I am using quaternions to describe 3D rotations which parametrized by Euler angles, and as a preliminary task I am trying to implement conversion routines that go between Euler angles and quaternions.
Using the XYZ (1,2,3) Tait-Bryan angle description, I have implemented the following function to obtain the quaternion that corresponds to a rotation of angle $\theta$ around the vector $v$:
qa[v_, θ_] := Quaternion[Cos[θ/2], Sin[θ/2] v[[1]], Sin[θ/2] v[[2]], Sin[θ/2] v[[3]]];
The quaternion thus generated can be multiplied as follows to obtain the rotated of a vector $v$
q[θ1_, θ2_, θ3_] := qa[{0, 0, 1}, θ3] ** qa[{0, 1, 0}, θ2] ** qa[{1, 0, 0}, θ1];
quat[v_] := Quaternion[0, v[[1]], v[[2]], v[[3]]];
q1[θ1_, θ2_, θ3_] := Conjugate[q[θ1, θ2, θ3]];
RotQuat[v_, θ_] := q1[θ[[1]], θ[[2]], θ[[3]]] ** quat[v] ** q[θ[[1]], θ[[2]], θ[[3]]]
where $\theta_i$ are the Euler angles, $q1$ is the inverse unit quaternion of $q$.
I now want to be able to invert the function q[θ[[1]], θ[[2]], θ[[3]]]
, to obtain the Euler angles from the unit quaternion $q$.
To do this, I have used the formulae given by Diebel here, Wikipedia (via Blanco, here):
QuatToEuler[q_] := {ArcTan[(2 q[[3]] q[[4]] + 2 q[[1]] q[[2]]), (q[[1]]^2 +
q[[4]]^2 - (q[[3]]^2 + q[[2]]^2))], -ArcSin[
2 q[[2]] q[[4]] - 2 q[[1]] q[[3]]],
ArcTan[(2 q[[2]] q[[3]] + 2 q[[1]] q[[4]]), (-q[[4]]^2 - q[[3]]^2 +
q[[2]]^2 + q[[1]]^2)]}
But I get unsatisfactory results. For instance:
q[π/4, 0, 0]
% // QuatToEuler // FullSimplify
Quaternion[Cos[π/8], Sin[π/8], 0, 0] {π/4, 0, π/2}
which is clearly not the desired output {$\pi$/4,0,0}.
atan2
, which could be expecting arguments (y,x). MMA wants (x,y). $\endgroup$