rodrigue 公式r-v v-r

来源：互联网发布：网络棋牌游戏出售编辑：程序博客网时间：2024/04/30 06:04

参考博客 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;}

0 0