Finish the implementation of the Hinge joint and some others modifications

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -143,7 +143,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
             int minAxis = d.getAbsoluteVector().getMinAxis();
 
             // Compute sin(60)
-            const decimal sin60 = sqrt(3.0) * 0.5;
+            const decimal sin60 = sqrt(3.0) * decimal(0.5);
 
             // Create a rotation quaternion to rotate the vector v1 to get the vectors
             // v2 and v3

src/configuration.h

@@ -81,6 +81,9 @@ const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
 /// Pi constant
 const decimal PI = decimal(3.14159265);
 
+/// 2*Pi constant
+const decimal PI_TIMES_2 = decimal(6.28318530);
+
 /// Default internal constant timestep in seconds
 const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0);
 

src/constraint/BallAndSocketJoint.cpp

@@ -27,10 +27,12 @@
 #include "BallAndSocketJoint.h"
 #include "../engine/ConstraintSolver.h"
 
+// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution)
+
 using namespace reactphysics3d;
 
 // Static variables definition
-const decimal BallAndSocketJoint::BETA = 0.2;
+const decimal BallAndSocketJoint::BETA = decimal(0.2);
 
 // Constructor
 BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)

src/constraint/HingeJoint.cpp

@@ -0,0 +1,591 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "HingeJoint.h"
+#include "../engine/ConstraintSolver.h"
+#include <cmath>
+
+// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution)
+
+using namespace reactphysics3d;
+
+// Static variables definition
+const decimal HingeJoint::BETA = decimal(0.2);
+
+// Constructor
+HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
+           : Constraint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0),
+             mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
+             mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
+             mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit),
+             mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
+             mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce) {
+
+    assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI);
+    assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI);
+
+    // Compute the local-space anchor point for each body
+    Transform transform1 = mBody1->getTransform();
+    Transform transform2 = mBody2->getTransform();
+    mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
+
+    // Compute the local-space hinge axis
+    mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
+    mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
+    mHingeLocalAxisBody1.normalize();
+    mHingeLocalAxisBody2.normalize();
+
+    // Compute the inverse of the initial orientation difference between the two bodies
+    mInitOrientationDifferenceInv = transform2.getOrientation() *
+                                 transform1.getOrientation().getInverse();
+    mInitOrientationDifferenceInv.normalize();
+    mInitOrientationDifferenceInv.inverse();
+}
+
+// Destructor
+HingeJoint::~HingeJoint() {
+
+}
+
+// Initialize before solving the constraint
+void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
+
+    // Initialize the bodies index in the velocity array
+    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+
+    // Get the bodies positions and orientations
+    const Vector3& x1 = mBody1->getTransform().getPosition();
+    const Vector3& x2 = mBody2->getTransform().getPosition();
+    const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
+    const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
+
+    // Get the inertia tensor of bodies
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the vector from body center to the anchor point in world-space
+    mR1World = orientationBody1 * mLocalAnchorPointBody1;
+    mR2World = orientationBody2 * mLocalAnchorPointBody2;
+
+    // Compute the current angle around the hinge axis
+    decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
+
+    // Check if the limit constraints are violated or not
+    decimal lowerLimitError = hingeAngle - mLowerLimit;
+    decimal upperLimitError = mUpperLimit - hingeAngle;
+    bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
+    mIsLowerLimitViolated = lowerLimitError <= 0;
+    if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
+        mImpulseLowerLimit = 0.0;
+    }
+    bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
+    mIsUpperLimitViolated = upperLimitError <= 0;
+    if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
+        mImpulseUpperLimit = 0.0;
+    }
+
+    decimal testAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
+
+    // Compute vectors needed in the Jacobian
+    mA1 = orientationBody1 * mHingeLocalAxisBody1;
+    Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2;
+    mA1.normalize();
+    a2.normalize();
+    const Vector3 b2 = a2.getOneUnitOrthogonalVector();
+    const Vector3 c2 = a2.cross(b2);
+    mB2CrossA1 = b2.cross(mA1);
+    mC2CrossA1 = c2.cross(mA1);
+
+    // Compute the corresponding skew-symmetric matrices
+    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
+    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
+
+    // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
+    decimal inverseMassBodies = 0.0;
+    if (mBody1->getIsMotionEnabled()) {
+        inverseMassBodies += mBody1->getMassInverse();
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        inverseMassBodies += mBody2->getMassInverse();
+    }
+    Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
+                                    0, inverseMassBodies, 0,
+                                    0, 0, inverseMassBodies);
+    if (mBody1->getIsMotionEnabled()) {
+        massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose();
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose();
+    }
+    mInverseMassMatrixTranslation.setToZero();
+    if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixTranslation = massMatrix.getInverse();
+    }
+
+    // Compute the bias "b" of the translation constraints
+    mBTranslation.setToZero();
+    decimal biasFactor = (BETA / constraintSolverData.timeStep);
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
+    }
+
+    // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
+    Vector3 I1B2CrossA1(0, 0, 0);
+    Vector3 I1C2CrossA1(0, 0, 0);
+    Vector3 I2B2CrossA1(0, 0, 0);
+    Vector3 I2C2CrossA1(0, 0, 0);
+    if (mBody1->getIsMotionEnabled()) {
+        I1B2CrossA1 = I1 * mB2CrossA1;
+        I1C2CrossA1 = I1 * mC2CrossA1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        I2B2CrossA1 = I2 * mB2CrossA1;
+        I2C2CrossA1 = I2 * mC2CrossA1;
+    }
+    const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
+                         mB2CrossA1.dot(I2B2CrossA1);
+    const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
+                         mB2CrossA1.dot(I2C2CrossA1);
+    const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
+                         mC2CrossA1.dot(I2B2CrossA1);
+    const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
+                         mC2CrossA1.dot(I2C2CrossA1);
+    const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
+    mInverseMassMatrixRotation.setToZero();
+    if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixRotation = matrixKRotation.getInverse();
+    }
+
+    // Compute the bias "b" of the rotation constraints
+    mBRotation.setToZero();
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2));
+    }
+
+    // If warm-starting is not enabled
+    if (!constraintSolverData.isWarmStartingActive) {
+
+        // Reset all the accumulated impulses
+        mImpulseTranslation.setToZero();
+        mImpulseRotation.setToZero();
+        mImpulseLowerLimit = 0.0;
+        mImpulseUpperLimit = 0.0;
+        mImpulseMotor = 0.0;
+    }
+
+    if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
+
+        // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
+        mInverseMassMatrixLimitMotor = 0.0;
+        if (mBody1->getIsMotionEnabled()) {
+            mInverseMassMatrixLimitMotor += mA1.dot(I1 * mA1);
+        }
+        if (mBody2->getIsMotionEnabled()) {
+            mInverseMassMatrixLimitMotor += mA1.dot(I2 * mA1);
+        }
+        mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
+                                  decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
+
+        // Compute the bias "b" of the lower limit constraint
+        mBLowerLimit = 0.0;
+        if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+            mBLowerLimit = biasFactor * lowerLimitError;
+        }
+
+        // Compute the bias "b" of the upper limit constraint
+        mBUpperLimit = 0.0;
+        if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+            mBUpperLimit = biasFactor * upperLimitError;
+        }
+    }
+}
+
+// Warm start the constraint (apply the previous impulse at the beginning of the step)
+void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    const decimal inverseMassBody1 = mBody1->getMassInverse();
+    const decimal inverseMassBody2 = mBody2->getMassInverse();
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the impulse P=J^T * lambda for the 3 translation constraints
+    Vector3 linearImpulseBody1 = -mImpulseTranslation;
+    Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
+    Vector3 linearImpulseBody2 = mImpulseTranslation;
+    Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
+
+    // Compute the impulse P=J^T * lambda for the 2 rotation constraints
+    Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
+    angularImpulseBody1 += rotationImpulse;
+    angularImpulseBody2 += -rotationImpulse;
+
+    // Compute the impulse P=J^T * lambda for the lower and upper limits constraints
+    const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1;
+    angularImpulseBody1 += limitsImpulse;
+    angularImpulseBody2 += -limitsImpulse;
+
+    // Compute the impulse P=J^T * lambda for the motor constraint
+    const Vector3 motorImpulse = -mImpulseMotor * mA1;
+    angularImpulseBody1 += motorImpulse;
+    angularImpulseBody2 += -motorImpulse;
+
+    // Apply the impulse to the bodies of the joint
+    if (mBody1->getIsMotionEnabled()) {
+        v1 += inverseMassBody1 * linearImpulseBody1;
+        w1 += I1 * angularImpulseBody1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        v2 += inverseMassBody2 * linearImpulseBody2;
+        w2 += I2 * angularImpulseBody2;
+    }
+}
+
+// Solve the velocity constraint
+void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    decimal inverseMassBody1 = mBody1->getMassInverse();
+    decimal inverseMassBody2 = mBody2->getMassInverse();
+    Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // --------------- Translation Constraints --------------- //
+
+    // Compute J*v
+    const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
+
+    // Compute the Lagrange multiplier lambda
+    const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation *
+                                          (-JvTranslation - mBTranslation);
+    mImpulseTranslation += deltaLambdaTranslation;
+
+    // Compute the impulse P=J^T * lambda
+    Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
+    Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World);
+    Vector3 linearImpulseBody2 = deltaLambdaTranslation;
+    Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World);
+
+    // Apply the impulse to the bodies of the joint
+    if (mBody1->getIsMotionEnabled()) {
+        v1 += inverseMassBody1 * linearImpulseBody1;
+        w1 += I1 * angularImpulseBody1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        v2 += inverseMassBody2 * linearImpulseBody2;
+        w2 += I2 * angularImpulseBody2;
+    }
+
+    // --------------- Rotation Constraints --------------- //
+
+    // Compute J*v for the 2 rotation constraints
+    const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2),
+                             -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2));
+
+    // Compute the Lagrange multiplier lambda for the 2 rotation constraints
+    Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation);
+    mImpulseRotation += deltaLambdaRotation;
+
+    // Compute the impulse P=J^T * lambda for the 2 rotation constraints
+    angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - mC2CrossA1 * deltaLambdaRotation.y;
+    angularImpulseBody2 = -angularImpulseBody1;
+
+    // Apply the impulse to the bodies of the joint
+    if (mBody1->getIsMotionEnabled()) {
+        w1 += I1 * angularImpulseBody1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        w2 += I2 * angularImpulseBody2;
+    }
+
+    // --------------- Limits Constraints --------------- //
+
+    if (mIsLimitEnabled) {
+
+        // If the lower limit is violated
+        if (mIsLowerLimitViolated) {
+
+            // Compute J*v for the lower limit constraint
+            const decimal JvLowerLimit = (w2 - w1).dot(mA1);
+
+            // Compute the Lagrange multiplier lambda for the lower limit constraint
+            decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit - mBLowerLimit);
+            decimal lambdaTemp = mImpulseLowerLimit;
+            mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
+            deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
+
+            // Compute the impulse P=J^T * lambda for the lower limit constraint
+            const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
+            const Vector3 angularImpulseBody2 = -angularImpulseBody1;
+
+            // Apply the impulse to the bodies of the joint
+            if (mBody1->getIsMotionEnabled()) {
+                w1 += I1 * angularImpulseBody1;
+            }
+            if (mBody2->getIsMotionEnabled()) {
+                w2 += I2 * angularImpulseBody2;
+            }
+        }
+
+        // If the upper limit is violated
+        if (mIsUpperLimitViolated) {
+
+            // Compute J*v for the upper limit constraint
+            const decimal JvUpperLimit = -(w2 - w1).dot(mA1);
+
+            // Compute the Lagrange multiplier lambda for the upper limit constraint
+            decimal deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit);
+            decimal lambdaTemp = mImpulseUpperLimit;
+            mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
+            deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
+
+            // Compute the impulse P=J^T * lambda for the upper limit constraint
+            const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
+            const Vector3 angularImpulseBody2 = -angularImpulseBody1;
+
+            // Apply the impulse to the bodies of the joint
+            if (mBody1->getIsMotionEnabled()) {
+                w1 += I1 * angularImpulseBody1;
+            }
+            if (mBody2->getIsMotionEnabled()) {
+                w2 += I2 * angularImpulseBody2;
+            }
+        }
+    }
+
+    // --------------- Motor --------------- //
+
+    if (mIsMotorEnabled) {
+
+        // Compute J*v for the motor
+        const decimal JvMotor = mA1.dot(w1 - w2);
+
+        // Compute the Lagrange multiplier lambda for the motor
+        const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep;
+        decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed);
+        decimal lambdaTemp = mImpulseMotor;
+        mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
+        deltaLambdaMotor = mImpulseMotor - lambdaTemp;
+
+        // Compute the impulse P=J^T * lambda for the motor
+        const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
+        const Vector3 angularImpulseBody2 = -angularImpulseBody1;
+
+        // Apply the impulse to the bodies of the joint
+        if (mBody1->getIsMotionEnabled()) {
+            w1 += I1 * angularImpulseBody1;
+        }
+        if (mBody2->getIsMotionEnabled()) {
+            w2 += I2 * angularImpulseBody2;
+        }
+    }
+}
+
+// Solve the position constraint
+void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
+
+}
+
+
+// Enable/Disable the limits of the joint
+void HingeJoint::enableLimit(bool isLimitEnabled) {
+
+    if (isLimitEnabled != mIsLimitEnabled) {
+
+        mIsLimitEnabled = isLimitEnabled;
+
+        // Reset the limits
+        resetLimits();
+    }
+}
+
+// Enable/Disable the motor of the joint
+void HingeJoint::enableMotor(bool isMotorEnabled) {
+
+    mIsMotorEnabled = isMotorEnabled;
+    mImpulseMotor = 0.0;
+
+    // TODO : Wake up the bodies of the joint here when sleeping is implemented
+}
+
+// Set the minimum angle limit
+void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
+
+    assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI);
+
+    if (lowerLimit != mLowerLimit) {
+
+        mLowerLimit = lowerLimit;
+
+        // Reset the limits
+        resetLimits();
+    }
+}
+
+// Set the maximum angle limit
+void HingeJoint::setMaxAngleLimit(decimal upperLimit) {
+
+    assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
+
+    if (upperLimit != mUpperLimit) {
+
+        mUpperLimit = upperLimit;
+
+        // Reset the limits
+        resetLimits();
+    }
+}
+
+// Reset the limits
+void HingeJoint::resetLimits() {
+
+    // Reset the accumulated impulses for the limits
+    mImpulseLowerLimit = 0.0;
+    mImpulseUpperLimit = 0.0;
+
+    // TODO : Wake up the bodies of the joint here when sleeping is implemented
+}
+
+// Set the motor speed
+void HingeJoint::setMotorSpeed(decimal motorSpeed) {
+
+    if (motorSpeed != mMotorSpeed) {
+
+        mMotorSpeed = motorSpeed;
+
+        // TODO : Wake up the bodies of the joint here when sleeping is implemented
+    }
+}
+
+// Set the maximum motor force
+void HingeJoint::setMaxMotorForce(decimal maxMotorForce) {
+
+    if (maxMotorForce != mMaxMotorForce) {
+
+        assert(mMaxMotorForce >= 0.0);
+        mMaxMotorForce = maxMotorForce;
+
+        // TODO : Wake up the bodies of the joint here when sleeping is implemented
+    }
+}
+
+// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
+decimal HingeJoint::computeNormalizedAngle(decimal angle) const {
+
+    // Convert it into the range [-2*pi; 2*pi]
+    angle = fmod(angle, PI_TIMES_2);
+
+    // Convert it into the range [-pi; pi]
+    if (angle < -PI) {
+        return angle + PI_TIMES_2;
+    }
+    else if (angle > PI) {
+        return angle - PI_TIMES_2;
+    }
+    else {
+        return angle;
+    }
+}
+
+// Given an "inputAngle" in the range [-pi, pi], this method returns an
+// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
+// two angle limits in arguments.
+decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
+                                                        decimal upperLimitAngle) const {
+    if (upperLimitAngle <= lowerLimitAngle) {
+        return inputAngle;
+    }
+    else if (inputAngle > upperLimitAngle) {
+        decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
+        decimal diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
+        return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle;
+    }
+    else if (inputAngle < lowerLimitAngle) {
+        decimal diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
+        decimal diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
+        return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
+    }
+    else {
+        return inputAngle;
+    }
+}
+
+// Compute the current angle around the hinge axis
+decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
+                                             const Quaternion& orientationBody2) {
+
+    decimal hingeAngle;
+
+    // Compute the current orientation difference between the two bodies
+    Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
+    currentOrientationDiff.normalize();
+
+    // Compute the relative rotation considering the initial orientation difference
+    Quaternion relativeRotation = currentOrientationDiff * mInitOrientationDifferenceInv;
+    relativeRotation.normalize();
+
+    // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
+    // length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
+    // |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any
+    // rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation
+    // axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
+    // has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
+    // about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
+    decimal cosHalfAngle = relativeRotation.w;
+    decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
+
+    // Compute the dot product of the relative rotation axis and the hinge axis
+    decimal dotProduct = relativeRotation.getVectorV().dot(mA1);
+
+    // If the relative rotation axis and the hinge axis are pointing the same direction
+    if (dotProduct >= decimal(0.0)) {
+        hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
+    }
+    else {
+        hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle);
+    }
+
+    // Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
+    hingeAngle = computeNormalizedAngle(hingeAngle);
+
+    // Compute and return the corresponding angle near one the two limits
+    return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit);
+}
+

