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

Start to replace PGS solver by sequential impulse and improve of persistent contact cache

Browse Source
This commit is contained in:
Daniel Chappuis 2012-12-10 07:52:57 +01:00
parent 4ca42f9392
commit a0800ac33d
6 changed files with 191 additions and 339 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/configuration.h
View File

@ -79,8 +79,8 @@ const reactphysics3d::decimal OBJECT_MARGIN = 0.04;
// Friction coefficient // Friction coefficient
const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4; const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4;
// Distance threshold for two contact points for a valid persistent contact // Distance threshold for two contact points for a valid persistent contact (in meters)
const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03;
// Maximum number of bodies // Maximum number of bodies
const int NB_MAX_BODIES = 100000; const int NB_MAX_BODIES = 100000;

261
src/engine/ConstraintSolver.cpp
View File

@ -34,9 +34,7 @@ using namespace std;
// Constructor // Constructor
ConstraintSolver::ConstraintSolver(DynamicsWorld* world) ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
:world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS), :world(world), nbConstraints(0), nbIterations(10) {
nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION),
isErrorCorrectionActive(false) {
} }
@ -47,43 +45,56 @@ ConstraintSolver::~ConstraintSolver() {
// Initialize the constraint solver before each solving // Initialize the constraint solver before each solving
void ConstraintSolver::initialize() { void ConstraintSolver::initialize() {
Constraint* constraint;
nbConstraints = 0; nbConstraints = 0;
nbConstraintsError = 0;
// TOOD : Use better allocation here
mContactConstraints = new ContactConstraint[world->getNbContactConstraints()];
uint nbContactConstraints = 0;
// For each constraint // For each constraint
vector<Constraint*>::iterator it; vector<Contact*>::iterator it;
for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { for (it = world->getContactConstraintsBeginIterator(); it != world->getContactConstraintsEndIterator(); ++it) {
constraint = *it; Contact* contact = *it;
// If the constraint is active // If the constraint is active
if (constraint->isActive()) { if (contact->isActive()) {
activeConstraints.push_back(constraint);
RigidBody* body1 = contact->getBody1();
RigidBody* body2 = contact->getBody2();
activeConstraints.push_back(contact);
// Add the two bodies of the constraint in the constraintBodies list // Add the two bodies of the constraint in the constraintBodies list
constraintBodies.insert(constraint->getBody1()); mConstraintBodies.insert(body1);
constraintBodies.insert(constraint->getBody2()); mConstraintBodies.insert(body2);
// Fill in the body number maping // Fill in the body number maping
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size())); mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size())); mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
// Update the size of the jacobian matrix // Update the size of the jacobian matrix
nbConstraints += constraint->getNbConstraints(); nbConstraints += contact->getNbConstraints();
// Update the size of the jacobian matrix for error correction projection ContactConstraint constraint = mContactConstraints[nbContactConstraints];
if (constraint->getType() == CONTACT) { constraint.indexBody1 = mMapBodyToIndex[body1];
nbConstraintsError++; constraint.indexBody2 = mMapBodyToIndex[body2];
} constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
constraint.isBody1Moving = body1->getIsMotionEnabled();
constraint.isBody2Moving = body2->getIsMotionEnabled();
constraint.massInverseBody1 = body1->getMassInverse();
constraint.massInverseBody2 = body2->getMassInverse();
nbContactConstraints++;
} }
} }
// Compute the number of bodies that are part of some active constraint // Compute the number of bodies that are part of some active constraint
nbBodies = bodyNumberMapping.size(); nbBodies = mMapBodyToIndex.size();
assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS); assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS);
assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES); assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
} }
@ -95,7 +106,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
// For each active constraint // For each active constraint
int noConstraint = 0; int noConstraint = 0;
int noConstraintError = 0;
for (int c=0; c<activeConstraints.size(); c++) { for (int c=0; c<activeConstraints.size(); c++) {
@ -125,33 +135,11 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
// If the constraint is a contact // If the constraint is a contact
if (constraint->getType() == CONTACT) { if (constraint->getType() == CONTACT) {
Contact* contact = dynamic_cast<Contact*>(constraint); Contact* contact = dynamic_cast<Contact*>(constraint);
decimal penetrationDepth = contact->getPenetrationDepth();
// If error correction with projection is active // Add the Baumgarte error correction term for contacts
if (isErrorCorrectionActive) { decimal slop = 0.005;
if (contact->getPenetrationDepth() > slop) {
// Fill in the error correction projection parameters errorValues[noConstraint] += 0.2 * oneOverDt * std::max(double(contact->getPenetrationDepth() - slop), 0.0);
lowerBoundsError[noConstraintError] = lowerBounds[noConstraint];
upperBoundsError[noConstraintError] = upperBounds[noConstraint];
for (int i=0; i<12; i++) {
J_spError[noConstraintError][i] = J_sp[noConstraint][i];
}
bodyMappingError[noConstraintError][0] = constraint->getBody1();
bodyMappingError[noConstraintError][1] = constraint->getBody2();
penetrationDepths[noConstraintError] = contact->getPenetrationDepth();
// If the penetration depth is small
if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
// Use the Baumgarte error correction term for contacts instead of
// first order world projection
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
}
noConstraintError++;
}
else { // If error correction with projection is not active
// Add the Baumgarte error correction term for contacts
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
} }
} }
@ -160,11 +148,11 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
// For each current body that is implied in some constraint // For each current body that is implied in some constraint
RigidBody* rigidBody; RigidBody* rigidBody;
Body* body; RigidBody* body;
uint b=0; uint b=0;
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); ++it, b++) { for (set<RigidBody*>::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it, b++) {
body = *it; body = *it;
uint bodyNumber = bodyNumberMapping[body]; uint bodyNumber = mMapBodyToIndex[body];
// TODO : Use polymorphism and remove this downcasting // TODO : Use polymorphism and remove this downcasting
rigidBody = dynamic_cast<RigidBody*>(body); rigidBody = dynamic_cast<RigidBody*>(body);
@ -189,16 +177,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
Vconstraint[bodyIndexArray + 4] = 0.0; Vconstraint[bodyIndexArray + 4] = 0.0;
Vconstraint[bodyIndexArray + 5] = 0.0; Vconstraint[bodyIndexArray + 5] = 0.0;
// Compute the vector Vconstraint with final constraint velocities
if (isErrorCorrectionActive) {
VconstraintError[bodyIndexArray] = 0.0;
VconstraintError[bodyIndexArray + 1] = 0.0;
VconstraintError[bodyIndexArray + 2] = 0.0;
VconstraintError[bodyIndexArray + 3] = 0.0;
VconstraintError[bodyIndexArray + 4] = 0.0;
VconstraintError[bodyIndexArray + 5] = 0.0;
}
// Compute the vector with forces and torques values // Compute the vector with forces and torques values
Vector3 externalForce = rigidBody->getExternalForce(); Vector3 externalForce = rigidBody->getExternalForce();
Vector3 externalTorque = rigidBody->getExternalTorque(); Vector3 externalTorque = rigidBody->getExternalTorque();
@ -232,8 +210,8 @@ void ConstraintSolver::computeVectorB(decimal dt) {
b[c] = errorValues[c] * oneOverDT; b[c] = errorValues[c] * oneOverDT;
// Substract 1.0/dt*J*V to the vector b // Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = mMapBodyToIndex[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = mMapBodyToIndex[bodyMapping[c][1]];
decimal multiplication = 0.0; decimal multiplication = 0.0;
int body1ArrayIndex = 6 * indexBody1; int body1ArrayIndex = 6 * indexBody1;
int body2ArrayIndex = 6 * indexBody2; int body2ArrayIndex = 6 * indexBody2;
@ -268,22 +246,6 @@ void ConstraintSolver::computeVectorB(decimal dt) {
} }
} }
// Compute the vector b for error correction projection
void ConstraintSolver::computeVectorBError(decimal dt) {
decimal oneOverDT = 1.0 / dt;
for (uint c = 0; c<nbConstraintsError; c++) {
// b = errorValues * oneOverDT;
if (penetrationDepths[c] >= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
bError[c] = penetrationDepths[c] * oneOverDT;
}
else {
bError[c] = 0.0;
}
}
}
// Compute the matrix B_sp // Compute the matrix B_sp
void ConstraintSolver::computeMatrixB_sp() { void ConstraintSolver::computeMatrixB_sp() {
uint indexConstraintArray, indexBody1, indexBody2; uint indexConstraintArray, indexBody1, indexBody2;
@ -292,8 +254,8 @@ void ConstraintSolver::computeMatrixB_sp() {
for (uint c = 0; c<nbConstraints; c++) { for (uint c = 0; c<nbConstraints; c++) {
indexConstraintArray = 6 * c; indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = mMapBodyToIndex[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = mMapBodyToIndex[bodyMapping[c][1]];
B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0]; B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1]; B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2]; B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
@ -312,36 +274,6 @@ void ConstraintSolver::computeMatrixB_sp() {
} }
} }
// Same as B_sp matrix but for error correction projection and
// therefore, only for contact constraints
void ConstraintSolver::computeMatrixB_spErrorCorrection() {
uint indexConstraintArray, indexBody1, indexBody2;
// For each contact constraint
for (uint c = 0; c<nbConstraintsError; c++) {
indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMappingError[c][0]];
indexBody2 = bodyNumberMapping[bodyMappingError[c][1]];
B_spError[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_spError[c][0];
B_spError[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_spError[c][1];
B_spError[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_spError[c][2];
B_spError[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_spError[c][6];
B_spError[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_spError[c][7];
B_spError[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_spError[c][8];
for (uint i=0; i<3; i++) {
B_spError[0][indexConstraintArray + 3 + i] = 0.0;
B_spError[1][indexConstraintArray + 3 + i] = 0.0;
for (uint j=0; j<3; j++) {
B_spError[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_spError[c][3 + j];
B_spError[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_spError[c][9 + j];
}
}
}
}
// Compute the vector V_constraint (which corresponds to the constraint part of // Compute the vector V_constraint (which corresponds to the constraint part of
// the final V2 vector) according to the formula // the final V2 vector) according to the formula
// V_constraint = dt * (M^-1 * J^T * lambda) // V_constraint = dt * (M^-1 * J^T * lambda)
@ -354,8 +286,8 @@ void ConstraintSolver::computeVectorVconstraint(decimal dt) {
// Compute dt * (M^-1 * J^T * lambda // Compute dt * (M^-1 * J^T * lambda
for (uint i=0; i<nbConstraints; i++) { for (uint i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]]; indexBody1Array = 6 * mMapBodyToIndex[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]]; indexBody2Array = 6 * mMapBodyToIndex[bodyMapping[i][1]];
indexConstraintArray = 6 * i; indexConstraintArray = 6 * i;
for (j=0; j<6; j++) { for (j=0; j<6; j++) {
Vconstraint[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i] * dt; Vconstraint[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i] * dt;
@ -364,24 +296,6 @@ void ConstraintSolver::computeVectorVconstraint(decimal dt) {
} }
} }
// Same as computeVectorVconstraint() but for error correction projection
void ConstraintSolver::computeVectorVconstraintError(decimal dt) {
uint indexBody1Array, indexBody2Array, indexConstraintArray;
uint j;
// Compute M^-1 * J^T * lambda
for (uint i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
indexConstraintArray = 6 * i;
for (j=0; j<6; j++) {
VconstraintError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
VconstraintError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
}
}
}
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm // Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector // This method outputs the result in the lambda vector
void ConstraintSolver::solveLCP() { void ConstraintSolver::solveLCP() {
@ -398,7 +312,6 @@ void ConstraintSolver::solveLCP() {
// Compute the vector a // Compute the vector a
computeVectorA(); computeVectorA();
// For each constraint // For each constraint
for (i=0; i<nbConstraints; i++) { for (i=0; i<nbConstraints; i++) {
uint indexConstraintArray = 6 * i; uint indexConstraintArray = 6 * i;
@ -408,10 +321,14 @@ void ConstraintSolver::solveLCP() {
} }
} }
for(iter=0; iter<nbIterationsLCP; iter++) { // For each iteration
for(iter=0; iter<nbIterations; iter++) {
// For each constraint
for (i=0; i<nbConstraints; i++) { for (i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]]; indexBody1Array = 6 * mMapBodyToIndex[bodyMapping[i][0]];
indexBody2Array = 6 * mMapBodyToIndex[bodyMapping[i][1]];
uint indexConstraintArray = 6 * i; uint indexConstraintArray = 6 * i;
deltaLambda = b[i]; deltaLambda = b[i];
for (uint j=0; j<6; j++) { for (uint j=0; j<6; j++) {
@ -429,54 +346,6 @@ void ConstraintSolver::solveLCP() {
} }
} }
// Solve a LCP problem for error correction projection
// using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void ConstraintSolver::solveLCPErrorCorrection() {
for (uint i=0; i<nbConstraintsError; i++) {
lambdaError[i] = 0;
}
uint indexBody1Array, indexBody2Array;
decimal deltaLambda;
decimal lambdaTemp;
uint i, iter;
// Compute the vector a
computeVectorAError();
// For each constraint
for (i=0; i<nbConstraintsError; i++) {
uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_spError[i][j] * B_spError[0][indexConstraintArray + j] + J_spError[i][6 + j] * B_spError[1][indexConstraintArray + j];
}
}
for(iter=0; iter<nbIterationsLCPErrorCorrection; iter++) {
for (i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
uint indexConstraintArray = 6 * i;
deltaLambda = bError[i];
for (uint j=0; j<6; j++) {
deltaLambda -= (J_spError[i][j] * aError[indexBody1Array + j] + J_spError[i][6 + j] * aError[indexBody2Array + j]);
}
deltaLambda /= d[i];
lambdaTemp = lambdaError[i];
lambdaError[i] = std::max(lowerBoundsError[i], std::min(lambdaError[i] + deltaLambda, upperBoundsError[i]));
deltaLambda = lambdaError[i] - lambdaTemp;
for (uint j=0; j<6; j++) {
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * deltaLambda;
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * deltaLambda;
}
}
}
}
// Compute the vector a used in the solve() method // Compute the vector a used in the solve() method
// Note that a = B * lambda // Note that a = B * lambda
void ConstraintSolver::computeVectorA() { void ConstraintSolver::computeVectorA() {
@ -489,8 +358,8 @@ void ConstraintSolver::computeVectorA() {
} }
for(i=0; i<nbConstraints; i++) { for(i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]]; indexBody1Array = 6 * mMapBodyToIndex[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]]; indexBody2Array = 6 * mMapBodyToIndex[bodyMapping[i][1]];
uint indexConstraintArray = 6 * i; uint indexConstraintArray = 6 * i;
for (uint j=0; j<6; j++) { for (uint j=0; j<6; j++) {
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i]; a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i];
@ -499,28 +368,6 @@ void ConstraintSolver::computeVectorA() {
} }
} }
// Same as computeVectorA() but for error correction projection
void ConstraintSolver::computeVectorAError() {
uint i;
uint indexBody1Array, indexBody2Array;
// Init the vector a with zero values
for (i=0; i<6*nbBodies; i++) {
aError[i] = 0.0;
}
for(i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
uint indexConstraintArray = 6 * i;
for (uint j=0; j<6; j++) {
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
}
}
}
// Cache the lambda values in order to reuse them in the next step // Cache the lambda values in order to reuse them in the next step
// to initialize the lambda vector // to initialize the lambda vector
void ConstraintSolver::cacheLambda() { void ConstraintSolver::cacheLambda() {

131
src/engine/ConstraintSolver.h
View File

@ -37,6 +37,51 @@ namespace reactphysics3d {
// Declarations // Declarations
class DynamicsWorld; class DynamicsWorld;
// Structure ContactPointConstraint
// Internal structure for a contact point constraint
struct ContactPointConstraint {
decimal penetrationImpulse; // Accumulated normal impulse
decimal friction1Impulse; // Accumulated impulse in the 1st friction direction
decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction
Vector3 normal; // Normal vector of the contact
Vector3 frictionVector1; // First friction vector in the tangent plane
Vector3 frictionVector2; // Second friction vector in the tangent plane
Vector3 oldFrictionVector1; // Old first friction vector in the tangent plane
Vector3 oldFrictionVector2; // Old second friction vector in the tangent plane
Vector3 r1; // Vector from the body 1 center to the contact point
Vector3 r2; // Vector from the body 2 center to the contact point
Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector
Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector
Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector
Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector
Vector3 r1CrossN; // Cross product of r1 with the contact normal
Vector3 r2CrossN; // Cross product of r2 with the contact normal
decimal penetrationDepth; // Penetration depth
decimal restitutionBias; // Velocity restitution bias
decimal inversePenetrationMass; // Inverse of the matrix K for the penenetration
decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction
decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction
};
// Structure ContactConstraint
struct ContactConstraint {
// TODO : Use a constant for the number of contact points
uint indexBody1; // Index of body 1 in the constraint solver
uint indexBody2; // Index of body 2 in the constraint solver
decimal massInverseBody1; // Inverse of the mass of body 1
decimal massInverseBody2; // Inverse of the mass of body 2
Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1
Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2
bool isBody1Moving; // True if the body 1 is allowed to move
bool isBody2Moving; // True if the body 2 is allowed to move
ContactPointConstraint contacts[4]; // Contact point constraints
uint nbContacts; // Number of contact points
decimal restitutionFactor; // Mix of the restitution factor for two bodies
};
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
@ -69,18 +114,15 @@ class ConstraintSolver {
DynamicsWorld* world; // Reference to the world DynamicsWorld* world; // Reference to the world
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
bool isErrorCorrectionActive; // True if error correction (with world order) is active bool isErrorCorrectionActive; // True if error correction (with world order) is active
uint nbIterationsLCP; // Number of iterations of the LCP solver uint nbIterations; // Number of iterations of the LCP solver
uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction
uint nbConstraints; // Total number of constraints (with the auxiliary constraints) uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints) uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints)
uint nbBodies; // Current number of bodies in the physics world uint nbBodies; // Current number of bodies in the physics world
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
// the pointer to the body that correspond to the 1x6 J_ij matrix in the // the pointer to the body that correspond to the 1x6 J_ij matrix in the
// J_sp matrix. An integer body index refers to its index in the "bodies" std::vector // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection
decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
// This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint) // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
// a 1x6 matrix // a 1x6 matrix
@ -112,89 +154,68 @@ class ConstraintSolver {
decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques
// Each cell contains a 6x1 vector with external force and torque. // Each cell contains a 6x1 vector with external force and torque.
// Contact constraints
ContactConstraint* mContactConstraints;
// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
// Map body to index
std::map<RigidBody*, uint> mMapBodyToIndex;
void initialize(); // Initialize the constraint solver before each solving void initialize(); // Initialize the constraint solver before each solving
void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(decimal dt); // Compute the vector b void computeVectorB(decimal dt); // Compute the vector b
void computeVectorBError(decimal dt); // Compute the vector b for error correction projection
void computeMatrixB_sp(); // Compute the matrix B_sp void computeMatrixB_sp(); // Compute the matrix B_sp
void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection
void computeVectorVconstraint(decimal dt); // Compute the vector V2 void computeVectorVconstraint(decimal dt); // Compute the vector V2
void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
void computeVectorA(); // Compute the vector a used in the solve() method void computeVectorA(); // Compute the vector a used in the solve() method
void computeVectorAError(); // Same as computeVectorA() but for error correction projection
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection
public: public:
ConstraintSolver(DynamicsWorld* world); // Constructor ConstraintSolver(DynamicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor virtual ~ConstraintSolver(); // Destructor
void solve(decimal dt); // Solve the current LCP problem void solve(decimal dt); // Solve the current LCP problem
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint bool isConstrainedBody(RigidBody* body) const; // Return true if the body is in at least one constraint
Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); // Return the constrained angular velocity of a body after solving the LCP problem
Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction
Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction
void cleanup(); // Cleanup of the constraint solver void cleanup(); // Cleanup of the constraint solver
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value
}; };
// Return true if the body is in at least one constraint // Return true if the body is in at least one constraint
inline bool ConstraintSolver::isConstrainedBody(Body* body) const { inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
if(constraintBodies.find(body) != constraintBodies.end()) { return mConstraintBodies.count(body) == 1;
return true;
}
return false;
} }
// Return the constrained linear velocity of a body after solving the LCP problem // Return the constrained linear velocity of a body after solving the LCP problem
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body)); assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body]; uint indexBodyArray = 6 * mMapBodyToIndex[body];
return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]); return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
} }
// Return the constrained angular velocity of a body after solving the LCP problem // Return the constrained angular velocity of a body after solving the LCP problem
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
assert(isConstrainedBody(body)); assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body]; uint indexBodyArray = 6 * mMapBodyToIndex[body];
return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]); return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
} }
// Return the constrained linear velocity of a body after solving the LCP problem for error correction
inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) {
//assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]);
}
// Return the constrained angular velocity of a body after solving the LCP problem for error correction
inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) {
//assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]);
}
// Cleanup of the constraint solver // Cleanup of the constraint solver
inline void ConstraintSolver::cleanup() { inline void ConstraintSolver::cleanup() {
bodyNumberMapping.clear(); mMapBodyToIndex.clear();
constraintBodies.clear(); mConstraintBodies.clear();
activeConstraints.clear(); activeConstraints.clear();
} }
// Set the number of iterations of the LCP solver // Set the number of iterations of the LCP solver
inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
nbIterationsLCP = nbIterations; nbIterations = nbIterations;
} }
// Set the isErrorCorrectionActive value
inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
this->isErrorCorrectionActive = isErrorCorrectionActive;
}
// Solve the current LCP problem // Solve the current LCP problem
inline void ConstraintSolver::solve(decimal dt) { inline void ConstraintSolver::solve(decimal dt) {
@ -206,30 +227,18 @@ inline void ConstraintSolver::solve(decimal dt) {
// Compute the vector b // Compute the vector b
computeVectorB(dt); computeVectorB(dt);
if (isErrorCorrectionActive) {
computeVectorBError(dt);
}
// Compute the matrix B // Compute the matrix B
computeMatrixB_sp(); computeMatrixB_sp();
if (isErrorCorrectionActive) {
computeMatrixB_spErrorCorrection();
}
// Solve the LCP problem (computation of lambda) // Solve the LCP problem (computation of lambda)
solveLCP(); solveLCP();
if (isErrorCorrectionActive) {
solveLCPErrorCorrection();
}
// Cache the lambda values in order to use them in the next step // Cache the lambda values in order to use them in the next step
cacheLambda(); cacheLambda();
// Compute the vector Vconstraint // Compute the vector Vconstraint
computeVectorVconstraint(dt); computeVectorVconstraint(dt);
if (isErrorCorrectionActive) {
computeVectorVconstraintError(dt);
}
} }
} // End of ReactPhysics3D namespace } // End of ReactPhysics3D namespace

