Start to replace PGS solver by sequential impulse and improve of persistent contact cache
This commit is contained in:
Daniel Chappuis
parent
4ca42f9392
commit
a0800ac33d
|
|
@ -79,8 +79,8 @@ const reactphysics3d::decimal OBJECT_MARGIN = 0.04;
|
|||
// Friction coefficient
|
||||
const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4;
|
||||
|
||||
// Distance threshold for two contact points for a valid persistent contact
|
||||
const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02;
|
||||
// Distance threshold for two contact points for a valid persistent contact (in meters)
|
||||
const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03;
|
||||
|
||||
// Maximum number of bodies
|
||||
const int NB_MAX_BODIES = 100000;
|
||||
|
|
|
|||
|
|
@ -34,9 +34,7 @@ using namespace std;
|
|||
|
||||
// Constructor
|
||||
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
|
||||
:world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS),
|
||||
nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION),
|
||||
isErrorCorrectionActive(false) {
|
||||
:world(world), nbConstraints(0), nbIterations(10) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -47,43 +45,56 @@ ConstraintSolver::~ConstraintSolver() {
|
|||
|
||||
// Initialize the constraint solver before each solving
|
||||
void ConstraintSolver::initialize() {
|
||||
Constraint* constraint;
|
||||
|
||||
nbConstraints = 0;
|
||||
nbConstraintsError = 0;
|
||||
|
||||
// TOOD : Use better allocation here
|
||||
mContactConstraints = new ContactConstraint[world->getNbContactConstraints()];
|
||||
|
||||
uint nbContactConstraints = 0;
|
||||
|
||||
// For each constraint
|
||||
vector<Constraint*>::iterator it;
|
||||
for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
|
||||
constraint = *it;
|
||||
vector<Contact*>::iterator it;
|
||||
for (it = world->getContactConstraintsBeginIterator(); it != world->getContactConstraintsEndIterator(); ++it) {
|
||||
Contact* contact = *it;
|
||||
|
||||
// If the constraint is active
|
||||
if (constraint->isActive()) {
|
||||
activeConstraints.push_back(constraint);
|
||||
if (contact->isActive()) {
|
||||
|
||||
RigidBody* body1 = contact->getBody1();
|
||||
RigidBody* body2 = contact->getBody2();
|
||||
|
||||
activeConstraints.push_back(contact);
|
||||
|
||||
// Add the two bodies of the constraint in the constraintBodies list
|
||||
constraintBodies.insert(constraint->getBody1());
|
||||
constraintBodies.insert(constraint->getBody2());
|
||||
mConstraintBodies.insert(body1);
|
||||
mConstraintBodies.insert(body2);
|
||||
|
||||
// Fill in the body number maping
|
||||
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
|
||||
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
|
||||
mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
|
||||
mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
|
||||
|
||||
// Update the size of the jacobian matrix
|
||||
nbConstraints += constraint->getNbConstraints();
|
||||
nbConstraints += contact->getNbConstraints();
|
||||
|
||||
// Update the size of the jacobian matrix for error correction projection
|
||||
if (constraint->getType() == CONTACT) {
|
||||
nbConstraintsError++;
|
||||
}
|
||||
ContactConstraint constraint = mContactConstraints[nbContactConstraints];
|
||||
constraint.indexBody1 = mMapBodyToIndex[body1];
|
||||
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
|
||||
nbBodies = bodyNumberMapping.size();
|
||||
nbBodies = mMapBodyToIndex.size();
|
||||
|
||||
assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
|
||||
assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS);
|
||||
assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
|
||||
}
|
||||
|
||||
|
|
@ -95,7 +106,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
|
|||
|
||||
// For each active constraint
|
||||
int noConstraint = 0;
|
||||
int noConstraintError = 0;
|
||||
|
||||
for (int c=0; c<activeConstraints.size(); c++) {
|
||||
|
||||
|
|
@ -125,33 +135,11 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
|
|||
// If the constraint is a contact
|
||||
if (constraint->getType() == CONTACT) {
|
||||
Contact* contact = dynamic_cast<Contact*>(constraint);
|
||||
decimal penetrationDepth = contact->getPenetrationDepth();
|
||||
|
||||
// If error correction with projection is active
|
||||
if (isErrorCorrectionActive) {
|
||||
|
||||
// Fill in the error correction projection parameters
|
||||
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();
|
||||
decimal slop = 0.005;
|
||||
if (contact->getPenetrationDepth() > slop) {
|
||||
errorValues[noConstraint] += 0.2 * oneOverDt * std::max(double(contact->getPenetrationDepth() - slop), 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -160,11 +148,11 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
|
|||
|
||||
// For each current body that is implied in some constraint
|
||||
RigidBody* rigidBody;
|
||||
Body* body;
|
||||
RigidBody* body;
|
||||
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;
|
||||
uint bodyNumber = bodyNumberMapping[body];
|
||||
uint bodyNumber = mMapBodyToIndex[body];
|
||||
|
||||
// TODO : Use polymorphism and remove this downcasting
|
||||
rigidBody = dynamic_cast<RigidBody*>(body);
|
||||
|
|
@ -189,16 +177,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
|
|||
Vconstraint[bodyIndexArray + 4] = 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
|
||||
Vector3 externalForce = rigidBody->getExternalForce();
|
||||
Vector3 externalTorque = rigidBody->getExternalTorque();
|
||||
|
|
@ -232,8 +210,8 @@ void ConstraintSolver::computeVectorB(decimal dt) {
|
|||
b[c] = errorValues[c] * oneOverDT;
|
||||
|
||||
// Substract 1.0/dt*J*V to the vector b
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||
indexBody1 = mMapBodyToIndex[bodyMapping[c][0]];
|
||||
indexBody2 = mMapBodyToIndex[bodyMapping[c][1]];
|
||||
decimal multiplication = 0.0;
|
||||
int body1ArrayIndex = 6 * indexBody1;
|
||||
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
|
||||
void ConstraintSolver::computeMatrixB_sp() {
|
||||
uint indexConstraintArray, indexBody1, indexBody2;
|
||||
|
|
@ -292,8 +254,8 @@ void ConstraintSolver::computeMatrixB_sp() {
|
|||
for (uint c = 0; c<nbConstraints; c++) {
|
||||
|
||||
indexConstraintArray = 6 * c;
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||
indexBody1 = mMapBodyToIndex[bodyMapping[c][0]];
|
||||
indexBody2 = mMapBodyToIndex[bodyMapping[c][1]];
|
||||
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 + 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
|
||||
// the final V2 vector) according to the formula
|
||||
// V_constraint = dt * (M^-1 * J^T * lambda)
|
||||
|
|
@ -354,8 +286,8 @@ void ConstraintSolver::computeVectorVconstraint(decimal dt) {
|
|||
|
||||
// Compute dt * (M^-1 * J^T * lambda
|
||||
for (uint 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]];
|
||||
indexConstraintArray = 6 * i;
|
||||
for (j=0; j<6; j++) {
|
||||
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
|
||||
// This method outputs the result in the lambda vector
|
||||
void ConstraintSolver::solveLCP() {
|
||||
|
|
@ -398,7 +312,6 @@ void ConstraintSolver::solveLCP() {
|
|||
// Compute the vector a
|
||||
computeVectorA();
|
||||
|
||||
|
||||
// For each constraint
|
||||
for (i=0; i<nbConstraints; 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++) {
|
||||
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;
|
||||
deltaLambda = b[i];
|
||||
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
|
||||
// Note that a = B * lambda
|
||||
void ConstraintSolver::computeVectorA() {
|
||||
|
|
@ -489,8 +358,8 @@ void ConstraintSolver::computeVectorA() {
|
|||
}
|
||||
|
||||
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;
|
||||
for (uint j=0; j<6; j++) {
|
||||
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
|
||||
// to initialize the lambda vector
|
||||
void ConstraintSolver::cacheLambda() {
|
||||
|
|
|
|||
|
|
@ -38,6 +38,51 @@ namespace reactphysics3d {
|
|||
// Declarations
|
||||
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
|
||||
};
|
||||
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class ConstrainSolver :
|
||||
|
|
@ -69,18 +114,15 @@ class ConstraintSolver {
|
|||
DynamicsWorld* world; // Reference to the world
|
||||
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
||||
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 nbConstraints; // Total number of constraints (with the auxiliary constraints)
|
||||
uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints)
|
||||
uint nbBodies; // Current number of bodies in the physics world
|
||||
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
|
||||
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
|
||||
RigidBody* 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
|
||||
// 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
|
||||
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
|
||||
// This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
|
||||
// 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 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.
|
||||
|
||||
// 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 fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem
|
||||
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_spErrorCorrection(); // Compute the matrix B_spError for error correction projection
|
||||
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 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 solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection
|
||||
|
||||
public:
|
||||
ConstraintSolver(DynamicsWorld* world); // Constructor
|
||||
virtual ~ConstraintSolver(); // Destructor
|
||||
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
|
||||
Vector3 getConstrainedLinearVelocityOfBody(Body* 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 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
|
||||
bool isConstrainedBody(RigidBody* body) const; // Return true if the body is in at least one constraint
|
||||
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); // Return the constrained linear 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
|
||||
void cleanup(); // Cleanup of the constraint 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
|
||||
inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
|
||||
if(constraintBodies.find(body) != constraintBodies.end()) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
|
||||
return mConstraintBodies.count(body) == 1;
|
||||
}
|
||||
|
||||
// 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));
|
||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
||||
uint indexBodyArray = 6 * mMapBodyToIndex[body];
|
||||
return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
|
||||
}
|
||||
|
||||
// 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));
|
||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
||||
uint indexBodyArray = 6 * mMapBodyToIndex[body];
|
||||
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
|
||||
inline void ConstraintSolver::cleanup() {
|
||||
bodyNumberMapping.clear();
|
||||
constraintBodies.clear();
|
||||
mMapBodyToIndex.clear();
|
||||
mConstraintBodies.clear();
|
||||
activeConstraints.clear();
|
||||
}
|
||||
|
||||
// Set the number of iterations of the LCP solver
|
||||
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
|
||||
inline void ConstraintSolver::solve(decimal dt) {
|
||||
|
||||
|
|
@ -206,30 +227,18 @@ inline void ConstraintSolver::solve(decimal dt) {
|
|||
|
||||
// Compute the vector b
|
||||
computeVectorB(dt);
|
||||
if (isErrorCorrectionActive) {
|
||||
computeVectorBError(dt);
|
||||
}
|
||||
|
||||
// Compute the matrix B
|
||||
computeMatrixB_sp();
|
||||
if (isErrorCorrectionActive) {
|
||||
computeMatrixB_spErrorCorrection();
|
||||
}
|
||||
|
||||
// Solve the LCP problem (computation of lambda)
|
||||
solveLCP();
|
||||
if (isErrorCorrectionActive) {
|
||||
solveLCPErrorCorrection();
|
||||
}
|
||||
|
||||
// Cache the lambda values in order to use them in the next step
|
||||
cacheLambda();
|
||||
|
||||
// Compute the vector Vconstraint
|
||||
computeVectorVconstraint(dt);
|
||||
if (isErrorCorrectionActive) {
|
||||
computeVectorVconstraintError(dt);
|
||||
}
|
||||
}
|
||||
|
||||
} // End of ReactPhysics3D namespace
|
||||
|
|
|
|||
|
|
@ -65,7 +65,8 @@ void DynamicsWorld::update() {
|
|||
|
||||
existCollision = false;
|
||||
|
||||
removeAllContactConstraints();
|
||||
// Remove all contact constraints
|
||||
mContactConstraints.clear();
|
||||
|
||||
// Compute the collision detection
|
||||
if (mCollisionDetection.computeCollisionDetection()) {
|
||||
|
|
@ -107,8 +108,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
decimal dt = mTimer.getTimeStep();
|
||||
Vector3 newLinearVelocity;
|
||||
Vector3 newAngularVelocity;
|
||||
Vector3 linearVelocityErrorCorrection;
|
||||
Vector3 angularVelocityErrorCorrection;
|
||||
|
||||
// For each body of thephysics world
|
||||
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
||||
|
|
@ -120,17 +119,12 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
if (rigidBody->getIsMotionEnabled()) {
|
||||
newLinearVelocity.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 (mConstraintSolver.isConstrainedBody(*it)) {
|
||||
// Get the constrained linear and angular velocities from the constraint solver
|
||||
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*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
|
||||
|
|
@ -143,8 +137,7 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
newAngularVelocity += rigidBody->getAngularVelocity();
|
||||
|
||||
// Update the position and the orientation of the body according to the new velocity
|
||||
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity,
|
||||
linearVelocityErrorCorrection, angularVelocityErrorCorrection);
|
||||
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
|
||||
|
||||
// Update the AABB of the rigid body
|
||||
rigidBody->updateAABB();
|
||||
|
|
@ -155,8 +148,9 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
// Update the position and orientation of a body
|
||||
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
|
||||
// orientation of the body
|
||||
void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
|
||||
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) {
|
||||
void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
|
||||
const Vector3& newLinVelocity,
|
||||
const Vector3& newAngVelocity) {
|
||||
decimal dt = mTimer.getTimeStep();
|
||||
|
||||
assert(rigidBody);
|
||||
|
|
@ -172,12 +166,8 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con
|
|||
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
|
||||
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
|
||||
|
||||
// Error correction projection
|
||||
Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection;
|
||||
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;
|
||||
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
||||
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
|
||||
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
rigidBody->setTransform(newTransform);
|
||||
|
|
@ -262,26 +252,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* 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
|
||||
void DynamicsWorld::removeAllConstraints() {
|
||||
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
|
||||
// to the set of constraints in the physics world
|
||||
for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
|
||||
addConstraint(overlappingPair->getContact(i));
|
||||
mContactConstraints.push_back(overlappingPair->getContact(i));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld {
|
|||
// All the rigid bodies of the physics world
|
||||
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;
|
||||
|
||||
// Gravity vector of the world
|
||||
|
|
@ -93,9 +96,7 @@ class DynamicsWorld : public CollisionWorld {
|
|||
|
||||
// Update the position and orientation of a body
|
||||
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity,
|
||||
const Vector3& newAngVelocity,
|
||||
const Vector3& linearVelocityErrorCorrection,
|
||||
const Vector3& angularVelocityErrorCorrection);
|
||||
const Vector3& newAngVelocity);
|
||||
|
||||
// Compute and set the interpolation factor to all bodies
|
||||
void setInterpolationFactorToAllBodies();
|
||||
|
|
@ -166,18 +167,24 @@ public :
|
|||
// Remove a constraint
|
||||
void removeConstraint(Constraint* constraint);
|
||||
|
||||
// Remove all collision contacts constraints
|
||||
void removeAllContactConstraints();
|
||||
|
||||
// Remove all constraints and delete them (free their memory)
|
||||
void removeAllConstraints();
|
||||
|
||||
// Return the number of contact constraints in the world
|
||||
uint getNbContactConstraints() const;
|
||||
|
||||
// Return a start iterator on the constraint list
|
||||
std::vector<Constraint*>::iterator getConstraintsBeginIterator();
|
||||
|
||||
// Return a end iterator on the constraint list
|
||||
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
|
||||
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
|
||||
|
||||
|
|
@ -202,11 +209,6 @@ inline void DynamicsWorld::setNbLCPIterations(uint 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
|
||||
inline void DynamicsWorld::resetBodiesMovementVariable() {
|
||||
|
||||
|
|
@ -275,6 +277,11 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
|
|||
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
|
||||
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
|
||||
return mConstraints.begin();
|
||||
|
|
@ -285,6 +292,16 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
|
|||
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
|
||||
|
|
|
|||
|
|
@ -42,18 +42,21 @@ PersistentContactCache::~PersistentContactCache() {
|
|||
// Add a contact in the cache
|
||||
void PersistentContactCache::addContact(Contact* contact) {
|
||||
|
||||
int indexNewContact = mNbContacts;
|
||||
|
||||
// For contact already in the cache
|
||||
for (uint i=0; i<mNbContacts; i++) {
|
||||
|
||||
// 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
|
||||
if (isApproxEqual(contact->getLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) {
|
||||
// already in the cache.
|
||||
decimal distance = (mContacts[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare();
|
||||
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
|
||||
|
||||
// Delete the new contact
|
||||
contact->Contact::~Contact();
|
||||
mMemoryPoolContacts.freeObject(contact);
|
||||
//removeContact(i);
|
||||
|
||||
return;
|
||||
//break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -62,11 +65,10 @@ void PersistentContactCache::addContact(Contact* contact) {
|
|||
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
|
||||
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
|
||||
removeContact(indexToRemove);
|
||||
indexNewContact = indexToRemove;
|
||||
}
|
||||
|
||||
// Add the new contact in the cache
|
||||
mContacts[indexNewContact] = contact;
|
||||
mContacts[mNbContacts] = contact;
|
||||
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()));
|
||||
}
|
||||
|
||||
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
|
||||
PERSISTENT_CONTACT_DIST_THRESHOLD;
|
||||
|
||||
// Remove the contacts that don't represent very well the persistent contact
|
||||
for (int i=mNbContacts-1; i>=0; i--) {
|
||||
assert(i>= 0 && i < mNbContacts);
|
||||
|
||||
// Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore)
|
||||
if (mContacts[i]->getPenetrationDepth() <= 0.0) {
|
||||
// Compute the distance between contact points in the normal direction
|
||||
decimal distanceNormal = -mContacts[i]->getPenetrationDepth();
|
||||
|
||||
// If the contacts points are too far from each other in the normal direction
|
||||
if (distanceNormal > squarePersistentContactThreshold) {
|
||||
removeContact(i);
|
||||
}
|
||||
else {
|
||||
// Compute the distance of the two contact points in the place orthogonal to the contact normal
|
||||
Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth();
|
||||
// Compute the distance of the two contact points in the plane orthogonal to the contact normal
|
||||
Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() +
|
||||
mContacts[i]->getNormal() * distanceNormal;
|
||||
Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user