From 6eb0cacd0a18b79a4b6dc3b1219ff3c19fce0bd7 Mon Sep 17 00:00:00 2001
From: Thompson Lee <tom.mai78101@gmail.com>
Date: Sat, 6 Aug 2016 23:02:17 -0400
Subject: [PATCH] Added Quat_FromMtx(). This converts a matrix to a quaternion.

Quat_FromMtx() added.
---
 include/c3d/maths.h         |  8 ++++++
 source/maths/quat_frommtx.c | 53 +++++++++++++++++++++++++++++++++++++
 2 files changed, 61 insertions(+)
 create mode 100644 source/maths/quat_frommtx.c

diff --git a/include/c3d/maths.h b/include/c3d/maths.h
index 1587231..9113cf3 100644
--- a/include/c3d/maths.h
+++ b/include/c3d/maths.h
@@ -621,6 +621,14 @@ C3D_FQuat Quat_RotateZ(C3D_FQuat q, float r, bool bRightSide);
  */
 void Mtx_FromQuat(C3D_Mtx* m, C3D_FQuat q);
 
+/**
+ * @brief Get Quaternion equivalent to 4x4 matrix
+ * @note If the matrix is orthogonal or special orthogonal, where determinant(matrix) = +1.0f, then the matrix can be converted. 
+ * @param[out]  q Output Quaternion
+ * @param[in]   m Input  Matrix
+ */
+void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m);
+
 /**
  * @brief Identity Quaternion
  * @return Identity Quaternion
diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c
new file mode 100644
index 0000000..eba7e96
--- /dev/null
+++ b/source/maths/quat_frommtx.c
@@ -0,0 +1,53 @@
+#include <c3d/maths.h>
+
+void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m)
+{
+	//Taken from Gamasutra:
+	//http://www.gamasutra.com/view/feature/131686/rotating_objects_using_quaternions.php
+	//Expanded upon from:
+	//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+	
+	//Variables we need.
+	float trace, sqrtTrace;
+	
+	//Check the main diagonal of the passed-in matrix for positive/negative signs.
+	trace = m.r[0].c[0] + m.r[1].c[1] + m.r[2].c[2];
+	if (trace > 0.0f)
+	{
+		//Diagonal is positive.
+		sqrtTrace = sqrtf(trace + 1.0f);
+		q->w = sqrtTrace / 2.0f;
+		sqrtTrace = 0.5 / sqrtTrace;
+		q->x = (m.r[1].c[2] - m.r[2].c[1]) * sqrtTrace;
+		q->y = (m.r[2].c[0] - m.r[0].c[2]) * sqrtTrace;
+		q->z = (m.r[0].c[1] - m.r[1].c[0]) * sqrtTrace;
+	}
+	else 
+	{
+		//Diagonal is negative or equals to zero. We need to identify which major diagonal element has the greatest value.
+		if (m.r[0].c[0] > m.r[1].c[1] && m.r[0].c[0] > m.r[2].c[2])
+		{
+			sqrtTrace = 2.0f * sqrtf(1.0f + m.r[0].c[0] - m.r[1].c[1] - m.r[2].c[2]);
+			q->w = (m.r[2].c[1] - m.r[1].c[2]) / sqrtTrace;
+			q->x = 0.25f * sqrtTrace;
+			q->y = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace;
+			q->z = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace;
+		}
+		else if (m.r[1].c[1] > m.r[2].c[2])
+		{
+			sqrtTrace = 2.0f * sqrtf(1.0f + m.r[1].c[1] - m.r[0].c[0] - m.r[2].c[2]);
+			q->w = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace;
+			q->x = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace;
+			q->y = 0.25f * sqrtTrace;
+			q->z = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace;
+		}
+		else 
+		{
+			sqrtTrace = 2.0f * sqrtf(1.0f + m.r[2].c[2] - m.r[0].c[0] - m.r[1].c[1]);
+			q->w = (m.r[1].c[0] - m.r[0].c[1]) / sqrtTrace;
+			q->x = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace;
+			q->y = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace;
+			q->z = 0.25f * sqrtTrace;
+		}
+	}
+}