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

Modify the contact solver so that its main loop is outside the solver

Browse Source
This commit is contained in:
Daniel Chappuis 2013-04-25 22:34:20 +02:00
parent ded465c105
commit fdda0b26a9
8 changed files with 301 additions and 268 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/fallingcubes/FallingCubes.cpp
View File

@ -66,12 +66,11 @@ int main(int argc, char** argv) {
glutMouseFunc(mouseButton);
glutMotionFunc(mouseMotion);
glutKeyboardFunc(keyboard);
glutCloseFunc(finish);
// Glut main looop
glutMainLoop();
finish();
return 0;
}
@ -115,8 +114,7 @@ void keyboard(unsigned char key, int x, int y) {
// Escape key
case 27:
finish();
exit(0);
glutLeaveMainLoop();
break;
// Space bar

6
src/engine/CollisionWorld.cpp
View File

@ -154,6 +154,7 @@ CollisionShape* CollisionWorld::createCollisionShape(const CollisionShape& colli
// A similar collision shape does not already exist in the world, so we create a
// new one and add it to the world
void* allocatedMemory = mMemoryAllocator.allocate(collisionShape.getSizeInBytes());
size_t test = collisionShape.getSizeInBytes();
CollisionShape* newCollisionShape = collisionShape.clone(allocatedMemory);
mCollisionShapes.push_back(newCollisionShape);
@ -181,11 +182,14 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
// Remove the shape from the set of shapes in the world
mCollisionShapes.remove(collisionShape);
// Compute the size (in bytes) of the collision shape
size_t nbBytesShape = collisionShape->getSizeInBytes();
// Call the destructor of the collision shape
collisionShape->CollisionShape::~CollisionShape();
// Deallocate the memory used by the collision shape
mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes());
mMemoryAllocator.release(collisionShape, nbBytesShape);
}
}

17
src/engine/ConstraintSolver.cpp
View File

@ -25,6 +25,7 @@
// Libraries
#include "ConstraintSolver.h"
#include "Profiler.h"
using namespace reactphysics3d;
@ -43,3 +44,19 @@ ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
ConstraintSolver::~ConstraintSolver() {
}
// Initialize the constraint solver
void ConstraintSolver::initialize(decimal dt) {
PROFILE("ConstraintSolver::initialize()");
// Set the current time step
mTimeStep = dt;
}
// Solve the constraints
void ConstraintSolver::solve() {
PROFILE("ConstraintSolver::solve()");
}

6
src/engine/ConstraintSolver.h
View File

@ -142,6 +142,12 @@ class ConstraintSolver {
/// Destructor
~ConstraintSolver();
/// Initialize the constraint solver
void initialize(decimal dt);
/// Solve the constraints
void solve();
};
}

405
src/engine/ContactSolver.cpp
View File

