公式
1. 旋转向量求解旋转矩阵
当刚体在三维空间中运动时,如果已知旋转向量,根据罗德里格斯公式是比较容易求得旋转矩阵的.
罗德里格斯公式如图所示
其中,I 是单位矩阵,n 是旋转向量的单位向量, theta是旋转向量的模长.
2. 旋转矩阵求解旋转向量
如果已知旋转矩阵,求解旋转向量时,theta是比较容易求解的.根据上图,对等式两端取迹便可以得到旋转向量的模长
记旋转向量的单位向量为 r(rx, ry, rz) ,通过下图公式便可求解得出 r 向量的反对成矩阵,即可得出 r 向量
代码