四元数

四元数

对于四元数的概念也许大家不太熟悉,这里介绍下四元数概念,四元数、矩阵和欧拉角之间的关系,四元数球面插值的概念。这还是从当年写的本科毕业论文中摘录下来的,相当的没有技术含量。四元数的代码实现,见下载页面。

一. 四元数

1.1 四元数的概念

四元数是由爱尔兰数学家威廉•卢云•哈密顿在1843年发现的数学概念,在图形学中有重要的应用。在3D程序中,通常用四元数来计算3D物体的旋转角度,与矩阵相比,四元数更加高效,占用的储存空间更小,此外也更便于插值。 可以把四元数看做一个标量和一个3D向量的组合。实部w表示标量,虚部表示向量标记为V或三个单独的分量(x,y,z),则四元数可以记为[ w, V]或[ w,(x,y,z)]。正规化四元数可以表示为:在三维中,可以用四元数表示绕着某个轴的旋转,如下公式所示:

α表示旋转的角度,cos(βx), cos(βy) 和cos(βz)表示定位旋转轴的方向余弦

根据欧拉旋转定理,任何两个坐标系的相对定向,可以由一组四个数字来设定;其中三个数字是方向余弦,用来设定特征矢量(固定轴);第四个数字是绕着固定轴旋转的角值。这样四个数字的一组称为四元数。上面这段话阐述了四元数的原理:三维空间内所有的旋转都可以用四个数来表示在通过四元数方法来计算旋转,已经替代了方向余弦方法,这是因为它能减少所需的工作,和它能减小舍入误差。在电脑图形学里,四元数与四元数之间,简易执行插值的能力是很有价值的。

1.2 旋转矩阵

旋转矩阵(Rotation Matrix)与一个向量相乘,会改变向量的方向但不改变大小的效果。旋转可分为主动旋转与被动旋转。主动旋转是指将向量逆时针围绕旋转轴所做出的旋转。被动旋转是对坐标轴本身进行的逆时针旋转,它相当于主动旋转的逆操作。在三维空间中,旋转矩阵有一个等于单位1的实特征值。只用三个实数就可以指定一个 3 维旋转矩阵。生成旋转矩阵的一种简单方式是把它作为三个基本旋转的序列复合。关于右手笛卡尔坐标系的 x-, y- 和 z-轴的旋转分别叫做 roll, pitch 和 yaw 旋转。因为这些旋转被表达为关于一个轴的旋转,它们的生成元很容易表达。

1.2 欧拉角

图1 欧拉角表示

设定 xyz-轴为参考系的参考轴,称 xy-平面与 XY-平面的相交为交点线,用英文字母(N)代表。zxz 顺规的欧拉角可以静态地这样定义:(1)a 是 x-轴与交点线的夹角;(2)B是 z-轴与Z-轴的夹角;(3)r 是交点线与X-轴的夹角。 欧拉角来描述刚体在三维欧几里得空间的取向,如图1所示。对于任何一个参考系,一个刚体的取向,是依照顺序,从这参考系,做三个欧拉角的旋转而设定的。所以,刚体的取向可以用三个基本旋转矩阵来决定。换句话说,任何关于刚体旋转的旋转矩阵是由三个基本旋转矩阵复合而成的。对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现。参考系又称为实验室参考系,是静止不动的。而坐标系则固定于刚体,随着刚体的旋转而旋转。 设定刚体取向的旋转矩阵[R]是由三个基本旋转矩阵合成的,单独分开作用,每个矩阵各自代表绕着其转动轴的旋转。但是,当它们照次序相乘,最里面的(最右的) 矩阵代表绕着 z 轴的旋转,最外面的(最左的) 矩阵代表绕着 Z 轴的旋转,在中间的矩阵代表绕着交点线的旋转。

1.3    旋转矩阵、欧拉角和四元数之间的转化关系

旋转矩阵、欧拉角、四元数都可以表示三维空间模型的旋转,三者间也可以相互转化。正规化四元数q0+iq1+jq2+kq3可以用矩阵形式表示:  欧拉角也可转化成矩阵形式,欧拉角φ, θ, ψ表示的顺时针旋转可以用下面的矩阵形式表示:  通过组合欧拉旋转(欧拉角分别是, θ, ψ)的四元数表达示,可以得到欧拉角到四元数的转化关系,如下面的公式所示:  同时可以得到,四元数到欧拉角的转换关系,如下面的公式所示:

