Java code to get rotation angle around an axis from quaternion

Question

I am really struggeling to find the correct way to get the rotation angle around a single axis from an arbitrary quaternion. So other words I want to find the portion of the expressed rotation around a specified axis (in my case the Z-axis of the coordinate system, but an arbitrary solution would be nice) in terms of the angle. Can anyone point out to achieve this? Ideally some java fragment would be nice.

I tried the solution proposed in 1 for attitude, which is:

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

However, this fails in some cases, e.g. a single rotation around the Z-axis with more than 0.6 * PI.

Answer 1

The answer can be found here Component of a quaternion rotation around an axis

"swing twist decomposition" from http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition/

Answer 2

Angle and rotation axis of a quaternion

Every quaternion q can be decomposed as some kind of polar decomposition

q = r * (c + s * e)

where

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

The axis of rotation of x ↦ q * x * q^(-1) is e, the angle is twice the angle α of the point (c,s)=(cos(α),sin(α)) on the unit circle.

To just compute the angle of rotation, the scaling by r is not that important, so

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

---

Theory for Euler angles

A rotation about the X-axis is represented by a quaternion ca+sa*i, rotation about the Y axis by quaternion cb+sb*j and Z-axis by cc+sc*k where ca²+sa²=1 represent the cosine-sine pair of half the rotation angle a etc. Later 2a, c2a and s2a etc. will denote the double angle and its cosine and sine values.

Multiplying in the order xyz of application to the object at the origin gives a product

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

Now interesting things happen in q*i*q^(-1) and q^(-1)*k*q, in that the inner terms commute and cancel, so that

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

which can then be used to isolate the angles 2a, 2b and 2c from

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

---

Resulting algorithm

Identifying expressions results in

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

or

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²))

This constructs the angles in a way that

c2b=sqrt( (qw²+qx²+qy²+qz²)²+8*qw*qx*qy*qz )

is positive, so 2b is between -pi/2 and pi/2. By some sign manipulations, one could also obtain a solution where c2b is negative.

---

Answer to the question on the asin formula

Obviously, a different kind of rotation order was used, where the Z-rotation is the middle rotation. To be precise,

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

where

2b = heading 2a = bank 2c = attitude

To handle attitude rotation angles 2c larger that 0.5*pi, you need to compute the full set of Euler angles, since they will then contain two flips around the other axes before and after the Z-rotation.

Or you need to detect this situation, either by keeping the cosine of bank positive or by checking for overly large angle changes, and apply sign modifications inside the atan formulas, changing their resulting angle by pi (+or-), and change the Z angle computation to pi-asin(...)

Or, to only manipulate the angles after computation, if (2a,2b,2c) is the computed solution, then

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

is another solution giving the same quaternion and rotation. Chose the one that is closest to the expected behavior.