src/constraint/HingeJoint.h

@@ -0,0 +1,342 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_HINGE_JOINT_H
+#define REACTPHYSICS3D_HINGE_JOINT_H
+
+// Libraries
+#include "Constraint.h"
+#include "../mathematics/mathematics.h"
+
+namespace reactphysics3d {
+
+// Structure HingeJointInfo
+/**
+ * This structure is used to gather the information needed to create a hinge joint.
+ * This structure will be used to create the actual hinge joint.
+ */
+struct HingeJointInfo : public ConstraintInfo {
+
+    public :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point (in world-space coordinates)
+        Vector3 anchorPointWorldSpace;
+
+        /// Hinge rotation axis (in world-space coordinates)
+        Vector3 rotationAxisWorld;
+
+        /// True if the slider limits are enabled
+        bool isLimitEnabled;
+
+        /// True if the slider motor is enabled
+        bool isMotorEnabled;
+
+        /// Minimum allowed rotation angle (in radian) if limits are enabled.
+        /// The angle must be in the range [-2*pi, 0]
+        decimal minAngleLimit;
+
+        /// Maximum allowed rotation angle (in radian) if limits are enabled.
+        /// The angle must be in the range [0, 2*pi]
+        decimal maxAngleLimit;
+
+        /// Motor speed (in radian/second)
+        decimal motorSpeed;
+
+        /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed
+        decimal maxMotorForce;
+
+        /// Constructor without limits and without motor
+        HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                               const Vector3& initAnchorPointWorldSpace,
+                               const Vector3& initRotationAxisWorld)
+                              : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
+                                anchorPointWorldSpace(initAnchorPointWorldSpace),
+                                rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
+                                isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), motorSpeed(0),
+                                maxMotorForce(0) {}
+
+        /// Constructor with limits but without motor
+        HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                               const Vector3& initAnchorPointWorldSpace,
+                               const Vector3& initRotationAxisWorld,
+                               decimal initMinAngleLimit, decimal initMaxAngleLimit)
+                              : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
+                                anchorPointWorldSpace(initAnchorPointWorldSpace),
+                                rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
+                                isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
+                                maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxMotorForce(0) {}
+
+        /// Constructor with limits and motor
+        HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                               const Vector3& initAnchorPointWorldSpace,
+                               const Vector3& initRotationAxisWorld,
+                               decimal initMinAngleLimit, decimal initMaxAngleLimit,
+                               decimal initMotorSpeed, decimal initMaxMotorForce)
+                              : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
+                                anchorPointWorldSpace(initAnchorPointWorldSpace),
+                                rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
+                                isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
+                                maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed),
+                                maxMotorForce(initMaxMotorForce) {}
+};
+
+// Class HingeJoint
+/**
+ * This class represents a hinge joint that allows arbitrary rotation
+ * between two bodies around a single axis.
+ */
+class HingeJoint : public Constraint {
+
+    private :
+
+        // -------------------- Constants -------------------- //
+
+        // Beta value for the bias factor of position correction
+        static const decimal BETA;
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point of body 1 (in local-space coordinates of body 1)
+        Vector3 mLocalAnchorPointBody1;
+
+        /// Anchor point of body 2 (in local-space coordinates of body 2)
+        Vector3 mLocalAnchorPointBody2;
+
+        /// Hinge rotation axis (in local-space coordinates of body 1)
+        Vector3 mHingeLocalAxisBody1;
+
+        /// Hinge rotation axis (in local-space coordiantes of body 2)
+        Vector3 mHingeLocalAxisBody2;
+
+        /// Hinge rotation axis (in world-space coordinates) computed from body 1
+        Vector3 mA1;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mR1World;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mR2World;
+
+        /// Cross product of vector b2 and a1
+        Vector3 mB2CrossA1;
+
+        /// Cross product of vector c2 and a1;
+        Vector3 mC2CrossA1;
+
+        /// Impulse for the 3 translation constraints
+        Vector3 mImpulseTranslation;
+
+        /// Impulse for the 2 rotation constraints
+        Vector2 mImpulseRotation;
+
+        /// Accumulated impulse for the lower limit constraint
+        decimal mImpulseLowerLimit;
+
+        /// Accumulated impulse for the upper limit constraint
+        decimal mImpulseUpperLimit;
+
+        /// Accumulated impulse for the motor constraint;
+        decimal mImpulseMotor;
+
+        /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints
+        Matrix3x3 mInverseMassMatrixTranslation;
+
+        /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints
+        Matrix2x2 mInverseMassMatrixRotation;
+
+        /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
+        decimal mInverseMassMatrixLimitMotor;
+
+        /// Inverse of mass matrix K=JM^-1J^t for the motor
+        decimal mInverseMassMatrixMotor;
+
+        /// Bias vector for the error correction for the translation constraints
+        Vector3 mBTranslation;
+
+        /// Bias vector for the error correction for the rotation constraints
+        Vector2 mBRotation;
+
+        /// Bias of the lower limit constraint
+        decimal mBLowerLimit;
+
+        /// Bias of the upper limit constraint
+        decimal mBUpperLimit;
+
+        /// Inverse of the initial orientation difference between the bodies
+        Quaternion mInitOrientationDifferenceInv;
+
+        /// True if the joint limits are enabled
+        bool mIsLimitEnabled;
+
+        /// True if the motor of the joint in enabled
+        bool mIsMotorEnabled;
+
+        /// Lower limit (minimum allowed rotation angle in radi)
+        decimal mLowerLimit;
+
+        /// Upper limit (maximum translation distance)
+        decimal mUpperLimit;
+
+        /// True if the lower limit is violated
+        bool mIsLowerLimitViolated;
+
+        /// True if the upper limit is violated
+        bool mIsUpperLimitViolated;
+
+        /// Motor speed
+        decimal mMotorSpeed;
+
+        /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed
+        decimal mMaxMotorForce;
+
+        // -------------------- Methods -------------------- //
+
+        /// Reset the limits
+        void resetLimits();
+
+        /// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
+        decimal computeNormalizedAngle(decimal angle) const;
+
+        /// Given an "inputAngle" in the range [-pi, pi], this method returns an
+        /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
+        /// two angle limits in arguments.
+        decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
+                                                    decimal upperLimitAngle) const;
+
+        /// Compute the current angle around the hinge axis
+        decimal computeCurrentHingeAngle(const Quaternion& orientationBody1,
+                                         const Quaternion& orientationBody2);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        HingeJoint(const HingeJointInfo& jointInfo);
+
+        /// Destructor
+        virtual ~HingeJoint();
+
+        /// Return true if the limits or the joint are enabled
+        bool isLimitEnabled() const;
+
+        /// Return true if the motor of the joint is enabled
+        bool isMotorEnabled() const;
+
+        /// Enable/Disable the limits of the joint
+        void enableLimit(bool isLimitEnabled);
+
+        /// Enable/Disable the motor of the joint
+        void enableMotor(bool isMotorEnabled);
+
+        /// Return the minimum angle limit
+        decimal getMinAngleLimit() const;
+
+        /// Set the minimum angle limit
+        void setMinAngleLimit(decimal lowerLimit);
+
+        /// Return the maximum angle limit
+        decimal getMaxAngleLimit() const;
+
+        /// Set the maximum angle limit
+        void setMaxAngleLimit(decimal upperLimit);
+
+        /// Return the motor speed
+        decimal getMotorSpeed() const;
+
+        /// Set the motor speed
+        void setMotorSpeed(decimal motorSpeed);
+
+        /// Return the maximum motor force
+        decimal getMaxMotorForce() const;
+
+        /// Set the maximum motor force
+        void setMaxMotorForce(decimal maxMotorForce);
+
+        /// Return the intensity of the current force applied for the joint motor
+        decimal getMotorForce(decimal timeStep) const;
+
+        /// Return the number of bytes used by the joint
+        virtual size_t getSizeInBytes() const;
+
+        /// Initialize before solving the constraint
+        virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
+
+        /// Warm start the constraint (apply the previous impulse at the beginning of the step)
+        virtual void warmstart(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the velocity constraint
+        virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the position constraint
+        virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
+};
+
+// Return true if the limits or the joint are enabled
+inline bool HingeJoint::isLimitEnabled() const {
+    return mIsLimitEnabled;
+}
+
+// Return true if the motor of the joint is enabled
+inline bool HingeJoint::isMotorEnabled() const {
+    return mIsMotorEnabled;
+}
+
+// Return the minimum angle limit
+inline decimal HingeJoint::getMinAngleLimit() const {
+    return mLowerLimit;
+}
+
+// Return the maximum angle limit
+inline decimal HingeJoint::getMaxAngleLimit() const {
+    return mUpperLimit;
+}
+
+// Return the motor speed
+inline decimal HingeJoint::getMotorSpeed() const {
+    return mMotorSpeed;
+}
+
+// Return the maximum motor force
+inline decimal HingeJoint::getMaxMotorForce() const {
+    return mMaxMotorForce;
+}
+
+// Return the intensity of the current force applied for the joint motor
+inline decimal HingeJoint::getMotorForce(decimal timeStep) const {
+    return mImpulseMotor / timeStep;
+}
+
+// Return the number of bytes used by the joint
+inline size_t HingeJoint::getSizeInBytes() const {
+    return sizeof(HingeJoint);
+}
+
+}
+
+
+#endif