arctan和arcsin的结果是[-π/2,π/2],这并不能覆盖所有朝向,因此需要用atan2来代替arctan,从而产生所有可能的朝向,得到新的公式如下面所示:

1.4    线性插值与球面插值

在二维空间对向量进行线性插值,如图2所示,从而在向量间取得线性平滑的插值。得到等式P(t)=P0+(P1-P0)*t , 0 <= t <=1,通常还要将它标准化,转化为P(t)=P(t)/|P(t)|,这种方式的缺点便是在以 |P0| 为半径的圆的曲线轨迹上不是恒速变化的。

图2线性插值

四元数在球面插值(Spherical Linear Interpolation)中仍只用于描述旋转,它是关于单位四元数构成的球表面上的操作,并且这一插值过程适用于在由三维向量升维构成的单位四元数中进行,所以基本上一次的插值过程可以降维到三维空间解释。其次,当向量仅发生旋转时,要求其起点向量到终点向量位于同一个二维空间,所以,球面插值本质上是处于二维空间的弧形插值。并且我们总是使用单位四元数来描述角度插值,因此在二维空间,被插值向量的长度是单位长度的,起始和终止向量以及由其长度为半径构成的圆中向量所夹的弧形便是我们观察的插值轨迹。 如图3球面插值所示,将 P(t) 分解为 P0 以及 P1 向量上的两个分量,根据平行四边形法则,我们首先观察在 P0 上的分量的长度 a(t),为了得到这个长度,作了 P0 末端到 A 以及 P(t) 末端到 B 的辅助线,根据相似三角形,各个边,得到比例关系a(t)/|P0|=sin(ang*(1-f))|P(t)|/(sin(ang)*|P0|),如果使用标准化的向量,简化为:a(t)=sin(ang*(1-f))/sin(ang)。使用同样的手段来计算另一个分量 b(t)=sin(ang*f)/sin(ang),最后得到这样的一组公式:P(t)=a(t)*P0+b(t)*P1。

图3 球面插值

二. 参考

【1】      http://en.wikipedia.org/wiki/Euler_angles

【2】      http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

【3】      http://en.wikipedia.org/wiki/Quaternion
转载:http://www.twinklingstar.cn/2013/534/intro-to-quaternion/



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

 

矩阵转四元素:

[cpp] view plaincopy
  1. FQuat::FQuat( const FMatrix& M )  
  2. {  
  3.     //const MeReal *const t = (MeReal *) tm;  
  4.     FLOAT   s;  
  5.   
  6.     // Check diagonal (trace)  
  7.     const FLOAT tr = M.M[0][0] + M.M[1][1] + M.M[2][2];  
  8.   
  9.     if (tr > 0.0f)   
  10.     {  
  11.         FLOAT InvS = appInvSqrt(tr + 1.f);  
  12.         this->W = 0.5f * (1.f / InvS);  
  13.         s = 0.5f * InvS;  
  14.   
  15.         this->X = (M.M[1][2] - M.M[2][1]) * s;  
  16.         this->Y = (M.M[2][0] - M.M[0][2]) * s;  
  17.         this->Z = (M.M[0][1] - M.M[1][0]) * s;  
  18.     }   
  19.     else   
  20.     {  
  21.         // diagonal is negative  
  22.         INT i = 0;  
  23.   
  24.         if (M.M[1][1] > M.M[0][0])  
  25.             i = 1;  
  26.   
  27.         if (M.M[2][2] > M.M[i][i])  
  28.             i = 2;  
  29.   
  30.         static const INT nxt[3] = { 1, 2, 0 };  
  31.         const INT j = nxt[i];  
  32.         const INT k = nxt[j];  
  33.   
  34.         s = M.M[i][i] - M.M[j][j] - M.M[k][k] + 1.0f;  
  35.   
  36.         FLOAT InvS = appInvSqrt(s);  
  37.   
  38.         FLOAT qt[4];  
  39.         qt[i] = 0.5f * (1.f / InvS);  
  40.   
  41.         s = 0.5f * InvS;  
  42.   
  43.         qt[3] = (M.M[j][k] - M.M[k][j]) * s;  
  44.         qt[j] = (M.M[i][j] + M.M[j][i]) * s;  
  45.         qt[k] = (M.M[i][k] + M.M[k][i]) * s;  
  46.   
  47.         this->X = qt[0];  
  48.         this->Y = qt[1];  
  49.         this->Z = qt[2];  
  50.         this->W = qt[3];  
  51.     }  
  52. }  
  53.   
  54.   
  55. //  
  56. // MSM: Fast float inverse square root using SSE.  
  57. // Accurate to within 1 LSB.  
  58. //  
  59. FORCEINLINE FLOAT appInvSqrt( FLOAT F )  
  60. {  
  61.     const FLOAT fThree = 3.0f;  
  62.     const FLOAT fOneHalf = 0.5f;  
  63.     FLOAT temp;  
  64.   
  65.     __asm  
  66.     {  
  67.         movss   xmm1,[F]  
  68.         rsqrtss xmm0,xmm1           // 1/sqrt estimate (12 bits)  
  69.   
  70.         // Newton-Raphson iteration (X1 = 0.5*X0*(3-(Y*X0)*X0))  
  71.         movss   xmm3,[fThree]  
  72.         movss   xmm2,xmm0  
  73.         mulss   xmm0,xmm1           // Y*X0  
  74.         mulss   xmm0,xmm2           // Y*X0*X0  
  75.         mulss   xmm2,[fOneHalf]     // 0.5*X0  
  76.         subss   xmm3,xmm0           // 3-Y*X0*X0  
  77.         mulss   xmm3,xmm2           // 0.5*X0*(3-Y*X0*X0)  
  78.         movss   [temp],xmm3  
  79.     }  
  80.   
  81.     return temp;  
  82. }  

