Bug fixes

Kazade · Dec 18, 2011 · 59e3af9 · 59e3af9
1 parent 48a1a18
commit 59e3af9
Show file tree

Hide file tree

Showing 4 changed files with 77 additions and 27 deletions.
diff --git a/include/kazmath/mat3.h b/include/kazmath/mat3.h
@@ -1,3 +1,6 @@
+#ifndef HEADER_8E9D0ABA3C76B989
+#define HEADER_8E9D0ABA3C76B989
+
 /*
 Copyright (c) 2008, Luke Benstead.
 All rights reserved.
@@ -56,6 +59,10 @@ struct kmVec3* const kmMat3RotationToAxisAngle(struct kmVec3* pAxis, kmScalar* r
 kmMat3* const kmMat3Assign(kmMat3* pOut, const kmMat3* pIn);
 const int  kmMat3AreEqual(const kmMat3* pM1, const kmMat3* pM2);
 
+struct kmVec3* const kmMat3GetUpVec3(struct kmVec3* pOut, const kmMat3* pIn);
+struct kmVec3* const kmMat3GetRightVec3(struct kmVec3* pOut, const kmMat3* pIn);
+struct kmVec3* const kmMat3GetForwardVec3(struct kmVec3* pOut, const kmMat3* pIn);
+
 kmMat3* const kmMat3RotationX(kmMat3* pOut, const kmScalar radians);
 kmMat3* const kmMat3RotationY(kmMat3* pOut, const kmScalar radians);
 kmMat3* const kmMat3RotationZ(kmMat3* pOut, const kmScalar radians);
@@ -73,3 +80,5 @@ struct kmVec3* const kmMat3RotationToAxisAngle(struct kmVec3* pAxis, kmScalar* r
 #endif
 #endif // MAT3_H_INCLUDED
 
+
+#endif // header guard
diff --git a/src/mat3.c b/src/mat3.c
@@ -368,3 +368,34 @@ kmMat3* const kmMat3RotationZ(kmMat3* pOut, const float radians)
 
 	return pOut;
 }
+
+kmVec3* const kmMat3GetUpVec3(kmVec3* pOut, const kmMat3* pIn) {
+	pOut->x = pIn->mat[3];
+	pOut->y = pIn->mat[4];
+	pOut->z = pIn->mat[5];
+
+	kmVec3Normalize(pOut, pOut);
+
+	return pOut;
+}
+
+kmVec3* const kmMat3GetRightVec3(kmVec3* pOut, const kmMat3* pIn) {
+	pOut->x = pIn->mat[0];
+	pOut->y = pIn->mat[1];
+	pOut->z = pIn->mat[2];
+
+	kmVec3Normalize(pOut, pOut);
+
+	return pOut;
+}
+
+kmVec3* const kmMat3GetForwardVec3(kmVec3* pOut, const kmMat3* pIn) {
+	pOut->x = pIn->mat[6];
+	pOut->y = pIn->mat[7];
+	pOut->z = pIn->mat[8];
+
+	kmVec3Normalize(pOut, pOut);
+
+	return pOut;
+}
+
diff --git a/src/mat4.c b/src/mat4.c
@@ -166,14 +166,14 @@ kmMat4* const kmMat4Inverse(kmMat4* pOut, const kmMat4* pM)
 {
     kmMat4 inv;
     kmMat4Assign(&inv, pM);
-    
+
     kmMat4 tmp;
     kmMat4Identity(&tmp);
-    
+
     if(gaussj(&inv, &tmp) == KM_FALSE) {
         return NULL;
     }
-    
+
     kmMat4Assign(pOut, &inv);
     return pOut;
 }
@@ -283,7 +283,7 @@ kmMat4* const kmMat4RotationAxisAngle(kmMat4* pOut, const kmVec3* axis, kmScalar
 {
 	float rcos = cosf(radians);
 	float rsin = sinf(radians);
-	
+
 	kmVec3 normalizedAxis;
 	kmVec3Normalize(&normalizedAxis, axis);
 
@@ -456,21 +456,31 @@ kmMat4* const kmMat4RotationPitchYawRoll(kmMat4* pOut, const kmScalar pitch, con
  */
 kmMat4* const kmMat4RotationQuaternion(kmMat4* pOut, const kmQuaternion* pQ)
 {
-	pOut->mat[0] = 1.0f - 2.0f * (pQ->y * pQ->y + pQ->z * pQ->z );
-	pOut->mat[1] = 2.0f * (pQ->x * pQ->y + pQ->z * pQ->w);
-	pOut->mat[2] = 2.0f * (pQ->x * pQ->z - pQ->y * pQ->w);
+    float x2 = pQ->x * pQ->x;
+    float y2 = pQ->y * pQ->y;
+    float z2 = pQ->z * pQ->z;
+    float xy = pQ->x * pQ->y;
+    float xz = pQ->x * pQ->z;
+    float yz = pQ->y * pQ->z;
+    float wx = pQ->w * pQ->x;
+    float wy = pQ->w * pQ->y;
+    float wz = pQ->w * pQ->z;
+
+	pOut->mat[0] = 1.0f - 2.0f * (y2 + z2);
+	pOut->mat[1] = 2.0f * (xy - wz);
+	pOut->mat[2] = 2.0f * (xz + wy);
 	pOut->mat[3] = 0.0f;
 
 	// Second row
-	pOut->mat[4] = 2.0f * ( pQ->x * pQ->y - pQ->z * pQ->w );
-	pOut->mat[5] = 1.0f - 2.0f * ( pQ->x * pQ->x + pQ->z * pQ->z );
-	pOut->mat[6] = 2.0f * (pQ->z * pQ->y + pQ->x * pQ->w );
+	pOut->mat[4] = 2.0f * (xy + wz);
+	pOut->mat[5] = 1.0f - 2.0f * (x2 + z2);
+	pOut->mat[6] = 2.0f * (yz - wx);
 	pOut->mat[7] = 0.0f;
 
 	// Third row
-	pOut->mat[8] = 2.0f * ( pQ->x * pQ->z + pQ->y * pQ->w );
-	pOut->mat[9] = 2.0f * ( pQ->y * pQ->z - pQ->x * pQ->w );
-	pOut->mat[10] = 1.0f - 2.0f * ( pQ->x * pQ->x + pQ->y * pQ->y );
+	pOut->mat[8] = 2.0f * (xz - wy);
+	pOut->mat[9] = 2.0f * (yz + wx);
+	pOut->mat[10] = 1.0f - 2.0f * (x2 + y2);
 	pOut->mat[11] = 0.0f;
 
 	// Fourth row
@@ -692,7 +702,7 @@ kmVec3* const kmMat4RotationToAxisAngle(kmVec3* pAxis, kmScalar* radians, const
     return pAxis;
 }
 
-/** Build a 4x4 OpenGL transformation matrix using a 3x3 rotation matrix, 
+/** Build a 4x4 OpenGL transformation matrix using a 3x3 rotation matrix,
  * and a 3d vector representing a translation. Assign the result to pOut,
  * pOut is also returned.
  */
@@ -702,29 +712,29 @@ kmMat4* const kmMat4RotationTranslation(kmMat4* pOut, const kmMat3* rotation, co
     pOut->mat[1] = rotation->mat[1];
     pOut->mat[2] = rotation->mat[2];
     pOut->mat[3] = 0.0f;
-    
+
     pOut->mat[4] = rotation->mat[3];
     pOut->mat[5] = rotation->mat[4];
     pOut->mat[6] = rotation->mat[5];
     pOut->mat[7] = 0.0f;
-    
+
     pOut->mat[8] = rotation->mat[6];
     pOut->mat[9] = rotation->mat[7];
     pOut->mat[10] = rotation->mat[8];
     pOut->mat[11] = 0.0f;
-    
+
     pOut->mat[12] = translation->x;
     pOut->mat[13] = translation->y;
     pOut->mat[14] = translation->z;
     pOut->mat[15] = 1.0f;
-    
+
     return pOut;
 }
 
 kmPlane* const kmMat4ExtractPlane(kmPlane* pOut, const kmMat4* pIn, const kmEnum plane)
 {
     float t = 1.0f;
-    
+
     switch(plane) {
         case KM_PLANE_RIGHT:
             pOut->a = pIn->mat[3] - pIn->mat[0];

diff --git a/src/quaternion.c b/src/quaternion.c
@@ -140,10 +140,10 @@ kmQuaternion* kmQuaternionMultiply(kmQuaternion* pOut,
 								 const kmQuaternion* q1,
 								 const kmQuaternion* q2)
 {
-	pOut->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
 	pOut->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
 	pOut->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
 	pOut->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
+    pOut->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
 
 	return pOut;
 }
@@ -216,21 +216,21 @@ taken from the Matrix and Quaternion FAQ
 
 /*	0 3 6
 	1 4 7
-	2 5 8 
+	2 5 8
 
 	0 1 2 3
 	4 5 6 7
 	8 9 10 11
 	12 13 14 15*/
 
-	m4x4[0]  = pIn->mat[0];	
-	m4x4[1]  = pIn->mat[3];	
+	m4x4[0]  = pIn->mat[0];
+	m4x4[1]  = pIn->mat[3];
 	m4x4[2]  = pIn->mat[6];
-	m4x4[4]  = pIn->mat[1];	
-	m4x4[5]  = pIn->mat[4];	
+	m4x4[4]  = pIn->mat[1];
+	m4x4[5]  = pIn->mat[4];
 	m4x4[6]  = pIn->mat[7];
-	m4x4[8]  = pIn->mat[2];	
-	m4x4[9]  = pIn->mat[5];	
+	m4x4[8]  = pIn->mat[2];
+	m4x4[9]  = pIn->mat[5];
 	m4x4[10] = pIn->mat[8];
 	m4x4[15] = 1;
 	pMatrix = &m4x4[0];