欧拉角，旋转向量，四元数与旋转矩阵

人体动作捕捉格式BVH及其与三维坐标的转换

1. 坐标系介绍与旋转角

1. 坐标系分为左手坐标系和右手坐标系，一般来说，右手坐标系更加常用一些，区别在于：
   四指从X轴弯曲到Y轴，若右手大拇指指向Z轴方向，则为右手坐标系。若左手大拇指指向Z轴方向，则为左手坐标系。
2. 在三维旋转中，物体的旋转次序也是十分重要的，例如，（a,b,c）在不同的旋转顺序下会有不同的结果。
   欧拉角的旋转顺序（顺规）有很多，例如：Z-X-Y,X-Y-Z,X-Y-X,Z-X-Y。 且任意旋转可以分解成以下三个轴上的转角：
   a. 绕物体的Z轴旋转，得到偏航角yaw;
   b. 绕旋转之后的Y轴旋转，得到俯仰角pitch;
   c. 绕旋转之后的X轴旋转，得到滚转角roll；

2. 欧拉角---->旋转矩阵

右手坐标系下的旋转矩阵可以拆解为以下三个矩阵：

//在右手系中绕X轴旋转x°  对应的矩阵Rx
Rx（x） =  |1|  0 |  0  |
      |0|cosx| sinx|
      |0|-sinx|cosx |
//在右手系中绕y轴旋转y°，对应旋转矩阵Ry. 
Ry (y) =  |cosy |  0 |-siny|
      | 0   |  1 | 0  |
      | siny|  0 |cosy|
//在右手系中绕z轴旋转z°，对应旋转矩阵Rz. 
Rz (z) =  |cosz| sinz|  0 |
      |-sinz| cosz|  0 |
      |  0 |  0  |  1 |

如果物体经过了Z-X-Y顺规的欧拉角变换（30°，20°，60°），则表示先绕z轴旋转60度，再绕x轴旋转30度，最后绕y轴旋转20度。则此时的旋转矩阵就是单个方向旋转矩阵的乘积。
例如Z-X-Y顺规的欧拉角对应的旋转矩阵就是 R = Rz * Rx * Ry 即 R = Rz(60 * 3.14/180) * Rx(30 * 3.14/180) * Ry(20 * 3.14/180) 。
同样的，X-Y-Z顺规的欧拉角对应的旋转矩阵为：R = Rx(30 * 3.14/180) * Ry(20 * 3.14/180) * Rz(60 * 3.14/180)。
上面这种旋转被称为被动旋转矩阵。

主动旋转矩阵，即物体围绕着惯性参考系或者世界坐标系作主动旋转的旋转矩阵（被称为外旋，左乘）。
与之对应的是被动旋转矩阵，物体不变，物体坐标系绕着自身旋转后的轴旋转的旋转矩阵（被称为内旋，右乘）。

性质：旋转矩阵的逆矩阵是它的转置矩阵

如MATLAB中的dcm2angle，angle2dcm 均对应的是被动旋转：
欧拉角转换到被动旋转矩阵在MATLAB中的实现为：

//使用函数 angle2dcm(r1,r2,r3,S) 其中若是S='XYZ’时， r1 = x; r2 = y; r3 = z
若是S='YXZ’时，r1 = y, r2 = x; r3 = z; R = angle2dcm(x,y,z,‘XYZ’); R =
angle2dcm(y,x,z,‘YXZ’); 将angle2dcm求得的旋转矩阵转化为主动旋转矩阵的话只需要加上转置即可。
[r1,r2,r3] = dcm2angle(R_m,S);
在使用dcm2angle的时候，需要确定R_m是被动旋转矩阵，如果直到的R_m是主动矩阵，则令R_m取转置，再用dcm2angle求其旋转角度。
且r1,r2,r3对应的旋转角度对应着S中的角度顺序。即如果S='XYZ’时，得到的r1对应着x轴的旋转角度，r2对应着y轴的旋转角度，r3对应着z轴旋转角度。以此类推。

旋转矩阵转为欧拉角的计算：

假设主动旋转矩阵为R：
x = atan2( R(3,2), R(3,3));
y = atan2(-R(3,1), aqrt(R(3,2)^2 + R(3,3)^2));
z = atan2(R(2,1),R(1,1));
angle = [x,y,z];//获得三个方向上的角度

3. 旋转向量与旋转矩阵

若是已知了欧拉角，就可以计算旋转向量和由旋转向量转化的旋转矩阵。
旋转向量就是将任意旋转用一个旋转轴和旋转角来表示的。
n 为旋转轴，θ为旋转角，R为相应的旋转矩阵，为旋转后的坐标系在原坐标系下的表达：
下式即为罗德里格斯公式：

而且，我们也可以将旋转向量从旋转矩阵中导出，即：

4. 四元数与旋转矩阵

由于欧拉角和旋转向量都是紧凑的，具有奇异性，因此在表达三维空间旋转时，用四元数来表示。四元数是紧凑的也没有奇异性。它能够很方便的刻画刚体绕任意轴的旋转。缺点就是四元数不够直观，运算复杂一些。
一个四元数拥有1个实部和3个虚部。可表示为如下所示：
乘上复数相当于逆时针把一个复向量旋转90°。

q = q0 + q1i + q2j + q3*k;

四元数表达和旋转向量之间的关系：

且使用四元数可以表示对一个点的旋转：

四元数到旋转矩阵的转换：

旋转矩阵到四元数的转换：

欧拉角到四元数的转换：

四元数转欧拉角的转换：

在MATLAB中使用函数来转换四元数和欧拉角和旋转矩阵：

matlab中其他操作四元数的函数：
p = angle2quat(x,y,z,‘XYZ’);//将欧拉角转化为四元数
q = quatnormalize§; //单位化（Normalize）：
R = quat2dcm(q);//q应该是单位四元数,且得到的旋转矩阵也是被动旋转矩阵。旋转方向与“欧拉角转化为四元数”的方向一致。
[q1,q2,q3] = dcm2angle(R，‘XYZ’);//就可以得到关于这个方向的欧拉角。
模（Modulus）：quatmod§
范数（Norm）：quatnorm§
求逆（Inverse）：quatinv§
四元数除法：quatdivide(q,p)
四元数乘法：quatmultiply(p,q)
共轭四元数：quatconj§
另外还有旋转函数quatrotate、四元数和欧拉角互换的函数quat2angle、angle2quat

5. 万向锁

万向锁发生在当一个以自身中心为坐标系中心的动态坐标系中，该物体会围绕三个坐标轴进行旋转。(内旋，右乘)

假设相机的旋转角度顺序为：’ZYX‘，假设首先物体围绕Z轴旋转30度，则此时X轴和Y轴仍然平行于地面，假设此时物体围绕Y轴旋转90度或者-90度，那么此时会导致X轴也朝下，即与刚开始物体没有运动时候的Z轴具有共同的一个轴。所以在下一步物体围绕X轴旋转的时候，会与之前围绕Z轴的旋转方向一致。导致这一次旋转无法令物体围绕初始的X轴进行左右偏移旋转。

解决方法：

1. 修改Y轴的旋转角度
2. 使用四元数