quaternion toEuler

xDan · Post by **xDan** » Sat Jul 10, 2010 6:54 pm

Hello. I think there is a bug in quaternion.toEuler. It seems to be related to gimbal lock.

If we read here, we need to account for this ("singularity at north/south pole"):
http://www.euclideanspace.com/maths/geo ... /index.htm

(not that I actually understand any of the maths)

I found the problem by passing some specific euler rotation into a quaternion, and when extracting them again with toEuler they would be completely wrong. It only occurs for certain angles, seemingly also only when the Y rotation == -90

In particular, I need to use slerp for interpolating stuff related to my game's camera. So with this problem, at certain rare circumstances I get the camera briefly flickering to some odd rotation. Not nice!

Here is some code showing the problem:

Code: Select all

#include <irrlicht.h>

using namespace irr;

bool vec_equals(const core::vector3df &a, const core::vector3df &b)
{
	return (core::equals(a.X,b.X,0.001f)
			&& core::equals(a.Y,b.Y,0.001f)
			&& core::equals(a.Z,b.Z,0.001f));
}

// This converts an euler rotation in degrees into a quaternion and then back again.
// For some rotations the result seems to be quite a bit different from the input.
void test_quaternion(const core::vector3df &rotEuler)
{
	printf("\nTest\n");
	printf("	Original rotation: %f,%f,%f\n", rotEuler.X,rotEuler.Y,rotEuler.Z);
	
	// Convert euler rotation to a quaternion
	core::quaternion quat( rotEuler * core::DEGTORAD );
	
	// And then convert back to an euler rotation
	core::vector3df resultRotEuler;
	quat.toEuler(resultRotEuler);
	resultRotEuler *= core::RADTODEG;
	
	// Result after passing through quaternion
	printf("	Quaternion result: %f,%f,%f\n", resultRotEuler.X,resultRotEuler.Y,resultRotEuler.Z);
	printf("\n");
	
	// Print both rotations as vectors as well
	// (since rotations may wrap around and stuff, this shows more clearly when the
	// the result is different from the original).
	core::vector3df vecOriginal = rotEuler.rotationToDirection();
	printf("	Original as vector: %f,%f,%f\n", vecOriginal.X,vecOriginal.Y,vecOriginal.Z);
	core::vector3df vecResult = resultRotEuler.rotationToDirection();
	printf("	Result as vector:   %f,%f,%f\n", vecResult.X,vecResult.Y,vecResult.Z);
	
	// Are the rotations the same?
	printf("\n	--> %s\n", vec_equals(vecOriginal,vecResult) ?
			"SUCCESS, rotations match" : "FAIL, rotations don't match");
}

// Only test the input of the quaternion
// ("does the quaternion applied to a vector perform the correct rotation?")
// If this fails too, it means it is the quaternion.set(eulerRot) function that has the bug.
// If this succeeds, then it means it is the quaternion.toEuler function that has the bug.
void test_quaternion_input_only(const core::vector3df &rotEuler)
{
	printf("\nTest input only\n");
	printf("	Original rotation: %f,%f,%f\n", rotEuler.X,rotEuler.Y,rotEuler.Z);
	
	// Convert euler rotation to a quaternion
	core::quaternion quat( rotEuler * core::DEGTORAD );
	

	core::vector3df vecOriginal = rotEuler.rotationToDirection();
	printf("	Original as vector: %f,%f,%f\n", vecOriginal.X,vecOriginal.Y,vecOriginal.Z);
	
	core::vector3df vecResult = quat * core::vector3df(0,0,1);
	printf("	Result as vector:   %f,%f,%f\n", vecResult.X,vecResult.Y,vecResult.Z);
	
	// Are the rotations the same?
	printf("\n	--> %s\n", vec_equals(vecOriginal,vecResult) ?
			"SUCCESS, rotations match" : "FAIL, rotations don't match");
}

// This code from "3D Math Primer for Graphics and Game Development"
// http://www.gamemath.com/downloads.htm
// Seems to work.
core::vector3df quaternion_to_euler(const core::quaternion &quat)
{
	f32 pitch,heading,bank;
	f32 sp = -2.0f * (quat.Y*quat.Z - quat.W*quat.X);

	if (fabs(sp) > 0.9999f)
	{
		pitch = core::HALF_PI * sp;
		heading = atan2(-quat.X*quat.Z + quat.W*quat.Y, 0.5f - quat.Y*quat.Y - quat.Z*quat.Z);
		bank = 0.0f;
	}
	else
	{
		pitch	= asin(sp);
		heading	= atan2(quat.X*quat.Z + quat.W*quat.Y, 0.5f - quat.X*quat.X - quat.Y*quat.Y);
		bank	= atan2(quat.X*quat.Y + quat.W*quat.Z, 0.5f - quat.X*quat.X - quat.Z*quat.Z);
	}
	return core::vector3df(pitch,heading,bank) * core::RADTODEG;
}

