旋转变换（三）四元数

1.简介

四元数是另一种描述三维旋转的方式，四元数使用4个分量来描述旋转，四元数的描述方式如下：

$$q = s + xi + yj + zk, (s, x, y, z \in \Bbb R）\quad i^2=j^2=k^2=ijk=-1$$

四元数的由来和复数很很大的关系，因此首先讨论一下关于复数的内容。

1.1 复数

复数可以定义如下：

$$z = a + bi \quad a,b\in \Bbb R \quad i^2 = -1$$
复数常用的基本运算如下：

复数中一个比较重要的概念是共轭复数，将复数的虚部取相反数，得到它的共轭复数：

$$z = a+bi \quad z^*=a-bi$$
复数的模，定义为：

复数还可以使用复平面来表示，复平面分为实轴和虚轴（类似于二维直角坐标系中的x轴和y轴），如下图所示：

当我们使用i去乘以一个复数时，当我们把得到的结果绘制在复平面上时，发现得到的位置正好是绕原点旋转90度的效果。

于是可以猜测，复数的乘法和旋转之间应该有某些关系。
我们可以通过定义一个复数

$$q= cos\theta+isin\theta$$
使用它作为一个旋转的因子，当与复数相乘时，得到：

写成矩阵的形式是：
$$\begin{bmatrix} a'\\ b'\\ \end{bmatrix}= \begin{bmatrix} cos\theta&-sin\theta\\ sin\theta&cos\theta\\ \end{bmatrix}* \begin{bmatrix} a\\ b\\ \end{bmatrix}$$
这个公式正好是二维的旋转公式，当把新的到的 $(a'+b'i)$绘制在复平面上时，得到的正好是原来的点( $a+bi$)旋转 $\theta$角之后的位置。

2. 四元数

既然使用复数的乘法可以描述二维的旋转，那么拓展一个维度是否能表示三维旋转呢，这个也正是四元数发明者William Hamilton最初的想法，也就是说使用

$$z = a + ib + jc \quad i^2=j^2=-1$$
但是很遗憾 “三维的复数”（这仅仅是我按概念杜撰的一个词，并不存在）的乘法并不是闭合的。也就是说有可能两个值相乘得到的结果并不是三维的复数。
William Hamilton经历了无数个日日夜夜，他绞尽脑汁也没想明白这个问题。终于有一天（1843年的一天），他意识到自己所需要的运算在三维空间中是不可能实现的，但在四维空间中是可以的，他是如此的兴奋，以至于把四元数的公式刻在了爱尔兰的一座桥上。

四元数可以写成下面的方式：

$$q = [s, \mathbf v]\quad s\in R \quad \mathbf v \in \Bbb R^3$$
或者写成
$$q = [s, xi+yj+zk] \quad s,x,y,z \in \Bbb R$$

2.1 四元数的运算

• 两四元数相加：
  A（a+bi+cj+dk） + B(e + fi + gj + hk) = C【 (a+e) + (b+f)i + (c+g)j + (d+h)k 】，实现代码：

      // Quat的成员_v[4]代表四元数的（x,y,z,w)
            inline Quat operator + (const Quat& rhs) const
            {
                return Quat(
                    _v[0] + rhs._v[0],
                    _v[1] + rhs._v[1],
                    _v[2] + rhs._v[2],
                    _v[3] + rhs._v[3]
                );
            }

• 两个四元数相减
  (sa,va) - (sb,vb) = (sa-sb,va-vb)

        inline Quat operator - (const Quat& rhs) const
        {
            return Quat
            (
                _v[0] - rhs._v[0],
                _v[1] - rhs._v[1],
                _v[2] - rhs._v[2],
                _v[3] - rhs._v[3]
            );
        }

• 两个四元数相乘

两个四元数相乘的规则和多项式乘法一样，
(a + i b + j c + k d)*(e + i f + j g + k h)
当有i，j，k参与时，规则如下：
i*i = j*j = k*k = -1
i*j = k,
j*i = -k
j*k = i,
k*j = -i
k*i = j,
i*k = -j
使用多项式乘法展开，可以得到：
a*e - b*f - c*g - d*h
+ i (b*e + a*f + c*h- d*g)
+ j (a*g - b*h+ c*e + d*f)
+ k (a*h + b*g - c*f + d*e)

实现代码：

        inline const Quat operator*(const Quat& rhs) const
        {
            return Quat( rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2] - rhs._v[2]*_v[1],
                 rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3] + rhs._v[2]*_v[0],
                 rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0] + rhs._v[2]*_v[3],
                 rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] - rhs._v[2]*_v[2] );
        }

