转-D3D中的四元数

http://blog.csdn.net/cppyin/article/details/6177742

 

在3D程序中,通常用quaternion来计算3D物体的旋转角度,与Matrix相比,quaternion更加高效,占用的储存空间更小,此外也更便于插值。在数学上,quaternion表示复数w+xi+yj+zk,其中i,j,k都是虚数单位:

i*i = j*j = k*k= -1

i*j = k, j*i = -k

可以把quaternion看做一个标量和一个3D向量的组合。实部w表示标量,虚部表示向量标记为V,或三个单独的分量(x,y,z)。所以quaternion可以记为[ w, V]或[ w,(x,y,x)]。对quaternion最大的误解在于认为w表示旋转角度,V表示旋转轴。正确的理解应该是w与旋转角度有关,v与旋转轴有关。例如,要表示以向量N为轴,轴旋α度,相对的quaternion应该是:

q = [ cos(α/ 2) , sin(α/ 2) N]

  =[ cos(α/ 2) , ( sina(α/ 2) Nx, sin(α/ 2)Ny, sin(α/ 2)Nz ) ]

为了计算方便,一般要求N为单位矢量。对quaternion来说使用四个值就能记录旋转,而不是Matrix所需的十六个值。为什么用quaternion来计算旋转很方便呢?先说过quaternion是一个复数,如果你还记得一点点复数的知识,那么应该知道复数乘法(叉乘)的几何意义实际上就是对复数进行旋转。对最简单的复数p= x + yi来说,和另一个复数q = ( conα,sinα)相乘,则表示把p沿逆时针方向旋转α:

p’ = pq

当然,x+yi的形式只能表示2D变换,对3D变换来说就需要使用 quaternion了,而且计算也要复杂一点。为了对3D空间中的一个点p(x,y,z)进行旋转,需要先把它转换为quaternion形式p = [0, ( x, y, z)],接下来前面讨论的内容,定义q = cos(α/ 2) , sin(α/ 2) N为旋转quaternion,这里N为单位矢量长度的旋转轴,α为旋转角度。那么旋转之后的点p’则为:

       p’ = qpq-1

 

1. 数学分析

1) 四元数是什么东西?

这个东西算盘、矩阵、复数是一类东西,即数学工具,数学家们创造了这个东西来解决一些数学问题。其实四元数是一种超复数,他不是只有一个虚数的复数,而是有三个虚数的复数。我们先回顾一下复数吧。

 

2) 虚数的来源

实数集中没有-1的平方根,因为没有哪个实数的平方等于-1,所以数学家们就创造它——虚数i,并且定义了i * i = -1。

所以我们可以计算sqrt(-4)了,sqrt(-4) = 2*i

 

3) 复数

定义:一个实数与一个虚数的和。 z = a + bi。

a叫实部,b叫虚部。

其中(a,b)为复平面上的点。这也是为什么3D图形运算能和四元数挂上关系了。

运算法则:

z1 = a + b*i

z2 = c + d*i

 

与标量相乘 k*z1 = k*a + K*b*i

复数相加 z1 + z2 = a + c + (b + d) * i

复数相乘 z1 * z2 = (a + b * i) * (c + d * i) = a * c + a * d * i + c * b * i - b * d = a*c - b*d + (a*d+b*c) * i

复数相除 z1 / z2 = (a + b*i) / (c + d*i)

z1的共轭z1* = a - b*i,作用是z×z*为实数

所以复数相除 z1 / z2 = (a + b*i)*(c - d*i) / (c2+d2) = ... = (a*c + b*d)/(a2+b2) + (b*c-a*d)*i)/(a2+b2)

z1的倒数 1/z = 1 / (a+b*i),同样可以转化为:a/(a2+b2) + b*i/(a2+b2)

z1的范数 |z| = sqrt(a2+b2) = sqrt(z×z*)

 

4) 超复数

