关闭

(四元素)Quaterninos

10587人阅读 评论(0) 收藏 举报

      欧拉描述法。它使用最简单的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)。但这种描述法却不适合插值。

 

  1. w = cos(theta/2)  
  2. x  = ax * sin(theta/2)  
  3. y  = ay * sin(theta/2)  
  4. 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 ||

如图:

(四元素)Quaterninos - wenpeng__851124 - 悻然的博客

 

尽管线性插值很有效,但不能以恒定的速率描述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函数就可以完成这个插值过程。

 

 

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

 

矩阵转四元素:

  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. }  

矩阵转欧拉角

  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) {}  

 

欧拉角转矩阵

 

 

  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<<ANGLE_BITS)-1)<<(16-ANGLE_BITS))};  
  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. };  

 

四元素转矩阵

 

  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) )  

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

 

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

  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.     {}  

 

2
0
查看评论

(四元素)Quaterninos

(四元素)Quaterninos  2013-07-17 08:42 4046人阅读 评论(0) 收藏 举报       欧拉描述法。它使用最简单的x,y,z值来分别表示在x,y,z轴上的旋...
  • yihaiyiren
  • yihaiyiren
  • 2016-08-28 13:50
  • 976

四元素(Quaternion)与旋转

四元素性质及其与欧拉角、旋转矩阵的转换关系
  • u010297353
  • u010297353
  • 2015-10-11 18:22
  • 1742

四元素还是向量?

四元素是描述运动物体姿态的终极武器。它是由威廉·哈密顿(William Rowan Hamilton)爵士1843年在爱尔兰发现的。会中文的数学家哈密顿生于爱尔兰,他在十三岁前都受其叔父语言学家詹姆斯照顾。哈密顿很喜欢文学,在大学期间,他不但修读数学,还有修读经典文学。因此,他总共精通十二...
  • wangxiaojun911
  • wangxiaojun911
  • 2009-10-09 07:30
  • 3202

为什么使用四元素

转载:http://www.game798.com/html/2007-05/3689.htm 好吧,我必须承认到目前为止我还没有完全理解四元数,我一度把四元数理解为轴、角表示的4维向量,也就在下午我才从和同事的争辩中理解了四元数不完全是角、轴这么简单,为此写点心得给那些同我一样搞了2年3D游戏的...
  • cy_weiyi
  • cy_weiyi
  • 2015-05-29 11:17
  • 1395

四元素与欧拉角之间的转换

转自:http://www.cppblog.com/heath/archive/2009/12/13/103127.html          在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值...
  • yinxingtianxia
  • yinxingtianxia
  • 2016-12-05 14:56
  • 1166

四元素与欧拉角的转换及定义、代码

在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换,计算公式采用3D笛卡尔坐标系: 图1 3D Cartesian coordinate System (from wikipedia)  ...
  • dianmao0917
  • dianmao0917
  • 2013-06-24 21:49
  • 2530

C/C++ 四元素转欧拉角 欧拉角转四元素 四元素矩阵旋转 附源码

最近在玩陀螺仪,分享些心得供大家参考,以下是源码:// Conversion_Euler_to_Quaternion.cpp : 定义控制台应用程序的入口点。 #include &quot;stdafx.h&quot; #include &amp;lt;stdio.h&...
  • tiandiyixian20
  • tiandiyixian20
  • 2017-12-08 15:12
  • 83

四元素与旋转矩阵

四元数与九轴
  • haoliliang88
  • haoliliang88
  • 2016-03-08 16:04
  • 2495

四元素姿态解算 ---地磁计融合解读

四元数姿态解算中的地磁计融合解读     笔者最近在做四轴,涉及到地磁计的融合算法,网上大多数是x-IMU的融合代码,但是这段代码对于地磁计的融合说明没有做过多的解释,网上没有相关讨论,仅在阿莫论坛看到一篇相关的代码解释,里面有关于地磁计融合部分的解说,个人觉得说的不是很清...
  • zsn15702422216
  • zsn15702422216
  • 2016-08-16 18:20
  • 3111

关于欧拉角与四元素的关系

1.欧拉角  在四元数出现之前先看下欧拉角:  对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现。为了后面的角度不混乱,我们要先区分参考系和坐标系的概念。  参考系即为大地参考系,是静止不动的。而坐标系则固定于四轴飞行器,随着四轴飞行器的旋...
  • u014035838
  • u014035838
  • 2015-04-29 10:49
  • 1209
    个人资料
    • 访问:758996次
    • 积分:8889
    • 等级:
    • 排名:第2557名
    • 原创:99篇
    • 转载:394篇
    • 译文:2篇
    • 评论:64条
    最新评论