空间三维坐标变换(转)-四元数-RowPitchYaw角互换

 

1、空间三维坐标系的平移(矢量加法获取)

 

解题思路：根据适量的加法即可求解

 =  + 

2、空间三维坐标系的旋转（旋转矩阵）

 

 

 

 关键是：姿态矩阵（或称之为“旋转矩阵”）

 

3、坐标系一般变换（平移+旋转）

 已知P点在坐标系B中的坐标为，B坐标系相对A坐标系任意旋转+平移，旋转矩阵为，移动向量为，求解P点在A坐标系中的坐标。

 

 关键需要获得： 旋转矩阵（姿态矩阵）和平移向量

 

4、空间三维坐标系的旋转加平移，齐次变换

 旋转使用矩阵乘法，平移使用矩阵加法计算，为了将旋转和平移统一成矩阵乘法的形式，将变量升维成齐次变换形式。

 

齐次变换矩阵的三个物理含义

4.1、坐标变换

作为两个坐标系之间的坐标变换矩阵。
已知P点在B坐标轴的坐标，B坐标系以 A 坐标系为基础的相对位置和姿态，求解P点在A坐标轴的坐标。

 

4.2、坐标系的描述

描述坐标系B相对A的位置和姿态；
以A为基坐标轴，B坐标系原点相对A坐标系原点的位置(x,y,z)；姿态(Bx在A轴三个方向投影的余弦角度；同理By；Bz等9个角度)；
因为在1.1中，单独考虑旋转，定义就是B轴x坐标与A坐标系三个轴的夹角；B坐标系的y，z轴同理。  

 

4.3、运动算子

作为运动算子来表示刚体的运动情况。

5、齐次变换矩阵乘法的物理意义

 

5.1、使用齐次矩阵相乘，可以实现：坐标系描述之间的转换。

已知C坐标系相对B坐标系的齐次矩阵；和B坐标系相对A坐标系的齐次矩阵；求取 C 坐标系相对 A 坐标系的齐次矩阵。

 

 

5.2、相对固定坐标系（A看作固定坐标系）先后发生的若干运动的合成(从右向左乘)

 

 固定坐标系A；初始状态ABC坐标系重合；先将C坐标系相对B运动；再将BC相对A运动(保证BC间相对位置不变)。先运动的齐次矩阵在乘号右边。

 

 

5.3、相对运动坐标系（）先后发生的若干运动的合成(从左向右乘)

 固定坐标系A；初始状态ABC坐标系重合；先将BC坐标系相对A运动；再将C相对B运动(保证BC间相对位置不变)。先运动的齐次矩阵在乘号左边。

 

6、齐次矩阵的逆变换

 　　齐次矩阵的逆物理含义为：齐次矩阵代表B坐标系相对A坐标系的位置和姿态；逆代表A坐标系相对B坐标系的位置和姿态。

　　齐次坐标系M表示：由B中坐标转化为A坐标系坐标的目的。

　　其次坐标系逆矩阵表示：由A坐标系坐标转化为B坐标系的坐标目的。

 

 

 

 

7、RPY角和欧拉角

问题：坐标系的一般变换=旋转+平移；需要 9 + 3 个变量来求解。一个三维的旋转需要使用 9 个角度的余弦求解；实际这9个余弦值只有3个是独立的，过于复杂。因此引入RPY和欧拉角的概念。

已知初始状态下，A、B两个坐标系重合，A坐标系固定不动，B坐标系进行旋转，存在两种情况：

（1）RPY 角(固定轴)：B坐标系绕A坐标的x，y，z三个固定轴进行旋转；固定坐标系的连续运动。

（2）欧拉 角(运动轴)：B坐标系绕B坐标的z轴旋转得到坐标系，然后绕坐标系的y轴旋转得到坐标系，最后然坐标系的x轴旋转得到最终的坐标系B。

 

　　 描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。

　　第一种是绕固定（参考）坐标轴旋转（固定旋转），称之为“RPY角描述”。