src/constraint/SliderJoint.cpp

@@ -26,6 +26,8 @@
 // Libraries
 #include "SliderJoint.h"
 
+// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution)
+
 using namespace reactphysics3d;
 
 // Static variables definition
@@ -36,7 +38,7 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
             : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
               mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
               mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
-              mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit),
+              mLowerLimit(jointInfo.minTranslationLimit), mUpperLimit(jointInfo.maxTranslationLimit),
               mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
               mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){
 
@@ -50,10 +52,11 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
     mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
     mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
 
-    // Compute the initial orientation difference between the two bodies
+    // Compute the inverse of the initial orientation difference between the two bodies
-    mInitOrientationDifference = transform2.getOrientation() *
+    mInitOrientationDifferenceInv = transform2.getOrientation() *
                                  transform1.getOrientation().getInverse();
-    mInitOrientationDifference.normalize();
+    mInitOrientationDifferenceInv.normalize();
+    mInitOrientationDifferenceInv.inverse();
 
     // Compute the slider axis in local-space of body 1
     mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() *
@@ -87,7 +90,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     mR1 = orientationBody1 * mLocalAnchorPointBody1;
     mR2 = orientationBody2 * mLocalAnchorPointBody2;
 
-    // Compute the vector u
+    // Compute the vector u (difference between anchor points)
     const Vector3 u = x2 + mR2 - x1 - mR1;
 
     // Compute the two orthogonal vectors to the slider axis in world-space
@@ -178,14 +181,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
         Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
         currentOrientationDifference.normalize();
-        const Quaternion qError = currentOrientationDifference *
+        const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
-                                  mInitOrientationDifference.getInverse();
         mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
     }
 
     if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
 
