git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@93 92aac97c-a6ce-11dd-a772-7fcde58d38e6

sources/reactphysics3d/mathematics/Quaternion.cpp

@@ -19,6 +19,7 @@
 
 // Libraries
 #include "Quaternion.h"
+#include "constants.h"
 #include <cassert>
 
 // Namespaces
@@ -53,13 +54,41 @@ Quaternion::~Quaternion() {
 
 }
 
-// --- Others functions --- //
-
 // Compute the spherical linear interpolation between two quaternions.
-// The t argument has to be such that 0 <= t <= 1
+// The t argument has to be such that 0 <= t <= 1.
-Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {
+// TODO : Test this method
-    //TODO : Implement this method
+Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {
     assert(t >= 0 && t <= 1);
+
+    double invert = 1;
+
+    // Compute cos(theta) using the quaternion scalar product
+    double cosineTheta = quaternion1.scalarProduct(quaternion2);
+
+    // Take care of the sign of cosineTheta
+    if (cosineTheta < 0) {
+			cosineTheta = -cosineTheta;
+			invert = -1;
+    }
+
+    // Because of precision, if cos(theta) is nearly 1, therefore theta is nearly 0 and we can write
+    // sin((1-t)*theta) as (1-t) and sin(t*theta) as t
+    if ((1.0-cosineTheta) < epsilon) {
+        return quaternion1 * (1-t) + quaternion2 * (t * invert);
+    }
+
+    // Compute the theta angle
+    double theta = acos(cosineTheta);
+
+    // Compute sin(theta)
+    double sineTheta = sin(theta);
+
+    // Compute the two coefficients that are in the spherical linear interpolation formula
+    double coeff1 = sin((1-t)*theta) / sineTheta;
+    double coeff2 = sin(t*theta) / sineTheta * invert;
+
+    // Compute and return the interpolated quaternion
+    return quaternion1 * coeff1 + quaternion2 * coeff2;
 }
 
 

sources/reactphysics3d/mathematics/Quaternion.h

@@ -61,7 +61,10 @@ class Quaternion
         Quaternion getUnit() const throw (MathematicsException);              // Return the unit quaternion
         Quaternion getConjugate() const;                                      // Return the conjugate quaternion
         Quaternion getInverse() const throw (MathematicsException);           // Return the inverse of the quaternion
-        double scalarProduct(const Quaternion& quaternion);                   // Scalar product between two quaternions
+        double scalarProduct(const Quaternion& quaternion) const;             // Scalar product between two quaternions
+
+        static Quaternion slerp(const Quaternion& quaternion1,
+                                const Quaternion& quaternion2, double t);     // Compute the spherical linear interpolation between two quaternions
 
         // --- Overloaded operators --- //
         Quaternion operator+(const Quaternion& quaternion) const;             // Overloaded operator for the addition
@@ -165,7 +168,7 @@ inline Quaternion Quaternion::getInverse() const throw(MathematicsException) {
 
 // Scalar product between two quaternions
 // TODO : Test for this method
-inline double Quaternion::scalarProduct(const Quaternion& quaternion) {
+inline double Quaternion::scalarProduct(const Quaternion& quaternion) const {
     return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
 }
 
@@ -212,11 +215,6 @@ inline bool Quaternion::operator==(const Quaternion& quaternion) const {
     return (x == quaternion.x && y == quaternion.y && z == quaternion.z && w == quaternion.w);
 }
 
-// --- Others functions --- //
-
-// Compute the spherical linear interpolation between two quaternions
-Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t);
-
 } // End of the ReactPhysics3D namespace
 
 #endif