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

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

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
src/engine/ConstraintSolver.cpp
View File

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

29
src/engine/DynamicsWorld.cpp
View File

@ -50,7 +50,6 @@ DynamicsWorld::~DynamicsWorld() {
// Update the physics simulation
void DynamicsWorld::update() {
bool existCollision = false;
assert(mTimer.getIsRunning());
@ -63,16 +62,16 @@ void DynamicsWorld::update() {
// While the time accumulator is not empty
while(mTimer.isPossibleToTakeStep()) {
existCollision = false;
// Remove all contact constraints
mContactConstraints.clear();
// Remove all contact manifolds
mContactManifolds.clear();
// Compute the collision detection
if (mCollisionDetection.computeCollisionDetection()) {
existCollision = true;
mCollisionDetection.computeCollisionDetection();
// Solve constraints
// If there are constraints or contacts
if (!mConstraints.empty() || !mContactManifolds.empty()) {
// Solve the constraints and contacts
mConstraintSolver.solve(mTimer.getTimeStep());
}
@ -86,9 +85,7 @@ void DynamicsWorld::update() {
updateAllBodiesMotion();
// Cleanup of the constraint solver
if (existCollision) {
mConstraintSolver.cleanup();
}
mConstraintSolver.cleanup();
}
// 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
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
// to the set of constraints in the physics world
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);
}

31
src/engine/DynamicsWorld.h
View File

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