-        // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix)
+        // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
         mInverseMassMatrixLimit = 0.0;
         if (mBody1->getIsMotionEnabled()) {
             mInverseMassMatrixLimit += mBody1->getMassInverse() +
@@ -469,8 +471,8 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
     // TODO : Wake up the bodies of the joint here when sleeping is implemented
 }
 
-// Set the lower limit
+// Set the minimum translation limit
-void SliderJoint::setLowerLimit(decimal lowerLimit) {
+void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
 
     assert(lowerLimit <= mUpperLimit);
 
@@ -483,8 +485,8 @@ void SliderJoint::setLowerLimit(decimal lowerLimit) {
     }
 }
 
-// Set the upper limit
+// Set the maximum translation limit
-void SliderJoint::setUpperLimit(decimal upperLimit) {
+void SliderJoint::setMaxTranslationLimit(decimal upperLimit) {
 
     assert(mLowerLimit <= upperLimit);
 

src/constraint/SliderJoint.h

@@ -55,11 +55,11 @@ struct SliderJointInfo : public ConstraintInfo {
         /// True if the slider motor is enabled
         bool isMotorEnabled;
 
-        /// Lower limit
+        /// Mininum allowed translation if limits are enabled
-        decimal lowerLimit;
+        decimal minTranslationLimit;
 
-        /// Upper limit
+        /// Maximum allowed translation if limits are enabled
-        decimal upperLimit;
+        decimal maxTranslationLimit;
 
         /// Motor speed
         decimal motorSpeed;
@@ -74,31 +74,34 @@ struct SliderJointInfo : public ConstraintInfo {
                        : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
                          anchorPointWorldSpace(initAnchorPointWorldSpace),
                          sliderAxisWorldSpace(initSliderAxisWorldSpace),
-                         isLimitEnabled(false), isMotorEnabled(false), lowerLimit(-1.0),
+                         isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0),
-                         upperLimit(1.0), motorSpeed(0), maxMotorForce(0) {}
+                         maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {}
 
         /// Constructor with limits and no motor
         SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
                         const Vector3& initAnchorPointWorldSpace,
                         const Vector3& initSliderAxisWorldSpace,
-                        decimal initLowerLimit, decimal initUpperLimit)
+                        decimal initMinTranslationLimit, decimal initMaxTranslationLimit)
                        : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
                          anchorPointWorldSpace(initAnchorPointWorldSpace),
                          sliderAxisWorldSpace(initSliderAxisWorldSpace),
-                         isLimitEnabled(true), isMotorEnabled(false), lowerLimit(initLowerLimit),
+                         isLimitEnabled(true), isMotorEnabled(false),
-                         upperLimit(initUpperLimit), motorSpeed(0), maxMotorForce(0) {}
+                         minTranslationLimit(initMinTranslationLimit),
+                         maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0),
+                         maxMotorForce(0) {}
 
         /// Constructor with limits and motor
         SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
                         const Vector3& initAnchorPointWorldSpace,
                         const Vector3& initSliderAxisWorldSpace,
