Make some modifications in the mathematics library

src/collision/shapes/CylinderShape.cpp

@@ -62,7 +62,7 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, decimal ma
     Vector3 w(direction.x, 0.0, direction.z);
     decimal lengthW = sqrt(direction.x * direction.x + direction.z * direction.z);
 
-    if (lengthW != 0.0) {
+    if (lengthW > MACHINE_EPSILON) {
         if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
         else supportPoint.y = mHalfHeight;
         supportPoint += (mRadius / lengthW) * w;

src/mathematics/Matrix3x3.cpp

@@ -73,21 +73,27 @@ Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
 
 // Return the inverse matrix
 Matrix3x3 Matrix3x3::getInverse() const {
+
     // Compute the determinant of the matrix
     decimal determinant = getDeterminant();
 
     // Check if the determinant is equal to zero
-    assert(determinant != 0.0);
+    assert(std::abs(determinant) > MACHINE_EPSILON);
-    decimal invDeterminant = 1.0 / determinant;
-    Matrix3x3 tempMatrix;
 
-    // Compute the inverse of the matrix
+    decimal invDeterminant = decimal(1.0) / determinant;
-    tempMatrix.setAllValues((mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]), -(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]), (mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1]),
+
-                            -(mArray[0][1]*mArray[2][2]-mArray[2][1]*mArray[0][2]), (mArray[0][0]*mArray[2][2]-mArray[2][0]*mArray[0][2]), -(mArray[0][0]*mArray[2][1]-mArray[2][0]*mArray[0][1]),
+    Matrix3x3 tempMatrix((mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]),
-                            (mArray[0][1]*mArray[1][2]-mArray[0][2]*mArray[1][1]), -(mArray[0][0]*mArray[1][2]-mArray[1][0]*mArray[0][2]), (mArray[0][0]*mArray[1][1]-mArray[0][1]*mArray[1][0]));
+                         -(mArray[0][1]*mArray[2][2]-mArray[2][1]*mArray[0][2]),
+                         (mArray[0][1]*mArray[1][2]-mArray[0][2]*mArray[1][1]),
+                            -(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]),
+                         (mArray[0][0]*mArray[2][2]-mArray[2][0]*mArray[0][2]),
+                         -(mArray[0][0]*mArray[1][2]-mArray[1][0]*mArray[0][2]),
+                            (mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1]),
+                         -(mArray[0][0]*mArray[2][1]-mArray[2][0]*mArray[0][1]),
+                         (mArray[0][0]*mArray[1][1]-mArray[0][1]*mArray[1][0]));
 
     // Return the inverse matrix
-    return (invDeterminant * tempMatrix.getTranspose());
+    return (invDeterminant * tempMatrix);
 }
 
 

src/mathematics/Quaternion.cpp

@@ -32,26 +32,24 @@
 using namespace reactphysics3d;
 
 // Constructor of the class
-Quaternion::Quaternion()
+Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {
-           :mX(0.0), mY(0.0), mZ(0.0), mW(0.0) {
 
 }
 
 // Constructor with arguments
-Quaternion::Quaternion(decimal x, decimal y, decimal z, decimal w)
+Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW)
-           :mX(x), mY(y), mZ(z), mW(w) {
+           :x(newX), y(newY), z(newZ), w(newW) {
 
 }
 
 // Constructor with the component w and the vector v=(x y z)
-Quaternion::Quaternion(decimal w, const Vector3& v)
+Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) {
-           :mX(v.x), mY(v.y), mZ(v.z), mW(w) {
 
 }
 
 // Copy-constructor
 Quaternion::Quaternion(const Quaternion& quaternion)
-           :mX(quaternion.mX), mY(quaternion.mY), mZ(quaternion.mZ), mW(quaternion.mW) {
+           :x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) {
 
 }
 
@@ -78,20 +76,20 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
                 s = 0.5 / r;
                 
                 // Compute the quaternion
-                mX = (array[2][0] + array[0][2])*s;
+                x = (array[2][0] + array[0][2])*s;
-                mY = (array[1][2] + array[2][1])*s;
+                y = (array[1][2] + array[2][1])*s;
-                mZ = 0.5*r;
+                z = 0.5*r;
-                mW = (array[1][0] - array[0][1])*s;
+                w = (array[1][0] - array[0][1])*s;
             }
             else {
                 r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0);
                 s = 0.5 / r;
 
                 // Compute the quaternion
