罗德里格旋转公式的矩阵实现

罗德里格公式：

在计算机图形学中，罗德里格向量旋转公式通常被用来填写旋转矩阵。如果把k和v分别写为列向量：

则旋转以后的向量可以表示为：

其中

其中E是3阶单位矩阵。需要注意的是，公式中的第二项不是点积，而是张量积，得到的是一个3行3列的矩阵。

C++，Eigen3程序：

 1 #include <iostream>
 2 #include <Eigen/Dense>
 3 
 4 using namespace Eigen;
 5 using namespace std;
 6 
 7 int main(int argc, char *argv[])
 8 {
 9     Vector3d k(1,0,0);
10     Vector3d v(0,1,0);
11     Vector3d vout;
12     Matrix3d E;
13     Matrix3d G;
14     float PI=3.1415926;
15     float angle=90.0*PI/180;
16 
17     E.setIdentity();
18 
19     G<<0    ,-k(2),k(1),
20        k(2) ,0    ,-k(0),
21        -k(1),k(0) ,0;
22 
23     vout = E*cos(angle)*v+(1-cos(angle))*k*k.transpose()*v+sin(angle)*G*v;
24     cout<<vout<<endl;
25 
26 }