Add support for rolling resistance in the contact solver
This commit is contained in:
Daniel Chappuis
parent
10549796c8
commit
3e98ab2282
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user