From 6d2a1118bc8fe46355d44a56d08e89cb907ec5c6 Mon Sep 17 00:00:00 2001
From: Lynix <lynix680@gmail.com>
Date: Thu, 16 May 2013 22:21:16 +0200
Subject: [PATCH] Implemented Matrix4::GetRotation

Former-commit-id: 90e0b6942497c1337ac3873d3990515284df4aa6
---
 include/Nazara/Math/Matrix4.inl | 48 +++++++++++++++++++++++++++++++++
 1 file changed, 48 insertions(+)

diff --git a/include/Nazara/Math/Matrix4.inl b/include/Nazara/Math/Matrix4.inl
index a1852ec73..bcf71684b 100644
--- a/include/Nazara/Math/Matrix4.inl
+++ b/include/Nazara/Math/Matrix4.inl
@@ -385,6 +385,54 @@ bool NzMatrix4<T>::GetInverseAffine(NzMatrix4* dest) const
 	}
 }
 
+template<typename T>
+NzQuaternion<T> NzMatrix4<T>::GetRotation() const
+{
+	// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+	NzQuaternion<T> quat;
+
+	T trace = m11 + m22 + m33;
+	if (trace > F(0.0))
+	{
+		float s = F(0.5)/std::sqrt(trace + F(1.0));
+		quat.w = F(0.25) / s;
+		quat.x = (m32 - m23) * s;
+		quat.y = (m13 - m31) * s;
+		quat.z = (m21 - m12) * s;
+	}
+	else
+	{
+		if (m11 > m22 && m11 > m33)
+		{
+			float s = F(2.0) * std::sqrt(F(1.0) + m11 - m22 - m33);
+
+			quat.w = (m32 - m23) / s;
+			quat.x = F(0.25) * s;
+			quat.y = (m12 + m21) / s;
+			quat.z = (m13 + m31) / s;
+		}
+		else if (m22 > m33)
+		{
+			float s = F(2.0) * std::sqrt(F(1.0) + m22 - m11 - m33);
+
+			quat.w = (m13 - m31) / s;
+			quat.x = (m12 + m21) / s;
+			quat.y = F(0.25) * s;
+			quat.z = (m23 + m32) / s;
+		}
+		else
+		{
+			float s = F(2.0) * std::sqrt(F(1.0) + m33 - m11 - m22);
+
+			quat.w = (m21 - m12) / s;
+			quat.x = (m13 + m31) / s;
+			quat.y = (m23 + m32) / s;
+			quat.z = F(0.25) * s;
+		}
+	}
+
+	return quat;
+}
 
 template<typename T>
 NzVector3<T> NzMatrix4<T>::GetScale() const