From ad267f050503447a275b40fc4c047878a161ae97 Mon Sep 17 00:00:00 2001
From: Nathan Bowhay <skienemy5555@gmail.com>
Date: Tue, 3 Feb 2015 12:16:06 -0800
Subject: [PATCH] Fixed angle conversion issues

Fixed a variable name and method that should be const.
Also fixed several angle conversion functions that didn't convert the
angle correct.
---
 Engine/source/math/mMatrix.cpp |  2 +-
 Engine/source/math/mQuat.cpp   | 67 +++++++++++++++++++++-------------
 Engine/source/math/mQuat.h     |  2 +-
 3 files changed, 43 insertions(+), 28 deletions(-)

diff --git a/Engine/source/math/mMatrix.cpp b/Engine/source/math/mMatrix.cpp
index 12af818ea..da1c137ca 100644
--- a/Engine/source/math/mMatrix.cpp
+++ b/Engine/source/math/mMatrix.cpp
@@ -163,7 +163,7 @@ EulerF MatrixF::toEuler() const
    const F32 * mat = m;
 
    EulerF r;
-   r.x = mAsin(mat[MatrixF::idx(2,1)]);
+   r.x = mAsin(mClampF(mat[MatrixF::idx(2,1)], -1.0, 1.0));
 
    if(mCos(r.x) != 0.f)
    {
diff --git a/Engine/source/math/mQuat.cpp b/Engine/source/math/mQuat.cpp
index 7ffc4eaa3..29180d837 100644
--- a/Engine/source/math/mQuat.cpp
+++ b/Engine/source/math/mQuat.cpp
@@ -32,33 +32,48 @@ const QuatF QuatF::Identity(0.0f,0.0f,0.0f,1.0f);
 
 QuatF& QuatF::set( const EulerF & e )
 {
-   F32 cx, sx;
-   F32 cy, sy;
-   F32 cz, sz;
-   mSinCos( e.x * 0.5f, sx, cx );
-   mSinCos( e.y * 0.5f, sy, cy );
-   mSinCos( e.z * 0.5f, sz, cz );
+	/*
+	F32 cx, sx;
+	F32 cy, sy;
+	F32 cz, sz;
+	mSinCos( -e.x * 0.5f, sx, cx );
+	mSinCos( -e.y * 0.5f, sy, cy );
+	mSinCos( -e.z * 0.5f, sz, cz );
 
-   // Qyaw(z) = [ (0, 0, sin z/2), cos z/2 ]
-   // Qpitch(x) = [ (sin x/2, 0, 0), cos x/2 ]
-   // Qroll(y) = [ (0, sin y/2, 0), cos y/2 ]
-   // this = Qresult = Qyaw*Qpitch*Qroll ZXY
-   //
-   // The code that folows is a simplification of:
-   // roll*=pitch;
-   // roll*=yaw;
-   // *this = roll;
-   F32 cycz, sysz, sycz, cysz;
-   cycz = cy*cz;
-   sysz = sy*sz;
-   sycz = sy*cz;
-   cysz = cy*sz;
-   w = cycz*cx - sysz*sx;
-   x = cycz*sx + sysz*cx;
-   y = sycz*cx + cysz*sx;
-   z = cysz*cx - sycz*sx;
+	// Qyaw(z)   = [ (0, 0, sin z/2), cos z/2 ]
+	// Qpitch(x) = [ (sin x/2, 0, 0), cos x/2 ]
+	// Qroll(y)  = [ (0, sin y/2, 0), cos y/2 ]
+	// this = Qresult = Qyaw*Qpitch*Qroll  ZXY
+	//
+	// The code that folows is a simplification of:
+	//    roll*=pitch;
+	//    roll*=yaw;
+	//    *this = roll;
+	F32 cycz, sysz, sycz, cysz;
+	cycz = cy*cz;
+	sysz = sy*sz;
+	sycz = sy*cz;
+	cysz = cy*sz;
+	w = cycz*cx + sysz*sx;
+	x = cycz*sx + sysz*cx;
+	y = sycz*cx - cysz*sx;
+	z = cysz*cx - sycz*sx;
+	*/
+	// Assuming the angles are in radians.
+	F32 c1 = mCos(e.y/2.0f);
+	F32 s1 = mSin(e.y/2.0f);
+	F32 c2 = mCos(e.z/2.0f);
+	F32 s2 = mSin(e.z/2.0f);
+	F32 c3 = mCos(e.x/2.0f);
+	F32 s3 = mSin(e.x/2.0f);
+	F32 c1c2 = c1*c2;
+	F32 s1s2 = s1*s2;
+	w =c1c2*c3 - s1s2*s3;
+	x =c1c2*s3 + s1s2*c3;
+	y =s1*c2*c3 + c1*s2*s3;
+	z =c1*s2*c3 - s1*c2*s3;
 
-   return *this;
+	return *this;
 }
 
 QuatF& QuatF::operator *=( const QuatF & b )
@@ -289,7 +304,7 @@ QuatF & QuatF::interpolate( const QuatF & q1, const QuatF & q2, F32 t )
    return *this;
 }
 
-Point3F & QuatF::mulP(const Point3F& p, Point3F* r)
+Point3F & QuatF::mulP(const Point3F& p, Point3F* r) const
 {
    QuatF qq;
    QuatF qi = *this;
diff --git a/Engine/source/math/mQuat.h b/Engine/source/math/mQuat.h
index 433b30155..2ea1b94c1 100644
--- a/Engine/source/math/mQuat.h
+++ b/Engine/source/math/mQuat.h
@@ -81,7 +81,7 @@ public:
    QuatF& interpolate( const QuatF & q1, const QuatF & q2, F32 t );
    F32  angleBetween( const QuatF & q );
 
-   Point3F& mulP(const Point3F& a, Point3F* b);   // r = p * this
+   Point3F& mulP(const Point3F& a, Point3F* r) const;   // r = p * this
    QuatF& mul(const QuatF& a, const QuatF& b);    // This = a * b
 
    // Vectors passed in must be normalized