Java代码,用于从四元数获取围绕轴的旋转角度

时间:2014-04-16 17:01:26

标签: java math geometry quaternions

我真的很想找到从任意四元数获得围绕单个轴的旋转角度的正确方法。所以其他的话我想在角度方面找到围绕指定轴(在我的情况下是坐标系的Z轴,但是任意解决方案会很好)的表达旋转的部分。任何人都可以指出实现这一目标吗?理想情况下,一些java片段会很好。

我尝试了1中提出的解决方案的态度,即:

asin(2*qx*qy + 2*qz*qw)

但是,在某些情况下,这种情况会失败,例如:围绕Z轴单次旋转,超过0.6 * PI。

2 个答案:

答案 0 :(得分:1)

四元数的角度和旋转轴

每个四元数q都可以分解为某种极性分解

  

q = r *(c + s * e)

其中

  

r = | q |,s = | imag(q / r)|,c = real(q / r)和e = imag(q / s / r)

x↦q* x * q ^( - 1)的旋转轴是e,角度是点(c,s)的角度α的两倍=(cos(α),sin(α))在单位圈。

要计算旋转角度,r的缩放并不重要,所以

angle = 2*atan2( norm(imag(q)), real(q) )  

欧拉角理论

围绕X轴的旋转由四元数ca+sa*i表示,由四元数cb+sb*j围绕Y轴旋转,由cc+sc*k围绕Z轴旋转,其中ca²+sa²= 1表示余弦旋转角度a的一半的余弦 - 正弦对。后来的2a,c2a和s2a等将表示双角度及其余弦和正弦值。

以xyz的应用顺序乘以原点上的对象,得到一个产品

q=qw+qx*i+qy*j+qz*k
=(cc+sc*k)*(cb+sb*j)*(ca+sa*i)

现在有趣的事情发生在q*i*q^(-1)q^(-1)*k*q,因为内部条款通勤和取消,所以

q*i*q^(-1)*(-i) = (cc+sc*k)*(cb+sb*j)*(cb+sb*j)*(cc+sc*k)
= (cc+sc*k)*(c2b+s2b*j)*(cc+sc*k)
= (c2c+s2c*k)*c2b+s2b*j 

(-k)*q^(-1)*k*q = (ca+sa*i)*(cb+sb*j)*(cb+sb*j)*(ca+sa*i)
=(ca+sa*i)*(c2b+s2b*j)*(ca+sa*i)
=(c2a+s2a*i)*c2b+s2b*j

然后可用于将角度2a,2b和2c与

隔离
q*i*q^(-1)*(-i) = (q*i)*(i*q)^(-1) 
   = (qw*i-qx-qy*k+qz*j)*(-qw*i-qx-qy*k+qz*j)
   =  (qw²+qx²-qy²-qz²)
    + 2*(qw*qy-qx*qz)*j
    + 2*(qw*qz+qx*qy)*k

(-k)*q^(-1)*k*q = (q*k)^(-1)*(k*q)
   = (-qw*k+qx*j-qy*i-qz)*(qw*k+qx*j-qy*i-qz)
   =   (qw²-qx²-qy²+qz²)
    + 2*(qw*qx+qy*qz)*i
    + 2*(qw*qy-qx*qz)*j

结果算法

识别表达式导致

s2b = 2*(qw*qy-qx*qz)
c2b*(c2a+s2a*i) = (qw²-qx²-qy²+qz²) + 2*(qw*qx+qy*qz)*i
c2b*(c2c+s2c*k) = (qw²+qx²-qy²-qz²) + 2*(qw*qz+qx*qy)*k

2a = atan2(2*(qw*qx+qy*qz), (qw²-qx²-qy²+qz²))
2b = asin(2*(qw*qy-qx*qz))
2c = atan2(2*(qw*qz+qx*qy), (qw²+qx²-qy²-qz²))

的方式构造角度
c2b=sqrt( (qw²+qx²+qy²+qz²)²+8*qw*qx*qy*qz )

是正的,因此2b在-pi / 2和pi / 2之间。通过一些符号操作,人们也可以获得c2b为负的解决方案。


回答关于asin公式的问题

显然,使用了不同类型的旋转顺序,其中Z旋转是中间旋转。确切地说,

q =(cb + sb * j)*(cc + sc * k)*(ca + sa * i)

,其中

2b =标题    2a =银行    2c =态度

要处理大于0.5 * pi的姿态旋转角度2c,您需要计算全部欧拉角度,因为它们将在Z旋转之前和之后包含围绕其他轴的两次翻转。

或者你需要检测这种情况,要么保持银行的余弦为正,要么检查过大的角度变化,并在atan公式内应用符号修改,用pi(+或 - )改变它们的结果角度,以及将Z角度计算更改为pi-asin(...)

或者,为了仅在计算后操纵角度,如果(2a,2b,2c)是计算的解,那么

(2a-sign(2a)*pi, 2b-sign(2b)*pi, sign(2c)*pi-2c)

是提供相同四元数和旋转的另一种解决方案。选择最接近预期行为的那个。

答案 1 :(得分:0)