-                        decimal initLowerLimit, decimal initUpperLimit,
+                        decimal initMinTranslationLimit, decimal initMaxTranslationLimit,
                         decimal initMotorSpeed, decimal initMaxMotorForce)
                        : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
                          anchorPointWorldSpace(initAnchorPointWorldSpace),
                          sliderAxisWorldSpace(initSliderAxisWorldSpace),
-                         isLimitEnabled(true), isMotorEnabled(true), lowerLimit(initLowerLimit),
+                         isLimitEnabled(true), isMotorEnabled(true),
-                         upperLimit(initUpperLimit), motorSpeed(initMotorSpeed),
+                         minTranslationLimit(initMinTranslationLimit),
+                         maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed),
                          maxMotorForce(initMaxMotorForce) {}
 };
 
@@ -126,8 +129,8 @@ class SliderJoint : public Constraint {
         /// Slider axis (in local-space coordinates of body 1)
         Vector3 mSliderAxisBody1;
 
-        /// Initial orientation difference between the two bodies
+        /// Inverse of the initial orientation difference between the two bodies
-        Quaternion mInitOrientationDifference;
+        Quaternion mInitOrientationDifferenceInv;
 
         /// First vector orthogonal to the slider axis local-space of body 1
         Vector3 mN1;
@@ -183,19 +186,19 @@ class SliderJoint : public Constraint {
         /// Inverse of mass matrix K=JM^-1J^t for the motor
         decimal mInverseMassMatrixMotor;
 
-        /// Impulse for the 2 translation constraints
+        /// Accumulated impulse for the 2 translation constraints
         Vector2 mImpulseTranslation;
 
-        /// Impulse for the 3 rotation constraints
+        /// Accumulated impulse for the 3 rotation constraints
         Vector3 mImpulseRotation;
 
-        /// Impulse for the lower limit constraint
+        /// Accumulated impulse for the lower limit constraint
         decimal mImpulseLowerLimit;
 
-        /// Impulse for the upper limit constraint
+        /// Accumulated impulse for the upper limit constraint
         decimal mImpulseUpperLimit;
 
-        /// Impulse for the motor
+        /// Accumulated impulse for the motor
         decimal mImpulseMotor;
 
         /// True if the slider limits are enabled
@@ -207,10 +210,10 @@ class SliderJoint : public Constraint {
         /// Slider axis in world-space coordinates
         Vector3 mSliderAxisWorld;
 
-        /// Lower limit
+        /// Lower limit (minimum translation distance)
         decimal mLowerLimit;
 
-        /// Upper limit
+        /// Upper limit (maximum translation distance)
         decimal mUpperLimit;
 
         /// True if the lower limit is violated
@@ -252,17 +255,17 @@ class SliderJoint : public Constraint {
         /// Enable/Disable the motor of the joint
         void enableMotor(bool isMotorEnabled);
 
-        /// Return the lower limit
+        /// Return the minimum translation limit
-        decimal getLowerLimit() const;
+        decimal getMinTranslationLimit() const;
 
-        /// Set the lower limit
+        /// Set the minimum translation limit
-        void setLowerLimit(decimal lowerLimit);
+        void setMinTranslationLimit(decimal lowerLimit);
 
-        /// Return the upper limit
+        /// Return the maximum translation limit
-        decimal getUpperLimit() const;
+        decimal getMaxTranslationLimit() const;
 
-        /// Set the upper limit
+        /// Set the maximum translation limit
-        void setUpperLimit(decimal upperLimit);
+        void setMaxTranslationLimit(decimal upperLimit);
 
         /// Return the motor speed
         decimal getMotorSpeed() const;
@@ -305,13 +308,13 @@ inline bool SliderJoint::isMotorEnabled() const {
     return mIsMotorEnabled;
 }
 
-// Return the lower limit
+// Return the minimum translation limit
-inline decimal SliderJoint::getLowerLimit() const {
+inline decimal SliderJoint::getMinTranslationLimit() const {
     return mLowerLimit;
 }
 
-// Return the upper limit
+// Return the maximum translation limit
-inline decimal SliderJoint::getUpperLimit() const {
+inline decimal SliderJoint::getMaxTranslationLimit() const {
     return mUpperLimit;
 }
 

src/engine/ConstraintSolver.cpp

@@ -37,7 +37,7 @@ ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
                  : mJoints(joints), mLinearVelocities(linearVelocities),
                    mAngularVelocities(angularVelocities),
                    mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                   mIsWarmStartingActive(false),
+                   mIsWarmStartingActive(true),
                    mIsNonLinearGaussSeidelPositionCorrectionActive(false),
                    mConstraintSolverData(linearVelocities,
                    angularVelocities, mapBodyToVelocityIndex){
@@ -77,6 +77,11 @@ void ConstraintSolver::initialize(decimal dt) {
 
         // Initialize the constraint before solving it
         joint->initBeforeSolve(mConstraintSolverData);
+
+        // Warm-start the constraint if warm-starting is enabled
+        if (mIsWarmStartingActive) {
+            joint->warmstart(mConstraintSolverData);
+        }
     }
 }
 

src/engine/DynamicsWorld.cpp

@@ -27,6 +27,7 @@
 #include "DynamicsWorld.h"
 #include "constraint/BallAndSocketJoint.h"
 #include "constraint/SliderJoint.h"
+#include "constraint/HingeJoint.h"
 
 // Namespaces
 using namespace reactphysics3d;
@@ -212,7 +213,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
     mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
     mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
 
-    double dt = mTimer.getTimeStep();
+    decimal dt = static_cast<decimal>(mTimer.getTimeStep());
 
     // Fill in the mapping of rigid body to their index in the constrained
     // velocities arrays
@@ -413,6 +414,15 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
             break;
         }
 
+        // Hinge joint
+        case HINGEJOINT:
+        {
+            void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint));
+            const HingeJointInfo& info = dynamic_cast<const HingeJointInfo&>(jointInfo);
+            newJoint = new (allocatedMemory) HingeJoint(info);
+            break;
+        }
+
         default:
         {
             assert(false);

src/mathematics/Quaternion.h

@@ -84,15 +84,24 @@ struct Quaternion {
         /// Set the quaternion to zero
         void setToZero();
 
+        /// Set to the identity quaternion
+        void setToIdentity();
+
         /// Return the vector v=(x y z) of the quaternion
         Vector3 getVectorV() const;
 
         /// Return the length of the quaternion
         decimal length() const;
 
+        /// Return the square of the length of the quaternion
+        decimal lengthSquare() const;
+
         /// Normalize the quaternion
         void normalize();
 
+        /// Inverse the quaternion
+        void inverse();
+
         /// Return the unit quaternion
         Quaternion getUnit() const;
 
@@ -156,6 +165,14 @@ inline void Quaternion::setToZero() {
     w = 0;
 }
 
+// Set to the identity quaternion
+inline void Quaternion::setToIdentity() {
+    x = 0;
+    y = 0;
+    z = 0;
+    w = 1;
+}
+
 // Return the vector v=(x y z) of the quaternion
 inline Vector3 Quaternion::getVectorV() const {
 
@@ -168,6 +185,11 @@ inline decimal Quaternion::length() const {
     return sqrt(x*x + y*y + z*z + w*w);
 }
 
+// Return the square of the length of the quaternion
+inline decimal Quaternion::lengthSquare() const {
+    return x*x + y*y + z*z + w*w;
+}
+
 // Normalize the quaternion
 inline void Quaternion::normalize() {
 
@@ -182,6 +204,21 @@ inline void Quaternion::normalize() {
     w /= l;
 }
 
+// Inverse the quaternion
+inline void Quaternion::inverse() {
+
+    // Get the square length of the quaternion
+    decimal lengthSquareQuaternion = lengthSquare();
+
+    assert (lengthSquareQuaternion > MACHINE_EPSILON);
+
+    // Compute and return the inverse quaternion
+    x /= -lengthSquareQuaternion;
+    y /= -lengthSquareQuaternion;
+    z /= -lengthSquareQuaternion;
+    w /= lengthSquareQuaternion;
+}
+
 // Return the unit quaternion
 inline Quaternion Quaternion::getUnit() const {
     decimal lengthQuaternion = length();
@@ -207,14 +244,13 @@ inline Quaternion Quaternion::getConjugate() const {
 // Return the inverse of the quaternion (inline)
 inline Quaternion Quaternion::getInverse() const {
 
-    decimal lengthQuaternion = length();
+    decimal lengthSquareQuaternion = lengthSquare();
-    lengthQuaternion = lengthQuaternion * lengthQuaternion;
 
-    assert (lengthQuaternion > MACHINE_EPSILON);
+    assert (lengthSquareQuaternion > MACHINE_EPSILON);
 
     // Compute and return the inverse quaternion
-    return Quaternion(-x / lengthQuaternion, -y / lengthQuaternion,
+    return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion,
-                      -z / lengthQuaternion, w / lengthQuaternion);
+                      -z / lengthSquareQuaternion, w / lengthSquareQuaternion);
 }
 
 // Scalar product between two quaternions

src/mathematics/mathematics_functions.h

@@ -29,6 +29,7 @@
 // Libraries
 #include "../configuration.h"
 #include "../decimal.h"
+#include <algorithm>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -43,6 +44,13 @@ inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON)
     return (difference < epsilon && difference > -epsilon);
 }
 
+/// Function that returns the result of the "value" clamped by
+/// two others values "lowerLimit" and "upperLimit"
+inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) {
+    assert(lowerLimit <= upperLimit);
+    return std::min(std::max(value, lowerLimit), upperLimit);
+}
+
 }
 
 

src/reactphysics3d.h

@@ -49,6 +49,7 @@
 #include "collision/shapes/AABB.h"
 #include "constraint/BallAndSocketJoint.h"
 #include "constraint/SliderJoint.h"
+#include "constraint/HingeJoint.h"
 
 /// Alias to the ReactPhysics3D namespace
 namespace rp3d = reactphysics3d;

test/tests/mathematics/TestQuaternion.h

@@ -120,6 +120,12 @@ class TestQuaternion : public Test {
             quaternion.setToZero();
             test(quaternion == Quaternion(0, 0, 0, 0));
 
+            // Tes the methods to get or set to identity
+            Quaternion identity1(1, 2, 3, 4);
+            identity1.setToIdentity();
+            test(identity1 == Quaternion(0, 0, 0, 1));
+            test(Quaternion::identity() == Quaternion(0, 0, 0, 1));
+
             // Test the method to get the vector (x, y, z)
             Vector3 v = mQuaternion1.getVectorV();
             test(v.x == mQuaternion1.x);
@@ -133,9 +139,12 @@ class TestQuaternion : public Test {
             test(conjugate.z == -mQuaternion1.z);
             test(conjugate.w == mQuaternion1.w);
 
-            // Test the inverse method
+            // Test the inverse methods
-            Quaternion inverse = mQuaternion1.getInverse();
+            Quaternion inverse1 = mQuaternion1.getInverse();
-            Quaternion product = mQuaternion1 * inverse;
+            Quaternion inverse2(mQuaternion1);
+            inverse2.inverse();
+            test(inverse2 == inverse1);
+            Quaternion product = mQuaternion1 * inverse1;
             test(approxEqual(product.x, mIdentity.x, decimal(10e-6)));
             test(approxEqual(product.y, mIdentity.y, decimal(10e-6)));
             test(approxEqual(product.z, mIdentity.z, decimal(10e-6)));