Commit

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

sources/reactphysics3d/mathematics/Matrix.h

@@ -22,7 +22,6 @@
 
 // Libraries
 #include "exceptions.h"
-
 #include <stdexcept>
 #include <iostream>
 
@@ -52,8 +51,8 @@ class Matrix {
         Matrix getTranspose() const;                                                    // Return the transposed matrixs
         Matrix getInverse() const throw(MathematicsException);                          // Return the inverse of the matrix if there exists
         double getDeterminant() const throw(MathematicsException);                      // Return the determinant of the matrix
-        double getTrace() const;                                                        // Return the trace of the matrix
+        double getTrace() const throw(MathematicsException);                            // Return the trace of a square matrix
-        static Matrix identityMatrix(int dimension) throw(std::invalid_argument);       // Return the identity matrix I of the given dimension
+        static Matrix identity(int dimension) throw(std::invalid_argument);             // Return the identity matrix I of the given dimension
 
         void display() const;                                                           // TO DELETE
 

sources/reactphysics3d/mathematics/Matrix3x3.cpp

@@ -44,7 +44,7 @@ Matrix3x3::Matrix3x3(const Matrix3x3& matrix2) {
                  matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]);
 }
 
-// Create a Matrix3x3 from a quaternion
+// Create a Matrix3x3 from a quaternion (the quaternion can be non-unit)
 Matrix3x3::Matrix3x3(const Quaternion& quaternion) {
     double x = quaternion.getX();
     double y = quaternion.getY();
@@ -58,9 +58,24 @@ Matrix3x3::Matrix3x3(const Quaternion& quaternion) {
         s = 2.0/nQ;
     }
 
+    // Computations used for optimization (less multiplications)
+    double xs = x*s;
+    double ys = y*s;
+    double zs = z*s;
+    double wxs = w*xs;
+    double wys = w*ys;
+    double wzs = w*zs;
+    double xxs = x*xs;
+    double xys = x*ys;
+    double xzs = x*zs;
+    double yys = y*ys;
+    double yzs = y*zs;
+    double zzs = z*zs;
+
     // Create the matrix corresponding to the quaternion
-    Matrix3x3(1.0-y*y*s-z*z*s, x*y*s-w*z*s, z*x*s + w*y*s, x*y*s + w*z*s, 1.0-x*x*s-z*z*s, y*z*s-w*x*s,
+    setAllValues(1.0-yys-zzs, xys-wzs, xzs + wys,
-                     z*x*s-w*y*s, y*z*s + w*x*s, 1.0-x*x*s-y*y*s);
+                 xys + wzs, 1.0-xxs-zzs, yzs-wxs,
+                 xzs-wys, yzs + wxs, 1.0-xxs-yys);
 }
 
 // Destructor
@@ -128,13 +143,13 @@ Quaternion Matrix3x3::getQuaternion() const {
     else {
         r = sqrt(trace + 1.0);
         s = 0.5/r;
-        return Quaternion(0.5 * r, (array[0][1]-array[1][0])*s, (array[2][0]-array[0][2])*s, (array[2][1]-array[1][2])*s);
+        return Quaternion((array[2][1]-array[1][2])*s, (array[0][2]-array[2][0])*s, (array[1][0]-array[0][1])*s, 0.5 * r);
     }
 }
 
 // Return the 3x3 identity matrix
