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.