-                mX = (array[0][1] + array[1][0])*s;
+                x = (array[0][1] + array[1][0])*s;
-                mY = 0.5 * r;
+                y = 0.5 * r;
-                mZ = (array[1][2] + array[2][1])*s;
+                z = (array[1][2] + array[2][1])*s;
-                mW = (array[0][2] - array[2][0])*s;
+                w = (array[0][2] - array[2][0])*s;
             }
         }
         else if (array[2][2] > array[0][0]) {
@@ -99,20 +97,20 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
             s = 0.5 / r;
 
             // Compute the quaternion
-            mX = (array[2][0] + array[0][2])*s;
+            x = (array[2][0] + array[0][2])*s;
-            mY = (array[1][2] + array[2][1])*s;
+            y = (array[1][2] + array[2][1])*s;
-            mZ = 0.5 * r;
+            z = 0.5 * r;
-            mW = (array[1][0] - array[0][1])*s;
+            w = (array[1][0] - array[0][1])*s;
         }
         else {
             r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0);
             s = 0.5 / r;
 
             // Compute the quaternion
-            mX = 0.5 * r;
+            x = 0.5 * r;
-            mY = (array[0][1] + array[1][0])*s;
+            y = (array[0][1] + array[1][0])*s;
-            mZ = (array[2][0] - array[0][2])*s;
+            z = (array[2][0] - array[0][2])*s;
-            mW = (array[2][1] - array[1][2])*s;
+            w = (array[2][1] - array[1][2])*s;
         }
     }
     else {
@@ -120,10 +118,10 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
         s = 0.5/r;
 
         // Compute the quaternion
-        mX = (array[2][1]-array[1][2])*s;
+        x = (array[2][1]-array[1][2])*s;
-        mY = (array[0][2]-array[2][0])*s;
+        y = (array[0][2]-array[2][0])*s;
-        mZ = (array[1][0]-array[0][1])*s;
+        z = (array[1][0]-array[0][1])*s;
-        mW = 0.5 * r;
+        w = 0.5 * r;
     }
 }
 
@@ -148,10 +146,10 @@ void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
     }
 
     // Compute the roation angle
-    angle = acos(quaternion.mW) * 2.0;
+    angle = acos(quaternion.w) * 2.0;
 
     // Compute the 3D rotation axis
-    Vector3 rotationAxis(quaternion.mX, quaternion.mY, quaternion.mZ);
+    Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z);
 
     // Normalize the rotation axis
     rotationAxis = rotationAxis.getUnit();
