Quaternion to Euler with some properties

Question

I am trying to create a map editor (for GTA SA-MP), and the source game data contains objects with quaternion rotation, whereas I need the editor to output the objects with Euler rotation (XYZ) in degrees, which can be used in SA-MP. I've found this algorithm:

double sqx = X*X;
double sqy = Y*Y;
double sqz = Z*Z;
double sqw = W*W;
double unit = sqx + sqy + sqz + sqw;
double test = X*Y + Z*W;
if(test > 0.499*unit)
{
    E_x = 2 * Math.Atan2(X,W)*180/Math.PI;
    E_y = 0;
    E_z = 90;
}else if(test < -0.499*unit)
{
    E_x = -2 * Math.Atan2(X,W)*180/Math.PI;
    E_y = 0;
    E_z = -90;
}else{
    E_x = Math.Atan2(2 * Y*W - 2 * X*Z, sqx - sqy - sqz + sqw)*180/Math.PI;
    E_y = Math.Atan2(2 * X*W - 2 * Y*Z, -sqx + sqy - sqz + sqw)*180/Math.PI;
    E_z = -Math.Asin(2 * test/unit)*180/Math.PI;
}

The problem is that it simply doesn't match either the input quaternion format or the output Euler format, like for (0 0 0.71 0.71) it should return (0 0 -90) and not (0 0 90) as it currently does.

To describe the Euler system in the game, I have placed an arrow in the game world and this table shows which directions it points to and which direction it faces (with its one side):

 X  Y  Z  points faces

 0  0  0  down   east
90  0  0  north  east
 0 90  0  west   down
 0  0 90  down   north
90 90  0  west   north
 0 90 90  south  down
90  0 90  west   north
90 90 90  south  west

To test the quaternion angles, I have placed a vehicle in the game world, which I can get the quaternion angles of. The following table shows the quaternion angles and whe direction the car points to and faces with its top:

  W   X   Y   Z  points faces

  1   0   0   0  north  up
 √½   0   0 -√½  west   up
  0   0   0   1  south  up
 √½   0   0  √½  east   up
  0   0   1   0  north  down
  0 -√½   ½   0  west   down
  0  -1   0   0  south  down
  0  √½   ½   0  east   down
  0   0 -√½ -√½  up     north
  ½  -½   ½   ½  up     west
 √½ -√½   0   0  up     south
  ½  -½  -½  -½  up     east
 √½  √½   0   0  down   north
  ½   ½   ½  -½  down   west
  0   0  √½ -√½  down   south
  ½   ½  -½   ½  down   east

A vehicle Y axis goes from its front to its rear, X axis is between its sides, and Z axis from its top to the bottom.

While not completely lost at maths, this quaternion stuff still puzzles me, so please be easy on me ☺. Thanks for your help!

Edit:

Here are some measured quaternion angles for a given set of Euler angles, which I can set for an object in the game:

Euler X Y Z
Quaternion W X Y Z

0, 0, 0
1, 0.000122, -0.000122, -0.000742

0, 0, 45
0.923879, 0.000000, 0.000000, -0.382683

0, 45, 0
0.921593, 0.000000, -0.388156, 0.000000

45, 0, 0
0.922824, -0.385187, 0.002027, -0.004683

0, 45, 45
0.852329, 0.147665, -0.356494, -0.353046

45, 0, 45
0.852672, -0.355673, -0.147325, -0.353186

45, 45, 0
0.859898, -0.349869, -0.353665, -0.114391

45, 45, 45
0.741364, -0.203753, -0.454495, -0.449773
0.728665, -0.190846, -0.466389, -0.463794 (another measure)

The measures are probably a bit off, mostly the last ones.

Answer 1

It looks like the rotation order is $y x z$, but angles negated for some reason. In other words, the rotation matrix for Euler angles $(\chi,\gamma,\theta)$ used is most likely $$\mathbf{R}(\chi,\gamma,\theta) = \left[\begin{matrix} \cos \gamma \cos \theta - \sin \chi \sin \gamma \sin \theta & \cos \gamma \sin \theta + \sin \chi \sin \gamma \cos \theta & -\cos \chi \sin \gamma \\ -\cos \chi \sin \theta & \cos \chi \cos \theta & \sin \chi \\ \sin \gamma \cos \theta + \sin \chi \cos \gamma \sin \theta & \sin \gamma \sin \theta - \sin \chi \cos \gamma \cos \theta & \cos \chi \cos \gamma \end{matrix}\right]$$ The rotation matrix corresponding to versor aka unit quaternion $\mathbf{Q} = (w, x, y, z)$ is $$\mathbf{R}(w,x,y,z) = \left[\begin{matrix} 1 - 2 (y^2 + z^2) & 2 (x y - z w) & 2 (x z + y w) \\ 2 (x y + z w) & 1 - 2 (x^2 + z^2) & 2 (y z - x w) \\ 2 (x z - y w) & 2 (y z + x w) & 1 - 2 (x^2 + y^2) \end{matrix}\right]$$ The stated problem is to find the correspondence between the two.

