Add the class Contact that contains the contact points between two bodies

This commit is contained in:
Daniel Chappuis 2012-12-12 08:19:03 +01:00
parent 2fc8beaa77
commit 5e997f1c5c
3 changed files with 70 additions and 56 deletions

View File

@ -49,45 +49,53 @@ void ConstraintSolver::initialize() {
nbConstraints = 0; nbConstraints = 0;
// TOOD : Use better allocation here // TOOD : Use better allocation here
mContactConstraints = new ContactConstraint[world->getNbContactConstraints()]; mContactConstraints = new ContactConstraint[world->getNbContactManifolds()];
uint nbContactConstraints = 0; uint nbContactConstraints = 0;
// For each constraint // For each contact manifold of the world
vector<Contact*>::iterator it; vector<ContactManifold>::iterator it;
for (it = world->getContactConstraintsBeginIterator(); it != world->getContactConstraintsEndIterator(); ++it) { for (it = world->getContactManifoldsBeginIterator(); it != world->getContactManifoldsEndIterator(); ++it) {
Contact* contact = *it; ContactManifold contactManifold = *it;
// If the constraint is active // For each contact point of the contact manifold
if (contact->isActive()) { for (uint c=0; c<contactManifold.nbContacts; c++) {
RigidBody* body1 = contact->getBody1(); // Get a contact point
RigidBody* body2 = contact->getBody2(); Contact* contact = contactManifold.contacts[c];
activeConstraints.push_back(contact); // If the constraint is active
if (contact->isActive()) {
// Add the two bodies of the constraint in the constraintBodies list RigidBody* body1 = contact->getBody1();
mConstraintBodies.insert(body1); RigidBody* body2 = contact->getBody2();
mConstraintBodies.insert(body2);
// Fill in the body number maping activeConstraints.push_back(contact);
mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
// Update the size of the jacobian matrix // Add the two bodies of the constraint in the constraintBodies list
nbConstraints += contact->getNbConstraints(); mConstraintBodies.insert(body1);
mConstraintBodies.insert(body2);
ContactConstraint constraint = mContactConstraints[nbContactConstraints];
constraint.indexBody1 = mMapBodyToIndex[body1]; // Fill in the body number maping
constraint.indexBody2 = mMapBodyToIndex[body2]; mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
constraint.isBody1Moving = body1->getIsMotionEnabled(); // Update the size of the jacobian matrix
constraint.isBody2Moving = body2->getIsMotionEnabled(); nbConstraints += contact->getNbConstraints();
constraint.massInverseBody1 = body1->getMassInverse();
constraint.massInverseBody2 = body2->getMassInverse(); 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++;
}
nbContactConstraints++;
} }
} }

View File

@ -50,7 +50,6 @@ DynamicsWorld::~DynamicsWorld() {
// Update the physics simulation // Update the physics simulation
void DynamicsWorld::update() { void DynamicsWorld::update() {
bool existCollision = false;
assert(mTimer.getIsRunning()); assert(mTimer.getIsRunning());
@ -63,16 +62,16 @@ void DynamicsWorld::update() {
// While the time accumulator is not empty // While the time accumulator is not empty
while(mTimer.isPossibleToTakeStep()) { while(mTimer.isPossibleToTakeStep()) {
existCollision = false; // Remove all contact manifolds
mContactManifolds.clear();
// Remove all contact constraints
mContactConstraints.clear();
// Compute the collision detection // Compute the collision detection
if (mCollisionDetection.computeCollisionDetection()) { mCollisionDetection.computeCollisionDetection();
existCollision = true;
// Solve constraints // If there are constraints or contacts
if (!mConstraints.empty() || !mContactManifolds.empty()) {
// Solve the constraints and contacts
mConstraintSolver.solve(mTimer.getTimeStep()); mConstraintSolver.solve(mTimer.getTimeStep());
} }
@ -86,9 +85,7 @@ void DynamicsWorld::update() {
updateAllBodiesMotion(); updateAllBodiesMotion();
// Cleanup of the constraint solver // Cleanup of the constraint solver
if (existCollision) { mConstraintSolver.cleanup();
mConstraintSolver.cleanup();
}
} }
// Compute and set the interpolation factor to all the bodies // Compute and set the interpolation factor to all the bodies
@ -302,9 +299,17 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const
// Add the contact to the contact cache of the corresponding overlapping pair // Add the contact to the contact cache of the corresponding overlapping pair
overlappingPair->addContact(contact); overlappingPair->addContact(contact);
// Create a contact manifold with the contact points of the two bodies
ContactManifold contactManifold;
contactManifold.nbContacts = 0;
// 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++) {
mContactConstraints.push_back(overlappingPair->getContact(i)); contactManifold.contacts[i] = overlappingPair->getContact(i);
contactManifold.nbContacts++;
} }
// Add the contact manifold to the world
mContactManifolds.push_back(contactManifold);
} }

View File

@ -28,6 +28,7 @@
// Libraries // Libraries
#include "CollisionWorld.h" #include "CollisionWorld.h"
#include "ContactManifold.h"
#include "../collision/CollisionDetection.h" #include "../collision/CollisionDetection.h"
#include "ConstraintSolver.h" #include "ConstraintSolver.h"
#include "../body/RigidBody.h" #include "../body/RigidBody.h"
@ -63,7 +64,7 @@ class DynamicsWorld : public CollisionWorld {
std::set<RigidBody*> mRigidBodies; std::set<RigidBody*> mRigidBodies;
// All the contact constraints // All the contact constraints
std::vector<Contact*> mContactConstraints; std::vector<ContactManifold> mContactManifolds;
// All the constraints (except contact constraints) // All the constraints (except contact constraints)
std::vector<Constraint*> mConstraints; std::vector<Constraint*> mConstraints;
@ -171,7 +172,7 @@ public :
void removeAllConstraints(); void removeAllConstraints();
// Return the number of contact constraints in the world // Return the number of contact constraints in the world
uint getNbContactConstraints() const; uint getNbContactManifolds() 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();
@ -179,11 +180,11 @@ public :
// 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 // Return a start iterator on the contact manifolds list
std::vector<Contact*>::iterator getContactConstraintsBeginIterator(); std::vector<ContactManifold>::iterator getContactManifoldsBeginIterator();
// Return a end iterator on the contact constraint list // Return a end iterator on the contact manifolds list
std::vector<Contact*>::iterator getContactConstraintsEndIterator(); std::vector<ContactManifold>::iterator getContactManifoldsEndIterator();
// 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();
@ -277,9 +278,9 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
return mRigidBodies.end(); return mRigidBodies.end();
} }
// Return the number of contact constraints in the world // Return the number of contact manifolds in the world
inline uint DynamicsWorld::getNbContactConstraints() const { inline uint DynamicsWorld::getNbContactManifolds() const {
return mContactConstraints.size(); return mContactManifolds.size();
} }
// Return a start iterator on the constraint list // Return a start iterator on the constraint list
@ -292,14 +293,14 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
return mConstraints.end(); return mConstraints.end();
} }
// Return a start iterator on the contact constraint list // Return a start iterator on the contact manifolds list
inline std::vector<Contact*>::iterator DynamicsWorld::getContactConstraintsBeginIterator() { inline std::vector<ContactManifold>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
return mContactConstraints.begin(); return mContactManifolds.begin();
} }
// Return a end iterator on the contact constraint list // Return a end iterator on the contact manifolds list
inline std::vector<Contact*>::iterator DynamicsWorld::getContactConstraintsEndIterator() { inline std::vector<ContactManifold>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
return mContactConstraints.end(); return mContactManifolds.end();
} }
} }