diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 0c16f0b0..84477450 100644 --- a/src/collision/ContactManifold.h +++ b/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); diff --git a/src/configuration.h b/src/configuration.h index 88bc83e5..18c3dcc9 100644 --- a/src/configuration.h +++ b/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; diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index afa8e37b..3666d86f 100644 --- a/src/constraint/ContactPoint.h +++ b/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; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 18036138..2967d6d8 100644 --- a/src/engine/ContactSolver.cpp +++ b/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.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); + internalManifold.frictionPointBody1 = Vector3::zero(); + 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); } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 9c4255f1..4300106b 100644 --- a/src/engine/ContactSolver.h +++ b/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) diff --git a/src/engine/Material.cpp b/src/engine/Material.cpp index 1eff58f8..a569fccc 100644 --- a/src/engine/Material.cpp +++ b/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) { } diff --git a/src/engine/Material.h b/src/engine/Material.h index 1db6729c..8fd9d421 100644 --- a/src/engine/Material.h +++ b/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