q = q0 + q1*i + q2*j + q3*k

上面就是四元数的表示,其中q0为实部,而虚部<q1,q2,q3>正好组成了3D复平面中的向量。

简写就是q = q0 + qv,其中qv是向量<q1, q2 ,q3>。

超复数的运算法则与复数相同,这里就不再重复了,但要特别说明四元数的倒数q-1

q×q-1 = 1

两边同时乘以q*:

q×q-1×q* = q*

因为:q×q* = |q|2

所以q-1=q* / |q|2

可以注意到,如果q是一个单位四元数的话,那么q的倒数就等于q的共轭。

这个特性非常重要,因为在四元数旋转中要使用。

 

5) 四元数旋转

对于一个3D向量<x, y, z>,我们把他表现为四元数形式,则是:vq= <0, x, y, z>。

以及给定一个表示旋转轴和旋转角度的单位四元数q,那么向量vq绕指定轴旋转指定角度的结果四元数vq'满足如下:

右手坐标系:

  顺时针旋转:vq' = q*×vq×q

  逆时针旋转:vq' = q×vq×q*

左手坐标系:

  顺时针旋转:vq' = q×vq×q*

  逆时针旋转:vq' = q*×vq×q

其中,对于那个用于存储旋转轴和旋转角度的单位四元数q,他与旋转轴v = <x0, y0, z0>和角度theta关系如下:

q = Cos(theta/2) + Sin(theta/2) × v

 

 

 

2. 代码实现

1) 四元数结构体定义

 

[cpp]  view plain copy
  1. typedef struct QUAT_TYPE // 四元数  
  2. {  
  3.     union  
  4.     {  
  5.         double M[4];  
  6.         struct  
  7.         {  
  8.             double w, x, y, z;  
  9.         };  
  10.         struct  
  11.         {  
  12.             double q0;  
  13.             VECTOR3D qv;  
  14.         };  
  15.     };  
  16. } QUAT, *QUAT_PTR;  

 

 

 

2) 四元数常用操作函数实现

 

