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;
}







  • 1
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值