参考 博客 http://blog.sina.com.cn/s/blog_5fb3f125010100hp.html
处理三维旋转问题时,通常采用旋转矩阵的方式来描述。一个向量乘以旋转矩阵等价于向量以某种方式进行旋转。除了采用旋转矩阵描述外,还可以用旋转向量来描述旋转,旋转向量的长度(模)表示绕轴逆时针旋转的角度(弧度)。旋转向量与旋转矩阵可以通过罗德里格斯(Rodrigues)变换进行转换。
式中,norm为求向量的模。
bool Rodrige(double *VRod,double *RRod,bool vtor)
{
if(vtor==true) //
{
double vrod[3];
vrod[0]=VRod[0];vrod[1]=VRod[1];vrod[2]=VRod[2];
double norm=(vrod[0]*vrod[0]+vrod[1]*vrod[1]+vrod[2]*vrod[2]);
norm=sqrt(norm);
for (int i=0;i<3;i++)
{
vrod[i]/=norm;
}
double costheta=cos(norm);
double costheta_1=1-cos(norm);
double sintheta=sin(norm);
RRod[0]=costheta+costheta_1*vrod[0]*vrod[0];
RRod[1]=costheta_1*vrod[0]*vrod[1]+sintheta*(-vrod[2]);
RRod[2]=costheta_1*vrod[0]*vrod[2]+sintheta*(vrod[1]);
RRod[3]=costheta_1*vrod[1]*vrod[0]+sintheta*(vrod[2]);
RRod[4]=costheta+costheta_1*vrod[1]*vrod[1];
RRod[5]=costheta_1*vrod[1]*vrod[2]+sintheta*(-vrod[0]);
RRod[6]=costheta_1*vrod[2]*vrod[0]+sintheta*(-vrod[1]);
RRod[7]=costheta_1*vrod[2]*vrod[1]+sintheta*(vrod[0]);
RRod[8]=costheta+costheta_1*vrod[2]*vrod[2];
return true;
}
else
{
double vrod[3];
vrod[0]=(RRod[7]-RRod[5])/2;
vrod[1]=(RRod[2]-RRod[6])/2;
vrod[2]=(RRod[3]-RRod[1])/2;
double sintheta=vrod[0]*vrod[0]+vrod[1]*vrod[1]+vrod[2]*vrod[2];
sintheta=sqrt(sintheta);
double vth=1./sintheta;
double c = (RRod[0] + RRod[4] + RRod[8] - 1)*0.5;
double theta = acos(c);
vth*=theta;
vrod[0]=vrod[0]*vth;
vrod[1]=vrod[1]*vth;
vrod[2]=vrod[2]*vth;
VRod[0]=vrod[0];
VRod[1]=vrod[1];
VRod[2]=vrod[2];
return true;
}
return false;
}