[cpp]  view plain copy
  1. void _CPPYIN_Math::QuatCreate(QUAT_PTR q, VECTOR3D_PTR v, double theta) // 创建用于旋转的四元数q, v必须为单位向量  
  2. {  
  3.     double theta_div_2 = (0.5)*theta;  
  4.     double sin_theta = sin(theta_div_2);  
  5.   
  6.     q->x = sin_theta * v->x;  
  7.     q->y = sin_theta * v->y;  
  8.     q->z = sin_theta * v->z;  
  9.     q->w = cos( theta_div_2 );  
  10. }  
  11.   
  12. void _CPPYIN_Math::QuatGetVectorAndTheta(QUAT_PTR q, VECTOR3D_PTR v, double *theta)  
  13. {  
  14.     *theta = acos(q->w);  
  15.     double sin_theta_inv = 1.0/sin(*theta);  
  16.   
  17.     v->x = q->x * sin_theta_inv;  
  18.     v->y = q->y * sin_theta_inv;  
  19.     v->z = q->z * sin_theta_inv;  
  20.   
  21.     *theta *= 2;  
  22. }  
  23.   
  24. void _CPPYIN_Math::QuatAdd(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum)  
  25. {  
  26.     qsum->x = q1->x + q2->x;  
  27.     qsum->y = q1->y + q2->y;  
  28.     qsum->z = q1->z + q2->z;  
  29.     qsum->w = q1->w + q2->w;  
  30. }  
  31.   
  32. void _CPPYIN_Math::QuatSub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff)  
  33. {  
  34.     qdiff->x = q1->x - q2->x;  
  35.     qdiff->y = q1->y - q2->y;  
  36.     qdiff->z = q1->z - q2->z;  
  37.     qdiff->w = q1->w - q2->w;  
  38. }  
  39.   
  40. void _CPPYIN_Math::QuatConjugate(QUAT_PTR q, QUAT_PTR qconj) // 求共轭  
  41. {  
  42.     qconj->x = -q->x;  
  43.     qconj->y = -q->y;  
  44.     qconj->z = -q->z;  
  45.     qconj->w = q->w;  
  46. }  
  47.   
  48. void _CPPYIN_Math::QuatScale(QUAT_PTR q, double scale, QUAT_PTR qs)  
  49. {  
  50.     qs->x = scale * q->x;  
  51.     qs->y = scale * q->y;  
  52.     qs->z = scale * q->z;  
  53.     qs->w = scale * q->w;  
  54. }  
  55.   
  56. double _CPPYIN_Math::QuatNorm(QUAT_PTR q)  
  57. {  
  58.     return sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);  
  59. }  
  60.   
  61. double _CPPYIN_Math::QuatNorm2(QUAT_PTR q)  
  62. {  
  63.     return q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z;  
  64. }  
  65.   
  66. void _CPPYIN_Math::QuatNormalize(QUAT_PTR q, QUAT_PTR qn)  
  67. {  
  68.     double qlength_inv = 1.0/(sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z));  
  69.   
  70.     qn->w = q->w * qlength_inv;  
  71.     qn->x = q->x * qlength_inv;  
  72.     qn->y = q->y * qlength_inv;  
  73.     qn->z = q->z * qlength_inv;  
  74. }  
  75.   
  76. void _CPPYIN_Math::QuatUnitInverse(QUAT_PTR q, QUAT_PTR qi) // 单位四元数的逆,等于求共轭  
  77. {  
  78.     qi->w =  q->w;  
  79.     qi->x = -q->x;  
  80.     qi->y = -q->y;  
  81.     qi->z = -q->z;  
  82. }  
  83.   
  84. void _CPPYIN_Math::QuatInverse(QUAT_PTR q, QUAT_PTR qi) // 非单位四元数的逆  
  85. {  
  86.     double norm2_inv = 1.0 / (q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);  
  87.   
  88.     qi->w =  q->w * norm2_inv;  
  89.     qi->x = -q->x * norm2_inv;  
  90.     qi->y = -q->y * norm2_inv;  
  91.     qi->z = -q->z * norm2_inv;  
  92. }  
  93.   
  94. void _CPPYIN_Math::QuatMul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod)  
  95. {  
  96.     double prd_0 = (q1->z - q1->y) * (q2->y - q2->z);  
  97.     double prd_1 = (q1->w + q1->x) * (q2->w + q2->x);  
  98.     double prd_2 = (q1->w - q1->x) * (q2->y + q2->z);  
  99.     double prd_3 = (q1->y + q1->z) * (q2->w - q2->x);  
  100.     double prd_4 = (q1->z - q1->x) * (q2->x - q2->y);  
  101.     double prd_5 = (q1->z + q1->x) * (q2->x + q2->y);  
  102.     double prd_6 = (q1->w + q1->y) * (q2->w - q2->z);  
  103.     double prd_7 = (q1->w - q1->y) * (q2->w + q2->z);  
  104.     double prd_8 = prd_5 + prd_6 + prd_7;  
  105.     double prd_9 = 0.5 * (prd_4 + prd_8);  
  106.   
  107.     qprod->w = prd_0 + prd_9 - prd_5;  
  108.     qprod->x = prd_1 + prd_9 - prd_8;  
  109.     qprod->y = prd_2 + prd_9 - prd_7;  
  110.     qprod->z = prd_3 + prd_9 - prd_6;  
  111. }  
  112.   
  113. void _CPPYIN_Math::QuatMul3(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod) // 三个四元数相乘,用于做旋转变换  
  114. {  
  115.     QUAT qtmp;  
  116.     QuatMul(q1, q2, &qtmp);  
  117.     QuatMul(&qtmp, q3, qprod);  
  118. }  

你可能感兴趣的:(3D)