天行健 君子當自強而不息

從四元數轉換到矩陣

為了將角位移從四元數轉換到矩陣形式，可以利用旋轉矩陣，它能計算繞任意軸的旋轉：

這個矩陣是用n和θ表示的，但四元數的分量是：

w = cos(θ/2)

x = n_x sin(θ/2)

y = n_y sin(θ/2)

z = n_z sin(θ/2)

讓我們看看能否將矩陣變形以代入w、x、y、z，矩陣的9個元素都必須這樣做。幸運的是，這個矩陣的結構非常好，一旦解出對角線上的一個元素，其他元素就能用同樣的方法求出。同樣，非對角線元素之間也是彼此類似的。

考慮矩陣對角線上的元素，我們將完整地解出m₁₁，m₂₂和m₃₃解法與之類似：

m₁₁ = n_x²(1 - cosθ) + cosθ

我們將從上式的變形開始，變形方法看起來像是在繞圈子，但你馬上就能理解這樣做的目的：

現在需要消去cosθ項，而代之以包含cosθ/2或sinθ/2的項，因為四元數的元素都是用它們表示的，像以前那樣，設α=θ/2，先用α寫出cos的倍角公式，再代入θ:

上式是正確的，但它和其他的標準形式不同，即:

m₁₁ = 1 - 2y² - 2z²

實際上，還有其他的形式存在。最著名的一個形式是m₁₁ = w² + x² - y²- z²，因為w² + x² + y² + z² = 1，所以這三種形式是等價的。現在回過頭來看看能不能直接導出其他標準形式，第一步，n是單位向量，n_x²+n_y^{2 +} n_z² = 1，則1 - n_x² = n_y^{2 +} n_z²。

m₁₁ = 1 - (1 - n_x²)(2sin²(θ/2))

       = 1 - (n_y^{2 +}n_z²)(2sin²(θ/2))

       = 1 - 2n_y²sin²(θ/2) - 2n_z²sin²(θ/2)

       = 1 - 2y² - 2z²

元素m₂₂和m₃₃可以用同樣的方法求得。

讓我們來看看非對角線元素，它們比對角線元素簡單一些，以m₁₂為例子：

m₁₂ = n_xn_y(1 - cosθ) + n_zsinθ

其他非對角線元素可用同樣的方法導出。

最后，給出從四元素構造的完整旋轉矩陣，如公式10.23所示：

從矩陣轉換到四元數

為了從旋轉矩陣中抽出相應的四元數，可以直接利用公式 10.23，檢查對角線元素的和（也稱作矩陣的軌跡）得到：

通過使軌跡中三個元素中的兩個為負，可以用類似的方法求得其他三個元素:

不幸的是，這種方法并不總是能正確工作，因為平方根的結果總是正值。（更加準確地說，沒有選擇正根還是負根的依據。）但是，q和-q代表相同的方位，我們能任意選擇用非負根作為4個分量中的一個并仍能得到正確的四元數，只是不能對四元數的所有4個數都用這種方法。

另一個技巧是檢查相對于對角線的對稱位置上元素的和與差：

那么應選用四種方法中的哪個呢？似乎最簡單的策略是總是先計算同一個分量，如w，然后再計算x、y、z。這伴隨著問題，如果w=0，除法就沒有定義；如果w非常小，將會出現數值不穩定。Shoemake建議先判斷w、x、y、z中哪個最大（不用做平方根運算），根據上面的表，用矩陣對角線計算該元素，再用它計算其他三個。

下面的代碼用一種非常直接的方式實現了這個方法。