From 3e8af92da1cd14b396978b5755252b8cdc62a494 Mon Sep 17 00:00:00 2001
From: Robert Osfield <robert@openscenegraph.com>
Date: Thu, 27 Jul 2006 11:14:28 +0000
Subject: [PATCH] From David Spilling, fix to Matrix::get(Quat&)

---
 src/osg/Matrix_implementation.cpp | 59 +++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/src/osg/Matrix_implementation.cpp b/src/osg/Matrix_implementation.cpp
index 3a3a696c7..1eb18d459 100644
--- a/src/osg/Matrix_implementation.cpp
+++ b/src/osg/Matrix_implementation.cpp
@@ -121,6 +121,64 @@ void Matrix_implementation::set(const Quat& q_in)
     _mat[3][3] = 1.0;
 }
 
+#if 1
+// David Spillings implementation
+void Matrix_implementation::get( Quat& q ) const
+{
+    value_type s;
+    value_type tq[4];
+    int    i, j;
+
+    // Use tq to store the largest trace
+     tq[0] = 1 + _mat[0][0]+_mat[1][1]+_mat[2][2];
+    tq[1] = 1 + _mat[0][0]-_mat[1][1]-_mat[2][2];
+    tq[2] = 1 - _mat[0][0]+_mat[1][1]-_mat[2][2];
+    tq[3] = 1 - _mat[0][0]-_mat[1][1]+_mat[2][2];
+ 
+    // Find the maximum (could also use stacked if's later)
+    j = 0;
+    for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;
+
+    // check the diagonal
+      if (j==0)
+    {
+        /* perform instant calculation */
+        QW = tq[0];
+        QX = _mat[1][2]-_mat[2][1]; 
+        QY = _mat[2][0]-_mat[0][2]; 
+        QZ = _mat[0][1]-_mat[1][0]; 
+    }
+    else if (j==1)
+    {
+        QW = _mat[1][2]-_mat[2][1]; 
+        QX = tq[1];
+        QY = _mat[0][1]+_mat[1][0]; 
+        QZ = _mat[2][0]+_mat[0][2]; 
+    }
+    else if (j==2)
+    {
+        QW = _mat[2][0]-_mat[0][2]; 
+        QX = _mat[0][1]+_mat[1][0]; 
+        QY = tq[2];
+        QZ = _mat[1][2]+_mat[2][1]; 
+    }
+    else /* if (j==3) */
+    {
+        QW = _mat[0][1]-_mat[1][0]; 
+        QX = _mat[2][0]+_mat[0][2]; 
+        QY = _mat[1][2]+_mat[2][1]; 
+        QZ = tq[3];
+    }
+
+    s = sqrt(0.25/tq[j]);
+    QW *= s;
+    QX *= s;
+    QY *= s;
+    QZ *= s;
+
+}
+#else
+// Original implementation
 void Matrix_implementation::get( Quat& q ) const
 {
     // Source: Gamasutra, Rotating Objects Using Quaternions
@@ -173,6 +231,7 @@ void Matrix_implementation::get( Quat& q ) const
         QW = tq[3];
     }
 }
+#endif
 
 int Matrix_implementation::compare(const Matrix_implementation& m) const
 {