diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index cca79f2b..b0b7ba66 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/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; diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index 06d121d8..07396882 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/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); - decimal invDeterminant = 1.0 / determinant; - Matrix3x3 tempMatrix; + assert(std::abs(determinant) > MACHINE_EPSILON); - // Compute the inverse of the matrix - 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]), - (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])); + decimal invDeterminant = decimal(1.0) / determinant; + + Matrix3x3 tempMatrix((mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]), + -(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); } diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index 3c33046a..cef6e1e3 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -32,26 +32,24 @@ using namespace reactphysics3d; // Constructor of the class -Quaternion::Quaternion() - :mX(0.0), mY(0.0), mZ(0.0), mW(0.0) { +Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { } // Constructor with arguments -Quaternion::Quaternion(decimal x, decimal y, decimal z, decimal w) - :mX(x), mY(y), mZ(z), mW(w) { +Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) + :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) - :mX(v.x), mY(v.y), mZ(v.z), mW(w) { +Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { } // 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; - mY = (array[1][2] + array[2][1])*s; - mZ = 0.5*r; - mW = (array[1][0] - array[0][1])*s; + x = (array[2][0] + array[0][2])*s; + y = (array[1][2] + array[2][1])*s; + z = 0.5*r; + 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; - mY = 0.5 * r; - mZ = (array[1][2] + array[2][1])*s; - mW = (array[0][2] - array[2][0])*s; + x = (array[0][1] + array[1][0])*s; + y = 0.5 * r; + z = (array[1][2] + array[2][1])*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; - mY = (array[1][2] + array[2][1])*s; - mZ = 0.5 * r; - mW = (array[1][0] - array[0][1])*s; + x = (array[2][0] + array[0][2])*s; + y = (array[1][2] + array[2][1])*s; + z = 0.5 * r; + 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; - mY = (array[0][1] + array[1][0])*s; - mZ = (array[2][0] - array[0][2])*s; - mW = (array[2][1] - array[1][2])*s; + x = 0.5 * r; + y = (array[0][1] + array[1][0])*s; + z = (array[2][0] - array[0][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; - mY = (array[0][2]-array[2][0])*s; - mZ = (array[1][0]-array[0][1])*s; - mW = 0.5 * r; + x = (array[2][1]-array[1][2])*s; + y = (array[0][2]-array[2][0])*s; + z = (array[1][0]-array[0][1])*s; + 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 ys = mY*s; - decimal zs = mZ*s; - decimal wxs = mW*xs; - decimal wys = mW*ys; - decimal wzs = mW*zs; - decimal xxs = mX*xs; - decimal xys = mX*ys; - decimal xzs = mX*zs; - decimal yys = mY*ys; - decimal yzs = mY*zs; - decimal zzs = mZ*zs; + decimal xs = x*s; + decimal ys = y*s; + decimal zs = z*s; + decimal wxs = w*xs; + decimal wys = w*ys; + decimal wzs = w*zs; + decimal xxs = x*xs; + decimal xys = x*ys; + decimal xzs = x*zs; + decimal yys = y*ys; + decimal yzs = y*zs; + decimal zzs = z*zs; // Create the matrix corresponding to the quaternion return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys, diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 02c0af88..140747e3 100644 --- a/src/mathematics/Quaternion.h +++ b/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; - - public : + decimal x, y, z, w; // -------------------- 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, - mZ/lengthQuaternion, mW/lengthQuaternion); + return Quaternion(x / lengthQuaternion, y / 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, - -mZ/lengthQuaternion, mW/lengthQuaternion); + return Quaternion(-x / lengthQuaternion, -y / 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, - mZ + quaternion.mZ, mW + quaternion.mW); + return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w); } // 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, - mZ - quaternion.mZ, mW - quaternion.mW); + 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*(decimal nb) const { - // Return the result - return Quaternion(nb*mX, nb*mY, nb*mZ, nb*mW); + 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 - return Quaternion(mW*quaternion.mW - vectorV().dot(quaternion.vectorV()), - mW*quaternion.vectorV()+quaternion.mW*vectorV() + + return Quaternion(w * quaternion.w - vectorV().dot(quaternion.vectorV()), + w * quaternion.vectorV() + quaternion.w * 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; - mY = quaternion.mY; - mZ = quaternion.mZ; - mW = quaternion.mW; + x = quaternion.x; + y = quaternion.y; + z = quaternion.z; + 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 && - mZ == quaternion.mZ && mW == quaternion.mW); + return (x == quaternion.x && y == quaternion.y && + z == quaternion.z && w == quaternion.w); } -} // End of the ReactPhysics3D namespace +} #endif diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp index d3895b21..cd8b08f6 100644 --- a/src/mathematics/Transform.cpp +++ b/src/mathematics/Transform.cpp @@ -30,27 +30,26 @@ using namespace reactphysics3d; // Constructor -Transform::Transform() { - mPosition = Vector3(0.0, 0.0, 0.0); - mOrientation = Quaternion::identity(); +Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { + } // Constructor -Transform::Transform(const Vector3& position, const Matrix3x3& orientation) { - mPosition = position; - mOrientation = Quaternion(orientation); +Transform::Transform(const Vector3& position, const Matrix3x3& orientation) + : mPosition(position), mOrientation(Quaternion(orientation)) { + } // Constructor -Transform::Transform(const Vector3& position, const Quaternion& orientation) { - mPosition = position; - mOrientation = orientation; +Transform::Transform(const Vector3& position, const Quaternion& orientation) + : mPosition(position), mOrientation(orientation) { + } // Copy-constructor -Transform::Transform(const Transform& transform) { - mPosition = transform.mPosition; - mOrientation = transform.mOrientation; +Transform::Transform(const Transform& transform) + : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { + } // Destructor