　　第二种是绕自身坐标轴旋转（相对旋转），称之为“欧拉角描述”。

　　其中：RPY角度，由于是绕固定坐标系旋转，则旋转矩阵为（$\mathrm{c\alpha is\; shorthand\; for cos\alpha , s\alpha is\; shorthand\; for sin\alpha ,and\; so\; on.）}$

 

 　　其中： 欧拉角描述，假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转$\mathrm{\alpha ，然后绕Y轴旋转\beta ，最后绕X轴旋转\gamma ，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转，则旋转矩阵为}$

　　

 

　　可以发现这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转$(\gamma ,\beta ,\alpha )和绕自身坐标轴Z-Y-X旋转(\alpha ,\beta ,\gamma )的最终结果一样，只是描述$

$的方法有差别而已。$

$绕定轴\; X-Y-Z\; 旋转（RPY\; 角）\; 等价于\; 绕动轴\; Z-Y-X\; 旋转（Euler\; 角）$

 

 

 

 

 

7.1、RPY角（Axis-Angle固定轴的角度，Roll-Pitch-Yaw旋转角）描述

 

　　RPY角就是：按照右手坐标系目视Z轴负半轴，Y轴正上方，X轴在右侧，如下图：

　　其中：Roll（横滚角度-围绕Z轴）-Pitch（俯仰角度-围绕X轴）-Yaw（偏航角度-围绕Y轴）旋转角

 

RPY 角(固定轴)：B坐标系绕A坐标的x，y，z三个轴进行旋转；固定坐标系的连续运动。计算旋转矩阵需要使用2个知识点：

a、矩阵依旧采用第二章中旋转矩阵的形式，只是角度转动比较特殊。

b、5.2节知识，绕固定轴连续转动，采用从右向左的方向乘。

 

7.1.1、特殊情况，每次绕轴旋转90度角

 

 

 根据旋转矩阵求解 三个角度；&lt;/p&gt;&lt;p&gt;&lt;img src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924125657061-902086347.png" alt="image" loading="lazy" data-mce-src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924125657061-902086347.png"&gt;&lt;/p&gt;&lt;p&gt;&amp;nbsp;&lt;/p&gt;&lt;h4 id="7.2%E3%80%81%E6%AC%A7%E6%8B%89%E8%A7%92"&gt;&lt;span style="font-size: 18pt;" data-mce-style="font-size: 18pt;"&gt;7.2、欧拉角（Euler角，）</p><p>　　欧拉角(运动轴)：B坐标系绕B坐标的z轴旋转得到坐标系<img src="https://latex.csdn.net/eq?B_%7BZ%7D" alt="B_{Z}" class="mathcode" data-mce-src="https://latex.csdn.net/eq?B_%7BZ%7D">，然后绕坐标系<img src="https://latex.csdn.net/eq?B_%7BZ%7D" alt="B_{Z}" class="mathcode" data-mce-src="https://latex.csdn.net/eq?B_%7BZ%7D">的y轴旋转得到坐标系<img src="https://latex.csdn.net/eq?B_%7BZY%7D" alt="B_{ZY}" class="mathcode" data-mce-src="https://latex.csdn.net/eq?B_%7BZY%7D">，最后然坐标系<img src="https://latex.csdn.net/eq?B_%7BZY%7D" alt="B_{ZY}" class="mathcode" data-mce-src="https://latex.csdn.net/eq?B_%7BZY%7D">的x轴旋转得到最终的坐标系 B。计算旋转矩阵需要使用2个知识点：</p><p>a、矩阵依旧采用第二章中旋转矩阵的形式，只是角度转动比较特殊。</p><p>b、5.2节知识，<strong><span style="color: #ff0000;" data-mce-style="color: #ff0000;">绕运动轴连续转动，采用从左向右的方向乘。

 

https://pic2.zhimg.com/v2-35da80e428ca5750491ffd2770e187e1_b.webp

 

7.1.1、特殊情况，每次绕轴旋转90度角

 

  RPY角和欧拉角的旋转矩阵相同；但是角度的定义和运动方式是不同的。

 

 

