mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Change the way to iterate over contacts

Browse Source
This commit is contained in:
Daniel Chappuis 2016-10-16 15:40:38 +02:00
parent 58ae61d6aa
commit d04cee7d0a
3 changed files with 339 additions and 296 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

561
src/engine/ContactSolver.cpp
View File

@ -36,7 +36,7 @@ using namespace std;
// Constants initialization // Constants initialization
const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP= decimal(0.01); const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor // Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex, ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
@ -49,8 +49,54 @@ ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocity
} }
// Initialize the contact constraints
void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
PROFILE("ContactSolver::init()");
mTimeStep = timeStep;
// TODO : Try not to count manifolds and contact points here
uint nbContactManifolds = 0;
uint nbContactPoints = 0;
for (uint i = 0; i < nbIslands; i++) {
uint nbManifoldsInIsland = islands[i]->getNbContactManifolds();
nbContactManifolds += nbManifoldsInIsland;
for (uint j=0; j < nbManifoldsInIsland; j++) {
nbContactPoints += islands[i]->getContactManifolds()[j]->getNbContactPoints();
}
}
mNbContactManifolds = 0;
mNbContactPoints = 0;
mContactConstraints = nullptr;
mContactPoints = nullptr;
if (nbContactManifolds == 0 || nbContactPoints == 0) return;
// TODO : Count exactly the number of constraints to allocate here
mContactPoints = static_cast<ContactPointSolver*>(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints));
assert(mContactPoints != nullptr);
mContactConstraints = static_cast<ContactManifoldSolver*>(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds));
assert(mContactConstraints != nullptr);
// For each island of the world
for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
if (islands[islandIndex]->getNbContactManifolds() > 0) {
initializeForIsland(islands[islandIndex]);
}
}
// Warmstarting
warmStart();
}
// Initialize the constraint solver for a given island // Initialize the constraint solver for a given island
void ContactSolver::initializeForIsland(decimal dt, Island* island) { void ContactSolver::initializeForIsland(Island* island) {
PROFILE("ContactSolver::initializeForIsland()"); PROFILE("ContactSolver::initializeForIsland()");
@ -60,22 +106,12 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
assert(mSplitLinearVelocities != nullptr); assert(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr); assert(mSplitAngularVelocities != nullptr);
// Set the current time step
mTimeStep = dt;
mNbContactManifolds = island->getNbContactManifolds();
mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
assert(mContactConstraints != nullptr);
// For each contact manifold of the island // For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifolds(); ContactManifold** contactManifolds = island->getContactManifolds();
for (uint i=0; i<mNbContactManifolds; i++) { for (uint i=0; i<island->getNbContactManifolds(); i++) {
ContactManifold* externalManifold = contactManifolds[i]; ContactManifold* externalManifold = contactManifolds[i];
ContactManifoldSolver& internalManifold = mContactConstraints[i];
assert(externalManifold->getNbContactPoints() > 0); assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact // Get the two bodies of the contact
@ -90,34 +126,33 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
// Initialize the internal contact manifold structure using the external // Initialize the internal contact manifold structure using the external
// contact manifold // contact manifold
internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
internalManifold.massInverseBody1 = body1->mMassInverse; mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
internalManifold.massInverseBody2 = body2->mMassInverse; mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
internalManifold.nbContacts = externalManifold->getNbContactPoints(); mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints();
internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mContactConstraints[mNbContactManifolds].restitutionFactor = computeMixedRestitutionFactor(body1, body2);
internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
internalManifold.externalContactManifold = externalManifold; mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold;
internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; mContactConstraints[mNbContactManifolds].isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
internalManifold.normal.setToZero(); mContactConstraints[mNbContactManifolds].isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
internalManifold.frictionPointBody1 = Vector3::zero(); mContactConstraints[mNbContactManifolds].normal.setToZero();
internalManifold.frictionPointBody2 = Vector3::zero(); mContactConstraints[mNbContactManifolds].frictionPointBody1 = Vector3::zero();
mContactConstraints[mNbContactManifolds].frictionPointBody2 = Vector3::zero();
// Get the velocities of the bodies // Get the velocities of the bodies
const Vector3& v1 = mLinearVelocities[internalManifold.indexBody1]; const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
const Vector3& w1 = mAngularVelocities[internalManifold.indexBody1]; const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
const Vector3& v2 = mLinearVelocities[internalManifold.indexBody2]; const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
const Vector3& w2 = mAngularVelocities[internalManifold.indexBody2]; const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
// For each contact point of the contact manifold // For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) { for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
ContactPointSolver& contactPoint = internalManifold.contacts[c];
// Get a contact point // Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c); ContactPoint* externalContact = externalManifold->getContactPoint(c);
@ -125,100 +160,104 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p1 = externalContact->getWorldPointOnBody1();
Vector3 p2 = externalContact->getWorldPointOnBody2(); Vector3 p2 = externalContact->getWorldPointOnBody2();
contactPoint.externalContact = externalContact; new (mContactPoints + mNbContactPoints) ContactPointSolver();
contactPoint.normal = externalContact->getNormal(); mContactPoints[mNbContactPoints].externalContact = externalContact;
contactPoint.r1 = p1 - x1; mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
contactPoint.r2 = p2 - x2; mContactPoints[mNbContactPoints].r1 = p1 - x1;
contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); mContactPoints[mNbContactPoints].r2 = p2 - x2;
contactPoint.isRestingContact = externalContact->getIsRestingContact(); mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true); externalContact->setIsRestingContact(true);
contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); mContactPoints[mNbContactPoints].oldFrictionVector1 = externalContact->getFrictionVector1();
contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); mContactPoints[mNbContactPoints].oldFrictionVector2 = externalContact->getFrictionVector2();
contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
contactPoint.penetrationSplitImpulse = 0.0; mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); mContactPoints[mNbContactPoints].friction1Impulse = externalContact->getFrictionImpulse1();
contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); mContactPoints[mNbContactPoints].friction2Impulse = externalContact->getFrictionImpulse2();
contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); mContactPoints[mNbContactPoints].rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
internalManifold.frictionPointBody1 += p1; mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1;
internalManifold.frictionPointBody2 += p2; mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2;
// Compute the velocity difference // Compute the velocity difference
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal); mContactPoints[mNbContactPoints].r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal); mContactPoints[mNbContactPoints].r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
// Compute the inverse mass matrix K for the penetration constraint // Compute the inverse mass matrix K for the penetration constraint
decimal massPenetration = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
((internalManifold.inverseInertiaTensorBody1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactPoints[mNbContactPoints].r1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) +
((internalManifold.inverseInertiaTensorBody2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactPoints[mNbContactPoints].r2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal);
contactPoint.inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0);
// Compute the restitution velocity bias "b". We compute this here instead // Compute the restitution velocity bias "b". We compute this here instead
// of inside the solve() method because we need to use the velocity difference // of inside the solve() method because we need to use the velocity difference
// at the beginning of the contact. Note that if it is a resting contact (normal // at the beginning of the contact. Note that if it is a resting contact (normal
// velocity bellow a given threshold), we do not add a restitution velocity bias // velocity bellow a given threshold), we do not add a restitution velocity bias
contactPoint.restitutionBias = 0.0; mContactPoints[mNbContactPoints].restitutionBias = 0.0;
decimal deltaVDotN = deltaV.dot(contactPoint.normal); decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
contactPoint.restitutionBias = internalManifold.restitutionFactor * deltaVDotN; mContactPoints[mNbContactPoints].restitutionBias = mContactConstraints[mNbContactManifolds].restitutionFactor * deltaVDotN;
} }
internalManifold.normal += contactPoint.normal; mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
mNbContactPoints++;
} }
mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts); mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts); mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1;
internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2;
internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
// Initialize the accumulated impulses with the previous step accumulated impulses // Initialize the accumulated impulses with the previous step accumulated impulses
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1();
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2();
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
// Compute the inverse K matrix for the rolling resistance constraint // Compute the inverse K matrix for the rolling resistance constraint
internalManifold.inverseRollingResistance.setToZero(); mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
if (internalManifold.rollingResistanceFactor > 0 && (internalManifold.isBody1DynamicType || internalManifold.isBody2DynamicType)) { if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (mContactConstraints[mNbContactManifolds].isBody1DynamicType || mContactConstraints[mNbContactManifolds].isBody2DynamicType)) {
internalManifold.inverseRollingResistance = internalManifold.inverseInertiaTensorBody1 + internalManifold.inverseInertiaTensorBody2; mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
internalManifold.inverseRollingResistance = internalManifold.inverseRollingResistance.getInverse(); mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
} }
internalManifold.normal.normalize(); mContactConstraints[mNbContactManifolds].normal.normalize();
Vector3 deltaVFrictionPoint = v2 + w2.cross(internalManifold.r2Friction) - Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
v1 - w1.cross(internalManifold.r1Friction); v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
// Compute the friction vectors // Compute the friction vectors
computeFrictionVectors(deltaVFrictionPoint, internalManifold); computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]);
// Compute the inverse mass matrix K for the friction constraints at the center of // Compute the inverse mass matrix K for the friction constraints at the center of
// the contact manifold // the contact manifold
internalManifold.r1CrossT1 = internalManifold.r1Friction.cross(internalManifold.frictionVector1); mContactConstraints[mNbContactManifolds].r1CrossT1 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
internalManifold.r1CrossT2 = internalManifold.r1Friction.cross(internalManifold.frictionVector2); mContactConstraints[mNbContactManifolds].r1CrossT2 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
internalManifold.r2CrossT1 = internalManifold.r2Friction.cross(internalManifold.frictionVector1); mContactConstraints[mNbContactManifolds].r2CrossT1 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
internalManifold.r2CrossT2 = internalManifold.r2Friction.cross(internalManifold.frictionVector2); mContactConstraints[mNbContactManifolds].r2CrossT2 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
decimal friction1Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + decimal friction1Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT1).cross(internalManifold.r1Friction)).dot( ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT1).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
internalManifold.frictionVector1) + mContactConstraints[mNbContactManifolds].frictionVector1) +
((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT1).cross(internalManifold.r2Friction)).dot( ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT1).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
internalManifold.frictionVector1); mContactConstraints[mNbContactManifolds].frictionVector1);
decimal friction2Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + decimal friction2Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT2).cross(internalManifold.r1Friction)).dot( ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT2).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
internalManifold.frictionVector2) + mContactConstraints[mNbContactManifolds].frictionVector2) +
((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT2).cross(internalManifold.r2Friction)).dot( ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT2).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
internalManifold.frictionVector2); mContactConstraints[mNbContactManifolds].frictionVector2);
decimal frictionTwistMass = internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody1 * decimal frictionTwistMass = mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 *
internalManifold.normal) + mContactConstraints[mNbContactManifolds].normal) +
internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 *
internalManifold.normal); mContactConstraints[mNbContactManifolds].normal);
internalManifold.inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0); mContactConstraints[mNbContactManifolds].inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0);
internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); mContactConstraints[mNbContactManifolds].inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0);
internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); mContactConstraints[mNbContactManifolds].inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0);
mNbContactManifolds++;
} }
} }
@ -228,41 +267,41 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
/// the solution of the linear system /// the solution of the linear system
void ContactSolver::warmStart() { void ContactSolver::warmStart() {
uint contactPointIndex = 0;
// For each constraint // For each constraint
for (uint c=0; c<mNbContactManifolds; c++) { for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
bool atLeastOneRestingContactPoint = false; bool atLeastOneRestingContactPoint = false;
for (uint i=0; i<contactManifold.nbContacts; i++) { for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
// If it is not a new contact (this contact was already existing at last time step) // If it is not a new contact (this contact was already existing at last time step)
if (contactPoint.isRestingContact) { if (mContactPoints[contactPointIndex].isRestingContact) {
atLeastOneRestingContactPoint = true; atLeastOneRestingContactPoint = true;
// --------- Penetration --------- // // --------- Penetration --------- //
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
Vector3 impulsePenetration = contactPoint.normal * contactPoint.penetrationImpulse; Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse;
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-impulsePenetration); mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-impulsePenetration);
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * contactPoint.penetrationImpulse); mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * mContactPoints[contactPointIndex].penetrationImpulse);
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * impulsePenetration; mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * contactPoint.penetrationImpulse); mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * mContactPoints[contactPointIndex].penetrationImpulse);
} }
else { // If it is a new contact point else { // If it is a new contact point
// Initialize the accumulated impulses to zero // Initialize the accumulated impulses to zero
contactPoint.penetrationImpulse = 0.0; mContactPoints[contactPointIndex].penetrationImpulse = 0.0;
contactPoint.friction1Impulse = 0.0; mContactPoints[contactPointIndex].friction1Impulse = 0.0;
contactPoint.friction2Impulse = 0.0; mContactPoints[contactPointIndex].friction2Impulse = 0.0;
contactPoint.rollingResistanceImpulse = Vector3::zero(); mContactPoints[contactPointIndex].rollingResistanceImpulse = Vector3::zero();
} }
contactPointIndex++;
} }
// If we solve the friction constraints at the center of the contact manifold and there is // If we solve the friction constraints at the center of the contact manifold and there is
@ -271,74 +310,74 @@ void ContactSolver::warmStart() {
// Project the old friction impulses (with old friction vectors) into the new friction // Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses // vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 + Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 +
contactManifold.friction2Impulse * contactManifold.oldFrictionVector2; mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2;
contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1); mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1);
contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector2); mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2);
// ------ First friction constraint at the center of the contact manifold ------ // // ------ First friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda // Compute the impulse P = J^T * lambda
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 *
contactManifold.friction1Impulse; mContactConstraints[c].friction1Impulse;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 *
contactManifold.friction1Impulse; mContactConstraints[c].friction1Impulse;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 *
contactManifold.friction1Impulse; mContactConstraints[c].friction1Impulse;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifold ----- // // ------ Second friction constraint at the center of the contact manifold ----- //
// Compute the impulse P = J^T * lambda // Compute the impulse P = J^T * lambda
angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse; angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse;
linearImpulseBody2 = contactManifold.frictionVector2 * contactManifold.friction2Impulse; linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse;
angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse; angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifold ------ // // ------ Twist friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda // Compute the impulse P = J^T * lambda
angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Rolling resistance at the center of the contact manifold ------ // // ------ Rolling resistance at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda // Compute the impulse P = J^T * lambda
angularImpulseBody2 = contactManifold.rollingResistanceImpulse; angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2); mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2);
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
} }
else { // If it is a new contact manifold else { // If it is a new contact manifold
// Initialize the accumulated impulses to zero // Initialize the accumulated impulses to zero
contactManifold.friction1Impulse = 0.0; mContactConstraints[c].friction1Impulse = 0.0;
contactManifold.friction2Impulse = 0.0; mContactConstraints[c].friction2Impulse = 0.0;
contactManifold.frictionTwistImpulse = 0.0; mContactConstraints[c].frictionTwistImpulse = 0.0;
contactManifold.rollingResistanceImpulse = Vector3::zero(); mContactConstraints[c].rollingResistanceImpulse.setToZero();
} }
} }
} }
@ -350,196 +389,195 @@ void ContactSolver::solve() {
decimal deltaLambda; decimal deltaLambda;
decimal lambdaTemp; decimal lambdaTemp;
uint contactPointIndex = 0;
// For each contact manifold // For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) { for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
decimal sumPenetrationImpulse = 0.0; decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities // Get the constrained velocities
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1]; const Vector3& v1 = mLinearVelocities[mContactConstraints[c].indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1]; const Vector3& w1 = mAngularVelocities[mContactConstraints[c].indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2]; const Vector3& v2 = mLinearVelocities[mContactConstraints[c].indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2]; const Vector3& w2 = mAngularVelocities[mContactConstraints[c].indexBody2];
for (uint i=0; i<contactManifold.nbContacts; i++) { for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
// --------- Penetration --------- // // --------- Penetration --------- //
// Compute J*v // Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal); decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal);
decimal Jv = deltaVDotN; decimal Jv = deltaVDotN;
// Compute the bias "b" of the constraint // Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA; decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0; decimal biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP)); max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias; decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) { if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) * deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) *
contactPoint.inversePenetrationMass; mContactPoints[contactPointIndex].inversePenetrationMass;
} }
else { else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass;
} }
lambdaTemp = contactPoint.penetrationImpulse; lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse +
deltaLambda, decimal(0.0)); deltaLambda, decimal(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp;
Vector3 linearImpulse = contactPoint.normal * deltaLambda; Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * deltaLambda); mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * deltaLambda);
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * deltaLambda); mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * deltaLambda);
sumPenetrationImpulse += contactPoint.penetrationImpulse; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
// If the split impulse position correction is active // If the split impulse position correction is active
if (mIsSplitImpulseActive) { if (mIsSplitImpulseActive) {
// Split impulse (position correction) // Split impulse (position correction)
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; const Vector3& v1Split = mSplitLinearVelocities[mContactConstraints[c].indexBody1];
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1];
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2];
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2];
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) -
v1Split - w1Split.cross(contactPoint.r1); v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
decimal JvSplit = deltaVSplit.dot(contactPoint.normal); decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
contactPoint.inversePenetrationMass; mContactPoints[contactPointIndex].inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max( mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse + mContactPoints[contactPointIndex].penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0)); deltaLambdaSplit, decimal(0.0));
deltaLambdaSplit = contactPoint.penetrationSplitImpulse - lambdaTempSplit; deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit;
Vector3 linearImpulse = contactPoint.normal * deltaLambdaSplit; Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); mSplitLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * mSplitAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 *
(-contactPoint.r1CrossN * deltaLambdaSplit); (-mContactPoints[contactPointIndex].r1CrossN * deltaLambdaSplit);
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 *
contactPoint.r2CrossN * deltaLambdaSplit; mContactPoints[contactPointIndex].r2CrossN * deltaLambdaSplit;
} }
contactPointIndex++;
} }
// ------ First friction constraint at the center of the contact manifol ------ // // ------ First friction constraint at the center of the contact manifol ------ //
// Compute J*v // Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction)
- v1 - w1.cross(contactManifold.r1Friction); - v1 - w1.cross(mContactConstraints[c].r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1); decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse; lambdaTemp = mContactConstraints[c].friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit, mContactConstraints[c].friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse + std::min(mContactConstraints[c].friction1Impulse +
deltaLambda, frictionLimit)); deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp; deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifol ----- // // ------ Second friction constraint at the center of the contact manifol ----- //
// Compute J*v // Compute J*v
deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2); Jv = deltaV.dot(mContactConstraints[c].frictionVector2);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
deltaLambda = -Jv * contactManifold.inverseFriction2Mass; deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction2Impulse; lambdaTemp = mContactConstraints[c].friction2Impulse;
contactManifold.friction2Impulse = std::max(-frictionLimit, mContactConstraints[c].friction2Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction2Impulse + std::min(mContactConstraints[c].friction2Impulse +
deltaLambda, frictionLimit)); deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction2Impulse - lambdaTemp; deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifol ------ // // ------ Twist friction constraint at the center of the contact manifol ------ //
// Compute J*v // Compute J*v
deltaV = w2 - w1; deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal); Jv = deltaV.dot(mContactConstraints[c].normal);
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.frictionTwistImpulse; lambdaTemp = mContactConstraints[c].frictionTwistImpulse;
contactManifold.frictionTwistImpulse = std::max(-frictionLimit, mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit,
std::min(contactManifold.frictionTwistImpulse std::min(mContactConstraints[c].frictionTwistImpulse
+ deltaLambda, frictionLimit)); + deltaLambda, frictionLimit));
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
angularImpulseBody2 = contactManifold.normal * deltaLambda; angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2); mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2);
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// --------- Rolling resistance constraint at the center of the contact manifold --------- // // --------- Rolling resistance constraint at the center of the contact manifold --------- //
if (contactManifold.rollingResistanceFactor > 0) { if (mContactConstraints[c].rollingResistanceFactor > 0) {
// Compute J*v // Compute J*v
const Vector3 JvRolling = w2 - w1; const Vector3 JvRolling = w2 - w1;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling);
decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse;
Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse;
contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit); deltaLambdaRolling, rollingLimit);
deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-deltaLambdaRolling); mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-deltaLambdaRolling);
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * deltaLambdaRolling; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
} }
} }
} }
@ -548,30 +586,30 @@ void ContactSolver::solve() {
// warm start the solver at the next iteration // warm start the solver at the next iteration
void ContactSolver::storeImpulses() { void ContactSolver::storeImpulses() {
uint contactPointIndex = 0;
// For each contact manifold // For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) { for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c]; for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
for (uint i=0; i<manifold.nbContacts; i++) { mContactPoints[contactPointIndex].externalContact->setPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse);
mContactPoints[contactPointIndex].externalContact->setFrictionImpulse1(mContactPoints[contactPointIndex].friction1Impulse);
mContactPoints[contactPointIndex].externalContact->setFrictionImpulse2(mContactPoints[contactPointIndex].friction2Impulse);
mContactPoints[contactPointIndex].externalContact->setRollingResistanceImpulse(mContactPoints[contactPointIndex].rollingResistanceImpulse);
ContactPointSolver& contactPoint = manifold.contacts[i]; mContactPoints[contactPointIndex].externalContact->setFrictionVector1(mContactPoints[contactPointIndex].frictionVector1);
mContactPoints[contactPointIndex].externalContact->setFrictionVector2(mContactPoints[contactPointIndex].frictionVector2);
contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse); contactPointIndex++;
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);
} }
manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); mContactConstraints[c].externalContactManifold->setFrictionImpulse1(mContactConstraints[c].friction1Impulse);
manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); mContactConstraints[c].externalContactManifold->setFrictionImpulse2(mContactConstraints[c].friction2Impulse);
manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); mContactConstraints[c].externalContactManifold->setFrictionTwistImpulse(mContactConstraints[c].frictionTwistImpulse);
manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); mContactConstraints[c].externalContactManifold->setRollingResistanceImpulse(mContactConstraints[c].rollingResistanceImpulse);
manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); mContactConstraints[c].externalContactManifold->setFrictionVector1(mContactConstraints[c].frictionVector1);
manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); mContactConstraints[c].externalContactManifold->setFrictionVector2(mContactConstraints[c].frictionVector2);
} }
} }
@ -634,12 +672,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// friction vector and the contact normal // friction vector and the contact normal
contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
} }
// Clean up the constraint solver
void ContactSolver::cleanup() {
if (mContactConstraints != nullptr) {
delete[] mContactConstraints;
mContactConstraints = nullptr;
}
}

