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

sources/reactphysics3d/mathematics/Quaternion.cpp

@@ -18,8 +18,7 @@
  ***************************************************************************/
 
 // Libraries
-#include "Quaternion.h"
-#include "constants.h"
+#include "mathematics.h"
 #include <cassert>
 
 // Namespaces
@@ -55,9 +54,8 @@ Quaternion::~Quaternion() {
 }
 
 // Compute the rotation angle (in radians) and the 3D rotation axis
-// This method is used to get the rotation angle and axis of an
-// orientation quaternion.
-// TODO : Test this method
+// This method is used to get the rotation angle (in radian) and the unit
+// rotation axis of an orientation quaternion.
 void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const {
     Quaternion quaternion;
 
@@ -84,26 +82,25 @@ void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const {
 }
 
 // Compute the spherical linear interpolation between two quaternions.
-// The t argument has to be such that 0 <= t <= 1.
-// TODO : Test this method
+// The t argument has to be such that 0 <= t <= 1. This method is static.
 Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {
-    assert(t >= 0 && t <= 1);
+    assert(t >= 0.0 && t <= 1.0);
 
-    double invert = 1;
+    double invert = 1.0;
 
     // Compute cos(theta) using the quaternion scalar product
     double cosineTheta = quaternion1.scalarProduct(quaternion2);
 
     // Take care of the sign of cosineTheta
-    if (cosineTheta < 0) {
+    if (cosineTheta < 0.0) {
 			cosineTheta = -cosineTheta;
-			invert = -1;
+			invert = -1.0;
     }
 
     // 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);
+    if(1-cosineTheta < EPSILON) {
+        return quaternion1 * (1.0-t) + quaternion2 * (t * invert);
     }
 
     // Compute the theta angle
@@ -113,7 +110,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& qu
     double sineTheta = sin(theta);
 
     // Compute the two coefficients that are in the spherical linear interpolation formula
-    double coeff1 = sin((1-t)*theta) / sineTheta;
+    double coeff1 = sin((1.0-t)*theta) / sineTheta;
     double coeff2 = sin(t*theta) / sineTheta * invert;
 
     // Compute and return the interpolated quaternion

sources/reactphysics3d/mathematics/constants.h

@@ -21,9 +21,9 @@
 #define CONSTANTS_H
 
 // Constant used for the precision
-const double epsilon = 0.00001;
+const double EPSILON = 0.00001;
 
 // Pi constant
-const double pi = 3.14159265;
+const double PI = 3.14159265;
 
 #endif

sources/reactphysics3d/mathematics/mathematics.h

@@ -19,8 +19,8 @@
 
 // TODO : Mathematics library : Check everywhere that in member methods we use attributes access instead of getter and setter.
 
-#ifndef DCMATHS_H
-#define DCMATHS_H
+#ifndef MATHEMATICS_H
+#define MATHEMATICS_H
 
 // Libraries
 #include "Matrix.h"
@@ -30,5 +30,15 @@
 #include "Vector3D.h"
 #include "constants.h"
 #include "exceptions.h"
+#include <cstdio>
+
+// ---------- Mathematics functions ---------- //
+
+// Method to test if two numbers are (almost) equal
+// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
+inline bool equal(double a, double b) {
+    double difference = a - b;
+    return (difference < EPSILON && difference > -EPSILON);
+}
 
 #endif