-Matrix3x3 Matrix3x3::identityMatrix() {
+Matrix3x3 Matrix3x3::identity() {
-    // Return the identity matrix
+    // Return the isdentity matrix
     return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
 }
 

sources/reactphysics3d/mathematics/Matrix3x3.h

@@ -53,8 +53,8 @@ class Matrix3x3 {
         double getDeterminant() const;                                                          // Return the determinant of the matrix
         double getTrace() const;                                                                // Return the trace of the matrix
         Matrix3x3 getInverse() const throw(MathematicsException);                               // Return the inverse matrix
-        Quaternion getQuaternion() const;                                                       // Return the quaternion corresponding to the matrix (it returns a unit quaternion)
+        Quaternion getQuaternion() const;                                                       // Return the unit quaternion corresponding to the matrix
-        static Matrix3x3 identityMatrix();                                                      // Return the 3x3 identity matrix
+        static Matrix3x3 identity();                                                            // Return the 3x3 identity matrix
 
         void display() const;                                                                   // TO DELETE
 

sources/reactphysics3d/mathematics/Quaternion.h

@@ -30,7 +30,8 @@ namespace reactphysics3d {
 
 /*  -------------------------------------------------------------------
     Class Quaternion :
-        This class represents a quaternion.
+        This class represents a quaternion. We use the notation :
+        q = (x*i, y*j, z*k, w) to represent a quaternion.
     -------------------------------------------------------------------
 */
 class Quaternion
@@ -64,6 +65,7 @@ class Quaternion
         // --- Overloaded operators --- //
         Quaternion operator+(const Quaternion& quaternion) const;             // Overloaded operator for the addition
         Quaternion operator-(const Quaternion& quaternion) const;             // Overloaded operator for the substraction
+        Quaternion operator*(double nb) const;                                // Overloaded operator for the multiplication with a constant
         Quaternion operator*(const Quaternion& quaternion) const;             // Overloaded operator for the multiplication
         Quaternion& operator=(const Quaternion& quaternion);                  // Overloaded operator for assignment
         bool operator==(const Quaternion& quaternion) const;                  // Overloaded operator for equality condition
@@ -127,6 +129,7 @@ inline Quaternion Quaternion::getUnit() const throw(MathematicsException) {
 
     // Check if the length is not equal to zero
     if (lengthQuaternion != 0.0) {
+
         // Compute and return the unit quaternion
         return Quaternion(x/lengthQuaternion, y/lengthQuaternion, z/lengthQuaternion, w/lengthQuaternion);
     }
@@ -138,7 +141,7 @@ inline Quaternion Quaternion::getUnit() const throw(MathematicsException) {
 
 // Return the conjugate of the quaternion (inline)
 inline Quaternion Quaternion::getConjugate() const {
-    return Quaternion(x, -y, -z, -w);
+    return Quaternion(-x, -y, -z, w);
 }
 
 // Return the inverse of the quaternion (inline)
@@ -148,8 +151,9 @@ inline Quaternion Quaternion::getInverse() const throw(MathematicsException) {
 
     // Check if the length is not equal to zero
     if (lengthQuaternion != 0.0) {
+
         // Compute and return the inverse quaternion
-        return Quaternion(x/lengthQuaternion, y/lengthQuaternion, z/lengthQuaternion, w/lengthQuaternion);
+        return Quaternion(-x/lengthQuaternion, -y/lengthQuaternion, -z/lengthQuaternion, w/lengthQuaternion);
     }
     else {
         // Throw an exception because the inverse cannot be computed
@@ -169,6 +173,12 @@ inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
     return Quaternion(x-quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w);
 }
 
+// Overloaded operator for the multiplication with a constant
+inline Quaternion Quaternion::operator*(double nb) const {
+    // Return the result
+    return Quaternion(nb*x, nb*y, nb*z, nb*w);
+}
+
 // Overloaded operator for the multiplication of two quaternions
 inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
     // Return the result of the multiplication

sources/reactphysics3d/mathematics/Vector.cpp

@@ -124,13 +124,6 @@ Vector Vector::crossProduct(const Vector& vector) const throw(MathematicsExcepti
     }
 }
 
-// TO DELETE
-void Vector::display() const {
-    for (int i=0; i<nbComponent; ++i) {
-        std::cout << tab[i] << std::endl;
-    }
-}
-
 // Overloaded operator for addition
 Vector Vector::operator+(const Vector& vector) const throw(MathematicsException) {
     // Check the size of the two vectors

sources/reactphysics3d/mathematics/Vector.h

@@ -50,8 +50,6 @@ class Vector {
         double scalarProduct(const Vector& vector) const throw(MathematicsException);   // Scalar product of two vectors
         Vector crossProduct(const Vector& vector) const throw(MathematicsException);    // Cross product of two vectors (in 3D only)
 
-        void display() const;                                                           // TO DELETE
-
         // --- Overloaded operators --- //
         Vector operator+(const Vector& vector) const throw(MathematicsException);   // Overloaded operator for addition
         Vector operator-(const Vector& vector) const throw(MathematicsException);   // Overloaded operator for substraction

sources/reactphysics3d/mathematics/Vector3D.cpp

@@ -63,11 +63,6 @@ Vector3D Vector3D::getUnit() const throw(MathematicsException) {
     }
 }
 
-// TO DELETE THIS IS JUST FOR TESTS
-void Vector3D::display() const {
-    std::cout << x << std::endl << y << std::endl << z << std::endl;
-}
-
 // Overloaded operator for addition
 Vector3D Vector3D::operator+(const Vector3D& vector) const {
     // Compute and return the sum of the two vectors

sources/reactphysics3d/mathematics/Vector3D.h

@@ -56,8 +56,6 @@ class Vector3D {
         double scalarProduct(const Vector3D& vector) const;     // Scalar product of two vectors
         Vector3D crossProduct(const Vector3D& vector) const;    // Cross product of two vectors
 
-        void display() const;                                   // TO DELETE
-
         // --- Overloaded operators --- //
         Vector3D operator+(const Vector3D& vector) const;     // Overloaded operator for addition
         Vector3D operator-(const Vector3D& vector) const ;    // Overloaded operator for substraction