Add support for rolling resistance in the contact solver

src/collision/ContactManifold.h

@@ -119,6 +119,9 @@ class ContactManifold {
         /// Twist friction constraint accumulated impulse
         decimal mFrictionTwistImpulse;
 
+        /// Accumulated rolling resistance impulse
+        Vector3 mRollingResistanceImpulse;
+
         /// True if the contact manifold has already been added into an island
         bool mIsAlreadyInIsland;
 
@@ -216,6 +219,9 @@ class ContactManifold {
         /// Set the friction twist accumulated impulse
         void setFrictionTwistImpulse(decimal frictionTwistImpulse);
 
+        /// Set the accumulated rolling resistance impulse
+        void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
+
         /// Return a contact point of the manifold
         ContactPoint* getContactPoint(uint index) const;
 
@@ -312,6 +318,11 @@ inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpuls
     mFrictionTwistImpulse = frictionTwistImpulse;
 }
 
+// Set the accumulated rolling resistance impulse
+inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse) {
+    mRollingResistanceImpulse = rollingResistanceImpulse;
+}
+
 // Return a contact point of the manifold
 inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
     assert(index >= 0 && index < mNbContactPoints);

src/configuration.h

@@ -94,6 +94,9 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
 /// Default bounciness factor for a rigid body
 const decimal DEFAULT_BOUNCINESS = decimal(0.5);
 
+/// Default rolling resistance
+const decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0);
+
 /// True if the spleeping technique is enabled
 const bool SPLEEPING_ENABLED = true;
 

src/constraint/ContactPoint.h

@@ -139,6 +139,9 @@ class ContactPoint {
 
         /// Cached second friction impulse
         decimal mFrictionImpulse2;
+
+        /// Cached rolling resistance impulse
+        Vector3 mRollingResistanceImpulse;
         
         // -------------------- Methods -------------------- //
 
@@ -191,6 +194,9 @@ class ContactPoint {
         /// Return the cached second friction impulse
         decimal getFrictionImpulse2() const;
 
+        /// Return the cached rolling resistance impulse
+        Vector3 getRollingResistanceImpulse() const;
+
         /// Set the cached penetration impulse
         void setPenetrationImpulse(decimal impulse);
 
@@ -200,6 +206,9 @@ class ContactPoint {
         /// Set the second cached friction impulse
         void setFrictionImpulse2(decimal impulse);
 
+        /// Set the cached rolling resistance impulse
+        void setRollingResistanceImpulse(const Vector3& impulse);
+
         /// Set the contact world point on body 1
         void setWorldPointOnBody1(const Vector3& worldPoint);
 
@@ -286,6 +295,11 @@ inline decimal ContactPoint::getFrictionImpulse2() const {
     return mFrictionImpulse2;
 }
 
+// Return the cached rolling resistance impulse
+inline Vector3 ContactPoint::getRollingResistanceImpulse() const {
+    return mRollingResistanceImpulse;
+}
+
 // Set the cached penetration impulse
 inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
     mPenetrationImpulse = impulse;
@@ -301,6 +315,11 @@ inline void ContactPoint::setFrictionImpulse2(decimal impulse) {
     mFrictionImpulse2 = impulse;
 }
 
+// Set the cached rolling resistance impulse
+inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) {
+    mRollingResistanceImpulse = impulse;
+}
+
 // Set the contact world point on body 1
 inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
     mWorldPointOnBody1 = worldPoint;

src/engine/ContactSolver.cpp

@@ -103,12 +103,15 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
         internalManifold.nbContacts = externalManifold->getNbContactPoints();
         internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
         internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
+        internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
         internalManifold.externalContactManifold = externalManifold;
+        internalManifold.isBody1DynamicType = body1->getType() == DYNAMIC;
+        internalManifold.isBody2DynamicType = body2->getType() == DYNAMIC;
 
         // If we solve the friction constraints at the center of the contact manifold
         if (mIsSolveFrictionAtContactManifoldCenterActive) {
-            internalManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0);
+            internalManifold.frictionPointBody1 = Vector3::zero();
-            internalManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0);
+            internalManifold.frictionPointBody2 = Vector3::zero();
         }
 
         // For each  contact point of the contact manifold
@@ -135,6 +138,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
             contactPoint.penetrationImpulse = 0.0;
             contactPoint.friction1Impulse = 0.0;
             contactPoint.friction2Impulse = 0.0;
+            contactPoint.rollingResistanceImpulse = Vector3::zero();
 
             // If we solve the friction constraints at the center of the contact manifold
             if (mIsSolveFrictionAtContactManifoldCenterActive) {
@@ -167,6 +171,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
                 internalManifold.friction1Impulse = 0.0;
                 internalManifold.friction2Impulse = 0.0;
                 internalManifold.frictionTwistImpulse = 0.0;
+                internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
             }
         }
     }
@@ -266,6 +271,7 @@ void ContactSolver::initializeContactConstraints() {
                 contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
                 contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
                 contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
+                contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
             }
 
             // Initialize the split impulses to zero
