From 765bf1623299e81f5f95b78a78e7d66997e4ad0c Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 9 Mar 2017 20:00:42 -0500
Subject: Fix wrong quat functions + Add matrix->quat conversion.

---
 redist/linmath.c | 58 ++++++++++++++++++++++++++++++++++++++++++++------------
 redist/linmath.h |  5 +++--
 2 files changed, 49 insertions(+), 14 deletions(-)

diff --git a/redist/linmath.c b/redist/linmath.c
index 1c5c25b..d76fbba 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -152,7 +152,7 @@ FLT quatmagnitude( const FLT * q )
 
 FLT quatinvsqmagnitude( const FLT * q )
 {
-	return ((FLT)1.)/((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
+	return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
 }
 
 
@@ -168,17 +168,17 @@ void quattomatrix(FLT * matrix44, const FLT * qin)
 	quatnormalize(q, qin);
 
 	//Reduced calulation for speed
-	FLT xx = 2 * q[0] * q[0];
-	FLT xy = 2 * q[0] * q[1];
-	FLT xz = 2 * q[0] * q[2];
-	FLT xw = 2 * q[0] * q[3];
+	FLT xx = 2 * q[1] * q[1];
+	FLT xy = 2 * q[1] * q[2];
+	FLT xz = 2 * q[1] * q[3];
+	FLT xw = 2 * q[1] * q[0];
 
-	FLT yy = 2 * q[1] * q[1];
-	FLT yz = 2 * q[1] * q[2];
-	FLT yw = 2 * q[1] * q[3];
+	FLT yy = 2 * q[2] * q[2];
+	FLT yz = 2 * q[2] * q[3];
+	FLT yw = 2 * q[2] * q[0];
 
-	FLT zz = 2 * q[2] * q[2];
-	FLT zw = 2 * q[2] * q[3];
+	FLT zz = 2 * q[3] * q[3];
+	FLT zw = 2 * q[3] * q[0];
 
 	//opengl major
 	matrix44[0] = 1 - yy - zz;
@@ -202,6 +202,40 @@ void quattomatrix(FLT * matrix44, const FLT * qin)
 	matrix44[15] = 1;
 }
 
+
+void quatfrommatrix( FLT * q, const FLT * matrix44 )
+{
+	//Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+	float tr = matrix44[0] + matrix44[5] + matrix44[10];
+
+	if (tr > 0) {
+		float S = sqrt(tr+1.0) * 2; // S=4*qw
+		q[0] = 0.25 * S;
+		q[1] = (matrix44[9] - matrix44[6]) / S;
+		q[2] = (matrix44[2] - matrix44[8]) / S;
+		q[3] = (matrix44[4] - matrix44[1]) / S;
+	} else if ((matrix44[0] > matrix44[5])&(matrix44[0] > matrix44[10])) {
+		float S = sqrt(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2; // S=4*qx
+		q[0] = (matrix44[9] - matrix44[6]) / S;
+		q[1] = 0.25 * S;
+		q[2] = (matrix44[1] + matrix44[4]) / S;
+		q[3] = (matrix44[2] + matrix44[8]) / S;
+	} else if (matrix44[5] > matrix44[10]) {
+		float S = sqrt(1.0 + matrix44[5] - matrix44[0] - matrix44[10]) * 2; // S=4*qy
+		q[0] = (matrix44[2] - matrix44[8]) / S;
+		q[1] = (matrix44[1] + matrix44[4]) / S;
+		q[2] = 0.25 * S;
+		q[3] = (matrix44[6] + matrix44[9]) / S;
+	} else {
+		float S = sqrt(1.0 + matrix44[10] - matrix44[0] - matrix44[5]) * 2; // S=4*qz
+		q[0] = (matrix44[4] - matrix44[1]) / S;
+		q[1] = (matrix44[2] + matrix44[8]) / S;
+		q[2] = (matrix44[6] + matrix44[9]) / S;
+		q[3] = 0.25 * S;
+	}
+}
+
+
 void quattomatrix33(FLT * matrix33, const FLT * qin)
 {
 	FLT q[4];
@@ -270,8 +304,8 @@ void quatrotateabout( FLT * qout, const FLT * a, const FLT * b )
 	FLT q1[4];
 	FLT q2[4];
 
-	quatnormalize( q1, a );
-	quatnormalize( q2, b );
+	//quatnormalize( q1, a );
+	//quatnormalize( q2, b );
 
 	qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
 	qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
diff --git a/redist/linmath.h b/redist/linmath.h
index caec281..ec20534 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -7,8 +7,8 @@
 #define DEFAULT_EPSILON 0.001
 
 //For printf
-#define PFTHREE(x) x[0], x[1], x[2]
-#define PFFOUR(x) x[0], x[1], x[2], x[3]
+#define PFTHREE(x) (x)[0], (x)[1], (x)[2]
+#define PFFOUR(x) (x)[0], (x)[1], (x)[2], (x)[3]
 
 #define LINMATHPI ((FLT)3.141592653589)
 
@@ -76,6 +76,7 @@ FLT quatmagnitude( const FLT * q );
 FLT quatinvsqmagnitude( const FLT * q );
 void quatnormalize( FLT * qout, const FLT * qin );  //Safe for in to be same as out.
 void quattomatrix( FLT * matrix44, const FLT * q );
+void quatfrommatrix( FLT * q, const FLT * matrix44 );
 void quatgetconjugate( FLT * qout, const FLT * qin );
 void quatgetreciprocal( FLT * qout, const FLT * qin );
 void quatsub( FLT * qout, const FLT * a, const FLT * b );
-- 
cgit v1.2.3