25
src/engine/ContactSolver.h
View File

@ -220,11 +220,8 @@ class ContactSolver {
/// Inverse inertia tensor of body 2 /// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2; Matrix3x3 inverseInertiaTensorBody2;
/// Contact point constraints
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Number of contact points /// Number of contact points
uint nbContacts; short int nbContacts;
/// True if the body 1 is of type dynamic /// True if the body 1 is of type dynamic
bool isBody1DynamicType; bool isBody1DynamicType;
@ -335,6 +332,12 @@ class ContactSolver {
/// Contact constraints /// Contact constraints
ContactManifoldSolver* mContactConstraints; ContactManifoldSolver* mContactConstraints;
/// Contact points
ContactPointSolver* mContactPoints;
/// Number of contact point constraints
uint mNbContactPoints;
/// Number of contact constraints /// Number of contact constraints
uint mNbContactManifolds; uint mNbContactManifolds;
@ -378,6 +381,9 @@ class ContactSolver {
void computeFrictionVectors(const Vector3& deltaVelocity, void computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contactPoint) const; ContactManifoldSolver& contactPoint) const;
/// Warm start the solver.
void warmStart();
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -389,8 +395,11 @@ class ContactSolver {
/// Destructor /// Destructor
~ContactSolver() = default; ~ContactSolver() = default;
/// Initialize the contact constraints
void init(Island** islands, uint nbIslands, decimal timeStep);
/// Initialize the constraint solver for a given island /// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island); void initializeForIsland(Island* island);
/// Set the split velocities arrays /// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@ -400,9 +409,6 @@ class ContactSolver {
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities); Vector3* constrainedAngularVelocities);
/// Warm start the solver.
void warmStart();
/// Store the computed impulses to use them to /// Store the computed impulses to use them to
/// warm start the solver at the next iteration /// warm start the solver at the next iteration
void storeImpulses(); void storeImpulses();
@ -415,9 +421,6 @@ class ContactSolver {
/// Activate or Deactivate the split impulses for contacts /// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive); void setIsSplitImpulseActive(bool isActive);
/// Clean up the constraint solver
void cleanup();
}; };
// Set the split velocities arrays // Set the split velocities arrays