50
src/engine/DynamicsWorld.cpp
View File

@ -65,7 +65,8 @@ void DynamicsWorld::update() {
existCollision = false; existCollision = false;
removeAllContactConstraints(); // Remove all contact constraints
mContactConstraints.clear();
// Compute the collision detection // Compute the collision detection
if (mCollisionDetection.computeCollisionDetection()) { if (mCollisionDetection.computeCollisionDetection()) {
@ -107,8 +108,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
decimal dt = mTimer.getTimeStep(); decimal dt = mTimer.getTimeStep();
Vector3 newLinearVelocity; Vector3 newLinearVelocity;
Vector3 newAngularVelocity; Vector3 newAngularVelocity;
Vector3 linearVelocityErrorCorrection;
Vector3 angularVelocityErrorCorrection;
// For each body of thephysics world // For each body of thephysics world
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
@ -120,17 +119,12 @@ void DynamicsWorld::updateAllBodiesMotion() {
if (rigidBody->getIsMotionEnabled()) { if (rigidBody->getIsMotionEnabled()) {
newLinearVelocity.setAllValues(0.0, 0.0, 0.0); newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
newAngularVelocity.setAllValues(0.0, 0.0, 0.0); newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
// If it's a constrained body // If it's a constrained body
if (mConstraintSolver.isConstrainedBody(*it)) { if (mConstraintSolver.isConstrainedBody(*it)) {
// Get the constrained linear and angular velocities from the constraint solver // Get the constrained linear and angular velocities from the constraint solver
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it); newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it); newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
linearVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody);
angularVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody);
} }
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
@ -143,8 +137,7 @@ void DynamicsWorld::updateAllBodiesMotion() {
newAngularVelocity += rigidBody->getAngularVelocity(); newAngularVelocity += rigidBody->getAngularVelocity();
// Update the position and the orientation of the body according to the new velocity // Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity, updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
linearVelocityErrorCorrection, angularVelocityErrorCorrection);
// Update the AABB of the rigid body // Update the AABB of the rigid body
rigidBody->updateAABB(); rigidBody->updateAABB();
@ -155,8 +148,9 @@ void DynamicsWorld::updateAllBodiesMotion() {
// Update the position and orientation of a body // Update the position and orientation of a body
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
// orientation of the body // orientation of the body
void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity, void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) { const Vector3& newLinVelocity,
const Vector3& newAngVelocity) {
decimal dt = mTimer.getTimeStep(); decimal dt = mTimer.getTimeStep();
assert(rigidBody); assert(rigidBody);
@ -171,13 +165,9 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con
// Get current position and orientation of the body // Get current position and orientation of the body
const Vector3& currentPosition = rigidBody->getTransform().getPosition(); const Vector3& currentPosition = rigidBody->getTransform().getPosition();
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
// Error correction projection Vector3 newPosition = currentPosition + newLinVelocity * dt;
Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection; Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt;
Vector3 newPosition = correctedPosition + newLinVelocity * dt;
Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt;
Transform newTransform(newPosition, newOrientation.getUnit()); Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform); rigidBody->setTransform(newTransform);
@ -262,26 +252,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
mMemoryPoolRigidBodies.freeObject(rigidBody); mMemoryPoolRigidBodies.freeObject(rigidBody);
} }
// Remove all collision contacts constraints
// TODO : This method should be in the collision detection class
void DynamicsWorld::removeAllContactConstraints() {
// For all constraints
for (vector<Constraint*>::iterator it = mConstraints.begin(); it != mConstraints.end(); ) {
// Try a downcasting
Contact* contact = dynamic_cast<Contact*>(*it);
// If the constraint is a contact
if (contact) {
// Remove it from the constraints of the physics world
it = mConstraints.erase(it);
}
else {
++it;
}
}
}
// Remove all constraints in the physics world // Remove all constraints in the physics world
void DynamicsWorld::removeAllConstraints() { void DynamicsWorld::removeAllConstraints() {
mConstraints.clear(); mConstraints.clear();
@ -335,6 +305,6 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const
// Add all the contacts in the contact cache of the two bodies // Add all the contacts in the contact cache of the two bodies
// to the set of constraints in the physics world // to the set of constraints in the physics world
for (uint i=0; i<overlappingPair->getNbContacts(); i++) { for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
addConstraint(overlappingPair->getContact(i)); mContactConstraints.push_back(overlappingPair->getContact(i));
} }
} }

