关于Rotation和Quaternion的一些问题【墨名夜星吧】_百度贴吧
&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&签到排名：今日本吧第个签到，本吧因你更精彩，明天继续来努力！
本吧签到人数：0可签7级以上的吧50个
本月漏签0次！成为超级会员，赠送8张补签卡连续签到：天&&累计签到：天超级会员单次开通12个月以上，赠送连续签到卡3张
关注：4,973贴子：
关于Rotation和Quaternion的一些问题
当我们使用unity的时候，面对一个物体，一个不可避免的问题就是：控制物体的旋转。unity的Transform组件的第二个属性Rotation为我们提供控制物体旋转的功能。在一个物体的Inspector面板中，我们可以看到三个信息（X,Y,Z）。但是unity同时还具有Quaternion()的功能，所以这里就有必要介绍一下——与四元数（能力有限，只是简单的为大家介绍一下），而unity采用这两种方式来表示旋转或者方向都是为了便于储存。（Euler angles）：简单来说就是一种定义物体方向的方法如图所示，为我们展示了，包括（α，β，γ）。另外还有一条特殊的绿色线N。对于上图作几点说明：蓝色坐标系xyz（小写）表示原始或初始坐标系位置（方向）；坐标系（XYZ）表示旋转后的坐标方向。绿色的线（line of nodes）是xy和XY坐标平面的交线，那么这条线也就同时正交垂直于zZ平面。α：x与N的夹角；β：z与Z的夹角 γ：N与X的夹角（Quaternion）很抱歉，对于的理解，现在我也不是很清楚，只是简单的对unity的Quaternion的几个函数方法进行了简单了解。下面是的解释，希望对你有所帮助：是简单的。 是由实数加上虚数单位 i 组成，其中i^2 = -1。 相似地，四元数都是由实数加上三个虚数单位 i、j、k 组成，而且它们有如下的关系： i^2 = j^2 = k^2 = -1， i^0 = j^0 = k^0 = 1 , 每个四元数都是 1、i、j 和 k 的线性组合，即是四元数一般可表示为a + bk+ cj + di，其中a、b、c 、d是实数。（）关于和之间的互相转换，这里就不再赘述，因为看不懂啊，等体会理解后，在为大家讲解这里有一篇关于Quaternion下函数方法详细介绍：说了这么多，其实我想为大家解释的是：下面这样一个问题：unity什么时候用欧拉角，什么时候使用四元数。在我们的接触中，unity大部分是接受的输入和输出的。比如说，我们想输出Transform组件的Rotation信息。直接这样写：transform.rotation是不行的，你会发现显示的是一组[-1,1]的数值。而需要调用localEulerAngles方法，才会获得我们所熟知的角度表示。总结来说：其实Transform.rotation是存储的四元数信息，而真正的欧拉角需要调用方法才会获得，即Euler Angles存储的Vector3类型的变量。我们如何在unity中将一个Vector3类型的欧拉角传给四元数形式的rotation呢？我们不需要复杂的数学运行，需要调用Quaternion下面的方法函数即可。transform.rotation=Quaternion.Euler(x,y,z)或者是transform.EulerAngles=new Vector(0.0,0.0,0.0)最后，介绍一下lerp与slerp的区别lerp()函数，我们都很了解，进行插值运算，对于物体平移，使用lerp函数，十分方便。这里就不过多说明了，详细请看unity 圣典。然而对于物体旋转方向的插值，效果就不那么理想了。其实unity还为我们提供了另一种插值函数---Slerp。Quaternion.lerp()。关于这个函数在圣典中的说明很短，只解释是球形插值。让人费解。关于这个问题我的理解是，如果想要对物体的旋转进行插值，我们应该选择slerp函数，这个函数利用，对旋转的角度进行插值，（至于更深层次的机制，我也不甚了解，希望大家留言补充）从而能够达到较为理想的效果。另外，还有Quaternion.lerp()函数，这个说是效果也不如slerp。大家可以自行尝试。下面是关于slerp函数的代码解释static function Slerp (from : Quaternion, to : Quaternion,t : float) : Quaternion 注意form和to都是Quaternion类型，而不是vector3类型。Quaternion.Slerp (Quaternion.Euler(rotation_begin),Quaternion.Euler(rotation_end),t); Euler()方法是为了将Vector3类型转换成类型。这是slerp函数的参数要求
……我只是随便点进来看看……你是学什么的……
贴吧热议榜
使用签名档&&
保存至快速回贴RPY角与Z-Y-X欧拉角
　　描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。第一种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转$\gamma$，然后绕{A}的Y轴旋转$\beta$，最后绕{A}的Z轴旋转$\alpha$，就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。
　　Roll:横滚
　　Pitch: 俯仰
Yaw: 偏航（航向）
　　由于是绕固定坐标系旋转，则旋转矩阵为（$c\alpha$ is shorthand for $\cos\alpha$, $s\alpha$ is shorthand for $\sin\alpha$,and so on.）
$$R_{XYZ}(\gamma,\beta,\alpha)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)=\begin{bmatrix}c\alpha c\beta & c\alpha s\beta s\gamma-s\alpha c\gamma & c\alpha s\beta c\gamma+s\alpha s\gamma\\ s\alpha c\beta & s\alpha s\beta s\gamma+c\alpha c\gamma & s\alpha s\beta c\gamma-c\alpha s\gamma\\ -s\beta&
c\beta s\gamma & c\beta c\gamma\end{bmatrix}$$
　　另一种姿态描述方式是绕自身坐标轴旋转：假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转$\alpha$，然后绕Y轴旋转$\beta$，最后绕X轴旋转$\gamma$，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转，则旋转矩阵为：
$$R_{Z'Y'X'}(\alpha,\beta,\gamma)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)=\begin{bmatrix}c\alpha c\beta & c\alpha s\beta s\gamma-s\alpha c\gamma & c\alpha s\beta c\gamma+s\alpha s\gamma\\ s\alpha c\beta & s\alpha s\beta s\gamma+c\alpha c\gamma & s\alpha s\beta c\gamma-c\alpha s\gamma\\ -s\beta&
c\beta s\gamma & c\beta c\gamma\end{bmatrix}$$
　　可以发现这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转$(\gamma,\beta,\alpha)$和绕自身坐标轴Z-Y-X旋转$(\alpha,\beta,\gamma)$的最终结果一样，只是描述的方法有差别而已。In gerenal: three rotations taken about fixed axes yield the same final orientation as the same three rotations taken in opposite order about the axes of the moving frame.
Axis-Angle与四元数
　　绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为$\vec{K}=[k_x,k_y,k_z]^T$，等效旋转角为$\theta$，则四元数$q=(x,y,z,w)$，其中：
$$\begin{align*} x &= k_x \cdot sin \frac{\theta}{2}\\y &= k_y \cdot sin \frac{\theta}{2}\\z &= k_z \cdot sin \frac{\theta}{2}\\w &= cos \frac{\theta}{2}\end{align*}$$
　　且有$x^2+y^2+z^2+w^2=1$
　　即四元数存储了旋转轴和旋转角的信息，它能方便的描述刚体绕任意轴的旋转。
　　四元数转换为旋转矩阵：
$$R=\begin{bmatrix}1-2y^2-2z^2 & 2(xy-zw) & 2(xz+yw)\\ 2(xy+zw) & 1-2x^2-2z^2 & 2(yz-xw)\\ 2(xz-yw)& 2(yz+xw) & 1-2x^2-2y^2\end{bmatrix}$$
&　　已知旋转矩阵为：
　　则对应的四元数为：
四元数与欧拉角的相互转换
　　定义两个四元数：
　　其中&表示矢量
四元数加法：
　　跟复数、向量和矩阵一样，两个四元数之和需要将不同的元素加起来。
　　加法遵循实数和复数的所有交换律和结合律。
四元数乘法：
　　四元数的乘法的意义类似于矩阵的乘法，可以表示旋转的合成。当有多次旋转操作时，使用四元数可以获得更高的计算效率。
　　由于四元数乘法的非可换性，pq并不等于qp，qp乘积的向量部分是：
　　Mathematica中有四元数相关的程序包，需要先导入才能使用。下面计算了三个四元数的乘积：
&&Quaternions`
（* This loads the package *）
Quaternion[2, 1, 1, 3] ** Quaternion[2, 1, 1, 0] ** Quaternion[1, 1, 1, 1]
(* Be sure to use&**&rather than&*&when multiplying quaternions *)
　　计算结果为：Quaternion[-12, 4, 14, 2]
　　那么将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转$\alpha$,$\beta$,$\gamma$角）转换为四元数：
$$q=\begin{bmatrix}\cos\frac{\gamma}{2}\\ 0\\ 0\\ \sin\frac{\gamma}{2}\end{bmatrix} \begin{bmatrix}\cos\frac{\beta}{2}\\ 0\\ \sin\frac{\beta}{2}\\ 0\end{bmatrix} \begin{bmatrix}\cos\frac{\alpha}{2}\\ \sin \frac{\alpha}{2}\\ 0\\ 0\end{bmatrix}=\begin{bmatrix}\cos\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \sin\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}\end{bmatrix}$$
&　　根据上面的公式可以求出逆解，即由四元数$q=(q_0,q_1,q_2,q_3)$或$q=(w,x,y,z)$到欧拉角的转换为：
$$\begin{bmatrix}\alpha\\ \beta\\ \gamma\end{bmatrix} = \begin{bmatrix} \arctan\frac{2(q_0q_1+q_2q_3)}{1-2(q_1^2+q_2^2)}\\ \arcsin(2(q_0q_2-q_1q_3))\\ \arctan\frac{2(q_0q_3+q_1q_2)}{1-2(q_2^2+q_3^2)}\end{bmatrix}$$
　　由于arctan和arcsin的取值范围在$\frac{-\pi}{2}$和$\frac{\pi}{2}$之间，只有180°，而绕某个轴旋转时范围是360°，因此要使用函数代替arctan函数：
$$\begin{bmatrix}\alpha\\ \beta\\ \gamma\end{bmatrix} = \begin{bmatrix} atan2(2(q_0q_1+q_2q_3),1-2(q_1^2+q_2^2))\\ \arcsin(2(q_0q_2-q_1q_3))\\ atan2(2(q_0 q_3+q_1 q_2),1-2(q_2^2+q_3^2))\end{bmatrix}$$
对于tan(θ) =&y&/&x ：
　　θ&=&ATan(y&/&x)求出的θ取值范围是[-PI/2, PI/2]；
　　θ&=&ATan2(y, x)求出的θ取值范围是[-PI, & PI]。
当 (x,&y) 在第一象限, 0 &&θ&&&PI/2
当&(x,&y)&在第二象限&PI/2 &&θ≤PI
当&(x,&y) 在第三象限, -PI&&&θ&& -PI/2
当&(x,&y)&在第四象限, -PI/2 &&θ&& 0
&　　将可以参考下面的代码。需要注意欧拉角有12种旋转次序，而上面推导的公式是按照Z-Y-X顺序进行的，所以有时会在网上看到不同的转换公式（因为对应着不同的旋转次序），在使用时一定要注意旋转次序是什么。比如ADAMS软件里就默认Body 3-1-3次序，即Z-X-Z欧拉角，而VREP中则按照X-Y-Z欧拉角旋转。
enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};
// COMPILE: g++ -o quat2EulerTest quat2EulerTest.cpp
#include &iostream&
#include &cmath&
using namespace
///////////////////////////////
// Quaternion struct
// Simple incomplete quaternion struct for demo purpose
///////////////////////////////
struct Quaternion{
Quaternion():x(<span style="color: #), y(<span style="color: #), z(<span style="color: #), w(<span style="color: #){};
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);
double norm(){
return std::sqrt(x*x + y*y + z*z + w*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[<span style="color: #] = atan2( r11, r12 );
res[<span style="color: #] = acos ( r21 );
res[<span style="color: #] = atan2( r31, r32 );
void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
res[<span style="color: #] = atan2( r31, r32 );
res[<span style="color: #] = asin ( r21 );
res[<span style="color: #] = atan2( r11, r12 );
void quaternion2Euler(const Quaternion& q, double res[], RotSeq rotSeq)
switch(rotSeq){
threeaxisrot( <span style="color: #*(q.x*q.y + q.w*q.z),
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
-<span style="color: #*(q.x*q.z - q.w*q.y),
<span style="color: #*(q.y*q.z + q.w*q.x),
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
twoaxisrot( <span style="color: #*(q.y*q.z - q.w*q.x),
<span style="color: #*(q.x*q.z + q.w*q.y),
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
<span style="color: #*(q.y*q.z + q.w*q.x),
-<span style="color: #*(q.x*q.z - q.w*q.y),
threeaxisrot( -<span style="color: #*(q.x*q.y - q.w*q.z),
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
<span style="color: #*(q.y*q.z + q.w*q.x),
-<span style="color: #*(q.x*q.z - q.w*q.y),
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
twoaxisrot( <span style="color: #*(q.x*q.z + q.w*q.y),
-<span style="color: #*(q.y*q.z - q.w*q.x),
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
<span style="color: #*(q.x*q.z - q.w*q.y),
<span style="color: #*(q.y*q.z + q.w*q.x),
threeaxisrot( <span style="color: #*(q.x*q.z + q.w*q.y),
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
-<span style="color: #*(q.y*q.z - q.w*q.x),
<span style="color: #*(q.x*q.y + q.w*q.z),
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
twoaxisrot( <span style="color: #*(q.x*q.y - q.w*q.z),
<span style="color: #*(q.y*q.z + q.w*q.x),
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
<span style="color: #*(q.x*q.y + q.w*q.z),
-<span style="color: #*(q.y*q.z - q.w*q.x),
threeaxisrot( -<span style="color: #*(q.x*q.z - q.w*q.y),
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
<span style="color: #*(q.x*q.y + q.w*q.z),
-<span style="color: #*(q.y*q.z - q.w*q.x),
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
twoaxisrot( <span style="color: #*(q.y*q.z + q.w*q.x),
-<span style="color: #*(q.x*q.y - q.w*q.z),
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
<span style="color: #*(q.y*q.z - q.w*q.x),
<span style="color: #*(q.x*q.y + q.w*q.z),
threeaxisrot( -<span style="color: #*(q.y*q.z - q.w*q.x),
q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
<span style="color: #*(q.x*q.z + q.w*q.y),
-<span style="color: #*(q.x*q.y - q.w*q.z),
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
twoaxisrot( <span style="color: #*(q.x*q.y + q.w*q.z),
-<span style="color: #*(q.x*q.z - q.w*q.y),
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
<span style="color: #*(q.x*q.y - q.w*q.z),
<span style="color: #*(q.x*q.z + q.w*q.y),
threeaxisrot( <span style="color: #*(q.y*q.z + q.w*q.x),
q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
-<span style="color: #*(q.x*q.y - q.w*q.z),
<span style="color: #*(q.x*q.z + q.w*q.y),
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
twoaxisrot( <span style="color: #*(q.x*q.z - q.w*q.y),
<span style="color: #*(q.x*q.y + q.w*q.z),
q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
<span style="color: #*(q.x*q.z + q.w*q.y),
-<span style="color: #*(q.x*q.y - q.w*q.z),
std::cout && "Unknown rotation sequence" && std::
///////////////////////////////
// Helper functions
///////////////////////////////
Quaternion operator*(Quaternion& q1, Quaternion& q2){
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;
ostream& operator &&(std::ostream& stream, const Quaternion& q) {
cout && q.w && " "&& showpos && q.x && "i " && q.y && "j " && q.z && "k";
double rad2deg(double rad){
return rad*<span style="color: #0.0/M_PI;
///////////////////////////////
///////////////////////////////
int main(){
Q // x,y,z,w
Quaternion qx45(sin(M_PI/<span style="color: #), <span style="color: #,<span style="color: #, cos(M_PI/<span style="color: #) );
Quaternion qy45(<span style="color: #, sin(M_PI/<span style="color: #), <span style="color: #, cos(M_PI/<span style="color: #));
Quaternion qz45(<span style="color: #, <span style="color: #, sin(M_PI/<span style="color: #), cos(M_PI/<span style="color: #));
Quaternion qx90(sin(M_PI/<span style="color: #), <span style="color: #,<span style="color: #, cos(M_PI/<span style="color: #) );
Quaternion qy90(<span style="color: #, sin(M_PI/<span style="color: #), <span style="color: #, cos(M_PI/<span style="color: #));
Quaternion qz90(<span style="color: #, <span style="color: #, sin(M_PI/<span style="color: #), cos(M_PI/<span style="color: #));
double res[<span style="color: #];
q = qz45*qx45;
q.normalize();
quaternion2Euler(q, res, zyx);
cout && "Rotation sequence: X-&Y-&Z" &&
cout && "x45 -& z45" &&
cout && "q: " && q &&
cout && "x: " && rad2deg(res[<span style="color: #]) && " y: " && rad2deg(res[<span style="color: #]) && " z: " && rad2deg(res[<span style="color: #]) && endl &&
q = qz90*qx90;
q.normalize();
quaternion2Euler(q, res, zyx);
cout && "Rotation sequence: X-&Y-&Z" &&
cout && "x90 -& z90" &&
cout && "q: " && q &&
cout && "x: " && rad2deg(res[<span style="color: #]) && " y: " && rad2deg(res[<span style="color: #]) && " z: " && rad2deg(res[<span style="color: #]) && endl &&
q = qx90*qz90;
q.normalize();
quaternion2Euler(q, res, xyz);
cout && "Rotation sequence: Z-&Y-&X" &&
cout && "z90 -& x90" &&
cout && "q: " && q &&
cout && "x: " && rad2deg(res[<span style="color: #]) && " y: " && rad2deg(res[<span style="color: #]) && " z: " && rad2deg(res[<span style="color: #]) &&
　　 上面的代码存在一个问题，即奇异性没有考虑。下面看一种特殊的情况（参考）：假设一架飞机绕Y轴旋转了90°（俯仰角pitch=90），机头垂直向上，此时如何计算航向角和横滚角？
　　这时会发生自由度丢失的情况，即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题：
　　$\arcsin(2(q_0q_2-q_1q_3))$的定义域为$[-1,1]$，因此$(q_0q_2-q_1q_3)\in[-0.5, 0.5]$，当$q_0q_2-q_1q_3=0.5$时（在程序中浮点数不能直接进行等于判断，要使用合理的阈值），俯仰角$\beta$为90°，将其带入正向公式计算出四元数$(q_0,q_1,q_2,q_3)$，然后可以发现逆向公式中atan2函数中的参数全部为0，即出现了$\frac{0}{0}$的情况！无法计算。
　　$\beta=\pi/2$时，$\sin\frac{\beta}{2}=\cos\frac{\beta}{2}=0.707$，将其带入公式中有
$$q=\begin{bmatrix}w\\ x\\ y\\ z\end{bmatrix} \begin{bmatrix}0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\gamma}{2})\\ 0.707(\sin\frac{\alpha}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\cos\frac{\gamma}{2})\end{bmatrix}=\begin{bmatrix}0.707\cos\frac{\alpha-\gamma}{2}\\ 0.707\sin\frac{\alpha-\gamma}{2}\\ 0.707\cos\frac{\alpha-\gamma}{2}\\ 0.707\sin\frac{\alpha-\gamma}{2}\end{bmatrix}$$
　　则$\frac{x}{w}=\frac{z}{y}=\tan\frac{\alpha-\gamma}{2}$，于是有
$$\alpha-\gamma = 2\cdot atan2(x,w)$$
&　　通常令$\alpha=0$，这时$\gamma = -2\cdot atan2(x,w)$。可以进行验证：当四元数为(w,x,y,z)=(0.653,-0.271,0.653,0.271)时，根据这些规则计算出来的ZYX欧拉角为α=0°，β=90°，γ=45°
　　当俯仰角为-90°，即机头竖直向下时的情况也与之类似，可以推导出奇异姿态时的计算公式。比较完整的四元数转欧拉角（Z-Y-X order）的代码如下：
CameraSpacePoint QuaternionToEuler(Vector4 q) // Z-Y-X Euler angles
CameraSpacePoint euler = { <span style="color: # };
const double Epsilon = <span style="color: #.f;
const double Threshold = <span style="color: #.5f - E
double TEST = q.w*q.y - q.x*q.z;
if (TEST & -Threshold || TEST & Threshold) // 奇异姿态,俯仰角为±90°
int sign = Sign(TEST);
euler.Z = -<span style="color: # * sign * (double)atan2(q.x, q.w); // yaw
euler.Y = sign * (PI / <span style="color: #.0); // pitch
euler.X = <span style="color: #; // roll
euler.X = atan2(<span style="color: # * (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(-<span style="color: # * (q.x*q.z - q.w*q.y));
euler.Z = atan2(<span style="color: # * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
　　在DirectXMath Library中有许多与刚体姿态变换相关的函数可以直接调用：
四元数乘法： method --Computes the product of two quaternions.
旋转矩阵转四元数： method --Computes a rotation quaternion from a rotation matrix.
四元数转旋转矩阵： method --&Builds a rotation matrix from a quaternion.
欧拉角转四元数： method --Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
四元数转Axis-Angle： method --Computes an axis and angle of rotation about that axis for a given quaternion.
欧拉角转旋转矩阵： --Builds a rotation matrix based on a given pitch, yaw, and roll (Euler angles).
Axis-Angle转旋转矩阵： method --Builds a matrix that rotates around an arbitrary axis.
构造绕X/Y/Z轴的旋转矩阵：&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(<span style="color: #, <span style="color: #, <span style="color: #, <span style="color: #);
XMVECTOR q2 = XMVectorSet(<span style="color: #, <span style="color: #, <span style="color: #, <span style="color: #);
XMVECTOR q3 = XMVectorSet(<span style="color: #, <span style="color: #, <span style="color: #, <span style="color: #);
XMVECTOR result = XMQuaternionMultiply(XMQuaternionMultiply(q3, q2), q1);
// Returns the product of two quaternions as q1*q2*q3
std::cout && "Quaternion Multiply:" && std::
std::cout && XMVectorGetX(result) && "," && XMVectorGetY(result) && "," && XMVectorGetZ(result) && "," && XMVectorGetW(result) && std::endl && std::
//------------------Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
float pitch = <span style="color: #.0 * PI / <span style="color: #0.0; // Angle of rotation around the x-axis, in radians.
float yaw = <span style="color: #;
// Angle of rotation around the y-axis, in radians.
float roll = <span style="color: #;
// Angle of rotation around the z - axis, in radians.
result = XMQuaternionRotationRollPitchYaw(pitch, yaw, roll);
std::cout && "RPY/Euler angles to Quaternion:" && std::
std::cout && XMVectorGetX(result) && "," && XMVectorGetY(result) && "," && XMVectorGetZ(result) && "," && XMVectorGetW(result) && std::endl && std::
//-----------------Computes a rotation quaternion from a rotation matrix.
float matrix[<span style="color: #] = { <span style="color: #, <span style="color: #, <span style="color: #, <span style="color: #,
<span style="color: #, <span style="color: #, <span style="color: #, <span style="color: #,
<span style="color: #, -<span style="color: #, <span style="color: #, <span style="color: #,
<span style="color: #, <span style="color: #, <span style="color: #, <span style="color: # };
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::
std::cout && XMVectorGetX(result) && "," && XMVectorGetY(result) && "," && XMVectorGetZ(result) && "," && XMVectorGetW(result) && std::endl && std::
//-----------------Builds a rotation matrix from a quaternion.
trans = XMMatrixRotationQuaternion(result);
XMFLOAT3X3 fV
XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3
std::cout && "Quaternion to Matrix:" && std::
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::
//-----------------Computes an axis and angle of rotation about that axis for a given quaternion.
float Angle = <span style="color: #;
XMVECTOR A
XMQuaternionToAxisAngle(&Axis, &Angle, result);
Axis = XMVector3Normalize(Axis); // Returns the normalized version of a 3D vector
std::cout && "Quaternion to Axis-Angle:" && std::
std::cout && "Axis: " && XMVectorGetX(Axis) && "," && XMVectorGetY(Axis) && "," && XMVectorGetZ(Axis) && std::
std::cout && "Angle: " && Angle*<span style="color: #0.0 / PI && std::endl && std::
//-----------------Builds a matrix that rotates around an arbitrary axis.
Angle = <span style="color: #.0 * PI / <span style="color: #0.0;
trans = XMMatrixRotationAxis(Axis, Angle);
XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3
std::cout && "Axis-Angle to Matrix:" && std::
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::
//-----------------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::
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::
//-----------------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::
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::
return <span style="color: #;
　　结果如下图所示:
Introduction to Robotics - Mechanics and Control. Chapter 2 Spatial descriptions and transformations
阅读(...) 评论()