41
src/engine/DynamicsWorld.cpp
View File

@ -342,23 +342,28 @@ void DynamicsWorld::solveContactsAndConstraints() {
// ---------- Solve velocity constraints for joints and contacts ---------- // // ---------- Solve velocity constraints for joints and contacts ---------- //
// Initialize the contact solver
mContactSolver.init(mIslands, mNbIslands, mTimeStep);
// For each island of the world // For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve // Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
if (!isConstraintsToSolve && !isContactsToSolve) continue; //if (!isConstraintsToSolve && !isContactsToSolve) continue;
// If there are contacts in the current island // If there are contacts in the current island
if (isContactsToSolve) { // if (isContactsToSolve) {
// Initialize the solver // // Initialize the solver
mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); // mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
// Warm start the contact solver // // Warm start the contact solver
mContactSolver.warmStart(); // if (mContactSolver.IsWarmStartingActive()) {
} // mContactSolver.warmStart();
// }
// }
// If there are constraints // If there are constraints
if (isConstraintsToSolve) { if (isConstraintsToSolve) {
@ -366,26 +371,32 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Initialize the constraint solver // Initialize the constraint solver
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
} }
}
// For each iteration of the velocity solver // For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) { for (uint i=0; i<mNbVelocitySolverIterations; i++) {
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Solve the constraints // Solve the constraints
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
if (isConstraintsToSolve) { if (isConstraintsToSolve) {
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]); mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
} }
}
mContactSolver.solve();
// Solve the contacts // Solve the contacts
if (isContactsToSolve) mContactSolver.solve(); // if (isContactsToSolve) {
// mContactSolver.resetTotalPenetrationImpulse();
// mContactSolver.solvePenetrationConstraints();
// mContactSolver.solveFrictionConstraints();
// }
} }
// Cache the lambda values in order to use them in the next
// step and cleanup the contact solver
if (isContactsToSolve) {
mContactSolver.storeImpulses(); mContactSolver.storeImpulses();
mContactSolver.cleanup();
}
}
} }
// Solve the position error correction of the constraints // Solve the position error correction of the constraints