8、旋转变换通式：旋转向量，向量长度为旋转角度。

旋转变量通式需要证明的2个问题：

 

8.1、正解，已知三维的单位向量k和角度，求解旋转矩阵&lt;img src="https://latex.csdn.net/eq?_%7BB%7D%5E%7BA%7D%5Ctextrm%7BR%7D" alt="_{B}^{A}\textrm{R}" class="mathcode" data-mce-src="https://latex.csdn.net/eq?_%7BB%7D%5E%7BA%7D%5Ctextrm%7BR%7D"&gt;&lt;/h4&gt;&lt;p&gt;&amp;nbsp; &amp;nbsp; &amp;nbsp;已知坐标系A，经过三维旋转得到坐标系B；三维旋转可以理解为以&lt;span style="color: #ff0000; background-color: #ffff00;" data-mce-style="color: #ff0000; background-color: #ffff00;"&gt;A坐标系为原点的某个<strong><span style="color: #ff0000; background-color: #ffff00;" data-mce-style="color: #ff0000; background-color: #ffff00;">单位向量k，将坐标系A绕向量k旋转&lt;span style="background-color: #ffff00; color: #ff0000;"&gt;角</strong>度得到坐标系B。</p><p>　　<span style="color: #ff0000; font-size: 14pt;" data-mce-style="color: #ff0000; font-size: 14pt;"><strong><span style="background-color: #ffff00;" data-mce-style="background-color: #ffff00;">注意：旋转轴k向量是在坐标系A中。将坐标系A围绕K进行旋转V角度后，才是坐标系B。

 

 

8.1.1、获取三个分量

 

 

 

 

理解要点：

将 拆分为三个旋转矩阵相乘的形式，将1次旋转变换拆分成&lt;strong&gt;&lt;span style="background-color: #ffff00;" data-mce-style="background-color: #ffff00;"&gt;3次旋转变换。</p><p><img src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924132217019-38745746.png" alt="image" loading="lazy" data-mce-src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924132217019-38745746.png"></p><p>&nbsp;</p><p>以上，就是将（k,thet）转化为一般变换矩阵的过程。</p><p>&nbsp;</p><p><img src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924132303029-477709636.png" alt="image" loading="lazy" data-mce-src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924132303029-477709636.png"></p><p>&nbsp;</p><p><img src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924132416416-79270638.png" alt="image" loading="lazy" data-mce-src="https://img2024.cnblogs.com/blog/700384/202509/700384-20250924132416416-79270638.png"></p><p>&nbsp;</p><p><strong><span style="font-size: 18px;" data-mce-style="font-size: 18px;">9.四元数：

四元数（Quaternion）既是紧凑的，又没有奇异性，缺点是不够直观，运算比较复杂。

参考：https://zhuanlan.zhihu.com/p/93563218

  四元数本质是一种具有三个虚数的高阶复数，四元数集英文表示为 ，对于任意四元数&amp;amp;amp;nbsp;&amp;amp;lt;span class="math inline"&amp;amp;gt;，可表示为如下形式：

　　

 

 

　　将四元数的实部和虚部分开表示，虚部用向量表示：

 

 

 

 

 四元数的运算：

 

 

 

 

 

 

 

 

 

 

10 四元数和RPY角度互换

 　　　　绕坐标轴的多次旋转（这里是RPY角旋转类型）可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为$\stackrel{}{}$

　　

　　

 

 　　四元数存储了旋转轴（Axis-Angle）和旋转角的信息，它能方便的描述刚体绕任意轴的旋转。

 （1）四元数转换为旋转矩阵：

　　已知四元数为：q=(x,y,z,w)

　　则旋转矩阵R：

 

 （2）旋转矩阵R转化为四元数

 　　对应的四元数为：

 

 

11. 四元数与Eular 欧拉角旋转