@@ -163,7 +161,7 @@ void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
 // Return the orientation matrix corresponding to this quaternion
 Matrix3x3 Quaternion::getMatrix() const {
 
-    decimal nQ = mX*mX + mY*mY + mZ*mZ + mW*mW;
+    decimal nQ = x*x + y*y + z*z + w*w;
     decimal s = 0.0;
 
     if (nQ > 0.0) {
@@ -171,18 +169,18 @@ Matrix3x3 Quaternion::getMatrix() const {
     }
 
     // Computations used for optimization (less multiplications)
-    decimal xs = mX*s;
+    decimal xs = x*s;
-    decimal ys = mY*s;
+    decimal ys = y*s;
-    decimal zs = mZ*s;
+    decimal zs = z*s;
-    decimal wxs = mW*xs;
+    decimal wxs = w*xs;
-    decimal wys = mW*ys;
+    decimal wys = w*ys;
-    decimal wzs = mW*zs;
+    decimal wzs = w*zs;
-    decimal xxs = mX*xs;
+    decimal xxs = x*xs;
-    decimal xys = mX*ys;
+    decimal xys = x*ys;
-    decimal xzs = mX*zs;
+    decimal xzs = x*zs;
-    decimal yys = mY*ys;
+    decimal yys = y*ys;
-    decimal yzs = mY*zs;
+    decimal yzs = y*zs;
-    decimal zzs = mZ*zs;
+    decimal zzs = z*zs;
 
     // Create the matrix corresponding to the quaternion
     return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,

src/mathematics/Quaternion.h

@@ -41,16 +41,14 @@ namespace reactphysics3d {
         q = (x*i, y*j, z*k, w) to represent a quaternion.
     -------------------------------------------------------------------
 */
-class Quaternion {
+struct Quaternion {
 
-    private :
+    public :
 
         // -------------------- Attributes -------------------- //
 
         // Components of the quaternion
-        decimal mX, mY, mZ, mW;
+        decimal x, y, z, w;
-
-    public :
 
         // -------------------- Methods -------------------- //
 
@@ -58,10 +56,10 @@ class Quaternion {
         Quaternion();
 
         // Constructor with arguments
-        Quaternion(decimal x, decimal y, decimal z, decimal w);
+        Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW);
 
         // Constructor with the component w and the vector v=(x y z)
-        Quaternion(decimal w, const Vector3& v);
+        Quaternion(decimal newW, const Vector3& v);
 
         // Copy-constructor
         Quaternion(const Quaternion& quaternion);
@@ -72,30 +70,6 @@ class Quaternion {
         // Destructor
         ~Quaternion();
 
-        // Return the component x of the quaternion
-        decimal getX() const;
-
-        // Return the component y of the quaternion
-        decimal getY() const;
-
-        // Return the component z of the quaternion
-        decimal getZ() const;
-
-        // Return the component w of the quaternion
-        decimal getW() const;
-
-        // Set the value x
-        void setX(decimal x);
-
-        // Set the value y
-        void setY(decimal y);
-
-        // Set the value z
-        void setZ(decimal z);
-
-        // Set the value w
-        void setW(decimal w);
-
         // Return the vector v=(x y z) of the quaternion
         Vector3 vectorV() const;
 
@@ -146,55 +120,16 @@ class Quaternion {
         bool operator==(const Quaternion& quaternion) const;
 };
 
-// Get the value x (inline)
-inline decimal Quaternion::getX() const {
-    return mX;
-}
-
-// Get the value y (inline)
-inline decimal Quaternion::getY() const {
-    return mY;
-}
-
-// Get the value z (inline)
-inline decimal Quaternion::getZ() const {
-    return mZ;
-}
-
-// Get the value w (inline)
-inline decimal Quaternion::getW() const {
-    return mW;
-}
-
-// Set the value x (inline)
-inline void Quaternion::setX(decimal x) {
-    mX = x;
-}
-
-// Set the value y (inline)
-inline void Quaternion::setY(decimal y) {
-    mY = y;
-}
-
-// Set the value z (inline)
-inline void Quaternion::setZ(decimal z) {
-    mZ = z;
-}
-
-// Set the value w (inline)
-inline void Quaternion::setW(decimal w) {
-    mW = w;
-}
-
 // Return the vector v=(x y z) of the quaternion
 inline Vector3 Quaternion::vectorV() const {
+
     // Return the vector v
-    return Vector3(mX, mY, mZ);
+    return Vector3(x, y, z);
 }
 
 // Return the length of the quaternion (inline)
 inline decimal Quaternion::length() const {
-    return sqrt(mX*mX + mY*mY + mZ*mZ + mW*mW);
+    return sqrt(x*x + y*y + z*z + w*w);
 }
 
 // Return the unit quaternion
@@ -202,11 +137,11 @@ inline Quaternion Quaternion::getUnit() const {
     decimal lengthQuaternion = length();
 
     // Check if the length is not equal to zero
-    assert (lengthQuaternion != 0.0);
+    assert (lengthQuaternion > MACHINE_EPSILON);
 
     // Compute and return the unit quaternion
-    return Quaternion(mX/lengthQuaternion, mY/lengthQuaternion,
+    return Quaternion(x / lengthQuaternion, y / lengthQuaternion,
-                      mZ/lengthQuaternion, mW/lengthQuaternion);
+                      z / lengthQuaternion, w / lengthQuaternion);
 }
 
 // Return the identity quaternion
@@ -216,62 +151,62 @@ inline Quaternion Quaternion::identity() {
 
 // Return the conjugate of the quaternion (inline)
 inline Quaternion Quaternion::getConjugate() const {
-    return Quaternion(-mX, -mY, -mZ, mW);
+    return Quaternion(-x, -y, -z, w);
 }
 
 // Return the inverse of the quaternion (inline)
 inline Quaternion Quaternion::getInverse() const {
+
     decimal lengthQuaternion = length();
     lengthQuaternion = lengthQuaternion * lengthQuaternion;
 
-    assert (lengthQuaternion != 0.0);
+    assert (lengthQuaternion > MACHINE_EPSILON);
 
     // Compute and return the inverse quaternion
-    return Quaternion(-mX/lengthQuaternion, -mY/lengthQuaternion,
+    return Quaternion(-x / lengthQuaternion, -y / lengthQuaternion,
-                      -mZ/lengthQuaternion, mW/lengthQuaternion);
+                      -z / lengthQuaternion, w / lengthQuaternion);
 }
 
 // Scalar product between two quaternions
 inline decimal Quaternion::dot(const Quaternion& quaternion) const {
-    return (mX*quaternion.mX + mY*quaternion.mY + mZ*quaternion.mZ + mW*quaternion.mW);
+    return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
 }
 
 // Overloaded operator for the addition of two quaternions
 inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const {
+
     // Return the result quaternion
-    return Quaternion(mX + quaternion.mX, mY + quaternion.mY,
+    return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w);
-                      mZ + quaternion.mZ, mW + quaternion.mW);
 }
 
 // Overloaded operator for the substraction of two quaternions
 inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
+
     // Return the result of the substraction
-    return Quaternion(mX-quaternion.mX, mY - quaternion.mY,
+    return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w);
-                      mZ - quaternion.mZ, mW - quaternion.mW);
 }
 
 // Overloaded operator for the multiplication with a constant
 inline Quaternion Quaternion::operator*(decimal nb) const {
-    // Return the result
+    return Quaternion(nb * x, nb * y, nb * z, nb * w);
-    return Quaternion(nb*mX, nb*mY, nb*mZ, nb*mW);
 }
 
 // Overloaded operator for the multiplication of two quaternions
 inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
-    // Return the result of the multiplication
+    return Quaternion(w * quaternion.w - vectorV().dot(quaternion.vectorV()),
-    return Quaternion(mW*quaternion.mW - vectorV().dot(quaternion.vectorV()),
+                      w * quaternion.vectorV() + quaternion.w * vectorV() +
-                      mW*quaternion.vectorV()+quaternion.mW*vectorV() +
                       vectorV().cross(quaternion.vectorV()));
 }
 
 // Overloaded operator for the assignment
 inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) {
+
     // Check for self-assignment
     if (this != &quaternion) {
-        mX = quaternion.mX;
+        x = quaternion.x;
-        mY = quaternion.mY;
+        y = quaternion.y;
-        mZ = quaternion.mZ;
+        z = quaternion.z;
-        mW = quaternion.mW;
+        w = quaternion.w;
     }
 
     // Return this quaternion
@@ -280,10 +215,10 @@ inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) {
 
 // Overloaded operator for equality condition
 inline bool Quaternion::operator==(const Quaternion& quaternion) const {
-    return (mX == quaternion.mX && mY == quaternion.mY &&
+    return (x == quaternion.x && y == quaternion.y &&
-            mZ == quaternion.mZ && mW == quaternion.mW);
+            z == quaternion.z && w == quaternion.w);
 }
 
-} // End of the ReactPhysics3D namespace
+}
 
 #endif

src/mathematics/Transform.cpp

@@ -30,27 +30,26 @@
 using namespace reactphysics3d;
 
 // Constructor
-Transform::Transform() {
+Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) {
-    mPosition = Vector3(0.0, 0.0, 0.0);
+
-    mOrientation = Quaternion::identity();
 }
 
 // Constructor
-Transform::Transform(const Vector3& position, const Matrix3x3& orientation) {
+Transform::Transform(const Vector3& position, const Matrix3x3& orientation)
-    mPosition = position;
+          : mPosition(position), mOrientation(Quaternion(orientation)) {
-    mOrientation = Quaternion(orientation);
+
 }
 
 // Constructor
-Transform::Transform(const Vector3& position, const Quaternion& orientation) {
+Transform::Transform(const Vector3& position, const Quaternion& orientation)
-    mPosition = position;
+          : mPosition(position), mOrientation(orientation) {
-    mOrientation = orientation;
+
 }
 
 // Copy-constructor
-Transform::Transform(const Transform& transform) {
+Transform::Transform(const Transform& transform)
-    mPosition = transform.mPosition;
+          : mPosition(transform.mPosition), mOrientation(transform.mOrientation) {
-    mOrientation = transform.mOrientation;
+
 }
 
 // Destructor