• 两四元数相除
  四元数一般来说不定义除法，因为四元数的乘法运算并不满足交换律。一般有四元数的类定义除法是，其实定义的是$q1*q2^{-1}$，其中
  $q^{-1} = conj(q) / |q^2|$，为什么定义这么奇怪的表达式呢，其实是为了让$q*q^{-1} = 1$，这个结论很容易推导出来。conj(q)称为q的共轭表达式，
  con(q) = w - xi - yj -zk，只需要四元数向量部分取负即可
  实现如下：

        inline const Quat operator/(const Quat& denom) const
        {
            return ( (*this) * denom.inverse() );
        }
        /// Conjugate
        inline Quat conj () const
        {
             return Quat( -_v[0], -_v[1], -_v[2], _v[3] );
        }

        /// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)
        inline const Quat inverse () const
        {
             return conj() / length2();
         }
                 value_type length2() const
        {
            return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
        }

2.2 四元数的旋转

四元数在三维图形学领域的一个重要应用是用它来描述三维旋转，四元数从某种意义上来说是四维空间的旋转，难以想象，了解它的结论和使用场景更加重要。欧拉定理告诉我们任意三维旋转都可以使用一个旋转向量和旋转角度来描述。因此四元数往往是使用旋转轴和旋转角来构造的，构造它的方法如下：

---

2.2.1 绕向量u旋转角度$\theta$构造四元数

可以用下面的四元数来表示：

$${\vec {u}}=(u_{x},u_{y},u_{z})=u_{x}\mathbf {i} +u_{y}\mathbf {j} +u_{z}\mathbf {k}$$
$$\mathbf {q} =e^{{\frac {\theta }{2}}{(u_{x}\mathbf {i} +u_{y}\mathbf {j} +u_{z}\mathbf {k} )}}=\cos {\frac {\theta }{2}}+(u_{x}\mathbf {i} +u_{y}\mathbf {j} +u_{z}\mathbf {k} )\sin {\frac {\theta }{2}}$$
实现代码如下：

   void makeRotate(value_type angle, value_type x, value_type y, value_type z)
        {
            const value_type epsilon = 1e-7;
            value_type length = sqrt(x*x + y*y + z*z);
            if (length < epsilon)
            {
                *this = Quat();
                return;
            }

            value_type  inversenorm = 1.0 / length;
            value_type  coshalfangle = cos(0.5*angle);
            value_type  sinhalfangle = sin(0.5*angle);

            _v[0] = x * sinhalfangle * inversenorm;
            _v[1] = y * sinhalfangle * inversenorm;
            _v[2] = z * sinhalfangle * inversenorm;
            _v[3] = coshalfangle;
        }

---

2.2.2 从一个向量旋转到另一个向量构造四元数

按照最原始的想法，从一个向量旋转到另一个向量，那么旋转轴可以通过两个向量的叉乘得到，旋转角度可以通过两个向量间的夹角得到。（向量间的夹角的余弦可以通过两向量点乘去除以它们的模，再通过反余弦函数计算），得到旋转轴和旋转角度之后就转换成2.2.1中的情形了。
也就是说最初的代码如下：

void makeRotate(Vec3<value_type>& u,  Vec3<value_type>& v)
        {
            u.normalize();
            v.normalize();

            double costheta = u*v;
            double angle = acos(costheta);

            Vec3<value_type> w = u^v;
            w.normalize();

            makeRotate(angle, w.x(), w.y(), w.z());
        }

有一种特殊情况需要考虑：两向量共线（包括方向相同和方向相反，也就是夹角是0度和180度的情形）

