(四元素)Quaterninos

      欧拉描述法。它使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里的旋转是针对世界坐标系说的，这意味着第一次的旋转不会影响第二、三次的转轴，简单的说，三角度系统无法表现任意轴的旋转，只要一开始旋转，物体本身就失去了任意轴的自主性，这也就导致了万向轴锁（Gimbal Lock）的问题。欧拉描述中针对x,y,z的旋转描述是世界坐标系下的值，所以当任意一轴旋转90°的时候会导致该轴同其他轴重合，此时旋转被重合的轴可能没有任何效果，这就是Gimbal Lock。

 

     四元素。还有一种是轴角的描述方法（即我一直以为的四元数的表示法），这种方法比欧拉描述要好，它避免了Gimbal Lock，它使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度，即(x,y,z,angle)，一般表示为(x,y,z,w)或者(v,w)。但这种描述法却不适合插值。

 

 w = cos(theta/2)
 x  = ax * sin(theta/2)
 y  = ay * sin(theta/2)
 z  = az * sin(theta/2)

 

      其中(ax,ay,az)表示轴的矢量，theta表示绕此轴的旋转角度，为什么是这样？和轴、角描述到底有什么不同？这是因为轴角描述的“四元组”并不是一个空间下的东西，首先（ax,ay,az）是一个3维坐标下的矢量，而theta则是级坐标下的角度，简单的将他们组合到一起并不能保证他们插值结果的稳定性，因为他们无法归一化，所以不能保证最终插值后得到的矢量长度（经过旋转变换后两点之间的距离）相等，而四元数在是在一个统一的4维空间中，方便归一化来插值，又能方便的得到轴、角这样用于3D图像的信息数据，所以用四元数再合适不过了。

      关于四元数的运算法则和推导这里有篇详细的文章介绍，重要的是一点，类似与Matrix的四元数的乘法是不可交换的，四元数的乘法的意义也类似于Matrix的乘法－可以将两个旋转合并，例如：

Q=Q1*Q2
 
表示Q的是先做Q2的旋转，再做Q1的旋转的结果，而多个四元数的旋转也是可以合并的，根据四元数乘法的定义，可以算出两个四元数做一次乘法需要16次乘法和加法，而3x3的矩阵则需要27运算，所以当有多次旋转操作时，使用四元数可以获得更高的计算效率。

 

 

关于插值

 

使用四元数的原因就是在于它非常适合插值，这是因为他是一个可以规格化的4维向量，最简单的插值算法就是线性插值，公式如：

q(t)=(1-t)q1+t q2

但这个结果是需要规格化的，否则q(t)的单位长度会发生变化，所以

q(t)=(1-t)q1+t q2 / || (1-t)q1+t q2 ||

如图:

 

尽管线性插值很有效，但不能以恒定的速率描述q1到q2之间的曲线，这也是其弊端，我们需要找到一种插值方法使得q1->q(t)之间的夹角θ是线性的，即θ(t)=(1-t)θ1+t*θ2，这样我们得到了球形线性插值函数q(t)，如下：

q(t)=q1 * sinθ(1-t)/sinθ + q2 * sinθt/sineθ

如果使用D3D，可以直接使用D3DXQuaternionSlerp函数就可以完成这个插值过程。

 

 

矩阵，欧拉角，四元素之间的相互转换

 

