1.公式
轴角转四元数。
q=[cos(theta/2),nx*sin(theta/2),ny*sin(theta/2),nz*sin(theta/2)]
四元数转轴角
theta=2arccosq0,
[nx,ny,nz].T=[q1,q2,q3].t/sin(theta/2)
经轴角旋转后的空间点
这时旋转矩阵是
也就是
2实际计算
这里的轴角变成了angle_axis=[nx,ny,nz].t*theta
2.1轴角变成四元数
那么[q1,q2,q3]=[a0,a1,a2]*sin(theta/2)/theta
这里theta=sqrt(a0*a0+a1*a1+a2*a2)
定义sin(theta/2)/theta为k
当角不为0的时候，如常计算k.
当角趋近于0的时候,k趋近于0.5.
2.2四元数变轴角
[a0,a1,a2]=[q1,q2,q3]*theta/sin(theta/2)
定义theta/sin(theta/2)为k
这里sin(theta/2)为sqrt(q1*q1+q2*q2+q3*q3)
阈值用sin(theta/2)的平方。
如果不为0，theta为2*arctan(-sin(theta/2),-q0)或者2*arctan(sin(theta/2),q0).
如果为0，k直接趋近于2.
2.3求旋转后的点，用轴角算的。
这里theta=sqrt(a0*a0+a1*a1+a2*a2),用theta的平方做了一个阈值判断。
如果不为0.计算w=[a0,a1,a2]/theta,也就是n.
计算w cross pt结果为w_cross_pt.(1-cos(theta))*w*pt为tmp.tmp是一个数值来着。
result[0]=cos(theta)*pt[0]+sin(theta)*w_cross_pt[0]+w[0]*tmp
如果为0.
cos(theta)=1,sin(theta)~theta
所以R=I+hat(w)*theta=I+hat(angle_axis)
令w_cross_pt为angle_axis和pt的叉乘结果
所以result[0]=pt[0]+w_cross_pt[0]