713 lines
38 KiB
C++
713 lines
38 KiB
C++
/** @file
|
|
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
* @author Daniel CHAPPUIS
|
|
* @author Edouard DUPIN
|
|
* @copyright 2010-2016, Daniel Chappuis
|
|
* @copyright 2017, Edouard DUPIN
|
|
* @license MPL v2.0 (see license file)
|
|
*/
|
|
#include <ephysics/engine/ContactSolver.hpp>
|
|
#include <ephysics/engine/DynamicsWorld.hpp>
|
|
#include <ephysics/body/RigidBody.hpp>
|
|
#include <ephysics/engine/Profiler.hpp>
|
|
|
|
using namespace ephysics;
|
|
|
|
float ContactSolver::BETA = float(0.2);
|
|
float ContactSolver::BETASPLITIMPULSE = float(0.2);
|
|
float ContactSolver::SLOP = float(0.01);
|
|
|
|
ContactSolver::ContactSolver( etk::Map<RigidBody*, int> mapBodyToVelocityIndex) :
|
|
this.splitLinearVelocities(null),
|
|
this.splitAngularVelocities(null),
|
|
this.linearVelocities(null),
|
|
this.angularVelocities(null),
|
|
this.mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
|
this.isWarmStartingActive(true),
|
|
this.isSplitImpulseActive(true),
|
|
this.isSolveFrictionAtContactManifoldCenterActive(true) {
|
|
|
|
}
|
|
|
|
void ContactSolver::initializeForIsland(float dt, Island* island) {
|
|
PROFILE("ContactSolver::initializeForIsland()");
|
|
assert(island != null);
|
|
assert(island->getNbBodies() > 0);
|
|
assert(island->getNbContactManifolds() > 0);
|
|
assert(this.splitLinearVelocities != null);
|
|
assert(this.splitAngularVelocities != null);
|
|
// Set the current time step
|
|
this.timeStep = dt;
|
|
this.contactConstraints.resize(island->getNbContactManifolds());
|
|
// For each contact manifold of the island
|
|
ContactManifold** contactManifolds = island->getContactManifold();
|
|
for (int iii=0; iii<this.contactConstraints.size(); ++iii) {
|
|
ContactManifold* externalManifold = contactManifolds[iii];
|
|
ContactManifoldSolver internalManifold = this.contactConstraints[iii];
|
|
assert(externalManifold->getNbContactPoints() > 0);
|
|
// Get the two bodies of the contact
|
|
RigidBody* body1 = staticcast<RigidBody*>(externalManifold->getContactPoint(0)->getBody1());
|
|
RigidBody* body2 = staticcast<RigidBody*>(externalManifold->getContactPoint(0)->getBody2());
|
|
assert(body1 != null);
|
|
assert(body2 != null);
|
|
// Get the position of the two bodies
|
|
vec3 x1 = body1->this.centerOfMassWorld;
|
|
vec3 x2 = body2->this.centerOfMassWorld;
|
|
// Initialize the internal contact manifold structure using the external
|
|
// contact manifold
|
|
internalManifold.indexBody1 = this.mapBodyToConstrainedVelocityIndex.find(body1)->second;
|
|
internalManifold.indexBody2 = this.mapBodyToConstrainedVelocityIndex.find(body2)->second;
|
|
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
|
|
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
|
|
internalManifold.massInverseBody1 = body1->this.massInverse;
|
|
internalManifold.massInverseBody2 = body2->this.massInverse;
|
|
internalManifold.nbContacts = externalManifold->getNbContactPoints();
|
|
internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
|
|
internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
|
|
internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
|
|
internalManifold.externalContactManifold = externalManifold;
|
|
internalManifold.isBody1DynamicType = body1->getType() == DYNAMIC;
|
|
internalManifold.isBody2DynamicType = body2->getType() == DYNAMIC;
|
|
// If we solve the friction raints at the center of the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
internalManifold.frictionPointBody1 = vec3(0.0f,0.0f,0.0f);
|
|
internalManifold.frictionPointBody2 = vec3(0.0f,0.0f,0.0f);
|
|
}
|
|
// For each contact point of the contact manifold
|
|
for (int ccc=0; ccc<externalManifold->getNbContactPoints(); ++ccc) {
|
|
ContactPointSolver contactPoint = internalManifold.contacts[ccc];
|
|
// Get a contact point
|
|
ContactPoint* externalContact = externalManifold->getContactPoint(ccc);
|
|
// Get the contact point on the two bodies
|
|
vec3 p1 = externalContact->getWorldPointOnBody1();
|
|
vec3 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();
|
|
externalContact->setIsRestingContact(true);
|
|
contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
|
|
contactPoint.oldFrictionvec2 = externalContact->getFrictionvec2();
|
|
contactPoint.penetrationImpulse = 0.0;
|
|
contactPoint.friction1Impulse = 0.0;
|
|
contactPoint.friction2Impulse = 0.0;
|
|
contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f);
|
|
// If we solve the friction raints at the center of the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
internalManifold.frictionPointBody1 += p1;
|
|
internalManifold.frictionPointBody2 += p2;
|
|
}
|
|
}
|
|
// If we solve the friction raints at the center of the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
internalManifold.frictionPointBody1 /=staticcast<float>(internalManifold.nbContacts);
|
|
internalManifold.frictionPointBody2 /=staticcast<float>(internalManifold.nbContacts);
|
|
internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
|
|
internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
|
|
internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
|
|
internalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2();
|
|
// If warm starting is active
|
|
if (this.isWarmStartingActive) {
|
|
// Initialize the accumulated impulses with the previous step accumulated impulses
|
|
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
|
|
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
|
|
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
|
|
} else {
|
|
// Initialize the accumulated impulses to zero
|
|
internalManifold.friction1Impulse = 0.0;
|
|
internalManifold.friction2Impulse = 0.0;
|
|
internalManifold.frictionTwistImpulse = 0.0;
|
|
internalManifold.rollingResistanceImpulse = vec3(0, 0, 0);
|
|
}
|
|
}
|
|
}
|
|
// Fill-in all the matrices needed to solve the LCP problem
|
|
initializeContactConstraints();
|
|
}
|
|
|
|
void ContactSolver::initializeContactConstraints() {
|
|
// For each contact raint
|
|
for (int c=0; c<this.contactConstraints.size(); c++) {
|
|
ContactManifoldSolver manifold = this.contactConstraints[c];
|
|
// Get the inertia tensors of both bodies
|
|
etk::Matrix3x3 I1 = manifold.inverseInertiaTensorBody1;
|
|
etk::Matrix3x3 I2 = manifold.inverseInertiaTensorBody2;
|
|
// If we solve the friction raints at the center of the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
manifold.normal = vec3(0.0, 0.0, 0.0);
|
|
}
|
|
// Get the velocities of the bodies
|
|
vec3 v1 = this.linearVelocities[manifold.indexBody1];
|
|
vec3 w1 = this.angularVelocities[manifold.indexBody1];
|
|
vec3 v2 = this.linearVelocities[manifold.indexBody2];
|
|
vec3 w2 = this.angularVelocities[manifold.indexBody2];
|
|
// For each contact point raint
|
|
for (int i=0; i<manifold.nbContacts; i++) {
|
|
ContactPointSolver contactPoint = manifold.contacts[i];
|
|
ContactPoint* externalContact = contactPoint.externalContact;
|
|
// Compute the velocity difference
|
|
vec3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
|
contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
|
|
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
|
|
// Compute the inverse mass matrix K for the penetration raint
|
|
float massPenetration = manifold.massInverseBody1
|
|
+ manifold.massInverseBody2
|
|
+ ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal)
|
|
+ ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
|
|
if (massPenetration > 0.0) {
|
|
contactPoint.inversePenetrationMass = 1.0f / massPenetration;
|
|
}
|
|
// If we do not solve the friction raints at the center of the contact manifold
|
|
if (!this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
// Compute the friction vectors
|
|
computeFrictionVectors(deltaV, contactPoint);
|
|
contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
|
|
contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2);
|
|
contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
|
|
contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2);
|
|
// Compute the inverse mass matrix K for the friction
|
|
// raints at each contact point
|
|
float friction1Mass = manifold.massInverseBody1
|
|
+ manifold.massInverseBody2
|
|
+ ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1)
|
|
+ ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1);
|
|
float friction2Mass = manifold.massInverseBody1
|
|
+ manifold.massInverseBody2
|
|
+ ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionvec2)
|
|
+ ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionvec2);
|
|
if (friction1Mass > 0.0) {
|
|
contactPoint.inverseFriction1Mass = 1.0f / friction1Mass;
|
|
}
|
|
if (friction2Mass > 0.0) {
|
|
contactPoint.inverseFriction2Mass = 1.0f / friction2Mass;
|
|
}
|
|
}
|
|
// 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;
|
|
float deltaVDotN = deltaV.dot(contactPoint.normal);
|
|
if (deltaVDotN < -RESTITUTIONVELOCITYTHRESHOLD) {
|
|
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
|
|
}
|
|
// If the warm starting of the contact solver is active
|
|
if (this.isWarmStartingActive) {
|
|
// Get the cached accumulated impulses from the previous step
|
|
contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
|
|
contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
|
|
contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
|
|
contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
|
|
}
|
|
// Initialize the split impulses to zero
|
|
contactPoint.penetrationSplitImpulse = 0.0;
|
|
// If we solve the friction raints at the center of the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
manifold.normal += contactPoint.normal;
|
|
}
|
|
}
|
|
// Compute the inverse K matrix for the rolling resistance raint
|
|
manifold.inverseRollingResistance.setZero();
|
|
if (manifold.rollingResistanceFactor > 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
|
|
manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
|
|
manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
|
|
}
|
|
// If we solve the friction raints at the center of the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
manifold.normal.normalize();
|
|
vec3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction)
|
|
- v1 - w1.cross(manifold.r1Friction);
|
|
// Compute the friction vectors
|
|
computeFrictionVectors(deltaVFrictionPoint, manifold);
|
|
// Compute the inverse mass matrix K for the friction raints at the center of
|
|
// the contact manifold
|
|
manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
|
|
manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2);
|
|
manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
|
|
manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2);
|
|
float friction1Mass = manifold.massInverseBody1
|
|
+ manifold.massInverseBody2
|
|
+ ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(manifold.frictionVector1)
|
|
+ ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(manifold.frictionVector1);
|
|
float friction2Mass = manifold.massInverseBody1
|
|
+ manifold.massInverseBody2
|
|
+ ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(manifold.frictionvec2)
|
|
+ ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(manifold.frictionvec2);
|
|
float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * manifold.normal)
|
|
+ manifold.normal.dot(manifold.inverseInertiaTensorBody2 * manifold.normal);
|
|
if (friction1Mass > 0.0) {
|
|
manifold.inverseFriction1Mass = 1.0f/friction1Mass;
|
|
}
|
|
if (friction2Mass > 0.0) {
|
|
manifold.inverseFriction2Mass = 1.0f/friction2Mass;
|
|
}
|
|
if (frictionTwistMass > 0.0) {
|
|
manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void ContactSolver::warmStart() {
|
|
// Check that warm starting is active
|
|
if (!this.isWarmStartingActive) {
|
|
return;
|
|
}
|
|
// For each raint
|
|
for (int ccc=0; ccc<this.contactConstraints.size(); ++ccc) {
|
|
ContactManifoldSolver contactManifold = this.contactConstraints[ccc];
|
|
boolean atLeastOneRestingContactPoint = false;
|
|
for (int iii=0; iii<contactManifold.nbContacts; ++iii) {
|
|
ContactPointSolver contactPoint = contactManifold.contacts[iii];
|
|
// If it is not a new contact (this contact was already existing at last time step)
|
|
if (contactPoint.isRestingContact) {
|
|
atLeastOneRestingContactPoint = true;
|
|
// --------- Penetration --------- //
|
|
// Compute the impulse P = J^T * lambda
|
|
Impulse impulsePenetration = computePenetrationImpulse(contactPoint.penetrationImpulse, contactPoint);
|
|
// Apply the impulse to the bodies of the raint
|
|
applyImpulse(impulsePenetration, contactManifold);
|
|
// If we do not solve the friction raints at the center of the contact manifold
|
|
if (!this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
// Project the old friction impulses (with old friction vectors) into
|
|
// the new friction vectors to get the new friction impulses
|
|
vec3 oldFrictionImpulse = contactPoint.friction1Impulse *
|
|
contactPoint.oldFrictionVector1 +
|
|
contactPoint.friction2Impulse *
|
|
contactPoint.oldFrictionvec2;
|
|
contactPoint.friction1Impulse = oldFrictionImpulse.dot(contactPoint.frictionVector1);
|
|
contactPoint.friction2Impulse = oldFrictionImpulse.dot(contactPoint.frictionvec2);
|
|
// --------- Friction 1 --------- //
|
|
// Compute the impulse P = J^T * lambda
|
|
Impulse impulseFriction1 = computeFriction1Impulse(contactPoint.friction1Impulse, contactPoint);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction1, contactManifold);
|
|
// --------- Friction 2 --------- //
|
|
// Compute the impulse P=J^T * lambda
|
|
Impulse impulseFriction2 = computeFriction2Impulse(contactPoint.friction2Impulse, contactPoint);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction2, contactManifold);
|
|
// ------ Rolling resistance------ //
|
|
if (contactManifold.rollingResistanceFactor > 0) {
|
|
// Compute the impulse P = J^T * lambda
|
|
Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), -contactPoint.rollingResistanceImpulse,
|
|
vec3(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseRollingResistance, contactManifold);
|
|
}
|
|
}
|
|
} 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 = vec3(0.0f,0.0f,0.0f);
|
|
}
|
|
}
|
|
// If we solve the friction raints at the center of the contact manifold and there is
|
|
// at least one resting contact point in the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj atLeastOneRestingContactPoint) {
|
|
// Project the old friction impulses (with old friction vectors) into the new friction
|
|
// vectors to get the new friction impulses
|
|
vec3 oldFrictionImpulse = contactManifold.friction1Impulse *
|
|
contactManifold.oldFrictionVector1 +
|
|
contactManifold.friction2Impulse *
|
|
contactManifold.oldFrictionvec2;
|
|
contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
|
|
contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2);
|
|
// ------ First friction raint at the center of the contact manifold ------ //
|
|
// Compute the impulse P = J^T * lambda
|
|
vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse;
|
|
vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse;
|
|
vec3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse;
|
|
vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse;
|
|
Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
|
|
linearImpulseBody2, angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction1, contactManifold);
|
|
// ------ Second friction raint at the center of the contact manifold ----- //
|
|
// Compute the impulse P = J^T * lambda
|
|
linearImpulseBody1 = -contactManifold.frictionvec2 * contactManifold.friction2Impulse;
|
|
angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse;
|
|
linearImpulseBody2 = contactManifold.frictionvec2 * contactManifold.friction2Impulse;
|
|
angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse;
|
|
Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
|
|
linearImpulseBody2, angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction2, contactManifold);
|
|
// ------ Twist friction raint at the center of the contact manifold ------ //
|
|
// Compute the impulse P = J^T * lambda
|
|
linearImpulseBody1 = vec3(0.0, 0.0, 0.0);
|
|
angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
|
|
linearImpulseBody2 = vec3(0.0, 0.0, 0.0);
|
|
angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
|
|
Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
|
|
linearImpulseBody2, angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseTwistFriction, contactManifold);
|
|
// ------ Rolling resistance at the center of the contact manifold ------ //
|
|
// Compute the impulse P = J^T * lambda
|
|
angularImpulseBody1 = -contactManifold.rollingResistanceImpulse;
|
|
angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
|
|
Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), angularImpulseBody1,
|
|
vec3(0.0f,0.0f,0.0f), angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseRollingResistance, contactManifold);
|
|
} 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 = vec3(0.0f,0.0f,0.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
void ContactSolver::solve() {
|
|
PROFILE("ContactSolver::solve()");
|
|
float deltaLambda;
|
|
float lambdaTemp;
|
|
// For each contact manifold
|
|
for (int ccc=0; ccc<this.contactConstraints.size(); ++ccc) {
|
|
ContactManifoldSolver contactManifold = this.contactConstraints[ccc];
|
|
float suthis.penetrationImpulse = 0.0;
|
|
// Get the rained velocities
|
|
vec3 v1 = this.linearVelocities[contactManifold.indexBody1];
|
|
vec3 w1 = this.angularVelocities[contactManifold.indexBody1];
|
|
vec3 v2 = this.linearVelocities[contactManifold.indexBody2];
|
|
vec3 w2 = this.angularVelocities[contactManifold.indexBody2];
|
|
for (int iii=0; iii<contactManifold.nbContacts; ++iii) {
|
|
ContactPointSolver contactPoint = contactManifold.contacts[iii];
|
|
// --------- Penetration --------- //
|
|
// Compute J*v
|
|
vec3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
|
float deltaVDotN = deltaV.dot(contactPoint.normal);
|
|
float Jv = deltaVDotN;
|
|
// Compute the bias "b" of the raint
|
|
float beta = this.isSplitImpulseActive ? BETASPLITIMPULSE : BETA;
|
|
float biasPenetrationDepth = 0.0;
|
|
if (contactPoint.penetrationDepth > SLOP) {
|
|
biasPenetrationDepth = -(beta/this.timeStep) * etk::max(0.0f, float(contactPoint.penetrationDepth - SLOP));
|
|
}
|
|
float b = biasPenetrationDepth + contactPoint.restitutionBias;
|
|
// Compute the Lagrange multiplier lambda
|
|
if (this.isSplitImpulseActive) {
|
|
deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass;
|
|
} else {
|
|
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
|
|
}
|
|
lambdaTemp = contactPoint.penetrationImpulse;
|
|
contactPoint.penetrationImpulse = etk::max(contactPoint.penetrationImpulse + deltaLambda, 0.0f);
|
|
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
|
|
// Compute the impulse P=J^T * lambda
|
|
Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint);
|
|
// Apply the impulse to the bodies of the raint
|
|
applyImpulse(impulsePenetration, contactManifold);
|
|
suthis.penetrationImpulse += contactPoint.penetrationImpulse;
|
|
// If the split impulse position correction is active
|
|
if (this.isSplitImpulseActive) {
|
|
// Split impulse (position correction)
|
|
vec3 v1Split = this.splitLinearVelocities[contactManifold.indexBody1];
|
|
vec3 w1Split = this.splitAngularVelocities[contactManifold.indexBody1];
|
|
vec3 v2Split = this.splitLinearVelocities[contactManifold.indexBody2];
|
|
vec3 w2Split = this.splitAngularVelocities[contactManifold.indexBody2];
|
|
vec3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1);
|
|
float JvSplit = deltaVSplit.dot(contactPoint.normal);
|
|
float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass;
|
|
float lambdaTempSplit = contactPoint.penetrationSplitImpulse;
|
|
contactPoint.penetrationSplitImpulse = etk::max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f);
|
|
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
|
|
// Compute the impulse P=J^T * lambda
|
|
Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint);
|
|
applySplitImpulse(splitImpulsePenetration, contactManifold);
|
|
}
|
|
// If we do not solve the friction raints at the center of the contact manifold
|
|
if (!this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
// --------- Friction 1 --------- //
|
|
// Compute J*v
|
|
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
|
Jv = deltaV.dot(contactPoint.frictionVector1);
|
|
// Compute the Lagrange multiplier lambda
|
|
deltaLambda = -Jv;
|
|
deltaLambda *= contactPoint.inverseFriction1Mass;
|
|
float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
|
|
lambdaTemp = contactPoint.friction1Impulse;
|
|
contactPoint.friction1Impulse = etk::max(-frictionLimit,
|
|
etk::min(contactPoint.friction1Impulse + deltaLambda, frictionLimit));
|
|
deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
|
|
// Compute the impulse P=J^T * lambda
|
|
Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction1, contactManifold);
|
|
// --------- Friction 2 --------- //
|
|
// Compute J*v
|
|
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
|
Jv = deltaV.dot(contactPoint.frictionvec2);
|
|
// Compute the Lagrange multiplier lambda
|
|
deltaLambda = -Jv;
|
|
deltaLambda *= contactPoint.inverseFriction2Mass;
|
|
frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
|
|
lambdaTemp = contactPoint.friction2Impulse;
|
|
contactPoint.friction2Impulse = etk::max(-frictionLimit, etk::min(contactPoint.friction2Impulse + deltaLambda, frictionLimit));
|
|
deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
|
|
// Compute the impulse P=J^T * lambda
|
|
Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction2, contactManifold);
|
|
// --------- Rolling resistance raint --------- //
|
|
if (contactManifold.rollingResistanceFactor > 0) {
|
|
// Compute J*v
|
|
vec3 JvRolling = w2 - w1;
|
|
// Compute the Lagrange multiplier lambda
|
|
vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
|
|
float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
|
|
vec3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
|
|
contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit);
|
|
deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
|
|
// Compute the impulse P=J^T * lambda
|
|
Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), -deltaLambdaRolling,
|
|
vec3(0.0f,0.0f,0.0f), deltaLambdaRolling);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseRolling, contactManifold);
|
|
}
|
|
}
|
|
}
|
|
// If we solve the friction raints at the center of the contact manifold
|
|
if (this.isSolveFrictionAtContactManifoldCenterActive) {
|
|
// ------ First friction raint at the center of the contact manifol ------ //
|
|
// Compute J*v
|
|
vec3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
|
|
- v1 - w1.cross(contactManifold.r1Friction);
|
|
float Jv = deltaV.dot(contactManifold.frictionVector1);
|
|
// Compute the Lagrange multiplier lambda
|
|
float deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
|
|
float frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse;
|
|
lambdaTemp = contactManifold.friction1Impulse;
|
|
contactManifold.friction1Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction1Impulse + deltaLambda, frictionLimit));
|
|
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
|
|
// Compute the impulse P=J^T * lambda
|
|
vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
|
|
vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
|
|
vec3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
|
|
vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
|
|
Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
|
|
linearImpulseBody2, angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction1, contactManifold);
|
|
// ------ Second friction raint 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.frictionvec2);
|
|
// Compute the Lagrange multiplier lambda
|
|
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
|
|
frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse;
|
|
lambdaTemp = contactManifold.friction2Impulse;
|
|
contactManifold.friction2Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction2Impulse + deltaLambda, frictionLimit));
|
|
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
|
|
// Compute the impulse P=J^T * lambda
|
|
linearImpulseBody1 = -contactManifold.frictionvec2 * deltaLambda;
|
|
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
|
|
linearImpulseBody2 = contactManifold.frictionvec2 * deltaLambda;
|
|
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
|
|
Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
|
|
linearImpulseBody2, angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseFriction2, contactManifold);
|
|
// ------ Twist friction raint at the center of the contact manifol ------ //
|
|
// Compute J*v
|
|
deltaV = w2 - w1;
|
|
Jv = deltaV.dot(contactManifold.normal);
|
|
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
|
|
frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse;
|
|
lambdaTemp = contactManifold.frictionTwistImpulse;
|
|
contactManifold.frictionTwistImpulse = etk::max(-frictionLimit, etk::min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit));
|
|
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
|
|
// Compute the impulse P=J^T * lambda
|
|
linearImpulseBody1 = vec3(0.0, 0.0, 0.0);
|
|
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
|
|
linearImpulseBody2 = vec3(0.0, 0.0, 0.0);;
|
|
angularImpulseBody2 = contactManifold.normal * deltaLambda;
|
|
Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
|
|
linearImpulseBody2, angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseTwistFriction, contactManifold);
|
|
// --------- Rolling resistance raint at the center of the contact manifold --------- //
|
|
if (contactManifold.rollingResistanceFactor > 0) {
|
|
// Compute J*v
|
|
vec3 JvRolling = w2 - w1;
|
|
// Compute the Lagrange multiplier lambda
|
|
vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
|
|
float rollingLimit = contactManifold.rollingResistanceFactor * suthis.penetrationImpulse;
|
|
vec3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
|
|
contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + deltaLambdaRolling,
|
|
rollingLimit);
|
|
deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
|
|
// Compute the impulse P=J^T * lambda
|
|
angularImpulseBody1 = -deltaLambdaRolling;
|
|
angularImpulseBody2 = deltaLambdaRolling;
|
|
Impulse impulseRolling(vec3(0.0f,0.0f,0.0f),
|
|
angularImpulseBody1,
|
|
vec3(0.0f,0.0f,0.0f),
|
|
angularImpulseBody2);
|
|
// Apply the impulses to the bodies of the raint
|
|
applyImpulse(impulseRolling, contactManifold);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void ContactSolver::storeImpulses() {
|
|
// For each contact manifold
|
|
for (int ccc=0; ccc<this.contactConstraints.size(); ++ccc) {
|
|
ContactManifoldSolver manifold = this.contactConstraints[ccc];
|
|
for (int iii=0; iii<manifold.nbContacts; ++iii) {
|
|
ContactPointSolver contactPoint = manifold.contacts[iii];
|
|
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->setFrictionvec2(contactPoint.frictionvec2);
|
|
}
|
|
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->setFrictionvec2(manifold.frictionvec2);
|
|
}
|
|
}
|
|
|
|
void ContactSolver::applyImpulse( Impulse impulse, ContactManifoldSolver manifold) {
|
|
// Update the velocities of the body 1 by applying the impulse P
|
|
this.linearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1;
|
|
this.angularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1;
|
|
// Update the velocities of the body 1 by applying the impulse P
|
|
this.linearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2;
|
|
this.angularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2;
|
|
}
|
|
|
|
void ContactSolver::applySplitImpulse( Impulse impulse, ContactManifoldSolver manifold) {
|
|
// Update the velocities of the body 1 by applying the impulse P
|
|
this.splitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1;
|
|
this.splitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1;
|
|
// Update the velocities of the body 1 by applying the impulse P
|
|
this.splitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2;
|
|
this.splitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2;
|
|
}
|
|
|
|
void ContactSolver::computeFrictionVectors( vec3 deltaVelocity, ContactPointSolver contactPoint) {
|
|
assert(contactPoint.normal.length() > 0.0);
|
|
// Compute the velocity difference vector in the tangential plane
|
|
vec3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
|
|
vec3 tangentVelocity = deltaVelocity - normalVelocity;
|
|
// If the velocty difference in the tangential plane is not zero
|
|
float lengthTangenVelocity = tangentVelocity.length();
|
|
if (lengthTangenVelocity > FLTEPSILON) {
|
|
// Compute the first friction vector in the direction of the tangent
|
|
// velocity difference
|
|
contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
|
|
} else {
|
|
// Get any orthogonal vector to the normal as the first friction vector
|
|
contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector();
|
|
}
|
|
// The second friction vector is computed by the cross product of the firs
|
|
// friction vector and the contact normal
|
|
contactPoint.frictionvec2 =contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalized();
|
|
}
|
|
|
|
void ContactSolver::computeFrictionVectors( vec3 deltaVelocity, ContactManifoldSolver contactManifold) {
|
|
assert(contactManifold.normal.length() > 0.0);
|
|
// Compute the velocity difference vector in the tangential plane
|
|
vec3 normalVelocity = deltaVelocity.dot(contactManifold.normal) * contactManifold.normal;
|
|
vec3 tangentVelocity = deltaVelocity - normalVelocity;
|
|
// If the velocty difference in the tangential plane is not zero
|
|
float lengthTangenVelocity = tangentVelocity.length();
|
|
if (lengthTangenVelocity > FLTEPSILON) {
|
|
// Compute the first friction vector in the direction of the tangent
|
|
// velocity difference
|
|
contactManifold.frictionVector1 = tangentVelocity / lengthTangenVelocity;
|
|
} else {
|
|
// Get any orthogonal vector to the normal as the first friction vector
|
|
contactManifold.frictionVector1 = contactManifold.normal.getOrthoVector();
|
|
}
|
|
// The second friction vector is computed by the cross product of the firs
|
|
// friction vector and the contact normal
|
|
contactManifold.frictionvec2 = contactManifold.normal.cross(contactManifold.frictionVector1).safeNormalized();
|
|
}
|
|
|
|
void ContactSolver::cleanup() {
|
|
this.contactConstraints.clear();
|
|
}
|
|
|
|
void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, vec3* splitAngularVelocities) {
|
|
assert(splitLinearVelocities != NULL);
|
|
assert(splitAngularVelocities != NULL);
|
|
this.splitLinearVelocities = splitLinearVelocities;
|
|
this.splitAngularVelocities = splitAngularVelocities;
|
|
}
|
|
|
|
void ContactSolver::setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities, vec3* rainedAngularVelocities) {
|
|
assert(rainedLinearVelocities != NULL);
|
|
assert(rainedAngularVelocities != NULL);
|
|
this.linearVelocities = rainedLinearVelocities;
|
|
this.angularVelocities = rainedAngularVelocities;
|
|
}
|
|
|
|
boolean ContactSolver::isSplitImpulseActive() {
|
|
return this.isSplitImpulseActive;
|
|
}
|
|
|
|
void ContactSolver::setIsSplitImpulseActive(boolean isActive) {
|
|
this.isSplitImpulseActive = isActive;
|
|
}
|
|
|
|
void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive) {
|
|
this.isSolveFrictionAtContactManifoldCenterActive = isActive;
|
|
}
|
|
|
|
float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) {
|
|
float restitution1 = body1->getMaterial().getBounciness();
|
|
float restitution2 = body2->getMaterial().getBounciness();
|
|
// Return the largest restitution factor
|
|
return (restitution1 > restitution2) ? restitution1 : restitution2;
|
|
}
|
|
|
|
float ContactSolver::computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2) {
|
|
// Use the geometric mean to compute the mixed friction coefficient
|
|
return sqrt(body1->getMaterial().getFrictionCoefficient() *
|
|
body2->getMaterial().getFrictionCoefficient());
|
|
}
|
|
|
|
float ContactSolver::computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) {
|
|
return 0.5f
|
|
* (body1->getMaterial().getRollingResistance()
|
|
+ body2->getMaterial().getRollingResistance());
|
|
}
|
|
|
|
Impulse ContactSolver::computePenetrationImpulse(float deltaLambda, ContactPointSolver contactPoint) {
|
|
return Impulse(-contactPoint.normal * deltaLambda,
|
|
-contactPoint.r1CrossN * deltaLambda,
|
|
contactPoint.normal * deltaLambda,
|
|
contactPoint.r2CrossN * deltaLambda);
|
|
}
|
|
|
|
Impulse ContactSolver::computeFriction1Impulse(float deltaLambda, ContactPointSolver contactPoint) {
|
|
return Impulse(-contactPoint.frictionVector1 * deltaLambda,
|
|
-contactPoint.r1CrossT1 * deltaLambda,
|
|
contactPoint.frictionVector1 * deltaLambda,
|
|
contactPoint.r2CrossT1 * deltaLambda);
|
|
}
|
|
|
|
Impulse ContactSolver::computeFriction2Impulse(float deltaLambda, ContactPointSolver contactPoint) {
|
|
return Impulse(-contactPoint.frictionvec2 * deltaLambda,
|
|
-contactPoint.r1CrossT2 * deltaLambda,
|
|
contactPoint.frictionvec2 * deltaLambda,
|
|
contactPoint.r2CrossT2 * deltaLambda);
|
|
}
|