From 355650ac1d311658aa274d5f279a3cbda6ac7f07 Mon Sep 17 00:00:00 2001
From: Robert Osfield <robert@openscenegraph.com>
Date: Thu, 27 Jan 2005 14:39:58 +0000
Subject: [PATCH] From Nicolas Brodu, new faster
 osg::Quat::makeRotate(Vec3d,Vec3d) implmentation.

From Robert Osfield, modes to osg::Quat to keep the original implmentation around
as makeRotate_original(,) and added tests into osgunittest to test the new
methods provide equivilant results to the original implemementation.  The
orignal implementation will be removed once the new method is more widely tested.
---
 examples/osgunittests/osgunittests.cpp | 27 ++++++++
 include/osg/Quat                       |  2 +
 src/osg/Quat.cpp                       | 96 +++++++++++++++++++++++++-
 3 files changed, 123 insertions(+), 2 deletions(-)

diff --git a/examples/osgunittests/osgunittests.cpp b/examples/osgunittests/osgunittests.cpp
index 0e0a80a6b..5640ebbe2 100644
--- a/examples/osgunittests/osgunittests.cpp
+++ b/examples/osgunittests/osgunittests.cpp
@@ -119,6 +119,21 @@ void sizeOfTest()
 }
 
 
+void testQuatRotate(const osg::Vec3d& from, const osg::Vec3d& to)
+{
+    osg::Quat q_nicolas;
+    q_nicolas.makeRotate(from,to);
+    
+    osg::Quat q_original;
+    q_original.makeRotate_original(from,to);
+    
+    std::cout<<"osg::Quat::makeRotate("<<from<<", "<<to<<")"<<std::endl;
+    std::cout<<"  q_nicolas = "<<q_nicolas<<std::endl;
+    std::cout<<"  q_original = "<<q_original<<std::endl;
+    std::cout<<"  from * M4x4(q_nicolas) = "<<from * osg::Matrixd::rotate(q_nicolas)<<std::endl;
+    std::cout<<"  from * M4x4(q_original) = "<<from * osg::Matrixd::rotate(q_original)<<std::endl;
+}
+
 void testQuat()
 {
     osg::Quat q1;
@@ -147,6 +162,18 @@ void testQuat()
     std::cout<<"m1*m2 = "<<qm1_2<<std::endl;
     std::cout<<"m2*m1 = "<<qm2_1<<std::endl;
 
+
+    testQuatRotate(osg::Vec3d(1.0,0.0,0.0),osg::Vec3d(0.0,1.0,0.0));
+    testQuatRotate(osg::Vec3d(0.0,1.0,0.0),osg::Vec3d(1.0,0.0,0.0));
+    testQuatRotate(osg::Vec3d(0.0,0.0,1.0),osg::Vec3d(0.0,1.0,0.0));
+    testQuatRotate(osg::Vec3d(1.0,1.0,1.0),osg::Vec3d(1.0,0.0,0.0));
+    testQuatRotate(osg::Vec3d(1.0,0.0,0.0),osg::Vec3d(1.0,0.0,0.0));
+    testQuatRotate(osg::Vec3d(1.0,0.0,0.0),osg::Vec3d(-1.0,0.0,0.0));
+    testQuatRotate(osg::Vec3d(-1.0,0.0,0.0),osg::Vec3d(1.0,0.0,0.0));
+    testQuatRotate(osg::Vec3d(0.0,1.0,0.0),osg::Vec3d(0.0,-1.0,0.0));
+    testQuatRotate(osg::Vec3d(0.0,-1.0,0.0),osg::Vec3d(0.0,1.0,0.0));
+    testQuatRotate(osg::Vec3d(0.0,0.0,1.0),osg::Vec3d(0.0,0.0,-1.0));
+    testQuatRotate(osg::Vec3d(0.0,0.0,-1.0),osg::Vec3d(0.0,0.0,1.0));
     
 }
 
diff --git a/include/osg/Quat b/include/osg/Quat
index 45ee68852..b477dc510 100644
--- a/include/osg/Quat
+++ b/include/osg/Quat
@@ -343,6 +343,8 @@ class SG_EXPORT Quat
             are co-incident or opposite in direction.*/
         void makeRotate( const Vec3d& vec1, const Vec3d& vec2 );
     
+        void makeRotate_original( const Vec3d& vec1, const Vec3d& vec2 );
+
         /** Return the angle and vector components represented by the quaternion.*/
         void getRotate ( value_type & angle, value_type & x, value_type & y, value_type & z ) const;
 
