diff --git a/sources/reactphysics3d/mathematics/Quaternion.cpp b/sources/reactphysics3d/mathematics/Quaternion.cpp index add81b21..ad9dace7 100755 --- a/sources/reactphysics3d/mathematics/Quaternion.cpp +++ b/sources/reactphysics3d/mathematics/Quaternion.cpp @@ -19,6 +19,7 @@ // Libraries #include "Quaternion.h" +#include "constants.h" #include // 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 -Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) { - //TODO : Implement this method +// The t argument has to be such that 0 <= t <= 1. +// TODO : Test 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; } diff --git a/sources/reactphysics3d/mathematics/Quaternion.h b/sources/reactphysics3d/mathematics/Quaternion.h index 89c57ea0..36dd7f31 100644 --- a/sources/reactphysics3d/mathematics/Quaternion.h +++ b/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