　　将四元数转换为欧拉角可以参考下面的代码。需要注意欧拉角有12种旋转次序，而上面推导的公式是按照Z-Y-X顺序进行的，所以有时会在网上看到不同的转换公式（因为对应着不同的旋转次序），在使用时一定要注意旋转次序是什么。

　　此处按照次序：将Z-Y-X欧拉角（（或RPY角：绕固定坐标系的X-Y-Z依次旋转$\mathrm{\alpha ,\beta ,\gamma 角）}$）转化为四元数：

 根据上面的公式可以求出逆解，即由四元数$\mathrm{q=(q0,q1,q2,q3)或q=(w,x,y,z)到欧拉角的转换为：}$

 由于arctan和arcsin的取值范围在$\frac{-\pi 2和\pi 2之间，只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数}{}$

 

 

// COMPILE: g++ -o quat2EulerTest quat2EulerTest.cpp 
#include <iostream>
#include <cmath> 

using namespace std;

///////////////////////////////
// Quaternion struct
// Simple incomplete quaternion struct for demo purpose
///////////////////////////////
struct Quaternion{
  Quaternion():x(0), y(0), z(0), w(1){};
  Quaternion(double x, double y, double z, double w):x(x), y(y), z(z), w(w){};
 
  void normalize(){
    double norm = std::sqrt(x*x + y*y + z*z + w*w);
    x /= norm;
    y /= norm;
    z /= norm;
    w /= norm;
  }
  
  double norm(){
    return std::sqrt(x*x + y*y + z*z + w*w);
  }

  double x;
  double y;
  double z;
  double w;  
  
};

///////////////////////////////
// Quaternion to Euler
///////////////////////////////
enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};

void twoaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
  res[0] = atan2( r11, r12 );
  res[1] = acos ( r21 );
  res[2] = atan2( r31, r32 );
}
    
void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
  res[0] = atan2( r31, r32 );
  res[1] = asin ( r21 );
  res[2] = atan2( r11, r12 );
}

void quaternion2Euler(const Quaternion& q, double res[], RotSeq rotSeq)
{
    switch(rotSeq){
    case zyx:
      threeaxisrot( 2*(q.x*q.y + q.w*q.z),
                     q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                    -2*(q.x*q.z - q.w*q.y),
                     2*(q.y*q.z + q.w*q.x),
                     q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                     res);
      break;
    
    case zyz:
      twoaxisrot( 2*(q.y*q.z - q.w*q.x),
                   2*(q.x*q.z + q.w*q.y),
                   q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                   2*(q.y*q.z + q.w*q.x),
                  -2*(q.x*q.z - q.w*q.y),
                  res);
      break;
                
    case zxy:
      threeaxisrot( -2*(q.x*q.y - q.w*q.z),
                      q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                      2*(q.y*q.z + q.w*q.x),
                     -2*(q.x*q.z - q.w*q.y),
                      q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                      res);
      break;

    case zxz:
      twoaxisrot( 2*(q.x*q.z + q.w*q.y),
                  -2*(q.y*q.z - q.w*q.x),
                   q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                   2*(q.x*q.z - q.w*q.y),
                   2*(q.y*q.z + q.w*q.x),
                   res);
      break;

    case yxz:
      threeaxisrot( 2*(q.x*q.z + q.w*q.y),
                     q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                    -2*(q.y*q.z - q.w*q.x),
                     2*(q.x*q.y + q.w*q.z),
                     q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                     res);
      break;

    case yxy:
      twoaxisrot( 2*(q.x*q.y - q.w*q.z),
                   2*(q.y*q.z + q.w*q.x),
                   q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                   2*(q.x*q.y + q.w*q.z),
                  -2*(q.y*q.z - q.w*q.x),
                  res);
      break;
      
    case yzx:
      threeaxisrot( -2*(q.x*q.z - q.w*q.y),
                      q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                      2*(q.x*q.y + q.w*q.z),
                     -2*(q.y*q.z - q.w*q.x),
                      q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                      res);
      break;

    case yzy:
      twoaxisrot( 2*(q.y*q.z + q.w*q.x),
                  -2*(q.x*q.y - q.w*q.z),
                   q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                   2*(q.y*q.z - q.w*q.x),
                   2*(q.x*q.y + q.w*q.z),
                   res);
      break;

    case xyz:
      threeaxisrot( -2*(q.y*q.z - q.w*q.x),
                    q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                    2*(q.x*q.z + q.w*q.y),
                   -2*(q.x*q.y - q.w*q.z),
                    q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                    res);
      break;
        
    case xyx:
      twoaxisrot( 2*(q.x*q.y + q.w*q.z),
                  -2*(q.x*q.z - q.w*q.y),
                   q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                   2*(q.x*q.y - q.w*q.z),
                   2*(q.x*q.z + q.w*q.y),
                   res);
      break;
        
    case xzy:
      threeaxisrot( 2*(q.y*q.z + q.w*q.x),
                     q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                    -2*(q.x*q.y - q.w*q.z),
                     2*(q.x*q.z + q.w*q.y),
                     q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                     res);
      break;
        
    case xzx:
      twoaxisrot( 2*(q.x*q.z - q.w*q.y),
                   2*(q.x*q.y + q.w*q.z),
                   q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                   2*(q.x*q.z + q.w*q.y),
                  -2*(q.x*q.y - q.w*q.z),
                  res);
      break;
    default:
      std::cout << "Unknown rotation sequence" << std::endl;
      break;
   }
}

