Change the way to iterate over contacts
This commit is contained in:
Daniel Chappuis
parent
58ae61d6aa
commit
d04cee7d0a
|
|
@ -36,7 +36,7 @@ using namespace std;
|
|||
// Constants initialization
|
||||
const decimal ContactSolver::BETA = 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
|
||||
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
|
||||
void ContactSolver::initializeForIsland(decimal dt, Island* island) {
|
||||
void ContactSolver::initializeForIsland(Island* island) {
|
||||
|
||||
PROFILE("ContactSolver::initializeForIsland()");
|
||||
|
||||
|
|
@ -60,22 +106,12 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
|
|||
assert(mSplitLinearVelocities != 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
|
||||
ContactManifold** contactManifolds = island->getContactManifolds();
|
||||
for (uint i=0; i<mNbContactManifolds; i++) {
|
||||
for (uint i=0; i<island->getNbContactManifolds(); i++) {
|
||||
|
||||
ContactManifold* externalManifold = contactManifolds[i];
|
||||
|
||||
ContactManifoldSolver& internalManifold = mContactConstraints[i];
|
||||
|
||||
assert(externalManifold->getNbContactPoints() > 0);
|
||||
|
||||
// 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
|
||||
// contact manifold
|
||||
internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
|
||||
internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
|
||||
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
|
||||
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
|
||||
internalManifold.massInverseBody1 = body1->mMassInverse;
|
||||
internalManifold.massInverseBody2 = body2->mMassInverse;
|
||||
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() == BodyType::DYNAMIC;
|
||||
internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
|
||||
internalManifold.normal.setToZero();
|
||||
internalManifold.frictionPointBody1 = Vector3::zero();
|
||||
internalManifold.frictionPointBody2 = Vector3::zero();
|
||||
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
|
||||
mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
|
||||
mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
|
||||
mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
|
||||
mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
|
||||
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints();
|
||||
mContactConstraints[mNbContactManifolds].restitutionFactor = computeMixedRestitutionFactor(body1, body2);
|
||||
mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
|
||||
mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
|
||||
mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold;
|
||||
mContactConstraints[mNbContactManifolds].isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
|
||||
mContactConstraints[mNbContactManifolds].isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
|
||||
mContactConstraints[mNbContactManifolds].normal.setToZero();
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1 = Vector3::zero();
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2 = Vector3::zero();
|
||||
|
||||
// Get the velocities of the bodies
|
||||
const Vector3& v1 = mLinearVelocities[internalManifold.indexBody1];
|
||||
const Vector3& w1 = mAngularVelocities[internalManifold.indexBody1];
|
||||
const Vector3& v2 = mLinearVelocities[internalManifold.indexBody2];
|
||||
const Vector3& w2 = mAngularVelocities[internalManifold.indexBody2];
|
||||
const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
|
||||
const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
|
||||
const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
|
||||
const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
|
||||
|
||||
// For each contact point of the contact manifold
|
||||
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
|
||||
|
||||
ContactPointSolver& contactPoint = internalManifold.contacts[c];
|
||||
|
||||
// Get a contact point
|
||||
ContactPoint* externalContact = externalManifold->getContactPoint(c);
|
||||
|
||||
|
|
@ -125,100 +160,104 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
|
|||
Vector3 p1 = externalContact->getWorldPointOnBody1();
|
||||
Vector3 p2 = externalContact->getWorldPointOnBody2();
|
||||
|
||||
contactPoint.externalContact = externalContact;
|
||||
contactPoint.normal = externalContact->getNormal();
|
||||
contactPoint.r1 = p1 - x1;
|
||||
contactPoint.r2 = p2 - x2;
|
||||
contactPoint.penetrationDepth = externalContact->getPenetrationDepth();
|
||||
contactPoint.isRestingContact = externalContact->getIsRestingContact();
|
||||
new (mContactPoints + mNbContactPoints) ContactPointSolver();
|
||||
mContactPoints[mNbContactPoints].externalContact = externalContact;
|
||||
mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
|
||||
mContactPoints[mNbContactPoints].r1 = p1 - x1;
|
||||
mContactPoints[mNbContactPoints].r2 = p2 - x2;
|
||||
mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
|
||||
mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
|
||||
externalContact->setIsRestingContact(true);
|
||||
contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
|
||||
contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
|
||||
contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
|
||||
contactPoint.penetrationSplitImpulse = 0.0;
|
||||
contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
|
||||
contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
|
||||
contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
|
||||
mContactPoints[mNbContactPoints].oldFrictionVector1 = externalContact->getFrictionVector1();
|
||||
mContactPoints[mNbContactPoints].oldFrictionVector2 = externalContact->getFrictionVector2();
|
||||
mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
|
||||
mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
|
||||
mContactPoints[mNbContactPoints].friction1Impulse = externalContact->getFrictionImpulse1();
|
||||
mContactPoints[mNbContactPoints].friction2Impulse = externalContact->getFrictionImpulse2();
|
||||
mContactPoints[mNbContactPoints].rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
|
||||
|
||||
internalManifold.frictionPointBody1 += p1;
|
||||
internalManifold.frictionPointBody2 += p2;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2;
|
||||
|
||||
// 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);
|
||||
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
|
||||
mContactPoints[mNbContactPoints].r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
|
||||
mContactPoints[mNbContactPoints].r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
|
||||
|
||||
// Compute the inverse mass matrix K for the penetration constraint
|
||||
decimal massPenetration = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
|
||||
((internalManifold.inverseInertiaTensorBody1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
|
||||
((internalManifold.inverseInertiaTensorBody2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
|
||||
contactPoint.inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0);
|
||||
decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
|
||||
((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactPoints[mNbContactPoints].r1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) +
|
||||
((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactPoints[mNbContactPoints].r2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal);
|
||||
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
|
||||
// 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
|
||||
// velocity bellow a given threshold), we do not add a restitution velocity bias
|
||||
contactPoint.restitutionBias = 0.0;
|
||||
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
|
||||
mContactPoints[mNbContactPoints].restitutionBias = 0.0;
|
||||
decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
|
||||
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++;
|
||||
}
|
||||
|
||||
|
||||
internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
|
||||
internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
|
||||
internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
|
||||
internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
|
||||
internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
|
||||
internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
|
||||
mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1;
|
||||
mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2;
|
||||
mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
|
||||
mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
|
||||
|
||||
// Initialize the accumulated impulses with the previous step accumulated impulses
|
||||
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
|
||||
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
|
||||
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
|
||||
mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1();
|
||||
mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2();
|
||||
mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
|
||||
|
||||
// Compute the inverse K matrix for the rolling resistance constraint
|
||||
internalManifold.inverseRollingResistance.setToZero();
|
||||
if (internalManifold.rollingResistanceFactor > 0 && (internalManifold.isBody1DynamicType || internalManifold.isBody2DynamicType)) {
|
||||
internalManifold.inverseRollingResistance = internalManifold.inverseInertiaTensorBody1 + internalManifold.inverseInertiaTensorBody2;
|
||||
internalManifold.inverseRollingResistance = internalManifold.inverseRollingResistance.getInverse();
|
||||
mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
|
||||
if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (mContactConstraints[mNbContactManifolds].isBody1DynamicType || mContactConstraints[mNbContactManifolds].isBody2DynamicType)) {
|
||||
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
|
||||
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
|
||||
}
|
||||
|
||||
internalManifold.normal.normalize();
|
||||
mContactConstraints[mNbContactManifolds].normal.normalize();
|
||||
|
||||
Vector3 deltaVFrictionPoint = v2 + w2.cross(internalManifold.r2Friction) -
|
||||
v1 - w1.cross(internalManifold.r1Friction);
|
||||
Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
|
||||
v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
|
||||
|
||||
// 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
|
||||
// the contact manifold
|
||||
internalManifold.r1CrossT1 = internalManifold.r1Friction.cross(internalManifold.frictionVector1);
|
||||
internalManifold.r1CrossT2 = internalManifold.r1Friction.cross(internalManifold.frictionVector2);
|
||||
internalManifold.r2CrossT1 = internalManifold.r2Friction.cross(internalManifold.frictionVector1);
|
||||
internalManifold.r2CrossT2 = internalManifold.r2Friction.cross(internalManifold.frictionVector2);
|
||||
decimal friction1Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
|
||||
((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT1).cross(internalManifold.r1Friction)).dot(
|
||||
internalManifold.frictionVector1) +
|
||||
((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT1).cross(internalManifold.r2Friction)).dot(
|
||||
internalManifold.frictionVector1);
|
||||
decimal friction2Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
|
||||
((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT2).cross(internalManifold.r1Friction)).dot(
|
||||
internalManifold.frictionVector2) +
|
||||
((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT2).cross(internalManifold.r2Friction)).dot(
|
||||
internalManifold.frictionVector2);
|
||||
decimal frictionTwistMass = internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody1 *
|
||||
internalManifold.normal) +
|
||||
internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody2 *
|
||||
internalManifold.normal);
|
||||
internalManifold.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);
|
||||
internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0);
|
||||
mContactConstraints[mNbContactManifolds].r1CrossT1 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
|
||||
mContactConstraints[mNbContactManifolds].r1CrossT2 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
|
||||
mContactConstraints[mNbContactManifolds].r2CrossT1 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
|
||||
mContactConstraints[mNbContactManifolds].r2CrossT2 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
|
||||
decimal friction1Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
|
||||
((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT1).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
|
||||
mContactConstraints[mNbContactManifolds].frictionVector1) +
|
||||
((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT1).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
|
||||
mContactConstraints[mNbContactManifolds].frictionVector1);
|
||||
decimal friction2Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
|
||||
((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT2).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
|
||||
mContactConstraints[mNbContactManifolds].frictionVector2) +
|
||||
((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT2).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
|
||||
mContactConstraints[mNbContactManifolds].frictionVector2);
|
||||
decimal frictionTwistMass = mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 *
|
||||
mContactConstraints[mNbContactManifolds].normal) +
|
||||
mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 *
|
||||
mContactConstraints[mNbContactManifolds].normal);
|
||||
mContactConstraints[mNbContactManifolds].inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0);
|
||||
mContactConstraints[mNbContactManifolds].inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : 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
|
||||
void ContactSolver::warmStart() {
|
||||
|
||||
uint contactPointIndex = 0;
|
||||
|
||||
// For each constraint
|
||||
for (uint c=0; c<mNbContactManifolds; c++) {
|
||||
|
||||
ContactManifoldSolver& contactManifold = mContactConstraints[c];
|
||||
|
||||
bool atLeastOneRestingContactPoint = false;
|
||||
|
||||
for (uint i=0; i<contactManifold.nbContacts; i++) {
|
||||
|
||||
ContactPointSolver& contactPoint = contactManifold.contacts[i];
|
||||
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
|
||||
|
||||
// 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;
|
||||
|
||||
// --------- Penetration --------- //
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
Vector3 impulsePenetration = contactPoint.normal * contactPoint.penetrationImpulse;
|
||||
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-impulsePenetration);
|
||||
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * contactPoint.penetrationImpulse);
|
||||
Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-impulsePenetration);
|
||||
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
|
||||
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * impulsePenetration;
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * contactPoint.penetrationImpulse);
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * mContactPoints[contactPointIndex].penetrationImpulse);
|
||||
}
|
||||
else { // If it is a new contact point
|
||||
|
||||
// Initialize the accumulated impulses to zero
|
||||
contactPoint.penetrationImpulse = 0.0;
|
||||
contactPoint.friction1Impulse = 0.0;
|
||||
contactPoint.friction2Impulse = 0.0;
|
||||
contactPoint.rollingResistanceImpulse = Vector3::zero();
|
||||
mContactPoints[contactPointIndex].penetrationImpulse = 0.0;
|
||||
mContactPoints[contactPointIndex].friction1Impulse = 0.0;
|
||||
mContactPoints[contactPointIndex].friction2Impulse = 0.0;
|
||||
mContactPoints[contactPointIndex].rollingResistanceImpulse = Vector3::zero();
|
||||
}
|
||||
|
||||
contactPointIndex++;
|
||||
}
|
||||
|
||||
// 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
|
||||
// vectors to get the new friction impulses
|
||||
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 +
|
||||
contactManifold.friction2Impulse * contactManifold.oldFrictionVector2;
|
||||
contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
|
||||
contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector2);
|
||||
Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 +
|
||||
mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2;
|
||||
mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1);
|
||||
mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2);
|
||||
|
||||
// ------ First friction constraint at the center of the contact manifold ------ //
|
||||
|
||||
// Compute the impulse P = J^T * lambda
|
||||
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
|
||||
contactManifold.friction1Impulse;
|
||||
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
|
||||
contactManifold.friction1Impulse;
|
||||
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
|
||||
contactManifold.friction1Impulse;
|
||||
Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 *
|
||||
mContactConstraints[c].friction1Impulse;
|
||||
Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 *
|
||||
mContactConstraints[c].friction1Impulse;
|
||||
Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 *
|
||||
mContactConstraints[c].friction1Impulse;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Second friction constraint at the center of the contact manifold ----- //
|
||||
|
||||
// Compute the impulse P = J^T * lambda
|
||||
angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse;
|
||||
linearImpulseBody2 = contactManifold.frictionVector2 * contactManifold.friction2Impulse;
|
||||
angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse;
|
||||
angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse;
|
||||
linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Twist friction constraint at the center of the contact manifold ------ //
|
||||
|
||||
// Compute the impulse P = J^T * lambda
|
||||
angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
|
||||
angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
|
||||
angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
|
||||
angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
|
||||
|
||||
// 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
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Rolling resistance at the center of the contact manifold ------ //
|
||||
|
||||
// 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
|
||||
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
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
}
|
||||
else { // If it is a new contact manifold
|
||||
|
||||
// Initialize the accumulated impulses to zero
|
||||
contactManifold.friction1Impulse = 0.0;
|
||||
contactManifold.friction2Impulse = 0.0;
|
||||
contactManifold.frictionTwistImpulse = 0.0;
|
||||
contactManifold.rollingResistanceImpulse = Vector3::zero();
|
||||
mContactConstraints[c].friction1Impulse = 0.0;
|
||||
mContactConstraints[c].friction2Impulse = 0.0;
|
||||
mContactConstraints[c].frictionTwistImpulse = 0.0;
|
||||
mContactConstraints[c].rollingResistanceImpulse.setToZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -350,196 +389,195 @@ void ContactSolver::solve() {
|
|||
|
||||
decimal deltaLambda;
|
||||
decimal lambdaTemp;
|
||||
uint contactPointIndex = 0;
|
||||
|
||||
// For each contact manifold
|
||||
for (uint c=0; c<mNbContactManifolds; c++) {
|
||||
|
||||
ContactManifoldSolver& contactManifold = mContactConstraints[c];
|
||||
|
||||
decimal sumPenetrationImpulse = 0.0;
|
||||
|
||||
// Get the constrained velocities
|
||||
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
|
||||
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
|
||||
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
|
||||
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
|
||||
const Vector3& v1 = mLinearVelocities[mContactConstraints[c].indexBody1];
|
||||
const Vector3& w1 = mAngularVelocities[mContactConstraints[c].indexBody1];
|
||||
const Vector3& v2 = mLinearVelocities[mContactConstraints[c].indexBody2];
|
||||
const Vector3& w2 = mAngularVelocities[mContactConstraints[c].indexBody2];
|
||||
|
||||
for (uint i=0; i<contactManifold.nbContacts; i++) {
|
||||
|
||||
ContactPointSolver& contactPoint = contactManifold.contacts[i];
|
||||
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
|
||||
|
||||
// --------- Penetration --------- //
|
||||
|
||||
// Compute J*v
|
||||
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
||||
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
|
||||
Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
|
||||
decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal);
|
||||
decimal Jv = deltaVDotN;
|
||||
|
||||
// Compute the bias "b" of the constraint
|
||||
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
|
||||
decimal biasPenetrationDepth = 0.0;
|
||||
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
|
||||
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
|
||||
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
|
||||
if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
|
||||
max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP));
|
||||
decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias;
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
if (mIsSplitImpulseActive) {
|
||||
deltaLambda = - (Jv + contactPoint.restitutionBias) *
|
||||
contactPoint.inversePenetrationMass;
|
||||
deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) *
|
||||
mContactPoints[contactPointIndex].inversePenetrationMass;
|
||||
}
|
||||
else {
|
||||
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
|
||||
deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass;
|
||||
}
|
||||
lambdaTemp = contactPoint.penetrationImpulse;
|
||||
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
|
||||
lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse +
|
||||
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
|
||||
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse);
|
||||
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * deltaLambda);
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * deltaLambda);
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse;
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * deltaLambda);
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
|
||||
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 (mIsSplitImpulseActive) {
|
||||
|
||||
// Split impulse (position correction)
|
||||
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
|
||||
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
|
||||
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
|
||||
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
|
||||
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
|
||||
v1Split - w1Split.cross(contactPoint.r1);
|
||||
decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
|
||||
const Vector3& v1Split = mSplitLinearVelocities[mContactConstraints[c].indexBody1];
|
||||
const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1];
|
||||
const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2];
|
||||
const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2];
|
||||
Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) -
|
||||
v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
|
||||
decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal);
|
||||
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
|
||||
contactPoint.inversePenetrationMass;
|
||||
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
|
||||
contactPoint.penetrationSplitImpulse = std::max(
|
||||
contactPoint.penetrationSplitImpulse +
|
||||
mContactPoints[contactPointIndex].inversePenetrationMass;
|
||||
decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse;
|
||||
mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max(
|
||||
mContactPoints[contactPointIndex].penetrationSplitImpulse +
|
||||
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
|
||||
mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse);
|
||||
mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
|
||||
(-contactPoint.r1CrossN * deltaLambdaSplit);
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 *
|
||||
(-mContactPoints[contactPointIndex].r1CrossN * deltaLambdaSplit);
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse;
|
||||
mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
|
||||
contactPoint.r2CrossN * deltaLambdaSplit;
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 *
|
||||
mContactPoints[contactPointIndex].r2CrossN * deltaLambdaSplit;
|
||||
}
|
||||
|
||||
contactPointIndex++;
|
||||
}
|
||||
|
||||
// ------ First friction constraint at the center of the contact manifol ------ //
|
||||
|
||||
// Compute J*v
|
||||
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
|
||||
- v1 - w1.cross(contactManifold.r1Friction);
|
||||
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
|
||||
Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction)
|
||||
- v1 - w1.cross(mContactConstraints[c].r1Friction);
|
||||
decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
|
||||
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = contactManifold.friction1Impulse;
|
||||
contactManifold.friction1Impulse = std::max(-frictionLimit,
|
||||
std::min(contactManifold.friction1Impulse +
|
||||
decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
|
||||
decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = mContactConstraints[c].friction1Impulse;
|
||||
mContactConstraints[c].friction1Impulse = std::max(-frictionLimit,
|
||||
std::min(mContactConstraints[c].friction1Impulse +
|
||||
deltaLambda, frictionLimit));
|
||||
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
|
||||
deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
|
||||
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
|
||||
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
|
||||
Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda;
|
||||
Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda;
|
||||
Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Second friction constraint at the center of the contact manifol ----- //
|
||||
|
||||
// Compute J*v
|
||||
deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction);
|
||||
Jv = deltaV.dot(contactManifold.frictionVector2);
|
||||
deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
|
||||
Jv = deltaV.dot(mContactConstraints[c].frictionVector2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
|
||||
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = contactManifold.friction2Impulse;
|
||||
contactManifold.friction2Impulse = std::max(-frictionLimit,
|
||||
std::min(contactManifold.friction2Impulse +
|
||||
deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
|
||||
frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = mContactConstraints[c].friction2Impulse;
|
||||
mContactConstraints[c].friction2Impulse = std::max(-frictionLimit,
|
||||
std::min(mContactConstraints[c].friction2Impulse +
|
||||
deltaLambda, frictionLimit));
|
||||
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
|
||||
deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
|
||||
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
|
||||
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
|
||||
angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda;
|
||||
linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda;
|
||||
angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Twist friction constraint at the center of the contact manifol ------ //
|
||||
|
||||
// Compute J*v
|
||||
deltaV = w2 - w1;
|
||||
Jv = deltaV.dot(contactManifold.normal);
|
||||
Jv = deltaV.dot(mContactConstraints[c].normal);
|
||||
|
||||
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
|
||||
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = contactManifold.frictionTwistImpulse;
|
||||
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
|
||||
std::min(contactManifold.frictionTwistImpulse
|
||||
deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
|
||||
frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = mContactConstraints[c].frictionTwistImpulse;
|
||||
mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit,
|
||||
std::min(mContactConstraints[c].frictionTwistImpulse
|
||||
+ deltaLambda, frictionLimit));
|
||||
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
|
||||
deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
|
||||
|
||||
// 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
|
||||
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
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// --------- Rolling resistance constraint at the center of the contact manifold --------- //
|
||||
|
||||
if (contactManifold.rollingResistanceFactor > 0) {
|
||||
if (mContactConstraints[c].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 +
|
||||
Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling);
|
||||
decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse;
|
||||
Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse;
|
||||
mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse +
|
||||
deltaLambdaRolling, rollingLimit);
|
||||
deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
|
||||
deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
|
||||
|
||||
// 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
|
||||
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
|
||||
void ContactSolver::storeImpulses() {
|
||||
|
||||
uint contactPointIndex = 0;
|
||||
|
||||
// For each contact manifold
|
||||
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);
|
||||
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);
|
||||
contactPointIndex++;
|
||||
}
|
||||
|
||||
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);
|
||||
mContactConstraints[c].externalContactManifold->setFrictionImpulse1(mContactConstraints[c].friction1Impulse);
|
||||
mContactConstraints[c].externalContactManifold->setFrictionImpulse2(mContactConstraints[c].friction2Impulse);
|
||||
mContactConstraints[c].externalContactManifold->setFrictionTwistImpulse(mContactConstraints[c].frictionTwistImpulse);
|
||||
mContactConstraints[c].externalContactManifold->setRollingResistanceImpulse(mContactConstraints[c].rollingResistanceImpulse);
|
||||
mContactConstraints[c].externalContactManifold->setFrictionVector1(mContactConstraints[c].frictionVector1);
|
||||
mContactConstraints[c].externalContactManifold->setFrictionVector2(mContactConstraints[c].frictionVector2);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -634,12 +672,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
|||
// friction vector and the contact normal
|
||||
contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
|
||||
}
|
||||
|
||||
// Clean up the constraint solver
|
||||
void ContactSolver::cleanup() {
|
||||
|
||||
if (mContactConstraints != nullptr) {
|
||||
delete[] mContactConstraints;
|
||||
mContactConstraints = nullptr;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -220,11 +220,8 @@ class ContactSolver {
|
|||
/// Inverse inertia tensor of body 2
|
||||
Matrix3x3 inverseInertiaTensorBody2;
|
||||
|
||||
/// Contact point constraints
|
||||
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
|
||||
|
||||
/// Number of contact points
|
||||
uint nbContacts;
|
||||
short int nbContacts;
|
||||
|
||||
/// True if the body 1 is of type dynamic
|
||||
bool isBody1DynamicType;
|
||||
|
|
@ -335,6 +332,12 @@ class ContactSolver {
|
|||
/// Contact constraints
|
||||
ContactManifoldSolver* mContactConstraints;
|
||||
|
||||
/// Contact points
|
||||
ContactPointSolver* mContactPoints;
|
||||
|
||||
/// Number of contact point constraints
|
||||
uint mNbContactPoints;
|
||||
|
||||
/// Number of contact constraints
|
||||
uint mNbContactManifolds;
|
||||
|
||||
|
|
@ -378,6 +381,9 @@ class ContactSolver {
|
|||
void computeFrictionVectors(const Vector3& deltaVelocity,
|
||||
ContactManifoldSolver& contactPoint) const;
|
||||
|
||||
/// Warm start the solver.
|
||||
void warmStart();
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
@ -389,8 +395,11 @@ class ContactSolver {
|
|||
/// Destructor
|
||||
~ContactSolver() = default;
|
||||
|
||||
/// Initialize the contact constraints
|
||||
void init(Island** islands, uint nbIslands, decimal timeStep);
|
||||
|
||||
/// Initialize the constraint solver for a given island
|
||||
void initializeForIsland(decimal dt, Island* island);
|
||||
void initializeForIsland(Island* island);
|
||||
|
||||
/// Set the split velocities arrays
|
||||
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
|
||||
|
|
@ -400,9 +409,6 @@ class ContactSolver {
|
|||
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
|
||||
Vector3* constrainedAngularVelocities);
|
||||
|
||||
/// Warm start the solver.
|
||||
void warmStart();
|
||||
|
||||
/// Store the computed impulses to use them to
|
||||
/// warm start the solver at the next iteration
|
||||
void storeImpulses();
|
||||
|
|
@ -415,9 +421,6 @@ class ContactSolver {
|
|||
|
||||
/// Activate or Deactivate the split impulses for contacts
|
||||
void setIsSplitImpulseActive(bool isActive);
|
||||
|
||||
/// Clean up the constraint solver
|
||||
void cleanup();
|
||||
};
|
||||
|
||||
// Set the split velocities arrays
|
||||
|
|
|
|||
|
|
@ -342,23 +342,28 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
|||
|
||||
// ---------- Solve velocity constraints for joints and contacts ---------- //
|
||||
|
||||
// Initialize the contact solver
|
||||
mContactSolver.init(mIslands, mNbIslands, mTimeStep);
|
||||
|
||||
// For each island of the world
|
||||
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
|
||||
|
||||
// Check if there are contacts and constraints to solve
|
||||
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
|
||||
bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
|
||||
if (!isConstraintsToSolve && !isContactsToSolve) continue;
|
||||
//bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
|
||||
//if (!isConstraintsToSolve && !isContactsToSolve) continue;
|
||||
|
||||
// If there are contacts in the current island
|
||||
if (isContactsToSolve) {
|
||||
// if (isContactsToSolve) {
|
||||
|
||||
// Initialize the solver
|
||||
mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
|
||||
// // Initialize the solver
|
||||
// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
|
||||
|
||||
// Warm start the contact solver
|
||||
mContactSolver.warmStart();
|
||||
}
|
||||
// // Warm start the contact solver
|
||||
// if (mContactSolver.IsWarmStartingActive()) {
|
||||
// mContactSolver.warmStart();
|
||||
// }
|
||||
// }
|
||||
|
||||
// If there are constraints
|
||||
if (isConstraintsToSolve) {
|
||||
|
|
@ -366,26 +371,32 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
|||
// Initialize the constraint solver
|
||||
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
|
||||
}
|
||||
}
|
||||
|
||||
// For each iteration of the velocity solver
|
||||
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
|
||||
// For each iteration of the velocity solver
|
||||
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
|
||||
|
||||
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
|
||||
// Solve the constraints
|
||||
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
|
||||
if (isConstraintsToSolve) {
|
||||
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
|
||||
}
|
||||
|
||||
// Solve the contacts
|
||||
if (isContactsToSolve) mContactSolver.solve();
|
||||
}
|
||||
|
||||
// Cache the lambda values in order to use them in the next
|
||||
// step and cleanup the contact solver
|
||||
if (isContactsToSolve) {
|
||||
mContactSolver.storeImpulses();
|
||||
mContactSolver.cleanup();
|
||||
}
|
||||
mContactSolver.solve();
|
||||
|
||||
// Solve the contacts
|
||||
// if (isContactsToSolve) {
|
||||
|
||||
// mContactSolver.resetTotalPenetrationImpulse();
|
||||
|
||||
// mContactSolver.solvePenetrationConstraints();
|
||||
// mContactSolver.solveFrictionConstraints();
|
||||
// }
|
||||
}
|
||||
|
||||
mContactSolver.storeImpulses();
|
||||
}
|
||||
|
||||
// Solve the position error correction of the constraints
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user