quaternion <-> Euler

Question

I'm using C# and in my program I receive a quaternion of a ridigbody in Quaternions but the axis do not correspond to the orientation of the axis that I use so I want to rotate the quaternion. In order to do this I convert the Quaternion to Euler angles, switch the pitch, yaw and roll so that it correnspond to my coordinate system, convert it back to quaternions and then I generate a rotation matrix to transform positions.

However something goes wrong, When I just convert the same input from quaternion to euler and back I get another quaternion ... so one of the functions does something wrong and I dont know where...

Quaternion q = new Quaternion(-0.4, -0.7, -0.8, 0.5);
double yaw = 0, pitch = 0, roll = 0;
toEuler(q, ref yaw, ref pitch, ref roll);
Quaternion quat = ToQ((float)(yaw), (float)(pitch), (float)(roll));
private void toEuler(Quaternion q, ref double x, ref double y, ref double z)
 {
 double test = q.X * q.Y + q.Z * q.W;
 if (test > 0.499) // singularity at north pole
 {
 y = 2.0F * System.Math.Atan2(q.X, q.W);
 z = Math.PI / 2.0F;
 x = 0.0F;
 return;
 }
 if (test < -0.499) // singularity at south pole
 {
 y = -2.0F * System.Math.Atan2(q.X, q.W);
 z = -Math.PI / 2.0F;
 x = 0.0F;
 return;
 }
 double sqx = q.X * q.X;
 double sqy = q.Y * q.Y;
 double sqz = q.Z * q.Z;
 y = System.Math.Atan2(2.0F * q.Y * q.W - 2.0 * q.X * q.Z, 1.0F - 2.0 * sqy - 2.0 * sqz);
 z = System.Math.Asin(2.0F * test);
 x = System.Math.Atan2(2.0 * q.X * q.W - 2.0 * q.Y * q.Z, 1.0F - 2.0 * sqx - 2.0 * sqz);
 }
 public Quaternion ToQ(float yaw, float pitch, float roll)
 {
 float rollOver2 = roll * 0.5f;
 float sinRollOver2 = (float)Math.Sin((double)rollOver2);
 float cosRollOver2 = (float)Math.Cos((double)rollOver2);
 float pitchOver2 = pitch * 0.5f;
 float sinPitchOver2 = (float)Math.Sin((double)pitchOver2);
 float cosPitchOver2 = (float)Math.Cos((double)pitchOver2);
 float yawOver2 = yaw * 0.5f;
 float sinYawOver2 = (float)Math.Sin((double)yawOver2);
 float cosYawOver2 = (float)Math.Cos((double)yawOver2);
 Quaternion result = new Quaternion();
 result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
 result.X = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
 result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
 result.Z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
 return result;
 }

Answer 1

Your quaternion does not seem normalized. Quaternions representing a rotation should have norm 1.

Answer 2

To expand on Ben's answer, normalize the quaternion by dividing by the square root of the sum of the squares.

If you need to rotate the quaternion and keep its length, I recommend using quaternions for the rotation. Wikipedia has a great page on this: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

Answer 3

Be sure that you are using same Euler Angels in convertion "to" and "from".

You can find good reference code from Ken Shoemake http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/

And article "Euler angle conversion" Ken Shoemake from "Graphics Gems" series

• Rebs: This link now 404s. Please instead provide the relevant information where possible.