///////////////////////////////
// Helper functions
///////////////////////////////
Quaternion operator*(Quaternion& q1, Quaternion& q2){
  Quaternion q;
  q.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
  q.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
  q.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x;
  q.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w;
  return q;
}

ostream& operator <<(std::ostream& stream, const Quaternion& q) {
  cout << q.w << " "<< showpos << q.x << "i " << q.y << "j " << q.z << "k"; 
  cout << noshowpos;
}

double rad2deg(double rad){
  return rad*180.0/M_PI;
}

///////////////////////////////
// Main
///////////////////////////////
int main(){

  Quaternion q; // x,y,z,w
  Quaternion qx45(sin(M_PI/8), 0,0, cos(M_PI/8) );
  Quaternion qy45(0, sin(M_PI/8), 0, cos(M_PI/8));
  Quaternion qz45(0, 0, sin(M_PI/8), cos(M_PI/8));
  Quaternion qx90(sin(M_PI/4), 0,0, cos(M_PI/4) );
  Quaternion qy90(0, sin(M_PI/4), 0, cos(M_PI/4));
  Quaternion qz90(0, 0, sin(M_PI/4), cos(M_PI/4));

  double res[3];

  q = qz45*qx45;
  q.normalize();
  quaternion2Euler(q, res, zyx);
  cout << "Rotation sequence: X->Y->Z" << endl;
  cout << "x45 -> z45" << endl;
  cout << "q: " << q << endl;
  cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl << endl;

  q = qz90*qx90;
  q.normalize();
  quaternion2Euler(q, res, zyx);
  cout << "Rotation sequence: X->Y->Z" << endl;
  cout << "x90 -> z90" << endl;
  cout << "q: " << q << endl;
  cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl << endl;

  q = qx90*qz90;
  q.normalize();
  quaternion2Euler(q, res, xyz);
  cout << "Rotation sequence: Z->Y->X" << endl;
  cout << "z90 -> x90" << endl;
  cout << "q: " << q << endl;
  cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl;
}

 

 

 12 奇异性情况

　　参考：https://www.cnblogs.com/21207-iHome/p/6894128.html

 

 　　下面看一种特殊的情况（参考Maths - Conversion Quaternion to Euler）：假设一架飞机绕Y轴旋转了90°（俯仰角pitch=90），机头垂直向上，此时如何计算航向角和横滚角？

　　注意：RPY角旋转永远是固定的旋转轴。

 