void makeRotate(const Vec3<value_type>& from, const Vec3<value_type>& to)
        {
            const value_type epsilon = 1e-7;

            value_type length1 = from.length();
            value_type length2 = to.length();

            value_type cosangle = from*to / (length1*length2);

            if (fabs(cosangle - 1) < epsilon)
            {
                makeRotate(0.0, 0.0, 0.0, 1.0);
            }
            else if (fabs(cosangle + 1.0) < epsilon)
            {
                Vec3<value_type> tmp;
                if ((fabs(from.x())) < fabs(from.y()))
                {
                    if (fabs(from.x()) < fabs(from.z()))
                    {
                        tmp.set(1.0, 0.0, 0.0);
                    }
                    else
                    {
                        tmp.set(0.0, 0.0, 1.0);
                    }
                }
                else if (fabs(from.y()) < fabs(from.z()))
                {
                    tmp.set(0.0, 1.0, 0.0);
                }
                else
                {
                    tmp.set(0.0, 0.0, 1.0);
                }

                Vec3<value_type> fromd(from.x(), from.y(), from.z());
                Vec3<value_type> axis(fromd^tmp);
                axis.normalize();

                _v[0] = axis[0];
                _v[1] = axis[1];
                _v[2] = axis[2];
                _v[3] = 0.0;
            }
            else
            {
                Vec3<value_type> axis(from^to);
                value_type angle = acos(cosangle);
                makeRotate(angle, axis.x(), axis.y(), axis.z());
            }
        }

上述的代码改进了之前代码，但是在计算过程中使用了反三角函数（相对比较耗时），可以通过三角函数公式，简化，不需要调用反三角函数：

$$\begin{align} \sin\frac{\theta}{2} &= \sqrt{\frac{1 - \cos\theta}{2}} \\ \cos\frac{\theta}{2} &= \sqrt{\frac{1 + \cos\theta}{2}} \end{align}$$
代码可以修改为：

//省略部分相同的代码
            else
            {
                Vec3<value_type> axis(from^to);

                //替换成下面几行
                //value_type angle = acos(cosangle);
                //makeRotate(angle, axis.x(), axis.y(), axis.z());

                axis.normalize();
                value_type half_cos = sqrt(0.5*(1+cosangle));
                value_type half_sin = sqrt(0.5*(1-cosangle));

                _v[0] = axis[0] * half_sin;
                _v[1] = axis[1] * half_sin;
                _v[2] = axis[2] * half_sin;
                _v[3] = half_cos;

            }

这样修改之后，去掉了算法中复杂的三角函数运算，事实上还可以进一步改进计算过程，考虑到代码中多次的归一化（normalize）的操作，需要进行多次开方运算，为了简化，可以考虑：

$$||\boldsymbol{u} \times \boldsymbol{v}|| = |\boldsymbol{u}| . |\boldsymbol{v}| . |\sin\theta|$$

$$\sin\theta = 2 \sin\frac{\theta}{2} \cos\frac{\theta}{2}$$

同时有：

$$sqrt(a)sqrt(b) = sqrt(ab)$$

于是代码可以修改为：

            else
            {
                value_type normFromAndTo = sqrt(from.length2()*to.length2());
                value_type cos_theta = from * to / normFromAndTo;
                value_type half_cos = sqrt(0.5 * (1+cos_theta));
                value_type half_sin = sqrt(0.5 * (1-cos_theta));

                Vec3<value_type> axis = from^to / (normFromAndTo*2*half_sin * half_cos);

                _v[0] = axis[0]*half_sin;
                _v[1] = axis[1]*half_sin;
                _v[2] = axis[2]*half_sin;
                _v[3] = half_cos;

            }

注意到_v[0]到_v[3]中乘以half_sin，之前axis计算的分母中就有half_sin，也就是说这一项可以被化简掉，于是代码简化成：

        else
        {
            value_type normFromAndTo = sqrt(from.length2()*to.length2());
            value_type cos_theta = from * to / normFromAndTo;
            value_type half_cos = sqrt(0.5 * (1+cos_theta));
            Vec3<value_type> axis = from^to / (normFromAndTo*2 * half_cos);

                _v[0] = axis[0];
                _v[1] = axis[1];
                _v[2] = axis[2];
                _v[3] = half_cos;

        }

2.2.3 从四元数获取旋转矩阵和旋转角

这个过程是上面的反过程，根据之前描述的公式反算就可以，得到的公式是：

 //设四元数是 xi+yj+zk+w，那么旋转角度和旋转轴(a,b,c)是：
angle = 2 * acos(w)
a = x / sqrt(1-w*w)
b = y / sqrt(1-w*w)
c = z / sqrt(1-w*w)

推导过程如下：
有之前的公式可以知道： $w = cos(\theta/2)$可以得到 角度 $\theta = 2 * acos(w)$