矩阵转四元素：

 FQuat::FQuat( const FMatrix& M )
 {
     //const MeReal *const t = (MeReal *) tm;
     FLOAT   s;

     // Check diagonal (trace)
     const FLOAT tr = M.M[0][0] + M.M[1][1] + M.M[2][2];

     if (tr > 0.0f)
     {
         FLOAT InvS = appInvSqrt(tr + 1.f);
         this->W = 0.5f * (1.f / InvS);
         s = 0.5f * InvS;

         this->X = (M.M[1][2] - M.M[2][1]) * s;
         this->Y = (M.M[2][0] - M.M[0][2]) * s;
         this->Z = (M.M[0][1] - M.M[1][0]) * s;
     }
     else
     {
         // diagonal is negative
         INT i = 0;

         if (M.M[1][1] > M.M[0][0])
             i = 1;

         if (M.M[2][2] > M.M[i][i])
             i = 2;

         static const INT nxt[3] = { 1, 2, 0 };
         const INT j = nxt[i];
         const INT k = nxt[j];

         s = M.M[i][i] - M.M[j][j] - M.M[k][k] + 1.0f;

         FLOAT InvS = appInvSqrt(s);

         FLOAT qt[4];
         qt[i] = 0.5f * (1.f / InvS);

         s = 0.5f * InvS;

         qt[3] = (M.M[j][k] - M.M[k][j]) * s;
         qt[j] = (M.M[i][j] + M.M[j][i]) * s;
         qt[k] = (M.M[i][k] + M.M[k][i]) * s;

         this->X = qt[0];
         this->Y = qt[1];
         this->Z = qt[2];
         this->W = qt[3];
     }
 }


 //
 // MSM: Fast float inverse square root using SSE.
 // Accurate to within 1 LSB.
 //
 FORCEINLINE FLOAT appInvSqrt( FLOAT F )
 {
     const FLOAT fThree = 3.0f;
     const FLOAT fOneHalf = 0.5f;
     FLOAT temp;

     __asm
     {
         movss   xmm1,[F]
         rsqrtss xmm0,xmm1           // 1/sqrt estimate (12 bits)

         // Newton-Raphson iteration (X1 = 0.5*X0*(3-(Y*X0)*X0))
         movss   xmm3,[fThree]
         movss   xmm2,xmm0
         mulss   xmm0,xmm1           // Y*X0
         mulss   xmm0,xmm2           // Y*X0*X0
         mulss   xmm2,[fOneHalf]     // 0.5*X0
         subss   xmm3,xmm0           // 3-Y*X0*X0
         mulss   xmm3,xmm2           // 0.5*X0*(3-Y*X0*X0)
         movss   [temp],xmm3
     }

     return temp;
 }

矩阵转欧拉角

 FRotator FMatrix::Rotator() const
 {
     const FVector       XAxis   = GetAxis( 0 );
     const FVector       YAxis   = GetAxis( 1 );
     const FVector       ZAxis   = GetAxis( 2 );

     FRotator    Rotator = FRotator(
                                     appRound(appAtan2( XAxis.Z, appSqrt(Square(XAxis.X)+Square(XAxis.Y)) ) * 32768.f / PI),
                                     appRound(appAtan2( XAxis.Y, XAxis.X ) * 32768.f / PI),
                                     0
                                 );

     const FVector       SYAxis  = FRotationMatrix( Rotator ).GetAxis(1);
     Rotator.Roll        = appRound(appAtan2( ZAxis | SYAxis, YAxis | SYAxis ) * 32768.f / PI);
     return Rotator;
 }

 FRotator( INT InPitch, INT InYaw, INT InRoll )
     :   Pitch(InPitch), Yaw(InYaw), Roll(InRoll) {}

 

欧拉角转矩阵

 

 

 FRotationMatrix(const FRotator& Rot)
     {
         const FLOAT SR  = GMath.SinTab(Rot.Roll);
         const FLOAT SP  = GMath.SinTab(Rot.Pitch);
         const FLOAT SY  = GMath.SinTab(Rot.Yaw);
         const FLOAT CR  = GMath.CosTab(Rot.Roll);
         const FLOAT CP  = GMath.CosTab(Rot.Pitch);
         const FLOAT CY  = GMath.CosTab(Rot.Yaw);

         M[0][0] = CP * CY;
         M[0][1] = CP * SY;
         M[0][2] = SP;
         M[0][3] = 0.f;

         M[1][0] = SR * SP * CY - CR * SY;
         M[1][1] = SR * SP * SY + CR * CY;
         M[1][2] = - SR * CP;
         M[1][3] = 0.f;

         M[2][0] = -( CR * SP * CY + SR * SY );
         M[2][1] = CY * SR - CR * SP * SY;
         M[2][2] = CR * CP;
         M[2][3] = 0.f;

         M[3][0] = 0.f;
         M[3][1] = 0.f;
         M[3][2] = 0.f;
         M[3][3] = 1.f;
     }
 class FGlobalMath
 {
 public:
     // Constants.
     enum {ANGLE_SHIFT   = 2};       // Bits to right-shift to get lookup value.
     enum {ANGLE_BITS    = 14};      // Number of valid bits in angles.
     enum {NUM_ANGLES    = 16384};   // Number of angles that are in lookup table.
     enum {ANGLE_MASK    =  (((1<<ANGLE_BITS)-1)<<(16-ANGLE_BITS))};

     // Basic math functions.
     FORCEINLINE FLOAT SinTab( int i ) const
     {
         return TrigFLOAT[((i>>ANGLE_SHIFT)&(NUM_ANGLES-1))];
     }
     FORCEINLINE FLOAT CosTab( int i ) const
     {
         return TrigFLOAT[(((i+16384)>>ANGLE_SHIFT)&(NUM_ANGLES-1))];
     }
     FLOAT SinFloat( FLOAT F ) const
     {
         return SinTab(appTrunc((F*65536.f)/(2.f*PI)));
     }
     FLOAT CosFloat( FLOAT F ) const
     {
         return CosTab(appTrunc((F*65536.f)/(2.f*PI)));
     }

     // Constructor.
     FGlobalMath();

 private:
     // Tables.
     FLOAT  TrigFLOAT        [NUM_ANGLES];
 };

 