void test_quaternion_othermethod(const core::vector3df &rotEuler)
{
	printf("\nTest other method\n");
	printf("	Original rotation: %f,%f,%f\n", rotEuler.X,rotEuler.Y,rotEuler.Z);
	
	// Convert euler rotation to a quaternion
	core::quaternion quat( rotEuler * core::DEGTORAD );
	
	// And then convert back to an euler rotation
	core::vector3df resultRotEuler = quaternion_to_euler(quat);
	
	// Result after passing through quaternion
	printf("	Quaternion result: %f,%f,%f\n", resultRotEuler.X,resultRotEuler.Y,resultRotEuler.Z);
	printf("\n");
	
	// Print both rotations as vectors as well
	// (since rotations may wrap around and stuff, this shows more clearly when the
	// the result is different from the original).
	core::vector3df vecOriginal = rotEuler.rotationToDirection();
	printf("	Original as vector: %f,%f,%f\n", vecOriginal.X,vecOriginal.Y,vecOriginal.Z);
	core::vector3df vecResult = resultRotEuler.rotationToDirection();
	printf("	Result as vector:   %f,%f,%f\n", vecResult.X,vecResult.Y,vecResult.Z);
	
	// Are the rotations the same?
	printf("\n	--> %s\n", vec_equals(vecOriginal,vecResult) ?
			"SUCCESS, rotations match" : "FAIL, rotations don't match");
}

int main()
{
	core::vector3df rotSuccess(-57.197481, -90, 0);
	core::vector3df rotFail(-57.187481, -90, 0);
	
	// This particular rotation appears to work
	test_quaternion( rotSuccess );
	
	// This rotation fails, the resulting euler rotation is much different from the original.
	// Yet it's almost exactly the same rotation as that which works above.
	test_quaternion( rotFail );
	
	// Again, this time only test that the quaternion contains the correct rotation.
	// Both these succeed so it must be quaternion.toEuler that is failing!?
	test_quaternion_input_only( rotSuccess );
	test_quaternion_input_only( rotFail );
	
	// And again, this time using a different method for calculating toEuler.
	// This method works!!
	test_quaternion_othermethod( rotSuccess );
	test_quaternion_othermethod( rotFail );

	return 0;
}

Output is

Test
Original rotation: -57.197479,-90.000000,0.000000
Quaternion result: 151.401260,-90.000000,151.401260

Original as vector: -0.541745,0.840543,0.000000
Result as vector: -0.541745,0.840543,-0.000000

--> SUCCESS, rotations match

Test
Original rotation: -57.187481,-90.000000,0.000000
Quaternion result: 0.000000,-90.000000,0.000000

Original as vector: -0.541892,0.840448,0.000000
Result as vector: -1.000000,0.000000,0.000000

--> FAIL, rotations don't match

Test input only
Original rotation: -57.197479,-90.000000,0.000000
Original as vector: -0.541745,0.840543,0.000000
Result as vector: -0.541745,0.840543,-0.000000

--> SUCCESS, rotations match

Test input only
Original rotation: -57.187481,-90.000000,0.000000
Original as vector: -0.541892,0.840448,0.000000
Result as vector: -0.541892,0.840448,0.000000

--> SUCCESS, rotations match

Test other method
Original rotation: -57.197479,-90.000000,0.000000
Quaternion result: -57.197479,-90.000008,0.000004

Original as vector: -0.541745,0.840543,0.000000
Result as vector: -0.541745,0.840543,-0.000000

--> SUCCESS, rotations match

Test other method
Original rotation: -57.187481,-90.000000,0.000000
Quaternion result: -57.187477,-89.999992,0.000000

Original as vector: -0.541892,0.840448,0.000000
Result as vector: -0.541892,0.840448,0.000000

--> SUCCESS, rotations match

The code there from "3D Math Primer for Graphics and Game Development" (page 192) seems to work perfectly. However I do not know the license for that code.

shadowslair · Post by **shadowslair** » Sat Jul 10, 2010 7:55 pm

xDan wrote:The code there from "3D Math Primer for Graphics and Game Development" (page 192) seems to work perfectly. However I do not know the license for that code.

The authors never mention any license- neither in the book, nor in the code. They`re using 25 books for references, and probably this code is from reference book #22 (Graphics Gems II). Too lazy to browse the archive right now. Should be fine.

xDan · Post by **xDan** » Tue Jul 13, 2010 6:14 pm

Maybe someone knows how to fix the currently code easily, or you have some other reason not to change it - but just in case, I sent an email to the author of the book and they have kindly given permission to use their code. (I can forward an admin the full email if you require it)

From: Fletcher Dunn <...>
To: Daniel Frith <...>

Certainly use away! I would appreciate a comment in the code with a
reference to where the formula came from. (I mean as an author and a coder
--- I always wonder when I see formulas in code, where the person writing
them got them from... )

And here also is their full annotated code

Code: Select all

void	EulerAngles::fromObjectToInertialQuaternion(const Quaternion &q) {

	// Extract sin(pitch)

	float sp = -2.0f * (q.y*q.z - q.w*q.x);

	// Check for Gimbel lock, giving slight tolerance for numerical imprecision

	if (fabs(sp) > 0.9999f) {

		// Looking straight up or down

		pitch = kPiOver2 * sp;

		// Compute heading, slam bank to zero

		heading = atan2(-q.x*q.z + q.w*q.y, 0.5f - q.y*q.y - q.z*q.z);
		bank = 0.0f;

	} else {

		// Compute angles.  We don't have to use the "safe" asin
		// function because we already checked for range errors when
		// checking for Gimbel lock

		pitch	= asin(sp);
		heading	= atan2(q.x*q.z + q.w*q.y, 0.5f - q.x*q.x - q.y*q.y);
		bank	= atan2(q.x*q.y + q.w*q.z, 0.5f - q.x*q.x - q.z*q.z);
	}
}