41
src/engine/DynamicsWorld.h
View File

@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld {
// All the rigid bodies of the physics world // All the rigid bodies of the physics world
std::set<RigidBody*> mRigidBodies; std::set<RigidBody*> mRigidBodies;
// List that contains all the current constraints // All the contact constraints
std::vector<Contact*> mContactConstraints;
// All the constraints (except contact constraints)
std::vector<Constraint*> mConstraints; std::vector<Constraint*> mConstraints;
// Gravity vector of the world // Gravity vector of the world
@ -93,9 +96,7 @@ class DynamicsWorld : public CollisionWorld {
// Update the position and orientation of a body // Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity,
const Vector3& newAngVelocity, const Vector3& newAngVelocity);
const Vector3& linearVelocityErrorCorrection,
const Vector3& angularVelocityErrorCorrection);
// Compute and set the interpolation factor to all bodies // Compute and set the interpolation factor to all bodies
void setInterpolationFactorToAllBodies(); void setInterpolationFactorToAllBodies();
@ -166,18 +167,24 @@ public :
// Remove a constraint // Remove a constraint
void removeConstraint(Constraint* constraint); void removeConstraint(Constraint* constraint);
// Remove all collision contacts constraints
void removeAllContactConstraints();
// Remove all constraints and delete them (free their memory) // Remove all constraints and delete them (free their memory)
void removeAllConstraints(); void removeAllConstraints();
// Return the number of contact constraints in the world
uint getNbContactConstraints() const;
// Return a start iterator on the constraint list // Return a start iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsBeginIterator(); std::vector<Constraint*>::iterator getConstraintsBeginIterator();
// Return a end iterator on the constraint list // Return a end iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsEndIterator(); std::vector<Constraint*>::iterator getConstraintsEndIterator();
// Return a start iterator on the contact constraint list
std::vector<Contact*>::iterator getContactConstraintsBeginIterator();
// Return a end iterator on the contact constraint list
std::vector<Contact*>::iterator getContactConstraintsEndIterator();
// Return an iterator to the beginning of the rigid bodies of the physics world // Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator(); std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@ -202,11 +209,6 @@ inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) {
mConstraintSolver.setNbLCPIterations(nbIterations); mConstraintSolver.setNbLCPIterations(nbIterations);
} }
// Set the isErrorCorrectionActive value
inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
mConstraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive);
}
// Reset the boolean movement variable of each body // Reset the boolean movement variable of each body
inline void DynamicsWorld::resetBodiesMovementVariable() { inline void DynamicsWorld::resetBodiesMovementVariable() {
@ -275,6 +277,11 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
return mRigidBodies.end(); return mRigidBodies.end();
} }
// Return the number of contact constraints in the world
inline uint DynamicsWorld::getNbContactConstraints() const {
return mContactConstraints.size();
}
// Return a start iterator on the constraint list // Return a start iterator on the constraint list
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() { inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
return mConstraints.begin(); return mConstraints.begin();
@ -285,6 +292,16 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
return mConstraints.end(); return mConstraints.end();
} }
// Return a start iterator on the contact constraint list
inline std::vector<Contact*>::iterator DynamicsWorld::getContactConstraintsBeginIterator() {
return mContactConstraints.begin();
}
// Return a end iterator on the contact constraint list
inline std::vector<Contact*>::iterator DynamicsWorld::getContactConstraintsEndIterator() {
return mContactConstraints.end();
}
} }
#endif #endif