@ -44,7 +44,6 @@ ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mContactManifolds(contactManifolds),
mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL),
mConstrainedLinearVelocities(constrainedLinearVelocities),
@ -61,7 +60,12 @@ ContactSolver::~ContactSolver() {
}
// Initialize the constraint solver
void ContactSolver::initialize() {
void ContactSolver::initialize(decimal dt) {
PROFILE("ContactSolver::initialize()");
// Set the current time step
mTimeStep = dt;
// TODO : Use better memory allocation here
mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
@ -187,6 +191,9 @@ void ContactSolver::initialize() {
// Initialize the split impulse velocities
initializeSplitImpulseVelocities();
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
}
// Initialize the split impulse velocities
@ -380,6 +387,9 @@ void ContactSolver::initializeContactConstraints() {
/// the solution of the linear system
void ContactSolver::warmStart() {
// Check that warm starting is active
if (!mIsWarmStartingActive) return;
// For each constraint
for (uint c=0; c<mNbContactManifolds; c++) {
@ -519,260 +529,231 @@ void ContactSolver::warmStart() {
}
}
// Solve the contact constraints by applying sequential impulses
void ContactSolver::solveContactConstraints() {
// Solve the contacts
void ContactSolver::solve() {
PROFILE("ContactSolver::solve()");
decimal deltaLambda;
decimal lambdaTemp;
uint iter;
// For each iteration of the contact solver
for(iter=0; iter<mNbIterations; iter++) {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
ContactManifoldSolver& contactManifold = mContactConstraints[c];
decimal sumPenetrationImpulse = 0.0;
decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
// Get the constrained velocities
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
for (uint i=0; i<contactManifold.nbContacts; i++) {
for (uint i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
ContactPointSolver& contactPoint = contactManifold.contacts[i];
// --------- Penetration --------- //
// --------- Penetration --------- //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
decimal Jv = deltaVDotN;
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
decimal Jv = deltaVDotN;
// Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass;
}
else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, decimal(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass;
}
else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, decimal(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
contactPoint);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
sumPenetrationImpulse += contactPoint.penetrationImpulse;
// If the split impulse position correction is active
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
v1Split - w1Split.cross(contactPoint.r1);
decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
contactPoint);
const Impulse splitImpulsePenetration = computePenetrationImpulse(
deltaLambdaSplit, contactPoint);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
sumPenetrationImpulse += contactPoint.penetrationImpulse;
// If the split impulse position correction is active
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
v1Split - w1Split.cross(contactPoint.r1);
decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda
const Impulse splitImpulsePenetration = computePenetrationImpulse(
deltaLambdaSplit, contactPoint);
applySplitImpulse(splitImpulsePenetration, contactManifold);
}
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// --------- Friction 1 --------- //
// Compute J*v
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector1);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction1Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// --------- Friction 2 --------- //
// Compute J*v
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction2Impulse;
contactPoint.friction2Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction2Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
}
applySplitImpulse(splitImpulsePenetration, contactManifold);
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// ------ First friction constraint at the center of the contact manifol ------ //
// --------- Friction 1 --------- //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector1);
// Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction1Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// ------ Second friction constraint at the center of the contact manifol ----- //
// --------- Friction 2 --------- //
// Compute J*v
deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2);
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction2Impulse;
contactManifold.friction2Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction2Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction2Impulse;
contactPoint.friction2Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction2Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ Twist friction constraint at the center of the contact manifol ------ //
// Compute J*v
deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal);
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.frictionTwistImpulse;
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
std::min(contactManifold.frictionTwistImpulse
+ deltaLambda, frictionLimit));
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
angularImpulseBody2 = contactManifold.normal * deltaLambda;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseTwistFriction, contactManifold);
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
// ------ First friction constraint at the center of the contact manifol ------ //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
// Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// ------ Second friction constraint at the center of the contact manifol ----- //
// Compute J*v
deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction2Impulse;
contactManifold.friction2Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction2Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ Twist friction constraint at the center of the contact manifol ------ //
// Compute J*v
deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal);
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.frictionTwistImpulse;
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
std::min(contactManifold.frictionTwistImpulse
+ deltaLambda, frictionLimit));
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
angularImpulseBody2 = contactManifold.normal * deltaLambda;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseTwistFriction, contactManifold);
}
}
}
// Solve the constraints
void ContactSolver::solve(decimal timeStep) {
PROFILE("ContactSolver::solve()");
// Set the current time step
mTimeStep = timeStep;
// Initialize the solver
initialize();
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
// Warm start the solver
if (mIsWarmStartingActive) {
warmStart();
}
// Solve the contact constraints
solveContactConstraints();
// Cache the lambda values in order to use them in the next step
storeImpulses();
}
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {

38
src/engine/ContactSolver.h
View File

@ -345,9 +345,6 @@ class ContactSolver {
/// Reference to all the contact manifold of the world
std::vector<ContactManifold*>& mContactManifolds;
/// Number of iterations of the contact solver
uint mNbIterations;
/// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
@ -389,25 +386,12 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Initialize the constraint solver
void initialize();
/// Initialize the split impulse velocities
void initializeSplitImpulseVelocities();
/// Initialize the contact constraints before solving the system
void initializeContactConstraints();
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
/// Warm start the solver.
void warmStart();
/// Solve the contact constraints by applying sequential impulses
void solveContactConstraints();
/// Apply an impulse to the two bodies of a constraint
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
@ -460,8 +444,18 @@ class ContactSolver {
/// Destructor
virtual ~ContactSolver();
/// Solve the constraints
void solve(decimal timeStep);
/// Initialize the constraint solver
void initialize(decimal dt);
/// Warm start the solver.
void warmStart();
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
/// Solve the contacts
void solve();
/// Return true if the body is in at least one constraint
bool isConstrainedBody(RigidBody* body) const;
@ -481,9 +475,6 @@ class ContactSolver {
/// Clean up the constraint solver
void cleanup();
/// Set the number of iterations of the constraint solver
void setNbIterationsSolver(uint nbIterations);
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
@ -511,11 +502,6 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
return mSplitAngularVelocities[indexBody];
}
// Set the number of iterations of the constraint solver
inline void ContactSolver::setNbIterationsSolver(uint nbIterations) {
mNbIterations = nbIterations;
}
// Activate or Deactivate the split impulses for contacts
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive;

72
src/engine/DynamicsWorld.cpp
View File

@ -38,6 +38,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex),
mNbSolverIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
mIsDeactivationActive(DEACTIVATION_ENABLED) {
}
@ -94,15 +95,11 @@ void DynamicsWorld::update() {
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
// Initialize the constrained velocities
initConstrainedVelocitiesArray();
// Integrate the velocities
integrateRigidBodiesVelocities();
// If there are contacts
if (!mContactManifolds.empty()) {
// Solve the contacts
mContactSolver.solve(static_cast<decimal>(mTimer.getTimeStep()));
}
// Solve the contacts and constraints
solveContactsAndConstraints();
// Update the timer
mTimer.nextStep();
@ -110,8 +107,8 @@ void DynamicsWorld::update() {
// Reset the movement boolean variable of each body to false
resetBodiesMovementVariable();
// Update the position and orientation of each body
updateRigidBodiesPositionAndOrientation();
// Integrate the position and orientation of each body
integrateRigidBodiesPositions();
// Cleanup of the contact solver
mContactSolver.cleanup();
@ -124,8 +121,8 @@ void DynamicsWorld::update() {
setInterpolationFactorToAllBodies();
}
// Update the position and orientation of the rigid bodies
void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
// Integrate position and orientation of the rigid bodies
void DynamicsWorld::integrateRigidBodiesPositions() {
PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()");
@ -200,8 +197,8 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
}
}
// Initialize the constrained velocities array at each step
void DynamicsWorld::initConstrainedVelocitiesArray() {
// Integrate the velocities of rigid bodies
void DynamicsWorld::integrateRigidBodiesVelocities() {
// TODO : Use better memory allocation here
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
@ -228,6 +225,50 @@ void DynamicsWorld::initConstrainedVelocitiesArray() {
assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
}
// Solve the contacts and constraints
void DynamicsWorld::solveContactsAndConstraints() {
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
// Get the current time step
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = !mJoints.empty();
bool isContactsToSolve = !mContactManifolds.empty();
if (!isConstraintsToSolve && !isContactsToSolve) return;
// If there are contacts
if (isContactsToSolve) {
// Initialize the solver
mContactSolver.initialize(dt);
// Warm start the contact solver
mContactSolver.warmStart();
}
// If there are constraints
if (isConstraintsToSolve) {
// Initialize the constraint solver
mConstraintSolver.initialize(dt);
}
// For each iteration of the solver
for (uint i=0; i<mNbSolverIterations; i++) {
// Solve the constraints
if (isConstraintsToSolve) mConstraintSolver.solve();
// Solve the contacts
if (isContactsToSolve) mContactSolver.solve();
}
// Cache the lambda values in order to use them in the next step
if (isContactsToSolve) mContactSolver.storeImpulses();
}
// Cleanup the constrained velocities array at each step
void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
@ -235,9 +276,6 @@ void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
mConstrainedLinearVelocities.clear();
mConstrainedAngularVelocities.clear();
// Clear the constrained bodies
mConstrainedBodies.clear();
// Clear the rigid body to velocities array index mapping
mMapBodyToConstrainedVelocityIndex.clear();
}

19
src/engine/DynamicsWorld.h
View File

@ -59,6 +59,9 @@ class DynamicsWorld : public CollisionWorld {
/// Constraint solver
ConstraintSolver mConstraintSolver;
/// Number of solver iterations for the Sequential Impulses technique
uint mNbSolverIterations;
/// True if the deactivation (sleeping) of inactive bodies is enabled
bool mIsDeactivationActive;
@ -71,9 +74,6 @@ class DynamicsWorld : public CollisionWorld {
/// All the joints of the world
std::set<Constraint*> mJoints;
/// All the bodies that are part of contacts or constraints
std::set<RigidBody*> mConstrainedBodies;
/// Gravity vector of the world
Vector3 mGravity;
@ -99,8 +99,8 @@ class DynamicsWorld : public CollisionWorld {
/// Private assignment operator
DynamicsWorld& operator=(const DynamicsWorld& world);
/// Compute the motion of all bodies and update their positions and orientations
void updateRigidBodiesPositionAndOrientation();
/// Integrate the positions and orientations of rigid bodies
void integrateRigidBodiesPositions();
/// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
@ -109,8 +109,11 @@ class DynamicsWorld : public CollisionWorld {
/// Compute and set the interpolation factor to all bodies
void setInterpolationFactorToAllBodies();
/// Initialize the constrained velocities array at each step
void initConstrainedVelocitiesArray();
/// Integrate the velocities of rigid bodies
void integrateRigidBodiesVelocities();
/// Solve the contacts and constraints
void solveContactsAndConstraints();
/// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray();
@ -212,7 +215,7 @@ inline void DynamicsWorld::stop() {
// Set the number of iterations of the constraint solver
inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
mContactSolver.setNbIterationsSolver(nbIterations);
mNbSolverIterations = nbIterations;
}
// Activate or Deactivate the split impulses for contacts