矩阵转欧拉角

[cpp]  view plain copy
  1. FRotator FMatrix::Rotator() const  
  2. {  
  3.     const FVector       XAxis   = GetAxis( 0 );  
  4.     const FVector       YAxis   = GetAxis( 1 );  
  5.     const FVector       ZAxis   = GetAxis( 2 );  
  6.   
  7.     FRotator    Rotator = FRotator(   
  8.                                     appRound(appAtan2( XAxis.Z, appSqrt(Square(XAxis.X)+Square(XAxis.Y)) ) * 32768.f / PI),   
  9.                                     appRound(appAtan2( XAxis.Y, XAxis.X ) * 32768.f / PI),   
  10.                                     0   
  11.                                 );  
  12.       
  13.     const FVector       SYAxis  = FRotationMatrix( Rotator ).GetAxis(1);  
  14.     Rotator.Roll        = appRound(appAtan2( ZAxis | SYAxis, YAxis | SYAxis ) * 32768.f / PI);  
  15.     return Rotator;  
  16. }  
  17.   
  18. FRotator( INT InPitch, INT InYaw, INT InRoll )  
  19.     :   Pitch(InPitch), Yaw(InYaw), Roll(InRoll) {}  

 

欧拉角转矩阵

 

 

[cpp]  view plain copy
  1. FRotationMatrix(const FRotator& Rot)  
  2.     {  
  3.         const FLOAT SR  = GMath.SinTab(Rot.Roll);  
  4.         const FLOAT SP  = GMath.SinTab(Rot.Pitch);  
  5.         const FLOAT SY  = GMath.SinTab(Rot.Yaw);  
  6.         const FLOAT CR  = GMath.CosTab(Rot.Roll);  
  7.         const FLOAT CP  = GMath.CosTab(Rot.Pitch);  
  8.         const FLOAT CY  = GMath.CosTab(Rot.Yaw);  
  9.   
  10.         M[0][0] = CP * CY;  
  11.         M[0][1] = CP * SY;  
  12.         M[0][2] = SP;  
  13.         M[0][3] = 0.f;  
  14.   
  15.         M[1][0] = SR * SP * CY - CR * SY;  
  16.         M[1][1] = SR * SP * SY + CR * CY;  
  17.         M[1][2] = - SR * CP;  
  18.         M[1][3] = 0.f;  
  19.   
  20.         M[2][0] = -( CR * SP * CY + SR * SY );  
  21.         M[2][1] = CY * SR - CR * SP * SY;  
  22.         M[2][2] = CR * CP;  
  23.         M[2][3] = 0.f;  
  24.   
  25.         M[3][0] = 0.f;  
  26.         M[3][1] = 0.f;  
  27.         M[3][2] = 0.f;  
  28.         M[3][3] = 1.f;  
  29.     }  
  30. class FGlobalMath  
  31. {  
  32. public:  
  33.     // Constants.  
  34.     enum {ANGLE_SHIFT   = 2};       // Bits to right-shift to get lookup value.  
  35.     enum {ANGLE_BITS    = 14};      // Number of valid bits in angles.  
  36.     enum {NUM_ANGLES    = 16384};   // Number of angles that are in lookup table.  
  37.     enum {ANGLE_MASK    =  (((1<
  38.   
  39.     // Basic math functions.  
  40.     FORCEINLINE FLOAT SinTab( int i ) const  
  41.     {  
  42.         return TrigFLOAT[((i>>ANGLE_SHIFT)&(NUM_ANGLES-1))];  
  43.     }  
  44.     FORCEINLINE FLOAT CosTab( int i ) const  
  45.     {  
  46.         return TrigFLOAT[(((i+16384)>>ANGLE_SHIFT)&(NUM_ANGLES-1))];  
  47.     }  
  48.     FLOAT SinFloat( FLOAT F ) const  
  49.     {  
  50.         return SinTab(appTrunc((F*65536.f)/(2.f*PI)));  
  51.     }  
  52.     FLOAT CosFloat( FLOAT F ) const  
  53.     {  
  54.         return CosTab(appTrunc((F*65536.f)/(2.f*PI)));  
  55.     }  
  56.   
  57.     // Constructor.  
  58.     FGlobalMath();  
  59.   
  60. private:  
  61.     // Tables.  
  62.     FLOAT  TrigFLOAT        [NUM_ANGLES];  
  63. };  

 

四元素转矩阵

 

[cpp]  view plain copy
  1. FQuatRotationTranslationMatrix(const FQuat& Q, const FVector& Origin)  
  2. {  
  3.     const FLOAT x2 = Q.X + Q.X;  const FLOAT y2 = Q.Y + Q.Y;  const FLOAT z2 = Q.Z + Q.Z;  
  4.     const FLOAT xx = Q.X * x2;   const FLOAT xy = Q.X * y2;   const FLOAT xz = Q.X * z2;  
  5.     const FLOAT yy = Q.Y * y2;   const FLOAT yz = Q.Y * z2;   const FLOAT zz = Q.Z * z2;  
  6.     const FLOAT wx = Q.W * x2;   const FLOAT wy = Q.W * y2;   const FLOAT wz = Q.W * z2;  
  7.   
  8.     M[0][0] = 1.0f - (yy + zz); M[1][0] = xy - wz;              M[2][0] = xz + wy;          M[3][0] = Origin.X;  
  9.     M[0][1] = xy + wz;          M[1][1] = 1.0f - (xx + zz);     M[2][1] = yz - wx;          M[3][1] = Origin.Y;  
  10.     M[0][2] = xz - wy;          M[1][2] = yz + wx;              M[2][2] = 1.0f - (xx + yy); M[3][2] = Origin.Z;  
  11.     M[0][3] = 0.0f;             M[1][3] = 0.0f;                 M[2][3] = 0.0f;             M[3][3] = 1.0f;  
  12. }  
  13.   
  14. QuatRotationTranslationMatrix( *this, FVector(0.f) )  

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

 

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

[cpp]  view plain copy
  1. FQuat FQuatFindBetween(const FVector& vec1, const FVector& vec2)  
  2. {  
  3.     const FVector cross = vec1 ^ vec2;  
  4.     const FLOAT crossMag = cross.Size();  
  5.   
  6.     if(crossMag < KINDA_SMALL_NUMBER)  
  7.     {  
  8.         const FLOAT Dot = vec1 | vec2;  
  9.         if(Dot > -KINDA_SMALL_NUMBER)  
  10.         {  
  11.             return FQuat::Identity; // no rotation  
  12.         }  
  13.         else  
  14.         {  
  15.             // rotation by 180 degrees around a vector orthogonal to vec1 & vec2  
  16.             FVector Vec = vec1.SizeSquared() > vec2.SizeSquared() ? vec1 : vec2;  
  17.             Vec.Normalize();  
  18.   
  19.             FVector AxisA, AxisB;  
  20.             Vec.FindBestAxisVectors(AxisA, AxisB);  
  21.   
  22.             return FQuat(AxisA.X, AxisA.Y, AxisA.Z, 0.f); // (axis*sin(pi/2), cos(pi/2)) = (axis, 0)  
  23.         }  
  24.     }  
  25.   
  26.     FLOAT angle = appAsin(crossMag);  
  27.   
  28.     const FLOAT dot = vec1 | vec2;  
  29.     if(dot < 0.0f)  
  30.     {  
  31.         angle = PI - angle;  
  32.     }  
  33.   
  34.     const FLOAT sinHalfAng = appSin(0.5f * angle);  
  35.     const FLOAT cosHalfAng = appCos(0.5f * angle);  
  36.     const FVector axis = cross / crossMag;  
  37.   
  38.     return FQuat(  
  39.         sinHalfAng * axis.X,  
  40.         sinHalfAng * axis.Y,  
  41.         sinHalfAng * axis.Z,  
  42.         cosHalfAng );  
  43. }  
  44.   
  45. #define KINDA_SMALL_NUMBER  (1.e-4)  
  46.   
  47. FQuat( FLOAT InX, FLOAT InY, FLOAT InZ, FLOAT InA )  
  48.     :   X(InX), Y(InY), Z(InZ), W(InA)  
  49.     {}  

 


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

 

矩阵转四元素:

[cpp] view plaincopy
  1. FQuat::FQuat( const FMatrix& M )  
  2. {  
  3.     //const MeReal *const t = (MeReal *) tm;  
  4.     FLOAT   s;  
  5.   
  6.     // Check diagonal (trace)  
  7.     const FLOAT tr = M.M[0][0] + M.M[1][1] + M.M[2][2];  
  8.   
  9.     if (tr > 0.0f)   
  10.     {  
  11.         FLOAT InvS = appInvSqrt(tr + 1.f);  
  12.         this->W = 0.5f * (1.f / InvS);  
  13.         s = 0.5f * InvS;  
  14.   
  15.         this->X = (M.M[1][2] - M.M[2][1]) * s;  
  16.         this->Y = (M.M[2][0] - M.M[0][2]) * s;  
  17.         this->Z = (M.M[0][1] - M.M[1][0]) * s;  
  18.     }   
  19.     else   
  20.     {  
  21.         // diagonal is negative  
  22.         INT i = 0;  
  23.   
  24.         if (M.M[1][1] > M.M[0][0])  
  25.             i = 1;  
  26.   
  27.         if (M.M[2][2] > M.M[i][i])  
  28.             i = 2;  
  29.   
  30.         static const INT nxt[3] = { 1, 2, 0 };  
  31.         const INT j = nxt[i];  
  32.         const INT k = nxt[j];  
  33.   
  34.         s = M.M[i][i] - M.M[j][j] - M.M[k][k] + 1.0f;  
  35.   
  36.         FLOAT InvS = appInvSqrt(s);  
  37.   
  38.         FLOAT qt[4];  
  39.         qt[i] = 0.5f * (1.f / InvS);  
  40.   
  41.         s = 0.5f * InvS;  
  42.   
  43.         qt[3] = (M.M[j][k] - M.M[k][j]) * s;  
  44.         qt[j] = (M.M[i][j] + M.M[j][i]) * s;  
  45.         qt[k] = (M.M[i][k] + M.M[k][i]) * s;  
  46.   
  47.         this->X = qt[0];  
  48.         this->Y = qt[1];  
  49.         this->Z = qt[2];  
  50.         this->W = qt[3];  
  51.     }  
  52. }  
  53.   
  54.   
  55. //  
  56. // MSM: Fast float inverse square root using SSE.  
  57. // Accurate to within 1 LSB.  
  58. //  
  59. FORCEINLINE FLOAT appInvSqrt( FLOAT F )  
  60. {  
  61.     const FLOAT fThree = 3.0f;  
  62.     const FLOAT fOneHalf = 0.5f;  
  63.     FLOAT temp;  
  64.   
  65.     __asm  
  66.     {  
  67.         movss   xmm1,[F]  
  68.         rsqrtss xmm0,xmm1           // 1/sqrt estimate (12 bits)  
  69.   
  70.         // Newton-Raphson iteration (X1 = 0.5*X0*(3-(Y*X0)*X0))  
  71.         movss   xmm3,[fThree]  
  72.         movss   xmm2,xmm0  
  73.         mulss   xmm0,xmm1           // Y*X0  
  74.         mulss   xmm0,xmm2           // Y*X0*X0  
  75.         mulss   xmm2,[fOneHalf]     // 0.5*X0  
  76.         subss   xmm3,xmm0           // 3-Y*X0*X0  
  77.         mulss   xmm3,xmm2           // 0.5*X0*(3-Y*X0*X0)  
  78.         movss   [temp],xmm3  
  79.     }  
  80.   
  81.     return temp;  
  82. }  

矩阵转欧拉角

[cpp]  view plain copy
  1. FRotator FMatrix::Rotator() const  
  2. {  
  3.     const FVector       XAxis   = GetAxis( 0 );  
  4.     const FVector       YAxis   = GetAxis( 1 );  
  5.     const FVector       ZAxis   = GetAxis( 2 );  
  6.   
  7.     FRotator    Rotator = FRotator(   
  8.                                     appRound(appAtan2( XAxis.Z, appSqrt(Square(XAxis.X)+Square(XAxis.Y)) ) * 32768.f / PI),   
  9.                                     appRound(appAtan2( XAxis.Y, XAxis.X ) * 32768.f / PI),   
  10.                                     0   
  11.                                 );  
  12.       
  13.     const FVector       SYAxis  = FRotationMatrix( Rotator ).GetAxis(1);  
  14.     Rotator.Roll        = appRound(appAtan2( ZAxis | SYAxis, YAxis | SYAxis ) * 32768.f / PI);  
  15.     return Rotator;  
  16. }  
  17.   
  18. FRotator( INT InPitch, INT InYaw, INT InRoll )  
  19.     :   Pitch(InPitch), Yaw(InYaw), Roll(InRoll) {}  

 

欧拉角转矩阵

 

 

[cpp]  view plain copy
  1. FRotationMatrix(const FRotator& Rot)  
  2.     {  
  3.         const FLOAT SR  = GMath.SinTab(Rot.Roll);  
  4.         const FLOAT SP  = GMath.SinTab(Rot.Pitch);  
  5.         const FLOAT SY  = GMath.SinTab(Rot.Yaw);  
  6.         const FLOAT CR  = GMath.CosTab(Rot.Roll);  
  7.         const FLOAT CP  = GMath.CosTab(Rot.Pitch);  
  8.         const FLOAT CY  = GMath.CosTab(Rot.Yaw);  
  9.   
  10.         M[0][0] = CP * CY;  
  11.         M[0][1] = CP * SY;  
  12.         M[0][2] = SP;  
  13.         M[0][3] = 0.f;  
  14.   
  15.         M[1][0] = SR * SP * CY - CR * SY;  
  16.         M[1][1] = SR * SP * SY + CR * CY;  
  17.         M[1][2] = - SR * CP;  
  18.         M[1][3] = 0.f;  
  19.   
  20.         M[2][0] = -( CR * SP * CY + SR * SY );  
  21.         M[2][1] = CY * SR - CR * SP * SY;  
  22.         M[2][2] = CR * CP;  
  23.         M[2][3] = 0.f;  
  24.   
  25.         M[3][0] = 0.f;  
  26.         M[3][1] = 0.f;  
  27.         M[3][2] = 0.f;  
  28.         M[3][3] = 1.f;  
  29.     }  
  30. class FGlobalMath  
  31. {  
  32. public:  
  33.     // Constants.  
  34.     enum {ANGLE_SHIFT   = 2};       // Bits to right-shift to get lookup value.  
  35.     enum {ANGLE_BITS    = 14};      // Number of valid bits in angles.  
  36.     enum {NUM_ANGLES    = 16384};   // Number of angles that are in lookup table.  
  37.     enum {ANGLE_MASK    =  (((1<
  38.   
  39.     // Basic math functions.  
  40.     FORCEINLINE FLOAT SinTab( int i ) const  
  41.     {  
  42.         return TrigFLOAT[((i>>ANGLE_SHIFT)&(NUM_ANGLES-1))];  
  43.     }  
  44.     FORCEINLINE FLOAT CosTab( int i ) const  
  45.     {  
  46.         return TrigFLOAT[(((i+16384)>>ANGLE_SHIFT)&(NUM_ANGLES-1))];  
  47.     }  
  48.     FLOAT SinFloat( FLOAT F ) const  
  49.     {  
  50.         return SinTab(appTrunc((F*65536.f)/(2.f*PI)));  
  51.     }  
  52.     FLOAT CosFloat( FLOAT F ) const  
  53.     {  
  54.         return CosTab(appTrunc((F*65536.f)/(2.f*PI)));  
  55.     }  
  56.   
  57.     // Constructor.  
  58.     FGlobalMath();  
  59.   
  60. private:  
  61.     // Tables.  
  62.     FLOAT  TrigFLOAT        [NUM_ANGLES];  
  63. };  

 

四元素转矩阵

 

[cpp]  view plain copy
  1. FQuatRotationTranslationMatrix(const FQuat& Q, const FVector& Origin)  
  2. {  
  3.     const FLOAT x2 = Q.X + Q.X;  const FLOAT y2 = Q.Y + Q.Y;  const FLOAT z2 = Q.Z + Q.Z;  
  4.     const FLOAT xx = Q.X * x2;   const FLOAT xy = Q.X * y2;   const FLOAT xz = Q.X * z2;  
  5.     const FLOAT yy = Q.Y * y2;   const FLOAT yz = Q.Y * z2;   const FLOAT zz = Q.Z * z2;  
  6.     const FLOAT wx = Q.W * x2;   const FLOAT wy = Q.W * y2;   const FLOAT wz = Q.W * z2;  
  7.   
  8.     M[0][0] = 1.0f - (yy + zz); M[1][0] = xy - wz;              M[2][0] = xz + wy;          M[3][0] = Origin.X;  
  9.     M[0][1] = xy + wz;          M[1][1] = 1.0f - (xx + zz);     M[2][1] = yz - wx;          M[3][1] = Origin.Y;  
  10.     M[0][2] = xz - wy;          M[1][2] = yz + wx;              M[2][2] = 1.0f - (xx + yy); M[3][2] = Origin.Z;  
  11.     M[0][3] = 0.0f;             M[1][3] = 0.0f;                 M[2][3] = 0.0f;             M[3][3] = 1.0f;  
  12. }  
  13.   
  14. QuatRotationTranslationMatrix( *this, FVector(0.f) )  

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

 

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

[cpp]  view plain copy
  1. FQuat FQuatFindBetween(const FVector& vec1, const FVector& vec2)  
  2. {  
  3.     const FVector cross = vec1 ^ vec2;  
  4.     const FLOAT crossMag = cross.Size();  
  5.   
  6.     if(crossMag < KINDA_SMALL_NUMBER)  
  7.     {  
  8.         const FLOAT Dot = vec1 | vec2;  
  9.         if(Dot > -KINDA_SMALL_NUMBER)  
  10.         {  
  11.             return FQuat::Identity; // no rotation  
  12.         }  
  13.         else  
  14.         {  
  15.             // rotation by 180 degrees around a vector orthogonal to vec1 & vec2  
  16.             FVector Vec = vec1.SizeSquared() > vec2.SizeSquared() ? vec1 : vec2;  
  17.             Vec.Normalize();  
  18.   
  19.             FVector AxisA, AxisB;  
  20.             Vec.FindBestAxisVectors(AxisA, AxisB);  
  21.   
  22.             return FQuat(AxisA.X, AxisA.Y, AxisA.Z, 0.f); // (axis*sin(pi/2), cos(pi/2)) = (axis, 0)  
  23.         }  
  24.     }  
  25.   
  26.     FLOAT angle = appAsin(crossMag);  
  27.   
  28.     const FLOAT dot = vec1 | vec2;  
  29.     if(dot < 0.0f)  
  30.     {  
  31.         angle = PI - angle;  
  32.     }  
  33.   
  34.     const FLOAT sinHalfAng = appSin(0.5f * angle);  
  35.     const FLOAT cosHalfAng = appCos(0.5f * angle);  
  36.     const FVector axis = cross / crossMag;  
  37.   
  38.     return FQuat(  
  39.         sinHalfAng * axis.X,  
  40.         sinHalfAng * axis.Y,  
  41.         sinHalfAng * axis.Z,  
  42.         cosHalfAng );  
  43. }  
  44.   
  45. #define KINDA_SMALL_NUMBER  (1.e-4)  
  46.   
  47. FQuat( FLOAT InX, FLOAT InY, FLOAT InZ, FLOAT InA )  
  48.     :   X(InX), Y(InY), Z(InZ), W(InA)  
  49.     {}  

 

你可能感兴趣的:(Unity3D,四元素,欧拉角,旋转矩阵)