@@ -277,6 +283,13 @@ void ContactSolver::initializeContactConstraints() {
             }
         }
 
+        // Compute the inverse K matrix for the rolling resistance constraint
+        manifold.inverseRollingResistance.setToZero();
+        if (manifold.rollingResistanceFactor > 0 && manifold.isBody1DynamicType || manifold.isBody2DynamicType) {
+            manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
+            manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
+        }
+
         // If we solve the friction constraints at the center of the contact manifold
         if (mIsSolveFrictionAtContactManifoldCenterActive) {
 
@@ -384,6 +397,18 @@ void ContactSolver::warmStart() {
 
                     // Apply the impulses to the bodies of the constraint
                     applyImpulse(impulseFriction2, contactManifold);
+
+                    // ------ Rolling resistance------ //
+
+                    if (contactManifold.rollingResistanceFactor > 0) {
+
+                        // Compute the impulse P = J^T * lambda
+                        const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse,
+                                                               Vector3::zero(), contactPoint.rollingResistanceImpulse);
+
+                        // Apply the impulses to the bodies of the constraint
+                        applyImpulse(impulseRollingResistance, contactManifold);
+                    }
                 }
             }
             else {  // If it is a new contact point
@@ -392,6 +417,7 @@ void ContactSolver::warmStart() {
                 contactPoint.penetrationImpulse = 0.0;
                 contactPoint.friction1Impulse = 0.0;
                 contactPoint.friction2Impulse = 0.0;
+                contactPoint.rollingResistanceImpulse = Vector3::zero();
             }
         }
 
@@ -456,6 +482,17 @@ void ContactSolver::warmStart() {
 
             // Apply the impulses to the bodies of the constraint
             applyImpulse(impulseTwistFriction, contactManifold);
+
+            // ------ Rolling resistance at the center of the contact manifold ------ //
+
+            // Compute the impulse P = J^T * lambda
+            angularImpulseBody1 = -contactManifold.rollingResistanceImpulse;
+            angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
+            const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1,
+                                                   Vector3::zero(), angularImpulseBody2);
+
+            // Apply the impulses to the bodies of the constraint
+            applyImpulse(impulseRollingResistance, contactManifold);
         }
         else {  // If it is a new contact manifold
 
@@ -463,6 +500,7 @@ void ContactSolver::warmStart() {
             contactManifold.friction1Impulse = 0.0;
             contactManifold.friction2Impulse = 0.0;
             contactManifold.frictionTwistImpulse = 0.0;
+            contactManifold.rollingResistanceImpulse = Vector3::zero();
         }
     }
 }
@@ -604,6 +642,29 @@ void ContactSolver::solve() {
 
                 // Apply the impulses to the bodies of the constraint
                 applyImpulse(impulseFriction2, contactManifold);
+
+                // --------- Rolling resistance constraint --------- //
+
+                if (contactManifold.rollingResistanceFactor > 0) {
+
+                    // Compute J*v
+                    const Vector3 JvRolling = w2 - w1;
+
+                    // Compute the Lagrange multiplier lambda
+                    Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+                    decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
+                    Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
+                    contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
+                                                                         deltaLambdaRolling, rollingLimit);
+                    deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
+
+                    // Compute the impulse P=J^T * lambda
+                    const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling,
+                                                 Vector3::zero(), deltaLambdaRolling);
+
+                    // Apply the impulses to the bodies of the constraint
+                    applyImpulse(impulseRolling, contactManifold);
+                }
             }
         }
 
@@ -688,6 +749,31 @@ void ContactSolver::solve() {
 
             // Apply the impulses to the bodies of the constraint
             applyImpulse(impulseTwistFriction, contactManifold);
+
+            // --------- Rolling resistance constraint at the center of the contact manifold --------- //
+
+            if (contactManifold.rollingResistanceFactor > 0) {
+
+                // Compute J*v
+                const Vector3 JvRolling = w2 - w1;
+
+                // Compute the Lagrange multiplier lambda
+                Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+                decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
+                Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
+                contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
+                                                                     deltaLambdaRolling, rollingLimit);
+                deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
+
+                // Compute the impulse P=J^T * lambda
+                angularImpulseBody1 = -deltaLambdaRolling;
+                angularImpulseBody2 = deltaLambdaRolling;
+                const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
+                                             Vector3::zero(), angularImpulseBody2);
+
+                // Apply the impulses to the bodies of the constraint
+                applyImpulse(impulseRolling, contactManifold);
+            }
         }
     }
 }
@@ -708,6 +794,7 @@ void ContactSolver::storeImpulses() {
             contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse);
             contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse);
             contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse);
+            contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse);
 
             contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
             contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
@@ -716,6 +803,7 @@ void ContactSolver::storeImpulses() {
         manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse);
         manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse);
         manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse);
+        manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse);
         manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
         manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
     }

src/engine/ContactSolver.h

