From d3313421c664db5bd1b672d39ba3faec0d430117 Mon Sep 17 00:00:00 2001
From: Jonathan Zrake <jonathan.zrake@gmail.com>
Date: Sun, 15 Feb 2015 14:29:49 -0500
Subject: [PATCH] Add functions eulerAngleXYZ and extractEulerAngleXYZ

---
 glm/gtx/euler_angles.hpp | 18 ++++++++++++-
 glm/gtx/euler_angles.inl | 56 +++++++++++++++++++++++++++++++++++++++-
 2 files changed, 72 insertions(+), 2 deletions(-)

diff --git a/glm/gtx/euler_angles.hpp b/glm/gtx/euler_angles.hpp
index 7a1cb364..e772bbf0 100644
--- a/glm/gtx/euler_angles.hpp
+++ b/glm/gtx/euler_angles.hpp
@@ -114,6 +114,14 @@ namespace glm
 		T const & angleZ,
 		T const & angleY);
 
+    /// Creates a 3D 4 * 4 homogeneous rotation matrix from euler angles (X * Y * Z).
+    /// @see gtx_euler_angles
+    template <typename T>
+    GLM_FUNC_DECL tmat4x4<T, defaultp> eulerAngleXYZ(
+        T const & t1,
+        T const & t2,
+        T const & t3);
+    
 	/// Creates a 3D 4 * 4 homogeneous rotation matrix from euler angles (Y * X * Z).
 	/// @see gtx_euler_angles
 	template <typename T>
@@ -121,7 +129,7 @@ namespace glm
 		T const & yaw,
 		T const & pitch,
 		T const & roll);
-
+    
 	/// Creates a 3D 4 * 4 homogeneous rotation matrix from euler angles (Y * X * Z).
 	/// @see gtx_euler_angles
 	template <typename T>
@@ -150,6 +158,14 @@ namespace glm
 	template <typename T, precision P>
 	GLM_FUNC_DECL tmat4x4<T, P> orientate4(tvec3<T, P> const & angles);
 
+    /// Extracts the (X * Y * Z) Euler angles from the rotation matrix M
+    /// @see gtx_euler_angles
+    template <typename T>
+    GLM_FUNC_DECL void extractEulerAngleXYZ(tmat4x4<T, defaultp> & M,
+                                            T & t1,
+                                            T & t2,
+                                            T & t3);
+    
 	/// @}
 }//namespace glm
 
diff --git a/glm/gtx/euler_angles.inl b/glm/gtx/euler_angles.inl
index 77fea425..80b30dbe 100644
--- a/glm/gtx/euler_angles.inl
+++ b/glm/gtx/euler_angles.inl
@@ -30,6 +30,8 @@
 /// @author Christophe Riccio
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 
+#include "compatibility.hpp" // glm::atan2
+
 namespace glm
 {
 	template <typename T>
@@ -157,7 +159,42 @@ namespace glm
 	{
 		return eulerAngleZ(angleZ) * eulerAngleY(angleY);
 	}
-
+    
+    template <typename T>
+    GLM_FUNC_QUALIFIER tmat4x4<T, defaultp> eulerAngleXYZ
+    (
+     T const & t1,
+     T const & t2,
+     T const & t3
+     )
+    {
+        T c1 = glm::cos(-t1);
+        T c2 = glm::cos(-t2);
+        T c3 = glm::cos(-t3);
+        T s1 = glm::sin(-t1);
+        T s2 = glm::sin(-t2);
+        T s3 = glm::sin(-t3);
+        
+        tmat4x4<T, defaultp> Result;
+        Result[0][0] = c2 * c3;
+        Result[0][1] =-c1 * s3 + s1 * s2 * c3;
+        Result[0][2] = s1 * s3 + c1 * s2 * c3;
+        Result[0][3] = static_cast<T>(0);
+        Result[1][0] = c2 * s3;
+        Result[1][1] = c1 * c3 + s1 * s2 * s3;
+        Result[1][2] =-s1 * c3 + c1 * s2 * s3;
+        Result[1][3] = static_cast<T>(0);
+        Result[2][0] =-s2;
+        Result[2][1] = s1 * c2;
+        Result[2][2] = c1 * c2;
+        Result[2][3] = static_cast<T>(0);
+        Result[3][0] = static_cast<T>(0);
+        Result[3][1] = static_cast<T>(0);
+        Result[3][2] = static_cast<T>(0);
+        Result[3][3] = static_cast<T>(1);
+        return Result;
+    }
+    
 	template <typename T>
 	GLM_FUNC_QUALIFIER tmat4x4<T, defaultp> eulerAngleYXZ
 	(
@@ -284,4 +321,21 @@ namespace glm
 	{
 		return yawPitchRoll(angles.z, angles.x, angles.y);
 	}
+    
+    template <typename T>
+    GLM_FUNC_DECL void extractEulerAngleXYZ(tmat4x4<T, defaultp> & M,
+                                            T & t1,
+                                            T & t2,
+                                            T & t3)
+    {
+        float T1 = glm::atan2<T, defaultp>(M[2][1], M[2][2]);
+        float C2 = glm::sqrt(M[0][0]*M[0][0] + M[1][0]*M[1][0]);
+        float T2 = glm::atan2<T, defaultp>(-M[2][0], C2);
+        float S1 = glm::sin(T1);
+        float C1 = glm::cos(T1);
+        float T3 = glm::atan2<T, defaultp>(S1*M[0][2] - C1*M[0][1], C1*M[1][1] - S1*M[1][2  ]);
+        t1 = -T1;
+        t2 = -T2;
+        t3 = -T3;
+    }
 }//namespace glm