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

罗德里格公式：

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

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

其中

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

C++，Eigen3程序：

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main(int argc, char *argv[])
{
    Vector3d k(1,0,0);
    Vector3d v(0,1,0);
    Vector3d vout;
    Matrix3d E;
    Matrix3d G;
    float PI=3.1415926;
    float angle=90.0*PI/180;

    E.setIdentity();

    G<<0    ,-k(2),k(1),
       k(2) ,0    ,-k(0),
       -k(1),k(0) ,0;

    vout = E*cos(angle)*v+(1-cos(angle))*k*k.transpose()*v+sin(angle)*G*v;
    cout<<vout<<endl;

}