From 4a804ebf325a651b364323e1ce68eddd8c87b0bc Mon Sep 17 00:00:00 2001
From: Robert Osfield <robert@openscenegraph.com>
Date: Tue, 27 May 2003 11:02:45 +0000
Subject: [PATCH] Reverted recent changes so the set(Matrix&) method.

---
 include/osg/Quat |  2 --
 src/osg/Quat.cpp | 86 ++++++++++++++++++++----------------------------
 2 files changed, 36 insertions(+), 52 deletions(-)

diff --git a/include/osg/Quat b/include/osg/Quat
index b1c817f95..d9d37971a 100644
--- a/include/osg/Quat
+++ b/include/osg/Quat
@@ -284,8 +284,6 @@ class SG_EXPORT Quat
         
     protected:
     
-        void _set(const Matrix& m );
-
 };	// end of class prototype
 
 inline std::ostream& operator << (std::ostream& output, const Quat& vec)
diff --git a/src/osg/Quat.cpp b/src/osg/Quat.cpp
index 423a77c43..ea9aaadb0 100644
--- a/src/osg/Quat.cpp
+++ b/src/osg/Quat.cpp
@@ -198,68 +198,54 @@ void Quat::slerp( float t, const Quat& from, const Quat& to )
 
 void Quat::set( const Matrix& m )
 {
-    // Source: 
+    // Source: Gamasutra, Rotating Objects Using Quaternions
     //
-    // http://mccammon.ucsd.edu/~adcock/matrixfaq.html#Q55
-    
-    float x_scale = sqrtf(osg::square(m(0,0))+osg::square(m(1,0))+osg::square(m(2,0)));
-    
-    if (osg::absolute(x_scale-1.0f)>1e-5)
-    {
-        osg::Matrix new_m(m*osg::Matrix::scale(1.0f/x_scale,1.0f/x_scale,1.0f/x_scale));
-        _set(new_m);
-    }
-    else
-    {
-        _set(m);
-    }
-}
-    
-void Quat::_set(const Matrix& m )
-{    
-    //std::cout<<"Matrix scaled "<<m<<std::endl;
+    //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
 
-    double S;
-    double tr = m(0,0) + m(1,1) + m(2,2) + 1.0;
+    float  tr, s;
+    float  tq[4];
+    int    i, j, k;
 
-    //cout << "tr="<<tr<<endl;
+    int nxt[3] = {1, 2, 0};
+
+    tr = m(0,0) + m(1,1) + m(2,2);
 
     // check the diagonal
-    if (tr > 2e-5/*0.00000001*/)
+    if (tr > 0.0)
     {
-        //cout << "path one"<<endl;
-    
-        S = 0.5/sqrt (tr);
-        QW = 0.25 / S;
-        QX = (m(1,2) - m(2,1)) * S;
-        QY = (m(2,0) - m(0,2)) * S;
-        QZ = (m(0,1) - m(1,0)) * S;
+        s = (float)sqrt (tr + 1.0);
+        QW = s / 2.0f;
+        s = 0.5f / s;
+        QX = (m(1,2) - m(2,1)) * s;
+        QY = (m(2,0) - m(0,2)) * s;
+        QZ = (m(0,1) - m(1,0)) * s;
     }
     else
     {
-        //cout << "path two"<<endl;
+        // diagonal is negative
+        i = 0;
+        if (m(1,1) > m(0,0))
+            i = 1;
+        if (m(2,2) > m(i,i))
+            i = 2;
+        j = nxt[i];
+        k = nxt[j];
 
-        if ( m(0,0) > m(1,1) && m(0,0) > m(2,2) )  {       // Column 0: 
-            S  = sqrt( 1.0 + m(0,0) - m(1,1) - m(2,2) ) * 2.0;
-            QX = 0.25 * S;
-            QY = (m(1,0) + m(0,1) ) / S;
-            QZ = (m(0,2) + m(2,0) ) / S;
-            QW = (m(2,1) - m(1,2) ) / S;
+        s = (float)sqrt ((m(i,i) - (m(j,j) + m(k,k))) + 1.0);
 
-        } else if ( m(1,1) > m(2,2) ) {                    // Column 1: 
-            S  = sqrt( 1.0 + m(1,1) - m(0,0) - m(2,2) ) * 2.0;
-            QX = (m(1,0) + m(0,1) ) / S;
-            QY = 0.25 * S;
-            QZ = (m(2,1) + m(1,2) ) / S;
-            QW = (m(0,2) - m(2,0) ) / S;
+        tq[i] = s * 0.5f;
 
-        } else {                                            // Column 2:
-            S  = sqrt( 1.0 + m(2,2) - m(0,0) - m(1,1) ) * 2.0;
-            QX = (m(0,2) + m(2,0) ) / S;
-            QY = (m(2,1) + m(1,2) ) / S;
-            QZ = 0.25f * S;
-            QW = (m(1,0) - m(0,1) ) / S;
-        }
+        if (s != 0.0f)
+            s = 0.5f / s;
+
+        tq[3] = (m(j,k) - m(k,j)) * s;
+        tq[j] = (m(i,j) + m(j,i)) * s;
+        tq[k] = (m(i,k) + m(k,i)) * s;
+
+        QX = tq[0];
+        QY = tq[1];
+        QZ = tq[2];
+        QW = tq[3];
     }
 }