diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c
index 0a34c1c..9c26d0e 100644
--- a/source/maths/quat_frommtx.c
+++ b/source/maths/quat_frommtx.c
@@ -2,54 +2,61 @@
 
 C3D_FQuat Quat_FromMtx(const 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;
+	// Original algorithm taken from here (with some optimizations):
+	//   https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
+	// Layout of the algorithm:
+	//   First,  we select a large (non-zero!) component "P" in the output quaternion (q)
+	//           (we can test this just by looking at the diagonal of the matrix)
+	//   Second, we calculate q' which is our desired output quaternion (q) scaled by 4P
+	//           (this can be done with simple additions; the 4P factor is large and non-zero thanks to above)
+	//   Third,  we normalize q' to finally obtain q
+	//           (this will work because normalize(kq) = q for any k scalar and q unit quaternion)
+
 	C3D_FQuat q;
-	
-	//Check the main diagonal of the passed-in matrix for positive/negative signs.
-	trace = m->r[0].x + m->r[1].y + m->r[2].z;
-	if (trace > 0.0f)
+	C3D_FVec  diagonal = FVec4_New(m->r[0].x, m->r[1].y, m->r[2].z, 1.0f);
+
+	// Check if x^2 + y^2 >= z^2 + w^2
+	if (diagonal.z <= 0.0f)
 	{
-		//Diagonal is positive.
-		sqrtTrace = sqrtf(trace + 1.0f);
-		q.w = sqrtTrace / 2.0f;
-		sqrtTrace = 0.5 / sqrtTrace;
-		q.x = (m->r[1].z - m->r[2].y) * sqrtTrace;
-		q.y = (m->r[2].x - m->r[0].z) * sqrtTrace;
-		q.z = (m->r[0].y - m->r[1].x) * 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].x > m->r[1].y && m->r[0].x > m->r[2].z)
+		// Check if |x| >= |y|
+		if (diagonal.x >= diagonal.y)
 		{
-			sqrtTrace = 2.0f * sqrtf(1.0f + m->r[0].x - m->r[1].y - m->r[2].z);
-			q.w = (m->r[2].y - m->r[1].z) / sqrtTrace;
-			q.x = 0.25f * sqrtTrace;
-			q.y = (m->r[0].y - m->r[1].x) / sqrtTrace;
-			q.z = (m->r[0].z - m->r[2].x) / sqrtTrace;
+			// X case
+			q.x = diagonal.w + diagonal.x - diagonal.y - diagonal.z;
+			q.y = m->r[1].x + m->r[0].y;
+			q.z = m->r[2].x + m->r[0].z;
+			q.w = m->r[2].y - m->r[1].z;
 		}
-		else if (m->r[1].y > m->r[2].z)
+		else
 		{
-			sqrtTrace = 2.0f * sqrtf(1.0f + m->r[1].y - m->r[0].x - m->r[2].z);
-			q.w = (m->r[0].z - m->r[2].x) / sqrtTrace;
-			q.x = (m->r[0].y - m->r[1].x) / sqrtTrace;
-			q.y = 0.25f * sqrtTrace;
-			q.z = (m->r[1].z - m->r[2].y) / sqrtTrace;
-		}
-		else 
-		{
-			sqrtTrace = 2.0f * sqrtf(1.0f + m->r[2].z - m->r[0].x - m->r[1].y);
-			q.w = (m->r[1].x - m->r[0].y) / sqrtTrace;
-			q.x = (m->r[0].z - m->r[2].x) / sqrtTrace;
-			q.y = (m->r[1].z - m->r[2].y) / sqrtTrace;
-			q.z = 0.25f * sqrtTrace;
+			// Y case
+			q.x = m->r[1].x + m->r[0].y;
+			q.y = diagonal.w - diagonal.x + diagonal.y - diagonal.z;
+			q.z = m->r[2].y + m->r[1].z;
+			q.w = m->r[0].z - m->r[2].x;
 		}
 	}
-	return q;
+	else
+	{
+		// Check if |z| >= |w|
+		if (-diagonal.x >= diagonal.y)
+		{
+			// Z case
+			q.x = m->r[2].x + m->r[0].z;
+			q.y = m->r[2].y + m->r[1].z;
+			q.z = diagonal.w - diagonal.x - diagonal.y + diagonal.z;
+			q.w = m->r[1].x - m->r[0].y;
+		}
+		else
+		{
+			// W case
+			q.x = m->r[2].y - m->r[1].z;
+			q.y = m->r[0].z - m->r[2].x;
+			q.z = m->r[1].x - m->r[0].y;
+			q.w = diagonal.w + diagonal.x + diagonal.y + diagonal.z;
+		}
+	}
+
+	// Normalize the quaternion
+	return Quat_Normalize(q);
 }
diff --git a/test/pc/main.cpp b/test/pc/main.cpp
index 4ad6b3b..e2c566e 100644
--- a/test/pc/main.cpp
+++ b/test/pc/main.cpp
@@ -953,13 +953,15 @@ check_quaternion(generator_t &gen, distribution_t &dist)
 
     // check conversion to matrix
     {
-      C3D_FQuat q = randomQuat(gen, dist);
+      C3D_FQuat q = Quat_Normalize(randomQuat(gen, dist));
       glm::quat g = loadQuat(q);
 
       C3D_Mtx m;
       Mtx_FromQuat(&m, q);
-
       assert(m == glm::mat4_cast(g));
+
+      C3D_FQuat q2 = Quat_FromMtx(&m);
+      assert(q2 == q || q2 == FVec4_Negate(q));
     }
   }
 }