Looking at $\mathbf{R}_{23}$ we can tell that $$\chi = \arcsin(2 y z - 2 x w)$$ Since $\mathbf{R}_{21}/\mathbf{R}_{22} = -\tan\theta$ if $\mathbf{R}_{22} \ne 0$, $$\theta = -\left\lbrace\begin{align} & -\arctan\left(\frac{x y + z w}{1/2 - x^2 - z^2}\right),\; & x^2 + z^2 \ne 1/2 \\ & -\frac{\pi}{2}, \; & x y + z w \ge 0 \\ & \frac{\pi}{2}, \; & x y + z w \lt 0 \end{align}\right.$$ Similarly, $\mathbf{R}_{13}/\mathbf{R}_{33} = -\tan\gamma$ if $\mathbf{R}_{33} \ne 0$, $$\gamma = \left\lbrace\begin{align} & -\arctan\left(\frac{x z + y w}{1/2 - x^2 - y^2}\right),\; & x^2 + y^2 \ne 1/2 \\ & -\frac{\pi}{2}, \; & x z + y w \ge 0 \\ & \frac{\pi}{2}, \; & x z + y w \lt 0 \end{align}\right.$$

As a result, I suggest trying the following pseudocode:

epsilon = 0.00001
halfpi = 0.5 * 3.14159265358979323846

function euler_angles(w, x, y, z):
    temp = 2*(y*z - x*w)
    if (temp >= 1 - epsilon):
        xa = halfpi
        ya = -atan2(y, w)
        za = -atan2(z, w)
    else if (-temp >= 1 - epsilon):
        xa = -halfpi
        ya = -atan2(y, w)
        za = -atan2(z, w)
    else:
        xa = asin(temp)
        ya = -atan2(x*z + y*w, 0.5 - x*x - y*y) 
        za = -atan2(x*y + z*w, 0.5 - x*x - z*z)

    return (xa, ya, za)

which utilizes the atan2(dy,dx) function available in most programming languages.

(Above pseudocode edited on 2017-09-30, to add the checks for gimbal lock, where xa = ±0.5 π. The epsilon represents the machine epsilon in this context, and depends on the precision of the variables; typically, it should be a compile-time constant that is easy to modify when porting or running the code on different architectures. The value shown should work well for visualization.)

The corresponding conversion from Euler angles to a quaternion is easy to compute, if one realizes that the three rotations as quaternions are easy to express: $$\begin{align} &\mathbf{R}_x(\chi) = \left( \cos\frac{\chi}{2} + \sin\frac{\chi}{2} \mathbf{i} \right )\\ &\mathbf{R}_y(\gamma) = \left( \cos\frac{\gamma}{2} + \sin\frac{\gamma}{2} \mathbf{j} \right )\\ &\mathbf{R}_z(\theta) = \left( \cos\frac{\theta}{2} + \sin\frac{\theta}{2} \mathbf{k} \right )\end{align}$$ Their Hamilton product $\mathbf{Q} = \mathbf{R}_y(\gamma)\mathbf{R}_x(\chi)\mathbf{R}_z(\theta)$ gives the desired quaternion here.

In pseudocode,

function quaternion(xa, ya, za):
    cx = cos(-0.5*xa)
    sx = sin(-0.5*xa)
    cy = cos(-0.5*ya)
    sy = sin(-0.5*ya)
    cz = cos(-0.5*za)
    sz = sin(-0.5*za)
    w = cx * cy * cz + sx * sy * sz
    i = cx * sy * sz + sx * cy * cz
    j = cx * sy * cz - sx * cy * sz
    k = cx * cy * sz - sx * sy * cz
    return (w, i, j, k)

Remember that both of these functions depend on the rotation order, and that the order of rotations here is $y x z$ with negated angles.