$$x = a * sin(\theta/2) \\ y = b * sin(\theta/2) \\ z = c * sin(\theta/2)$$
先分析x这个等式，带入求出的 $\theta$角，得到：
$$a = x / sin(acos(w))$$ ，参考下图：

得到 sin(acosw) = sqrt(1-w*w)，同理可以推出其他的结论。
但是还需要考虑其他两个特殊情况：也就是共线的情形（角度 $\theta$是0度或者180度）

• 0度的情况：
  当时0度的时候，得到w=1，会导致计算公式中分母是0，除以0出现无穷大，因此需要单独讨论

• 180度的情况
  当180度是 w=0，可以通过计算得到
  a = x， b=y，c=z
  计算过程是正确的，因此这种情况不需要特殊的去分析。
  综合上面整体的描述，代码如下：

        void getRotate(value_type& angle, value_type& x, value_type& y, value_type& z) const 
        {
            Quat q1 = *this;
            if (_v[3] > 1)
                q1.normalize();

            angle = 2 * acos(q1.w());
            value_type s = sqrt(1 - q1.w()*q1.w());

            if (s < 1e-6)
            {
                x = q1.x();
                y = q1.y();
                z = q1.z();
            }
            else
            {
                x = q1.x() / s;
                y = q1.y() / s;
                z = q1.z() / s;
            }
        }

2.3 向量使用四元数进行旋转

这么辛苦写了四元数的类，为的就是使用它对顶点和向量进行旋转的操作，也就是说我们需要完成下面的函数实现：

        //Rotate a vector by this quaternion
        Vec3<value_type> operator* (const Vec3<value_type>& v);

四元数变换向量的算法如下：
1. 创建一个以v为虚部的纯虚的向量，（v.x + v.y + v.z + 0)
2. 左乘四元数 q 接着右乘四元数q的共轭四元数 q*
3. 计算得到的结果也是一个纯的四元数，它的虚部就是变换之后的向量v’
尽管这样做可以得到变换后的向量，如果计算过程完全按照四元数乘法法则去展开计算，计算量略大 ，可以使用下面的方式优化一下：

        Vec3<value_type> operator* (const Vec3<value_type>& v)
        {
            // nVidia SDK implementation
            Vec3<value_type> uv, uuv;
            Vec3<value_type> qvec(_v[0], _v[1], _v[2]);
            uv = qvec ^ v;
            uuv = qvec ^ uv;
            uv *= ( 2.0f * _v[3] );
            uuv *= 2.0f;
            return v + uv + uuv;
        }

2.4 四元数的插值

使用四元数来表示旋转，在插值时非常的方便和平滑，如果使用欧拉角来进行插值运算，除了会出现万向节死锁外，插值的效果显得十分的生硬。四元数的球面插值使用下面的公式：

其中：
- qm：插值的四元数
- qa： 插值四元数的第一个值（起点）
- qb：插值四元数的第二个值（终点）
- t： （0.0,1.0）之间的一个数
- $\theta$: qa和qb夹角的一半

实现如下：

void dslerp( value_type t, const Quat& from, const Quat& to )
{
    const double epsilon = 0.00001;
    double omega, cosomega, sinomega, scale_from, scale_to ;

    osg::Quat quatTo(to);
    // this is a dot product

    cosomega = from.asVec4() * to.asVec4();

    if ( cosomega <0.0 )
    {
        cosomega = -cosomega;
        quatTo = -to;
    }

    if( (1.0 - cosomega) > epsilon )
    {
        omega= acos(cosomega) ;  // 0 <= omega <= Pi (see man acos)
        sinomega = sin(omega) ;  // this sinomega should always be +ve so
        // could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
        scale_from = sin((1.0-t)*omega)/sinomega ;
        scale_to = sin(t*omega)/sinomega ;
    }
    else
    {
        /* --------------------------------------------------
           The ends of the vectors are very close
           we can use simple linear interpolation - no need
           to worry about the "spherical" interpolation
           -------------------------------------------------- */
        scale_from = 1.0 - t ;
        scale_to = t ;
    }

    *this = (from*scale_from) + (quatTo*scale_to);
}

参考文献：

1. Understanding Quaternions
2. 如何形象地理解四元数？知乎Yang Eninala的解答
3. Quaternion
4. Quaternion
5. Maths - Quaternions
6. Beautiful maths simplification: quaternion from two vectors
7. Rotating vector3 by a quaternion
8. quaternion vector product四元数变换向量算法的原理