3D数学基础--3D中的方位与角位移(3)

• 前言：前面我们讲解了3D中描述方位和角位移的三种形式：矩阵，欧拉角，四元数。这节我们将比较它们之间的区别与选择建议，然后讲下它们之间的相互转换。

各方法比较

先来看下下面的总结表：

不同的方位表示方法适用于不同的情况，下面是一些选择的建议：

• 欧拉角最容易使用，如为世界中的物体指定方位，简化人机交互，包括直接的键盘输入方位，在代码中指定方位(如为渲染设定摄像机)，在调试中测试等，容易使用这个优点不应被忽视，不要以名义上的优化来牺牲易用性，除非你确定这个优化的确有效。
• 如果需要在不同坐标系之间转换向量，建议选择矩阵形式。当然也可以先用其他形式保存方位，再在需要的时候转换为矩阵形式就行了。
• 当需要大量保存方位数据(如动画)时，就使用欧拉角或四元数。欧拉角比四元数还少占25%的内存，但它在转换到矩阵时要稍微慢一些。如果动画数据需要嵌套坐标系之间的连接，四元数就比矩阵快。所以说如果内存不是很要紧，这种情况一般选择四元数要好。
• 平滑的插值只能用四元数完成。

各种形式之间的转换

• 从欧拉角转换到矩阵：欧拉角描述了一个旋转序列(按一定顺序分别绕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计算pitch
    float 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,bank
    if(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;
    }

• 从欧拉角转换到四元数
  这个和从欧拉角到矩阵类似，都是分别把欧拉角每个分量单独表示出来再叉乘连接得到。注意欧拉角有两种情况：惯性到物体的欧拉角和物体到惯性的欧拉角，不过它们对应的四元数互为共轭。先看从惯性坐标系到物体坐标系的变换：
  将每个分量单独表示成四元数，如下：
  
  再连接得到：
  
  惯性到物体与物体到惯性的四元数互为共轭，所以它的公式如下：
  

• 从四元数转换到欧拉角
  总体推导同样和以前类似，都是根据以有公式代入求解，下面直接贴出公式：
  

  从惯性–物体四元数转换到欧拉角代码实例：

//用全局变量保存输入输出
    float w, x, y, z;
    float h, p, b;

    //计算sin(pitch)
    float sp = -2.0f * (y * z + w * x);

    //检查万向锁并计算出欧拉角
    if(fabs(sp) > 0.9999f)
    {
        p = 1.570796f * sp;
        h = atan2(-x * z - w * y, 0.5f - y * y - z * z);
        b = 0.0f;
    }
    else
    {
        p = asin(sp);
        h = atan2(x * z - w * y, 0.5f - x * x - y * y);
        b = atan2(x * y - w * z, 0.5f - x * x - z * z);
    }

从物体–惯性四元数转换到欧拉角代码实例(只是将x,y,z值变负，因为共轭)：

//用全局变量保存输入输出
    float w, x, y, z;
    float h, p, b;

    //计算sin(pitch)
    float sp = -2.0f * (y * z - w * x);

    //检查万向锁并计算出欧拉角
    if(fabs(sp) > 0.9999f)
    {
        p = 1.570796f * sp;
        h = atan2(-x * z + w * y, 0.5f - y * y - z * z);
        b = 0.0f;
    }
    else
    {
        p = asin(sp);
        h = atan2(x * z + w * y, 0.5f - x * x - y * y);
        b = atan2(x * y + w * z, 0.5f - x * x - z * z);
    }