@@ -132,6 +132,9 @@ class ContactSolver {
             /// Accumulated split impulse for penetration correction
             decimal penetrationSplitImpulse;
 
+            /// Accumulated rolling resistance impulse
+            Vector3 rollingResistanceImpulse;
+
             /// Normal vector of the contact
             Vector3 normal;
 
@@ -224,12 +227,21 @@ class ContactSolver {
             /// Number of contact points
             uint nbContacts;
 
+            /// True if the body 1 is of type dynamic
+            bool isBody1DynamicType;
+
+            /// True if the body 2 is of type dynamic
+            bool isBody2DynamicType;
+
             /// Mix of the restitution factor for two bodies
             decimal restitutionFactor;
 
             /// Mix friction coefficient for the two bodies
             decimal frictionCoefficient;
 
+            /// Rolling resistance factor between the two bodies
+            decimal rollingResistanceFactor;
+
             /// Pointer to the external contact manifold
             ContactManifold* externalContactManifold;
 
@@ -271,6 +283,9 @@ class ContactSolver {
             /// Matrix K for the twist friction constraint
             decimal inverseTwistFrictionMass;
 
+            /// Matrix K for the rolling resistance constraint
+            Matrix3x3 inverseRollingResistance;
+
             /// First friction direction at contact manifold center
             Vector3 frictionVector1;
 
@@ -291,6 +306,9 @@ class ContactSolver {
 
             /// Twist friction impulse at contact manifold center
             decimal frictionTwistImpulse;
+
+            /// Rolling resistance impulse
+            Vector3 rollingResistanceImpulse;
         };
 
         // -------------------- Constants --------------------- //
@@ -360,6 +378,9 @@ class ContactSolver {
         decimal computeMixedFrictionCoefficient(RigidBody* body1,
                                                 RigidBody* body2) const;
 
+        /// Compute th mixed rolling resistance factor between two bodies
+        decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
+
         /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
         /// plane for a contact point. The two vectors have to be
         /// such that : t1 x t2 = contactNormal.
@@ -481,6 +502,12 @@ inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
                 body2->getMaterial().getFrictionCoefficient());
 }
 
+// Compute th mixed rolling resistance factor between two bodies
+inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
+                                                            RigidBody* body2) const {
+    return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
+}
+
 // Compute a penetration constraint impulse
 inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
                                                           const ContactPointSolver& contactPoint)

src/engine/Material.cpp

@@ -30,13 +30,16 @@ using namespace reactphysics3d;
 
 // Constructor
 Material::Material()
-         : mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT), mBounciness(DEFAULT_BOUNCINESS) {
+         : mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT),
+           mBounciness(DEFAULT_BOUNCINESS),
+           mRollingResistance(DEFAULT_ROLLING_RESISTANCE) {
 
 }
 
 // Copy-constructor
 Material::Material(const Material& material)
-         : mFrictionCoefficient(material.mFrictionCoefficient), mBounciness(material.mBounciness) {
+         : mFrictionCoefficient(material.mFrictionCoefficient), mBounciness(material.mBounciness),
+           mRollingResistance(material.mRollingResistance) {
 
 }
 

src/engine/Material.h

@@ -47,6 +47,9 @@ class Material {
         /// Friction coefficient (positive value)
         decimal mFrictionCoefficient;
 
+        /// Rolling resistance factor (positive value)
+        decimal mRollingResistance;
+
         /// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body
         decimal mBounciness;
 
@@ -75,6 +78,12 @@ class Material {
         /// Set the friction coefficient.
         void setFrictionCoefficient(decimal frictionCoefficient);
 
+        /// Return the rolling resistance factor
+        decimal getRollingResistance() const;
+
+        /// Set the rolling resistance factor
+        void setRollingResistance(decimal rollingResistance);
+
         /// Overloaded assignment operator
         Material& operator=(const Material& material);
 };
@@ -117,6 +126,27 @@ inline void Material::setFrictionCoefficient(decimal frictionCoefficient) {
     mFrictionCoefficient = frictionCoefficient;
 }
 
+// Return the rolling resistance factor. If this value is larger than zero,
+// it will be used to slow down the body when it is rolling
+// against another body.
+/**
+ * @return The rolling resistance factor (positive value)
+ */
+inline decimal Material::getRollingResistance() const {
+    return mRollingResistance;
+}
+
+// Set the rolling resistance factor. If this value is larger than zero,
+// it will be used to slow down the body when it is rolling
+// against another body.
+/**
+ * @param rollingResistance The rolling resistance factor
+ */
+inline void Material::setRollingResistance(decimal rollingResistance) {
+    assert(rollingResistance >= 0);
+    mRollingResistance = rollingResistance;
+}
+
 // Overloaded assignment operator
 inline Material& Material::operator=(const Material& material) {
 
@@ -124,6 +154,7 @@ inline Material& Material::operator=(const Material& material) {
     if (this != &material) {
         mFrictionCoefficient = material.mFrictionCoefficient;
         mBounciness = material.mBounciness;
+        mRollingResistance = material.mRollingResistance;
     }
 
     // Return this material