四元素转矩阵

 

 FQuatRotationTranslationMatrix(const FQuat& Q, const FVector& Origin)
 {
     const FLOAT x2 = Q.X + Q.X;  const FLOAT y2 = Q.Y + Q.Y;  const FLOAT z2 = Q.Z + Q.Z;
     const FLOAT xx = Q.X * x2;   const FLOAT xy = Q.X * y2;   const FLOAT xz = Q.X * z2;
     const FLOAT yy = Q.Y * y2;   const FLOAT yz = Q.Y * z2;   const FLOAT zz = Q.Z * z2;
     const FLOAT wx = Q.W * x2;   const FLOAT wy = Q.W * y2;   const FLOAT wz = Q.W * z2;

     M[0][0] = 1.0f - (yy + zz); M[1][0] = xy - wz;              M[2][0] = xz + wy;          M[3][0] = Origin.X;
     M[0][1] = xy + wz;          M[1][1] = 1.0f - (xx + zz);     M[2][1] = yz - wx;          M[3][1] = Origin.Y;
     M[0][2] = xz - wy;          M[1][2] = yz + wx;              M[2][2] = 1.0f - (xx + yy); M[3][2] = Origin.Z;
     M[0][3] = 0.0f;             M[1][3] = 0.0f;                 M[2][3] = 0.0f;             M[3][3] = 1.0f;
 }

 QuatRotationTranslationMatrix( *this, FVector(0.f) )

 四元素与欧拉角的转换都是通过矩阵来做中间跳板！

 

从一个向量旋转到另一个向量的四元素：

 FQuat FQuatFindBetween(const FVector& vec1, const FVector& vec2)
 {
     const FVector cross = vec1 ^ vec2;
     const FLOAT crossMag = cross.Size();

     if(crossMag < KINDA_SMALL_NUMBER)
     {
         const FLOAT Dot = vec1 | vec2;
         if(Dot > -KINDA_SMALL_NUMBER)
         {
             return FQuat::Identity; // no rotation
         }
         else
         {
             // rotation by 180 degrees around a vector orthogonal to vec1 & vec2
             FVector Vec = vec1.SizeSquared() > vec2.SizeSquared() ? vec1 : vec2;
             Vec.Normalize();

             FVector AxisA, AxisB;
             Vec.FindBestAxisVectors(AxisA, AxisB);

             return FQuat(AxisA.X, AxisA.Y, AxisA.Z, 0.f); // (axis*sin(pi/2), cos(pi/2)) = (axis, 0)
         }
     }

     FLOAT angle = appAsin(crossMag);

     const FLOAT dot = vec1 | vec2;
     if(dot < 0.0f)
     {
         angle = PI - angle;
     }

     const FLOAT sinHalfAng = appSin(0.5f * angle);
     const FLOAT cosHalfAng = appCos(0.5f * angle);
     const FVector axis = cross / crossMag;

     return FQuat(
         sinHalfAng * axis.X,
         sinHalfAng * axis.Y,
         sinHalfAng * axis.Z,
         cosHalfAng );
 }

 #define KINDA_SMALL_NUMBER  (1.e-4)

 FQuat( FLOAT InX, FLOAT InY, FLOAT InZ, FLOAT InA )
     :   X(InX), Y(InY), Z(InZ), W(InA)
     {}