stole lightfeather's quaternion::toEuler function by mm765 (with permission)...

stole lightfeather's quaternion::toEuler function by mm765 (with permission) as it deals with singularities. added a constant for pi/2.

git-svn-id: svn://svn.code.sf.net/p/irrlicht/code/trunk@795 dfc29bdd-3216-0410-991c-e03cc46cb475

include/irrMath.h

@@ -41,6 +41,9 @@ namespace core
 	//! Constant for reciprocal of PI.
 	const f32 RECIPROCAL_PI		= 1.0f/PI;
 
+	//! Constant for half of PI.
+	const f32 HALF_PI		= PI/2.0f;
+
 	//! Constant for 64bit PI.
 	const f64 PI64			= 3.1415926535897932384626433832795028841971693993751;
 

include/quaternion.h

@@ -103,6 +103,9 @@ class quaternion
 		//! set quaternion to identity
 		void makeIdentity();
 
+		//! sets quaternion to represent a rotation from one angle to another
+		void rotationFromTo(const vector3df& from, const vector3df& to);
+
 		f32 X, Y, Z, W;
 };
 
@@ -492,19 +495,37 @@ inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
 
 inline void quaternion::toEuler(vector3df& euler) const
 {
-	double sqw = W*W;
-	double sqx = X*X;
-	double sqy = Y*Y;
-	double sqz = Z*Z;
+	/* new version after euclideanspace.com by Matthias Meyer
+	old version had a problem where the initial quaternion
+	would return -0 for Y value and NaN for rotation of 1.5708
+	around Y-axis when using fromangleaxis() */
 
-	// heading = rotation about z-axis
-	euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
+	f64 testValue = X * Y + Z * W;
+	if(testValue > 0.499f) // north pole singularity
+	{
+		euler.Y = (f32) (2 * atan2(X,W));
+		euler.Z = (f32) (HALF_PI);
+		euler.X = 0.0f;
+		return;
+	}
+	if(testValue < -0.499f) // south pole singularity
+	{
+		euler.Y = (f32) (-2 * atan2(X,W));
+		euler.Z = (f32) (-HALF_PI);
+		euler.X = 0.0f;
+		return;
+	}
+	f64 sqx = X*X;
+	f64 sqy = Y*Y;
+	f64 sqz = Z*Z;
+	f64 sqw = W*W;
+	f64 unit = sqx + sqy + sqz + sqw;
 
-	// bank = rotation about x-axis
-	euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
+	euler.Y = (f32) atan2( 2.0*Y*W - 2.0*X*Z, sqx - sqy - sqz + sqw );
+	euler.Z = (f32) asin ( 2.0 * testValue / unit );
+	euler.X = (f32) atan2( 2.0*X*W - 2.0*Y*Z, -sqx + sqy - sqz + sqw );
 
-	// attitude = rotation about y-axis
-	euler.Y = (f32) (asin( clamp(-2.0 * (X*Z - Y*W), -1.0, 1.0) ));
+	return;
 }
 
 inline vector3df quaternion::operator* (const vector3df& v) const
@@ -530,6 +551,31 @@ inline void quaternion::makeIdentity()
 	Z = 0.f;
 }
 
+inline void quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
+{
+	// Based on Stan Melax's article in Game Programming Gems
+	// Copy, since cannot modify local
+	vector3df v0 = from;
+	vector3df v1 = to;
+	v0.normalize();
+	v1.normalize();
+
+	vector3df c = v0.crossProduct(v1);
+
+	f32 d = v0.dotProduct(v1);
+	if (d >= 1.0f) // If dot == 1, vectors are the same
+	{
+		*this=quaternion(0,0,0,1); //IDENTITY;
+	}
+	f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
+	f32 invs = 1 / s;
+
+	X = c.X * invs;
+	Y = c.Y * invs;
+	Z = c.Z * invs;
+	W = s * 0.5f;
+}
+
 
 } // end namespace core
 } // end namespace irr