rodrigue 公式r-v v-r

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