43
src/engine/PersistentContactCache.cpp
View File

@ -41,19 +41,22 @@ PersistentContactCache::~PersistentContactCache() {
// Add a contact in the cache // Add a contact in the cache
void PersistentContactCache::addContact(Contact* contact) { void PersistentContactCache::addContact(Contact* contact) {
int indexNewContact = mNbContacts;
// For contact already in the cache // For contact already in the cache
for (uint i=0; i<mNbContacts; i++) { for (uint i=0; i<mNbContacts; i++) {
// Check if the new point point does not correspond to a same contact point // Check if the new point point does not correspond to a same contact point
// already in the cache. If it's the case, we do not add the new contact // already in the cache.
if (isApproxEqual(contact->getLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) { decimal distance = (mContacts[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare();
// Delete the new contact if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
contact->Contact::~Contact();
mMemoryPoolContacts.freeObject(contact); // Delete the new contact
contact->Contact::~Contact();
return; mMemoryPoolContacts.freeObject(contact);
//removeContact(i);
return;
//break;
} }
} }
@ -62,11 +65,10 @@ void PersistentContactCache::addContact(Contact* contact) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContact(indexToRemove); removeContact(indexToRemove);
indexNewContact = indexToRemove;
} }
// Add the new contact in the cache // Add the new contact in the cache
mContacts[indexNewContact] = contact; mContacts[mNbContacts] = contact;
mNbContacts++; mNbContacts++;
} }
@ -104,21 +106,28 @@ void PersistentContactCache::update(const Transform& transform1, const Transform
mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal())); mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal()));
} }
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD;
// Remove the contacts that don't represent very well the persistent contact // Remove the contacts that don't represent very well the persistent contact
for (int i=mNbContacts-1; i>=0; i--) { for (int i=mNbContacts-1; i>=0; i--) {
assert(i>= 0 && i < mNbContacts); assert(i>= 0 && i < mNbContacts);
// Compute the distance between contact points in the normal direction
decimal distanceNormal = -mContacts[i]->getPenetrationDepth();
// Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore) // If the contacts points are too far from each other in the normal direction
if (mContacts[i]->getPenetrationDepth() <= 0.0) { if (distanceNormal > squarePersistentContactThreshold) {
removeContact(i); removeContact(i);
} }
else { else {
// Compute the distance of the two contact points in the place orthogonal to the contact normal // Compute the distance of the two contact points in the plane orthogonal to the contact normal
Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth(); Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() +
mContacts[i]->getNormal() * distanceNormal;
Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1; Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance threshold, we remove the contact // If the orthogonal distance is larger than the valid distance threshold, we remove the contact
if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) { if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
removeContact(i); removeContact(i);
} }
} }
@ -224,4 +233,4 @@ void PersistentContactCache::clear() {
mMemoryPoolContacts.freeObject(mContacts[i]); mMemoryPoolContacts.freeObject(mContacts[i]);
} }
mNbContacts = 0; mNbContacts = 0;
} }