　　飞机围绕Pitch轴旋转90或-90度，如下图情况，此时状态的Row轴已经和Row轴重合，变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题：
$\mathrm{arcsin(2(q0q2-q1q3))的定义域为[-1,1]，因此(q0q2-q1q3)\in [-0.5,0.5]，当q0q2-q1q3=0.5时（在程序中浮点数不能直接进行等于判断，要使用合理的阈值），俯仰角\beta 为90°，将其带入正向公式计算出四元数(q0,q1,q2,q3)，然后可以发现逆向公式中atan2函数中的参数全部为0，即出现了0/0的情况！无法计算。}$

$\mathrm{当俯仰角为-90°，即机头竖直向下时的情况也与之类似，可以推导出奇异姿态时的计算公式。比较完整的四元数转欧拉角（Z-Y-X\; order）的代码如下：}$

　　四元数在线演示：https://quaternions.online/

 

 

 

CameraSpacePoint QuaternionToEuler(Vector4 q) // Z-Y-X Euler angles
{
    CameraSpacePoint euler = { 0 };
    const double Epsilon = 0.0009765625f;
    const double Threshold = 0.5f - Epsilon;

    double TEST = q.w*q.y - q.x*q.z;

    if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
    {
        int sign = Sign(TEST);

        euler.Z = -2 * sign * (double)atan2(q.x, q.w); // yaw

        euler.Y = sign * (PI / 2.0); // pitch

        euler.X = 0; // roll

    }
    else
    {
        euler.X = atan2(2 * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
        euler.Y = asin(-2 * (q.x*q.z - q.w*q.y));
        euler.Z = atan2(2 * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
    }
        
    return euler;
}

　　在DirectXMath Library中有许多与刚体姿态变换相关的函数可以直接调用：

• 四元数乘法：XMQuaternionMultiply method --Computes the product of two quaternions.
• 旋转矩阵转四元数：XMQuaternionRotationMatrix method --Computes a rotation quaternion from a rotation matrix.
• 四元数转旋转矩阵：XMMatrixRotationQuaternion method -- Builds a rotation matrix from a quaternion.
• 欧拉角转四元数：XMQuaternionRotationRollPitchYaw method --Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
• 四元数转Axis-Angle：XMQuaternionToAxisAngle method --Computes an axis and angle of rotation about that axis for a given quaternion.
• 欧拉角转旋转矩阵：XMMatrixRotationRollPitchYaw method --Builds a rotation matrix based on a given pitch, yaw, and roll (Euler angles).
• Axis-Angle转旋转矩阵：XMMatrixRotationAxis method --Builds a matrix that rotates around an arbitrary axis.
• 构造绕X/Y/Z轴的旋转矩阵：XMMatrixRotationX method --Builds a matrix that rotates around the x-axis.(Angles are measured clockwise when looking along the rotation axis toward the origin)

 　　下面的代码中坐标系绕X轴旋转90°（注意这里不是按照右手定则的方向，而是沿着坐标轴向原点看过去以顺时针方式旋转，因此与传统的右手定则刚好方向相反），来进行变换：

 

 

#include "stdafx.h"
#include<iostream>

#include <DirectXMath.h>
using namespace DirectX;

#define PI 3.1415926

int _tmain(int argc, _TCHAR* argv[])
{
    //四元数相乘-------------------Computes the product of two quaternions
    XMVECTOR q1 = XMVectorSet(1, 1, 3, 2);
    XMVECTOR q2 = XMVectorSet(1, 1, 0, 2);
    XMVECTOR q3 = XMVectorSet(1, 1, 1, 1);

    XMVECTOR result = XMQuaternionMultiply(XMQuaternionMultiply(q3, q2), q1);   // Returns the product of two quaternions as q1*q2*q3

    std::cout << "Quaternion Multiply:" << std::endl;
    std::cout << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << "," << XMVectorGetW(result) << std::endl << std::endl;



    //欧拉角转化四元数------------------Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
    float pitch = 90.0 * PI / 180.0; // X轴俯仰角度 Angle of rotation around the x-axis, in radians.
    float yaw = 0;                     // y轴偏航角度 Angle of rotation around the y-axis, in radians.
    float roll = 0;                     // z轴横滚角度 Angle of rotation around the z - axis, in radians.

    result = XMQuaternionRotationRollPitchYaw(pitch, yaw, roll);

    std::cout << "RPY/Euler angles to Quaternion:" << std::endl;
    std::cout << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << "," << XMVectorGetW(result) << std::endl << std::endl;



    //从旋转矩阵计算四元数-----------------Computes a rotation quaternion from a rotation matrix.
    float matrix[16] = { 1, 0, 0, 0,   0, 0, 1, 0,   0, -1, 0, 0,   0, 0, 0, 1 };
    XMMATRIX trans(matrix); // Initializes a new instance of the XMMATRIX structure from a sixteen element float array.

    result = XMQuaternionRotationMatrix(trans); // This function only uses the upper 3x3 portion of the XMMATRIX.

    std::cout << "Matrix to Quaternion:" << std::endl;
    std::cout << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << "," << XMVectorGetW(result) << std::endl << std::endl;



    //四元数转化旋转矩阵-----------------Builds a rotation matrix from a quaternion.
    trans = XMMatrixRotationQuaternion(result);

    XMFLOAT3X3 fView;
    XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3
    std::cout << "Quaternion to Matrix:" << std::endl;
    std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl
        << fView._21 << "," << fView._22 << "," << fView._23 << std::endl
        << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl;



    //从四元数计算旋转轴和旋转角度-----------------Computes an axis and angle of rotation about that axis for a given quaternion.
    float Angle = 0;
    XMVECTOR Axis;

    XMQuaternionToAxisAngle(&Axis, &Angle, result);

    Axis = XMVector3Normalize(Axis); // Returns the normalized version of a 3D vector
    std::cout << "Quaternion to Axis-Angle:" << std::endl;
    std::cout << "Axis: " << XMVectorGetX(Axis) << "," << XMVectorGetY(Axis) << "," << XMVectorGetZ(Axis) << std::endl;
    std::cout << "Angle: " << Angle*180.0 / PI << std::endl << std::endl;



    //根据任意旋转轴和角度构建旋转矩阵-----------------Builds a matrix that rotates around an arbitrary axis.
    Angle = 90.0 * PI / 180.0;

    trans = XMMatrixRotationAxis(Axis, Angle);

    XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3
    std::cout << "Axis-Angle to Matrix:" << std::endl;
    std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl
              << fView._21 << "," << fView._22 << "," << fView._23 << std::endl
              << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl;



    //从给定欧拉角构建旋转矩阵-----------------Builds a rotation matrix based on a given pitch, yaw, and roll(Euler angles).
    trans = XMMatrixRotationRollPitchYaw(pitch, yaw, roll);

    XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3
    std::cout << "RPY/Euler angles to Matrix:" << std::endl;
    std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl
        << fView._21 << "," << fView._22 << "," << fView._23 << std::endl
        << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl;


    //围绕X轴旋转一定角度，计算旋转矩阵-----------------Builds a matrix that rotates around the x - axis.
    trans = XMMatrixRotationX(Angle); // Angles are measured clockwise when looking along the rotation axis toward the origin.

    XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3
    std::cout << "Builds a matrix that rotates around the x-axis.:" << std::endl;
    std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl
        << fView._21 << "," << fView._22 << "," << fView._23 << std::endl
        << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl;

    return 0;
}

 

 

 

 

 

 

 

 

 

 

 

　　https://www.cnblogs.com/21207-iHome/p/6894128.html

 

 

 

 

 

 

 

 

参考：

　　1. B栈视频：https://www.bilibili.com/video/BV1yK411k7Mm/?spm_id_from=333.788&vd_source=4eff17e358f404eb42dcda49f33fe336

　   2. CSDN博客：　https://blog.csdn.net/paul1994/article/details/134111671

　　3.四元数和RPY角度互换： https://www.cnblogs.com/21207-iHome/p/6894128.html