利用四元数表示空间向量的旋转及代码示例

1.四元数的定义

四元数(Quaternion)是带有一个实部和三个虚部的一种扩展复数，其表达式如下：

i,j,k之间的关系可以理解为相互正交的三个单位向量，于是四元数又可以表示为标量+向量的形式，即：

2.四元数相关运算

假设：

共轭：

逆：

模：

乘法：

3.四元数表示旋转

从二维平面复数的乘法中，我们知道表示某条射线的复数和表示旋转角度的复数其乘积为该射线按旋转角度旋转后的射线所表示的复数，那么四元数可以理解为其在三维空间的拓展应用。实际上，四元数是一种表示三维姿态的方法，其特点是紧凑、易于迭代、又不会出现奇异值。

接下来，推导两个四元数运算后为空间向量绕单位轴向量旋转后的结果对应的公式。

对于四元数：

将向量视为三维空间中的一个向量，令标量s为0，则可表示为三维空间中的一个向量。若想将其旋转，必须保证旋转前后的角度，向量长度，坐标系手性都不变。通过数学证明可得满足上述条件的旋转变换公式：

其中为单位四元数。

进一步地可通过数学推导求得：

需要注意的是，由上式可知取角会得到的共轭，即​​​​​​​，方便后续编程。

因此，三维空间向量绕单位轴向量旋转后的向量满足：

4.代码示例

代码参考：http://blog.sina.com.cn/s/blog_557d254601018dfv.html

/// Quaternion.cpp 

#include <math.h> 
#include <iostream.h> 
  
/// Define Data type 
typedef struct 
{ 
              double t; // real-component 
              double x; // x-component 
              double y; // y-component 
              double z; // z-component 
} quaternion; 
  

 Bill 注：Kakezan 在日语里是 “乘法”的意思
quaternion Kakezan(quaternion left, quaternion right) 
{ 
              quaternion ans; 
              double d1, d2, d3, d4; 
  
              d1 =  left.t * right.t; 
              d2 = -left.x * right.x; 
              d3 = -left.y * right.y; 
              d4 = -left.z * right.z; 
              ans.t = d1+ d2+ d3+ d4; 
  
              d1 =  left.t * right.x; 
              d2 =  right.t * left.x; 
              d3 =  left.y * right.z; 
              d4 = -left.z * right.y; 
              ans.x =  d1+ d2+ d3+ d4; 
  
              d1 =  left.t * right.y; 
              d2 =  right.t * left.y; 
              d3 =  left.z * right.x; 
              d4 = -left.x * right.z; 
              ans.y =  d1+ d2+ d3+ d4; 
  
              d1 =  left.t * right.z; 
              d2 =  right.t * left.z; 
              d3 =  left.x * right.y; 
              d4 = -left.y * right.x; 
              ans.z =  d1+ d2+ d3+ d4; 
              
              return ans; 
} 
  
 Make Rotational quaternion 
quaternion MakeRotationalQuaternion(double radian, double AxisX, double AxisY, double AxisZ) 
{ 
              quaternion ans; 
              double norm; 
              double ccc, sss; 
              
              ans.t = ans.x = ans.y = ans.z = 0.0; 
  
              norm = AxisX *  AxisX +  AxisY *  AxisY +  AxisZ *  AxisZ; 
              if(norm <= 0.0) return ans; 
  
              norm = 1.0 / sqrt(norm); 
              AxisX *= norm; 
              AxisY *= norm; 
              AxisZ *= norm; 
  
              ccc = cos(0.5 * radian); 
              sss = sin(0.5 * radian); 
  
              ans.t = ccc; 
              ans.x = sss * AxisX; 
              ans.y = sss * AxisY; 
              ans.z = sss * AxisZ; 
  
              return ans; 
} 
  
 Put XYZ into  quaternion 
quaternion PutXYZToQuaternion(double PosX, double PosY, double PosZ) 
{ 
              quaternion ans; 
  
              ans.t = 0.0; 
              ans.x = PosX; 
              ans.y = PosY; 
              ans.z = PosZ; 
  
              return ans; 
} 
  
/ main 
int main() 
{ 
              double px, py, pz; 
              double ax, ay, az, th; 
              quaternion ppp, qqq, rrr; 
  
              cout << "Point Position (x, y, z) " << endl; 
              cout << "  x = "; 
              cin >> px; 
              cout << "  y = "; 
              cin >> py; 
              cout << "  z = "; 
              cin >> pz; 
              ppp = PutXYZToQuaternion(px, py, pz); 
  
              while(1) { 
                            cout << "\nRotation Degree ? (Enter 0 to Quit) " << endl; 
                            cout << "  angle = "; 
                            cin >> th; 
                            if(th == 0.0) break; 
  
                            cout << "Rotation Axis Direction ? (x, y, z) " << endl; 
                            cout << "  x = "; 
                            cin >> ax; 
                            cout << "  y = "; 
                            cin >> ay; 
                            cout << "  z = "; 
                            cin >> az; 
  
  
                            th *= 3.1415926535897932384626433832795 / 180.0; /// Degree -> radian; 
  
                            qqq = MakeRotationalQuaternion(th, ax, ay, az); 
                            rrr = MakeRotationalQuaternion(-th, ax, ay, az); 
  
                            ppp = Kakezan(rrr, ppp); 
                            ppp = Kakezan(ppp, qqq); 
  
                            cout << "\nAnser X = " << ppp.x 
                                          <<  "\n      Y = " << ppp.y 
                                          <<  "\n      Z = " << ppp.z << endl; 
  
              } 
  
              return 0; 
}  

参考文献

1.Mathematics for 3D Game Programming and Computer Graphics

2.http://blog.sina.com.cn/s/blog_557d254601018dfv.html 

3.四元数和旋转(Quaternion & rotation) - 知乎
https://zhuanlan.zhihu.com/p/78987582

4.Quaternions and rotation sequences  a primer with applications to orbits, aerospace, and virtual reality.