diff --git a/sources/reactphysics3d/mathematics/Matrix.h b/sources/reactphysics3d/mathematics/Matrix.h index ff04a454..6440462b 100644 --- a/sources/reactphysics3d/mathematics/Matrix.h +++ b/sources/reactphysics3d/mathematics/Matrix.h @@ -22,7 +22,6 @@ // Libraries #include "exceptions.h" - #include #include @@ -41,7 +40,7 @@ class Matrix { double** array; // Dynamic array that contains the values of the matrix public : - Matrix(int nbRow, int nbColum) throw(std::invalid_argument); // Constructor of the class Matrix + Matrix(int nbRow, int nbColum) throw(std::invalid_argument); // Constructor of the class Matrix Matrix(const Matrix& matrix); // Copy constructor of the class Matrix virtual ~Matrix(); // Destructor of the class Matrix double getValue(int i, int j) const throw(std::invalid_argument); // Return a value in the matrix @@ -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 - static Matrix identityMatrix(int dimension) throw(std::invalid_argument); // Return the identity matrix I of the given dimension + double getTrace() const throw(MathematicsException); // Return the trace of a square matrix + static Matrix identity(int dimension) throw(std::invalid_argument); // Return the identity matrix I of the given dimension void display() const; // TO DELETE diff --git a/sources/reactphysics3d/mathematics/Matrix3x3.cpp b/sources/reactphysics3d/mathematics/Matrix3x3.cpp index 127f61a1..a2506c98 100644 --- a/sources/reactphysics3d/mathematics/Matrix3x3.cpp +++ b/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, - z*x*s-w*y*s, y*z*s + w*x*s, 1.0-x*x*s-y*y*s); + setAllValues(1.0-yys-zzs, xys-wzs, xzs + wys, + 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() { - // Return the identity matrix +Matrix3x3 Matrix3x3::identity() { + // Return the isdentity matrix return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); } diff --git a/sources/reactphysics3d/mathematics/Matrix3x3.h b/sources/reactphysics3d/mathematics/Matrix3x3.h index c4c7f046..ede01190 100644 --- a/sources/reactphysics3d/mathematics/Matrix3x3.h +++ b/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) - static Matrix3x3 identityMatrix(); // Return the 3x3 identity matrix + Quaternion getQuaternion() const; // Return the unit quaternion corresponding to the matrix + static Matrix3x3 identity(); // Return the 3x3 identity matrix void display() const; // TO DELETE diff --git a/sources/reactphysics3d/mathematics/Quaternion.h b/sources/reactphysics3d/mathematics/Quaternion.h index a5da00e6..59cb96b0 100644 --- a/sources/reactphysics3d/mathematics/Quaternion.h +++ b/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 diff --git a/sources/reactphysics3d/mathematics/Vector.cpp b/sources/reactphysics3d/mathematics/Vector.cpp index 84f5d056..017a34e5 100755 --- a/sources/reactphysics3d/mathematics/Vector.cpp +++ b/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