From 5c662669339c87ee53388c397af2ad3398cc4626 Mon Sep 17 00:00:00 2001
From: Robert Rouhani <robert.rouhani@gmail.com>
Date: Thu, 18 Jul 2013 13:14:30 -0700
Subject: [PATCH] Added Quaterniond version of FromMatrix

---
 Source/OpenTK/Math/Quaternion.cs  |  6 ++-
 Source/OpenTK/Math/Quaterniond.cs | 72 +++++++++++++++++++++++++++++++
 2 files changed, 77 insertions(+), 1 deletion(-)

diff --git a/Source/OpenTK/Math/Quaternion.cs b/Source/OpenTK/Math/Quaternion.cs
index 44d9a24a..5913e1dd 100644
--- a/Source/OpenTK/Math/Quaternion.cs
+++ b/Source/OpenTK/Math/Quaternion.cs
@@ -252,7 +252,7 @@ namespace OpenTK
         /// <summary>
         /// Defines the identity quaternion.
         /// </summary>
-        public static Quaternion Identity = new Quaternion(0, 0, 0, 1);
+        public static readonly Quaternion Identity = new Quaternion(0, 0, 0, 1);
 
         #endregion
 
@@ -507,6 +507,8 @@ namespace OpenTK
 
         #endregion
 
+        #region FromMatrix
+
         /// <summary>
         /// Builds a quaternion from the given rotation matrix
         /// </summary>
@@ -575,6 +577,8 @@ namespace OpenTK
             }
         }
 
+        #endregion
+
         #region Slerp
 
         /// <summary>
diff --git a/Source/OpenTK/Math/Quaterniond.cs b/Source/OpenTK/Math/Quaterniond.cs
index 8d384312..9806f694 100644
--- a/Source/OpenTK/Math/Quaterniond.cs
+++ b/Source/OpenTK/Math/Quaterniond.cs
@@ -507,6 +507,78 @@ namespace OpenTK
 
         #endregion
 
+        #region FromMatrix
+
+        /// <summary>
+        /// Builds a quaternion from the given rotation matrix
+        /// </summary>
+        /// <param name="matrix">A rotation matrix</param>
+        /// <returns>The equivalent quaternion</returns>
+        public static Quaterniond FromMatrix(Matrix3d matrix)
+        {
+            Quaterniond result;
+            FromMatrix(ref matrix, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Builds a quaternion from the given rotation matrix
+        /// </summary>
+        /// <param name="matrix">A rotation matrix</param>
+        /// <param name="result">The equivalent quaternion</param>
+        public static void FromMatrix(ref Matrix3d matrix, out Quaterniond result)
+        {
+            double trace = matrix.Trace;
+
+            if (trace > 0)
+            {
+                double s = Math.Sqrt(trace + 1) * 2;
+                double invS = 1.0 / s;
+
+                result.w = s * 0.25;
+                result.xyz.X = (matrix.Row2.Y - matrix.Row1.Z) * invS;
+                result.xyz.Y = (matrix.Row0.Z - matrix.Row2.X) * invS;
+                result.xyz.Z = (matrix.Row1.X - matrix.Row0.Y) * invS;
+            }
+            else
+            {
+                double m00 = matrix.Row0.X, m11 = matrix.Row1.Y, m22 = matrix.Row2.Z;
+
+                if (m00 > m11 && m00 > m22)
+                {
+                    double s = Math.Sqrt(1 + m00 - m11 - m22) * 2;
+                    double invS = 1.0 / s;
+
+                    result.w = (matrix.Row2.Y - matrix.Row1.Z) * invS;
+                    result.xyz.X = s * 0.25;
+                    result.xyz.Y = (matrix.Row0.Y + matrix.Row1.X) * invS;
+                    result.xyz.Z = (matrix.Row0.Z + matrix.Row2.X) * invS;
+                }
+                else if (m11 > m22)
+                {
+                    double s = Math.Sqrt(1 + m11 - m00 - m22) * 2;
+                    double invS = 1.0 / s;
+
+                    result.w = (matrix.Row0.Z - matrix.Row2.X) * invS;
+                    result.xyz.X = (matrix.Row0.Y + matrix.Row1.X) * invS;
+                    result.xyz.Y = s * 0.25;
+                    result.xyz.Z = (matrix.Row1.Z + matrix.Row2.Y) * invS;
+                }
+                else
+                {
+                    double s = Math.Sqrt(1 + m22 - m00 - m11) * 2;
+                    double invS = 1.0 / s;
+
+                    result.w = (matrix.Row1.X - matrix.Row0.Y) * invS;
+                    result.xyz.X = (matrix.Row0.Z + matrix.Row2.X) * invS;
+                    result.xyz.Y = (matrix.Row1.Z + matrix.Row2.Y) * invS;
+                    result.xyz.Z = s * 0.25;
+                }
+            }
+        }
+
+        #endregion
+
         #region Slerp
 
         /// <summary>