前言：前面我们讲解了3D中描述方位和角位移的三种形式：矩阵，，欧拉角，四元数。这节我们将比较它们之间的区别与选择建议，然后讲下它们之间的相互转换。各方法比较
先来看下下面的总结表：
不同的方位表示方法适用于不同的情况，下面是一些选择的建议：
各种形式之间的转换
从欧拉角转换到矩阵：欧拉角描述了一个旋转序列(按一定顺序分别绕x,y,z轴旋转)，所以这个转换就是分别计算出每个旋转的矩阵再将它们连接成一个矩阵得到。公式如下(H,P,B分别是heading,pitch,bank)：
从矩阵转换到欧拉角： 先看下说明：
具体的计算其实就是利用给定的矩阵公式，一个个代入，分别解出h,p,b值。下面直接贴出公式： 矩阵公式：
计算p：
计算h：
计算b：
最后，从惯性到物体旋转矩阵中提取欧拉角实例代码：
//设矩阵保存在下面这些变量中float m11, m12, m13;float m21, m22, m23;float m31, m32, m33;//以弧度形式计算欧拉角并保存在以下变量中(360° == 2π弧度)float h, p, b;//从m23计算pitchfloat sp = -m23;if(sp <= -1.0f){p = -1.570796f; //p = -90°}else if(sp >= 1.0f){p = 1.570796f; //p = 90°}else{p = asin(sp);}//检查万向锁情况并计算heading,bankif(sp > 0.9999f){b = 0.0f;h = atan2f(-m31, m11);}else{h = atan2(m13, m33);b = atan2(m21, m22);}
从四元数转换到矩阵 首先看看书上讲解的转换原理，其实就是公式间的代入转化求解：
下面直接给出公式：
从矩阵转换到四元数 首先看四元数中w的计算：
通过使轨迹中三个元素中的两个为负，用类似方法求得其他三个元素：
上面已经计算出了四元数的四个分量，但从上可以看出，都是要通过开平方得到其值，而开平方得到的是正值，所以用这个公式没有选择正根还是负根的依据，但是，q和-q是代表相同的方位。下面来看下怎么对上面公式进行扩展：
从矩阵转换到四元数具体实例代码：
//输入矩阵float m11, m12, m13;float m21, m22, m23;float m31, m32, m33;//输入四元数float w, x, y, z;//检测w, x, y, z中的最大绝对值float fourWSquaredMinus1 = m11 + m22 + m33;float fourXSquaredMinus1 = m11 – m22 – m33;float fourYSquaredMinus1 = m22 – m11 – m33;float fourZSquaredMinus1 = m33 – m11 – m22;int biggestIndex = 0;float fourBiggestSquaredMinus1 = fourWSquaredMinus1;if(fourXSquaredMinus1 > fourBiggestSquaredMinus1){fourBiggestSquaredMinus1 = fourXSquaredMinus1;biggestIndex = 1;}if(fourYSquaredMinus1 > fourBiggestSquaredMinus1){fourBiggestSquaredMinus1 = fourYSquaredMinus1;biggestIndex = 2;}if(fourZSquaredMinus1 > fourBiggestSquaredMinus1){fourBiggestSquaredMinus1 = fourZSquaredMinus1;biggestIndex = 3;}//计算平方根和除数float biggestVal = sqrt(fourBiggestSquaredMinus1 + 1.0f) * 0.5f;float mult = 0.25f / biggestVal;//计算四元数的四个分量switch (biggestIndex) {case 0:w = biggestVal;x = (m23 – m32) * mult;y = (m31 – m13) * mult;z = (m12 – m21) * mult;break;case 1:x = biggestVal;w = (m23 – m32) * mult;y = (m12 + m21) * mult;z = (m31 + m13) * mult;break;case 2:y = biggestVal;w = (m31 – m13) * mult;x = (m12 + m21) * mult;z = (m23 + m32) * mult;break;case 3:z = biggestVal;w = (m12 – m21) * mult;x = (m31 + m13) * mult;y = (m23 + m32) * mult;break;}用积极的拼搏迎接雨后的彩虹,相信自己