diff --git a/src/osg/Quat.cpp b/src/osg/Quat.cpp
index 49bc6bd34..01b5d7d29 100644
--- a/src/osg/Quat.cpp
+++ b/src/osg/Quat.cpp
@@ -107,12 +107,105 @@ void Quat::makeRotate( const Vec3f& from, const Vec3f& to )
     makeRotate( Vec3d(from), Vec3d(to) );
 }
 
+/** Make a rotation Quat which will rotate vec1 to vec2
+
+This routine uses only fast geometric transforms, without costly acos/sin computations. 
+It's exact, fast, and with less degenerate cases than the acos/sin method.
+
+For an explanation of the math used, you may see for example: 
+http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf
+
+@note This is the rotation with shortest angle, which is the one equivalent to the 
+acos/sin transform method. Other rotations exists, for example to additionally keep 
+a local horizontal attitude.
+
+@author Nicolas Brodu
+*/
+void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
+{
+
+    // This routine takes any vector as argument but normalized 
+    // vectors are necessary, if only for computing the dot product.
+    // Too bad the API is that generic, it leads to performance loss.
+    // Even in the case the 2 vectors are not normalized but same length,
+    // the sqrt could be shared, but we have no way to know beforehand
+    // at this point, while the caller may know.
+    // So, we have to test... in the hope of saving at least a sqrt
+    Vec3d sourceVector = from;
+    Vec3d targetVector = to;
+    
+    value_type fromLen2 = from.length2();
+    value_type fromLen;
+    // normalize only when necessary, epsilon test
+    if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) {
+        fromLen = sqrt(fromLen2);
+        sourceVector /= fromLen;
+    } else fromLen = 1.0;
+    
+    value_type toLen2 = to.length2();
+    // normalize only when necessary, epsilon test
+    if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
+        value_type toLen;
+        // re-use fromLen for case of mapping 2 vectors of the same length
+        if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) {
+            toLen = fromLen;
+        } 
+        else toLen = sqrt(toLen2);
+        targetVector /= toLen;
+    }
+
+    
+    // Now let's get into the real stuff
+    // Use "dot product plus one" as test as it can be re-used later on
+    double dotProdPlus1 = 1.0 + sourceVector * targetVector;
+    
+    // Check for degenerate case of full u-turn. Use epsilon for detection
+    if (dotProdPlus1 < 1e-7) {
+    
+        // Get an orthogonal vector of the given vector
+        // in a plane with maximum vector coordinates.
+        // Then use it as quaternion axis with pi angle
+        // Trick is to realize one value at least is >0.6 for a normalized vector.
+        if (fabs(sourceVector.x()) < 0.6) {
+            const double norm = sqrt(1.0 - sourceVector.x() * sourceVector.x());
+            _v[0] = 0.0; 
+            _v[1] = sourceVector.z() / norm;
+            _v[2] = -sourceVector.y() / norm;
+            _v[3] = 0.0;
+        } else if (fabs(sourceVector.y()) < 0.6) {
+            const double norm = sqrt(1.0 - sourceVector.y() * sourceVector.y());
+            _v[0] = -sourceVector.z() / norm;
+            _v[1] = 0.0;
+            _v[2] = sourceVector.x() / norm;
+            _v[3] = 0.0;
+        } else {
+            const double norm = sqrt(1.0 - sourceVector.z() * sourceVector.z());
+            _v[0] = sourceVector.y() / norm;
+            _v[1] = -sourceVector.x() / norm;
+            _v[2] = 0.0;
+            _v[3] = 0.0;
+        }
+    }
+    
+    else {
+        // Find the shortest angle quaternion that transforms normalized vectors
+        // into one other. Formula is still valid when vectors are colinear
+        const double s = sqrt(0.5 * dotProdPlus1);
+        const Vec3d tmp = sourceVector ^ targetVector / (2.0*s);
+        _v[0] = tmp.x();
+        _v[1] = tmp.y();
+        _v[2] = tmp.z();
+        _v[3] = s;
+    }
+}
+
+
 // Make a rotation Quat which will rotate vec1 to vec2
 // Generally take adot product to get the angle between these
 // and then use a cross product to get the rotation axis
 // Watch out for the two special cases of when the vectors
 // are co-incident or opposite in direction.
-void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
+void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to )
 {
     const value_type epsilon = 0.0000001;
 
@@ -165,7 +258,6 @@ void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
     }
 }
 
-
 void Quat::getRotate( value_type& angle, Vec3f& vec ) const
 {
     value_type x,y,z;