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

Add persistent contact caching in collision detection

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@443 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-10-18 22:03:05 +00:00
parent c72bf4aeca
commit 4049f6ae3b
41 changed files with 608 additions and 550 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/body/Body.cpp
View File

@ -30,8 +30,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
Body::Body(const Transform& transform, Shape* shape, double mass) Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned int id)
: shape(shape), transform(transform), mass(mass) { : shape(shape), transform(transform), mass(mass), id(id) {
assert(mass > 0.0); assert(mass > 0.0);
assert(shape); assert(shape);

12
src/body/Body.h
View File

@ -31,6 +31,7 @@
#include "../mathematics/Transform.h" #include "../mathematics/Transform.h"
#include "../shapes/AABB.h" #include "../shapes/AABB.h"
#include "../shapes/Shape.h" #include "../shapes/Shape.h"
#include "../constants.h"
// Namespace reactphysics3d // Namespace reactphysics3d
namespace reactphysics3d { namespace reactphysics3d {
@ -52,11 +53,13 @@ class Body {
bool isMotionEnabled; // True if the body is able to move bool isMotionEnabled; // True if the body is able to move
bool isCollisionEnabled; // True if the body can collide with others bodies bool isCollisionEnabled; // True if the body can collide with others bodies
AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection
luint id; // ID of the body
public : public :
Body(const Transform& transform, Shape* shape, double mass); // Constructor Body(const Transform& transform, Shape* shape, double mass, long unsigned int id); // Constructor
virtual ~Body(); // Destructor virtual ~Body(); // Destructor
luint getID() const; // Return the id of the body
Shape* getShape() const; // Return the collision shape Shape* getShape() const; // Return the collision shape
void setShape(Shape* shape); // Set the collision shape void setShape(Shape* shape); // Set the collision shape
double getMass() const; // Return the mass of the body double getMass() const; // Return the mass of the body
@ -74,6 +77,11 @@ class Body {
void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates
}; };
// Return the id of the body
inline luint Body::getID() const {
return id;
}
// Return the collision shape // Return the collision shape
inline Shape* Body::getShape() const { inline Shape* Body::getShape() const {
assert(shape); assert(shape);

4
src/body/RigidBody.cpp
View File

@ -30,8 +30,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape) RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape, long unsigned id)
: Body(transform, shape, mass), inertiaTensorLocal(inertiaTensorLocal), : Body(transform, shape, mass, id), inertiaTensorLocal(inertiaTensorLocal),
inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) { inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) {
restitution = 1.0; restitution = 1.0;

22
src/body/RigidBody.h
View File

@ -43,19 +43,19 @@ namespace reactphysics3d {
*/ */
class RigidBody : public Body { class RigidBody : public Body {
protected : protected :
Vector3 linearVelocity; // Linear velocity of the body Vector3 linearVelocity; // Linear velocity of the body
Vector3 angularVelocity; // Angular velocity of the body Vector3 angularVelocity; // Angular velocity of the body
Vector3 externalForce; // Current external force on the body Vector3 externalForce; // Current external force on the body
Vector3 externalTorque; // Current external torque on the body Vector3 externalTorque; // Current external torque on the body
Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates) Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates)
Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates) Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates)
double massInverse; // Inverse of the mass of the body double massInverse; // Inverse of the mass of the body
double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
public : public :
RigidBody(const Transform& transform, double mass, RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal,
const Matrix3x3& inertiaTensorLocal, Shape* shape); // Constructor // Copy-constructor Shape* shape, long unsigned int id); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor virtual ~RigidBody(); // Destructor
Vector3 getLinearVelocity() const; // Return the linear velocity Vector3 getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body

3
src/collision/BroadPhaseAlgorithm.cpp
View File

@ -29,7 +29,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm() { BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:collisionDetection(collisionDetection) {
} }

13
src/collision/BroadPhaseAlgorithm.h
View File

@ -32,6 +32,9 @@
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
// Declarations
class CollisionDetection;
/* -------------------------------------------------------------------- /* --------------------------------------------------------------------
Class BroadPhaseAlgorithm : Class BroadPhaseAlgorithm :
This class is an abstract class that represents an algorithm This class is an abstract class that represents an algorithm
@ -47,13 +50,15 @@ namespace reactphysics3d {
*/ */
class BroadPhaseAlgorithm { class BroadPhaseAlgorithm {
protected : protected :
CollisionDetection& collisionDetection; // Reference to the collision detection object
public : public :
BroadPhaseAlgorithm(); // Constructor BroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~BroadPhaseAlgorithm(); // Destructor virtual ~BroadPhaseAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies, virtual void computePossibleCollisionPairs()=0; // Compute the possible collision pairs of bodies
std::vector<std::pair<Body*, Body*> >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies virtual void notifyAddedBodies(std::vector<Body*> bodies)=0; // Notify the broad-phase algorithm about new bodies in the physics world
virtual void notifyRemovedBodies(std::vector<Body*> bodies)=0; // Notify the broad-phase algorithm about removed bodies in the physics world
}; };
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

262
src/collision/CollisionDetection.cpp
View File

@ -30,232 +30,144 @@
#include "../body/Body.h" #include "../body/Body.h"
#include "../shapes/BoxShape.h" #include "../shapes/BoxShape.h"
#include "../body/RigidBody.h" #include "../body/RigidBody.h"
#include "../constants.h"
#include <cassert> #include <cassert>
#include <complex> #include <complex>
#include <set>
#include <utility>
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
using namespace std; using namespace std;
// Constructor // Constructor
CollisionDetection::CollisionDetection(PhysicsWorld* world) { CollisionDetection::CollisionDetection(PhysicsWorld* world)
this->world = world; : world(world), memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS) {
// Create the broad-phase algorithm that will be used (Sweep and Prune with AABB) // Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
broadPhaseAlgorithm = new SAPAlgorithm(); broadPhaseAlgorithm = new SAPAlgorithm(*this);
// Create the narrow-phase algorithm that will be used (Separating axis algorithm) // Create the narrow-phase algorithm that will be used (Separating axis algorithm)
narrowPhaseAlgorithm = new GJKAlgorithm(); // TODO : Use GJK algo here narrowPhaseAlgorithm = new GJKAlgorithm(*this);
} }
// Destructor // Destructor
CollisionDetection::~CollisionDetection() { CollisionDetection::~CollisionDetection() {
// Delete the remaining overlapping pairs
for (map<std::pair<luint, luint>, OverlappingPair*>::iterator it=overlappingPairs.begin(); it != overlappingPairs.end(); it++) {
// Delete the overlapping pair
(*it).second->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject((*it).second);
}
} }
// Compute the collision detection // Compute the collision detection
bool CollisionDetection::computeCollisionDetection() { bool CollisionDetection::computeCollisionDetection() {
world->removeAllContactConstraints(); world->removeAllContactConstraints();
possibleCollisionPairs.clear();
contactInfos.clear();
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
computeNarrowPhase(); bool collisionExists = computeNarrowPhase();
// Compute all the new contacts
computeAllContacts();
// Return true if at least one contact has been found // Return true if at least one contact has been found
return (contactInfos.size() > 0); return collisionExists;
} }
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() { void CollisionDetection::computeBroadPhase() {
// Clear the set of possible colliding pairs of bodies // Notify the broad-phase algorithm about new and removed bodies in the physics world
possibleCollisionPairs.clear(); broadPhaseAlgorithm->notifyAddedBodies(world->getAddedBodies());
broadPhaseAlgorithm->notifyRemovedBodies(world->getRemovedBodies());
// Compute the set of possible collision pairs of bodies // Clear the set of the overlapping pairs in the current step
broadPhaseAlgorithm->computePossibleCollisionPairs(world->getAddedBodies(), world->getRemovedBodies(), possibleCollisionPairs); currentStepOverlappingPairs.clear();
// Execute the broad-phase collision algorithm in order to compute the overlapping pairs of bodies
broadPhaseAlgorithm->computePossibleCollisionPairs();
// At this point, the pairs in the set lastStepOverlappingPairs contains
// only the pairs that are not overlapping anymore. Therefore, we can
// remove them from the overlapping pairs map
for (set<pair<luint, luint> >::iterator it = lastStepOverlappingPairs.begin(); it != lastStepOverlappingPairs.end(); it++) {
// Remove the overlapping pair from the memory pool
overlappingPairs.at((*it))->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject(overlappingPairs.at((*it)));
overlappingPairs.erase(*it);
}
// The current overlapping pairs become the last step overlapping pairs
lastStepOverlappingPairs = currentStepOverlappingPairs;
} }
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() { bool CollisionDetection::computeNarrowPhase() {
bool collisionExists = false;
map<std::pair<luint, luint>, OverlappingPair*>::iterator it;
// For each possible collision pair of bodies // For each possible collision pair of bodies
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) { for (it = overlappingPairs.begin(); it != overlappingPairs.end(); it++) {
ContactInfo* contactInfo = NULL; ContactInfo* contactInfo = NULL;
Body* const body1 = possibleCollisionPairs.at(i).first; Body* const body1 = (*it).second->getBody1();
Body* const body2 = possibleCollisionPairs.at(i).second; Body* const body2 = (*it).second->getBody2();
// Update the contact cache of the overlapping pair
(*it).second->update();
// Use the narrow-phase collision detection algorithm to check if there really are a contact // Use the narrow-phase collision detection algorithm to check if there really are a contact
if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(), if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(),
body2->getShape(), body2->getTransform(), contactInfo)) { body2->getShape(), body2->getTransform(), contactInfo)) {
assert(contactInfo); assert(contactInfo);
collisionExists = true;
// Add the contact info the current list of collision informations // Create a new contact
contactInfos.push_back(contactInfo); Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(contactInfo);
// Add the contact to the contact cache of the corresponding overlapping pair
(*it).second->addContact(contact);
// 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<(*it).second->getNbContacts(); i++) {
world->addConstraint((*it).second->getContact(i));
}
} }
} }
return collisionExists;
} }
// Compute all the contacts from the contact info list
void CollisionDetection::computeAllContacts() {
// For each possible contact info (computed during narrow-phase collision detection)
for (unsigned int i=0; i<contactInfos.size(); i++) {
ContactInfo* contactInfo = contactInfos.at(i);
assert(contactInfo);
// Compute one or several new contacts and add them into the physics world // Allow the broadphase to notify the collision detection about an overlapping pair
computeContactGJK(contactInfo); // TODO : Call computeContactGJK() here // This method is called by a broad-phase collision detection algorithm
} void CollisionDetection::broadPhaseNotifyOverlappingPair(Body* body1, Body* body2) {
// Construct the pair of index
pair<luint, luint> indexPair = body1->getID() < body2->getID() ? make_pair(body1->getID(), body2->getID()) :
make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second);
// Add the pair to the overlapping pairs of the current step
currentStepOverlappingPairs.insert(indexPair);
// Remove the pair from the set of overlapping pairs in the last step
// if the pair of bodies were already overlapping (used to compute the
// set of pair that were overlapping in the last step but are not overlapping
// in the current one anymore
lastStepOverlappingPairs.erase(indexPair);
// Add the pair into the set of overlapping pairs (if not there yet)
OverlappingPair* newPair = new (memoryPoolOverlappingPairs.allocateObject()) OverlappingPair(body1, body2, memoryPoolContacts);
pair<map<pair<luint, luint>, OverlappingPair*>::iterator, bool> check = overlappingPairs.insert(make_pair(indexPair, newPair));
// If the overlapping pair was already in the set of overlapping pair
if (!check.second) {
// Delete the new pair
newPair->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject(newPair);
}
} }
// Compute a contact for GJK (and add it to the physics world)
void CollisionDetection::computeContactGJK(const ContactInfo* const contactInfo) {
// TODO : Compute PersisentContact here instead
// Create a new contact
Contact* contact = new Contact(contactInfo);
// Add the contact to the physics world
world->addConstraint(contact);
}
/* TODO : DELETE THIS
// Compute a contact for the SAT algorithm (and add it to the physics world) for two colliding bodies
// This method only works for collision between two OBB bounding volumes
void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo) {
// Extract informations from the contact info structure
const OBB* const obb1 = dynamic_cast<const OBB* const>(contactInfo->body1->getNarrowBoundingVolume());
const OBB* const obb2 = dynamic_cast<const OBB* const>(contactInfo->body2->getNarrowBoundingVolume());
Vector3D normal = contactInfo->normal;
double penetrationDepth = contactInfo->penetrationDepth;
const vector<Vector3D> obb1ExtremePoints = obb1->getExtremeVertices(normal);
const vector<Vector3D> obb2ExtremePoints = obb2->getExtremeVertices(normal.getOpposite());
unsigned int nbVerticesExtremeOBB1 = obb1ExtremePoints.size();
unsigned int nbVerticesExtremeOBB2 = obb2ExtremePoints.size();
assert(nbVerticesExtremeOBB1==1 || nbVerticesExtremeOBB1==2 || nbVerticesExtremeOBB1==4);
assert(nbVerticesExtremeOBB2==1 || nbVerticesExtremeOBB2==2 || nbVerticesExtremeOBB2==4);
assert(approxEqual(normal.length(), 1.0));
// If it's a Vertex-Something contact
if (nbVerticesExtremeOBB1 == 1) {
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb1ExtremePoints));
}
else if(nbVerticesExtremeOBB2 == 1) { // If its a Vertex-Something contact
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb2ExtremePoints));
}
else if (nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-edge contact
// Compute the two vectors of the segment lines
Vector3D d1 = obb1ExtremePoints[1] - obb1ExtremePoints[0];
Vector3D d2 = obb2ExtremePoints[1] - obb2ExtremePoints[0];
double alpha, beta;
vector<Vector3D> contactSet;
// If the two edges are parallel
if (d1.isParallelWith(d2)) {
Vector3D contactPointA;
Vector3D contactPointB;
// Compute the intersection between the two edges
computeParallelSegmentsIntersection(obb1ExtremePoints[0], obb1ExtremePoints[1], obb2ExtremePoints[0], obb2ExtremePoints[1],
contactPointA, contactPointB);
// Add the two contact points in the contact set
contactSet.push_back(contactPointA);
contactSet.push_back(contactPointB);
}
else { // If the two edges are not parallel
// Compute the closest two points between the two line segments
closestPointsBetweenTwoLines(obb1ExtremePoints[0], d1, obb2ExtremePoints[0], d2, &alpha, &beta);
Vector3D pointA = obb1ExtremePoints[0] + d1 * alpha;
Vector3D pointB = obb2ExtremePoints[0] + d2 * beta;
// Compute the contact point as halfway between the 2 closest points
Vector3D contactPoint = 0.5 * (pointA + pointB);
// Add the contact point into the contact set
contactSet.push_back(contactPoint);
}
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, contactSet));
}
else if(nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 4) { // If it's an edge-face contact
// Compute the projection of the edge of OBB1 onto the same plane of the face of OBB2
vector<Vector3D> edge = projectPointsOntoPlane(obb1ExtremePoints, obb2ExtremePoints[0], normal);
// Clip the edge of OBB1 using the face of OBB2
vector<Vector3D> clippedEdge = clipSegmentWithRectangleInPlane(edge, obb2ExtremePoints);
// TODO : Correct this bug
// The following code is to correct a bug when the projected "edge" is not inside the clip rectangle
// of obb1ExtremePoints. Therefore, we compute the nearest two points that are on the rectangle.
if (clippedEdge.size() != 2) {
edge.clear();
edge.push_back(computeNearestPointOnRectangle(edge[0], obb2ExtremePoints));
edge.push_back(computeNearestPointOnRectangle(edge[1], obb2ExtremePoints));
clippedEdge = clipSegmentWithRectangleInPlane(edge, obb2ExtremePoints);
}
// Move the clipped edge halway between the edge of OBB1 and the face of OBB2
clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal.getOpposite());
assert(clippedEdge.size() == 2);
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge));
}
else if(nbVerticesExtremeOBB1 == 4 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-face contact
// Compute the projection of the edge of OBB2 onto the same plane of the face of OBB1
vector<Vector3D> edge = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal);
// Clip the edge of OBB2 using the face of OBB1
vector<Vector3D> clippedEdge = clipSegmentWithRectangleInPlane(edge, obb1ExtremePoints);
// TODO : Correct this bug
// The following code is to correct a bug when the projected "edge" is not inside the clip rectangle
// of obb1ExtremePoints. Therefore, we compute the nearest two points that are on the rectangle.
if (clippedEdge.size() != 2) {
edge.clear();
edge.push_back(computeNearestPointOnRectangle(edge[0], obb1ExtremePoints));
edge.push_back(computeNearestPointOnRectangle(edge[1], obb1ExtremePoints));
clippedEdge = clipSegmentWithRectangleInPlane(edge, obb1ExtremePoints);
}
// Move the clipped edge halfway between the face of OBB1 and the edge of OBB2
clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal);
assert(clippedEdge.size() == 2);
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge));
}
else { // If it's a face-face contact
// Compute the projection of the face vertices of OBB2 onto the plane of the face of OBB1
vector<Vector3D> faceOBB2 = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal);
// Clip the face of OBB2 using the face of OBB1
vector<Vector3D> clippedFace = clipPolygonWithRectangleInPlane(faceOBB2, obb1ExtremePoints);
// Move the clipped face halfway between the face of OBB1 and the face of OBB2
clippedFace = movePoints(clippedFace, penetrationDepth/2.0 * normal);
assert(clippedFace.size() >= 3);
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace));
}
}
*/

53
src/collision/CollisionDetection.h
View File

@ -26,45 +26,66 @@
#define COLLISION_DETECTION_H #define COLLISION_DETECTION_H
// Libraries // Libraries
#include "BroadPhaseAlgorithm.h"
#include "NarrowPhaseAlgorithm.h"
#include "../body/Body.h" #include "../body/Body.h"
#include "OverlappingPair.h"
#include "../engine/PhysicsWorld.h" #include "../engine/PhysicsWorld.h"
#include "../memory/MemoryPool.h"
#include "ContactInfo.h" #include "ContactInfo.h"
#include <vector> #include <vector>
#include <map>
#include <set>
#include <utility>
// TODO : Add a broadphase and a narrowphase folder in the src/collision/ folder
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
// Declarations
class BroadPhaseAlgorithm;
class NarrowPhaseAlgorithm;
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class CollisionDetection : Class CollisionDetection :
This class computes the collision detection algorithms. We first This class computes the collision detection algorithms. We first
perfom a broad-phase algorithm to know wich pairs of bodies can perfom a broad-phase algorithm to know which pairs of bodies can
collide and then we run a narrow-phase algorithm to compute the collide and then we run a narrow-phase algorithm to compute the
collision contacts between bodies. collision contacts between bodies.
------------------------------------------------------------------- -------------------------------------------------------------------
*/ */
class CollisionDetection { class CollisionDetection {
private : private :
PhysicsWorld* world; // Pointer to the physics world PhysicsWorld* world; // Pointer to the physics world
std::vector<std::pair<Body*, Body*> > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) std::map<std::pair<luint, luint>, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies
std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase) std::set<std::pair<luint, luint> > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm std::set<std::pair<luint, luint> > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
MemoryPool<Contact> memoryPoolContacts; // Memory pool for the contacts
MemoryPool<OverlappingPair> memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs
void computeBroadPhase(); // Compute the broad-phase collision detection void computeBroadPhase(); // Compute the broad-phase collision detection
void computeNarrowPhase(); // Compute the narrow-phase collision detection bool computeNarrowPhase(); // Compute the narrow-phase collision detection
void computeAllContacts(); // Compute all the contacts from the collision info list
//void computeContactSAT(const ContactInfo* const contactInfo); // Compute a contact for SAT (and add it to the physics world) for two colliding bodies
void computeContactGJK(const ContactInfo* const contactInfo); // Compute a contact for GJK (and add it to the physics world)
public : public :
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor ~CollisionDetection(); // Destructor
bool computeCollisionDetection(); // Compute the collision detection OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null
bool computeCollisionDetection(); // Compute the collision detection
void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair
}; };
// Return an overlapping pair of bodies according to the given bodies ID
// The method returns null if the pair of bodies is not overlapping
inline OverlappingPair* CollisionDetection::getOverlappingPair(luint body1ID, luint body2ID) {
std::pair<luint, luint> pair = (body1ID < body2ID) ? std::make_pair(body1ID, body2ID) : std::make_pair(body2ID, body1ID);
if (overlappingPairs.count(pair) == 1) {
return overlappingPairs[pair];
}
return NULL;
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

13
src/collision/GJK/GJKAlgorithm.cpp
View File

@ -38,7 +38,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
GJKAlgorithm::GJKAlgorithm() { GJKAlgorithm::GJKAlgorithm(CollisionDetection& collisionDetection)
:NarrowPhaseAlgorithm(collisionDetection) {
} }
@ -58,7 +59,8 @@ GJKAlgorithm::~GJKAlgorithm() {
// origin, they we give that simplex polytope to the EPA algorithm which will compute // origin, they we give that simplex polytope to the EPA algorithm which will compute
// the correct penetration depth and contact points between the enlarged objects. // the correct penetration depth and contact points between the enlarged objects.
bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform1, bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2, ContactInfo*& contactInfo) { const Shape* shape2, const Transform& transform2,
ContactInfo*& contactInfo) {
assert(shape1 != shape2); assert(shape1 != shape2);
@ -86,10 +88,9 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
// Get the last point V (last separating axis) // Get the previous point V (last cached separating axis)
// TODO : Implement frame coherence. For each pair of body, store OverlappingPair* overlappingPair = collisionDetection.getOverlappingPair(body1->getID(), body2->getID());
// the last separating axis and use it to initialize the v vector Vector3 v = (overlappingPair) ? overlappingPair->getCachedSeparatingAxis() : Vector3(1.0, 1.0, 1.0);
Vector3 v(1.0, 1.0, 1.0);
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
double distSquare = DBL_MAX; double distSquare = DBL_MAX;

4
src/collision/GJK/GJKAlgorithm.h
View File

@ -66,8 +66,8 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects
public : public :
GJKAlgorithm(); // Constructor GJKAlgorithm(CollisionDetection& collisionDetection); // Constructor
~GJKAlgorithm(); // Destructor ~GJKAlgorithm(); // Destructor
virtual bool testCollision(const Shape* shape1, const Transform& transform1, virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2, const Shape* shape2, const Transform& transform2,

3
src/collision/NarrowPhaseAlgorithm.cpp
View File

@ -29,7 +29,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() { NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(CollisionDetection& collisionDetection)
:collisionDetection(collisionDetection) {
} }

8
src/collision/NarrowPhaseAlgorithm.h
View File

@ -28,6 +28,7 @@
// Libraries // Libraries
#include "../body/Body.h" #include "../body/Body.h"
#include "ContactInfo.h" #include "ContactInfo.h"
#include "CollisionDetection.h"
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
@ -41,11 +42,12 @@ namespace reactphysics3d {
------------------------------------------------------------------- -------------------------------------------------------------------
*/ */
class NarrowPhaseAlgorithm { class NarrowPhaseAlgorithm {
private : protected :
CollisionDetection& collisionDetection; // Reference to the collision detection object
public : public :
NarrowPhaseAlgorithm(); // Constructor NarrowPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const Shape* shape1, const Transform& transform1, virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2, const Shape* shape2, const Transform& transform2,

58
src/collision/NoBroadPhaseAlgorithm.cpp
View File

@ -24,12 +24,15 @@
// Libraries // Libraries
#include "NoBroadPhaseAlgorithm.h" #include "NoBroadPhaseAlgorithm.h"
#include "CollisionDetection.h"
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
using namespace std;
// Constructor // Constructor
NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm() { NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection)
: BroadPhaseAlgorithm(collisionDetection) {
} }
@ -38,37 +41,30 @@ NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
} }
// Compute the possible collision pairs of bodies // Compute the possible collision pairs of bodies. This broad-phase algorithm
// The arguments "addedBodies" and "removedBodies" are respectively the set // doesn't do anything
// of bodies that have been added and removed since the last broad-phase
// computation. Before the call, the argument "possibleCollisionPairs"
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument. This broad-phase
// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set
// must contains all the possible pairs of bodies
void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs) {
// Add the new bodies
for (std::vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
bodies.push_back(*it);
}
// Remove the bodies to be removed void NoBroadPhaseAlgorithm::computePossibleCollisionPairs() {
for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) { // For each pair of bodies
bodies.erase(std::find(bodies.begin(), bodies.end(), *it)); for (vector<Body*>::iterator it1 = bodies.begin(); it1 != bodies.end(); it1++) {
} for (vector<Body*>::iterator it2 = it1+1; it2 != bodies.end(); it2++) {
collisionDetection.broadPhaseNotifyOverlappingPair(*it1, *it2);
// If the set of bodies have been changed
if (addedBodies.size() + removedBodies.size() > 0) {
// Recompute all the possible pairs of bodies
possibleCollisionPairs.clear();
for (std::vector<Body*>::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) {
for (std::vector<Body*>::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) {
if (*it1 != *it2) {
possibleCollisionPairs.push_back(std::make_pair(*it1, *it2));
}
}
} }
} }
} }
// Notify the broad-phase algorithm about new bodies in the physics world
void NoBroadPhaseAlgorithm::notifyAddedBodies(vector<Body*> addedBodies) {
// Add the new bodies
for (vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
bodies.push_back(*it);
}
}
// Notify the broad-phase algorithm about removed bodies in the physics world
void NoBroadPhaseAlgorithm::notifyRemovedBodies(vector<Body*> removedBodies) {
// Remove the bodies to be removed
for (vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
}
}

9
src/collision/NoBroadPhaseAlgorithm.h
View File

@ -44,11 +44,12 @@ class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
std::vector<Body*> bodies; // All bodies of the engine std::vector<Body*> bodies; // All bodies of the engine
public : public :
NoBroadPhaseAlgorithm(); // Constructor NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~NoBroadPhaseAlgorithm(); // Destructor virtual ~NoBroadPhaseAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies, virtual void computePossibleCollisionPairs(); // Compute the possible collision pairs of bodies
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies virtual void notifyAddedBodies(std::vector<Body*> bodies); // Notify the broad-phase algorithm about new bodies in the physics world
virtual void notifyRemovedBodies(std::vector<Body*> bodies); // Notify the broad-phase algorithm about removed bodies in the physics world
}; };
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

40
src/collision/OverlappingPair.cpp Normal file
View File

@ -0,0 +1,40 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2011 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "OverlappingPair.h"
using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(Body* body1, Body* body2, MemoryPool<Contact>& memoryPoolContacts)
: body1(body1), body2(body2), contactsCache(body1, body2, memoryPoolContacts), cachedSeparatingAxis(1.0, 1.0, 1.0) {
}
// Destructor
OverlappingPair::~OverlappingPair() {
// TODO : MAYBE DELETE THE CONTACTS HERE
}

102
src/collision/OverlappingPair.h Normal file
View File

@ -0,0 +1,102 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2011 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef OVERLAPPING_PAIR_H
#define OVERLAPPING_PAIR_H
// Libraries
#include "../engine/PersistentContactCache.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class OverlappingPair :
This class represents a pair of two bodies that are overlapping
during the broad-phase collision detection. It is created when
the two bodies start to overlap and is destroy when they do not
overlap anymore. This class contains the cache with all the
current contacts between the bodies.
-------------------------------------------------------------------
*/
class OverlappingPair {
private:
Body* const body1; // Pointer to the first body of the contact
Body* const body2; // Pointer to the second body of the contact
PersistentContactCache contactsCache; // Persistent contact cache
Vector3 cachedSeparatingAxis; // Cached previous separating axis
public:
OverlappingPair(Body* body1, Body* body2, MemoryPool<Contact>& memoryPoolContacts); // Constructor
~OverlappingPair(); // Destructor
Body* const getBody1() const; // Return the pointer to first body
Body* const getBody2() const; // Return the pointer to second body
void addContact(Contact* contact); // Add a contact to the contact cache
void update(); // Update the contact cache
Vector3 getCachedSeparatingAxis() const; // Return the cached separating axis
uint getNbContacts() const; // Return the number of contacts in the cache
Contact* getContact(uint index) const; // Return a contact of the cache
};
// Return the pointer to first body
inline Body* const OverlappingPair::getBody1() const {
return body1;
}
// Return the pointer to second body
inline Body* const OverlappingPair::getBody2() const {
return body2;
}
// Add a contact to the contact cache
inline void OverlappingPair::addContact(Contact* contact) {
contactsCache.addContact(contact);
}
// Update the contact cache
inline void OverlappingPair::update() {
contactsCache.update(body1->getTransform(), body2->getTransform());
}
// Return the cached separating axis
inline Vector3 OverlappingPair::getCachedSeparatingAxis() const {
return cachedSeparatingAxis;
}
// Return the number of contacts in the cache
inline uint OverlappingPair::getNbContacts() const {
return contactsCache.getNbContacts();
}
// Return a contact of the cache
inline Contact* OverlappingPair::getContact(uint index) const {
return contactsCache.getContact(index);
}
} // End of the ReactPhysics3D namespace
#endif

41
src/collision/SAPAlgorithm.cpp
View File

@ -24,6 +24,7 @@
// Libraries // Libraries
#include "SAPAlgorithm.h" #include "SAPAlgorithm.h"
#include "CollisionDetection.h"
#include <algorithm> #include <algorithm>
// Namespaces // Namespaces
@ -34,7 +35,8 @@ using namespace std;
unsigned short int SAPAlgorithm::sortAxis = 0; unsigned short int SAPAlgorithm::sortAxis = 0;
// Constructor // Constructor
SAPAlgorithm::SAPAlgorithm() { SAPAlgorithm::SAPAlgorithm(CollisionDetection& collisionDetection)
:BroadPhaseAlgorithm(collisionDetection) {
} }
@ -43,8 +45,9 @@ SAPAlgorithm::~SAPAlgorithm() {
} }
// Remove the AABB representation of a given set of bodies from the sortedAABBs set // Notify the broad-phase algorithm about new bodies in the physics world
void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) { // This method removes the AABB representation of a given set of bodies from the sortedAABBs set
void SAPAlgorithm::notifyRemovedBodies(vector<Body*> bodies) {
vector<const AABB*>::iterator elemToRemove; vector<const AABB*>::iterator elemToRemove;
const AABB* aabb; const AABB* aabb;
@ -58,8 +61,9 @@ void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
} }
} }
// Add the AABB representation of a given body in the sortedAABBs set // Notify the broad-phase algorithm about new bodies in the physics world
void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) { // This method adds the AABB representation of a given body in the sortedAABBs set
void SAPAlgorithm::notifyAddedBodies(vector<Body*> bodies) {
const AABB* aabb; const AABB* aabb;
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) { for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
@ -70,32 +74,21 @@ void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) {
} }
} }
// Compute the possible collision pairs of bodies // This method computes the possible collision pairs of bodies and notify
// The arguments "addedBodies" and "removedBodies" are respectively the set // the collision detection object about overlapping pairs using the
// of bodies that have been added and removed since the last broad-phase // broadPhaseNotifyOverlappingPair() method from the CollisionDetection class
// computation. Before the call, the argument "possibleCollisionPairs" void SAPAlgorithm::computePossibleCollisionPairs() {
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument.
void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vector<Body*> removedBodies,
vector<pair<Body*, Body*> >& possibleCollisionPairs) {
double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis
double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis
double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis
vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set
vector<const AABB*>::iterator it2; // Second iterator vector<const AABB*>::iterator it2; // Second iterator
Vector3 center3D; // Center of the current AABB Vector3 center3D; // Center of the current AABB
double center[3]; // Coordinates of the center of the current AABB double center[3]; // Coordinates of the center of the current AABB
int i; int i;
const Body* body; // Body pointer on the body corresponding to an AABB const Body* body; // Body pointer on the body corresponding to an AABB
uint nbAABBs = sortedAABBs.size(); // Number of AABBs uint nbAABBs = sortedAABBs.size(); // Number of AABBs
// Removed the bodies to remove
removeBodiesAABB(removedBodies);
// Add the bodies to add
addBodiesAABB(addedBodies);
// Sort the set of AABBs // Sort the set of AABBs
sort(sortedAABBs.begin(), sortedAABBs.end(), compareAABBs); sort(sortedAABBs.begin(), sortedAABBs.end(), compareAABBs);
@ -121,7 +114,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
} }
// Test collision against all possible overlapping AABBs following the current one // Test collision against all possible overlapping AABBs following the current one
for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) { for (it2 = it + 1; it2 != sortedAABBs.end(); it2++) {
// Stop when the tested AABBs are beyond the end of the current AABB // Stop when the tested AABBs are beyond the end of the current AABB
if ((*it2)->getMinCoordinates()[sortAxis] > (*it)->getMaxCoordinates()[sortAxis]) { if ((*it2)->getMinCoordinates()[sortAxis] > (*it)->getMaxCoordinates()[sortAxis]) {
break; break;
@ -131,8 +124,8 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
// Test if both AABBs overlap // Test if both AABBs overlap
if (body->getIsCollisionEnabled() && (*it)->testCollision(*(*it2))) { if (body->getIsCollisionEnabled() && (*it)->testCollision(*(*it2))) {
// Add the current pair of AABBs into the possibleCollisionPairs set // Notify the collision detection object about this current overlapping pair of bodies
possibleCollisionPairs.push_back(make_pair((*it)->getBodyPointer(), (*it2)->getBodyPointer())); collisionDetection.broadPhaseNotifyOverlappingPair((*it)->getBodyPointer(), (*it2)->getBodyPointer());
} }
} }
} }

14
src/collision/SAPAlgorithm.h
View File

@ -29,6 +29,8 @@
#include "BroadPhaseAlgorithm.h" #include "BroadPhaseAlgorithm.h"
#include "../shapes/AABB.h" #include "../shapes/AABB.h"
// TODO : Rename this class SweepAndPruneAlgorithm
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
@ -54,15 +56,15 @@ class SAPAlgorithm : public BroadPhaseAlgorithm {
static unsigned short int sortAxis; // Current sorting axis (0 for x, 1 for y, 2 for z axis) static unsigned short int sortAxis; // Current sorting axis (0 for x, 1 for y, 2 for z axis)
static bool compareAABBs(const AABB* a, const AABB* b); // Static method that compare two AABBs (in order to sort them) static bool compareAABBs(const AABB* a, const AABB* b); // Static method that compare two AABBs (in order to sort them)
void removeBodiesAABB(std::vector<Body*> bodies); // Remove the AABB representation of a given set of bodies from the sortedAABBs set
void addBodiesAABB(std::vector<Body*> bodies); // Add the AABB representation of a given set of bodies in the sortedAABBs set
public : public :
SAPAlgorithm(); // Constructor SAPAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~SAPAlgorithm(); // Destructor virtual ~SAPAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies, virtual void computePossibleCollisionPairs(); // Compute the possible collision pairs of bodies
std::vector<std::pair<Body*, Body*> >& possibleCollisionPairs); // Compute the possible collision pairs of bodies virtual void notifyAddedBodies(std::vector<Body*> bodies); // Notify the broad-phase algorithm about new bodies in the physics world
virtual void notifyRemovedBodies(std::vector<Body*> bodies); // Notify the broad-phase algorithm about removed bodies in the physics world
}; };
// Static method that compare two AABBs. This method will be used to compare to AABBs // Static method that compare two AABBs. This method will be used to compare to AABBs

7
src/constants.h
View File

@ -31,6 +31,7 @@
// Type definitions // Type definitions
typedef unsigned int uint; typedef unsigned int uint;
typedef long unsigned int luint;
// Mathematical constants // Mathematical constants
const double EPSILON = 1.0e-10; // Epsilon value const double EPSILON = 1.0e-10; // Epsilon value
@ -47,13 +48,13 @@ const double DEFAULT_TIMESTEP = 0.002;
const double OBJECT_MARGIN = 0.04; // Object margin for collision detection const double OBJECT_MARGIN = 0.04; // Object margin for collision detection
// Contact constants // Contact constants
const double FRICTION_COEFFICIENT = 1.0; // Friction coefficient const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient
const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the
// penetration depth in order to calculate the correct impulse for the contact // penetration depth in order to calculate the correct impulse for the contact
const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact
// TODO : Change this number const int NB_MAX_CONTACTS = 100000; // Maximum number of contacts (for memory pool allocation)
const int NB_MAX_CONTACTS = 10000; // Maximum number of contacts (for memory pool allocation) const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation)
// Constraint solver constants // Constraint solver constants
const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem

4
src/constraint/Constraint.cpp
View File

@ -32,6 +32,10 @@ using namespace reactphysics3d;
Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active) Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active)
:body1(body1), body2(body2), active(active), nbConstraints(nbConstraints) { :body1(body1), body2(body2), active(active), nbConstraints(nbConstraints) {
// Initialize the cached lambda values
for (int i=0; i<nbConstraints; i++) {
cachedLambdas.push_back(0.0);
}
} }
// Destructor // Destructor

25
src/constraint/Constraint.h
View File

@ -42,10 +42,11 @@ namespace reactphysics3d {
*/ */
class Constraint { class Constraint {
protected : protected :
Body* const body1; // Pointer to the first body of the constraint Body* const body1; // Pointer to the first body of the constraint
Body* const body2; // Pointer to the second body of the constraint Body* const body2; // Pointer to the second body of the constraint
bool active; // True if the constraint is active bool active; // True if the constraint is active
uint nbConstraints; // Number mathematical constraints associated with this Constraint uint nbConstraints; // Number mathematical constraints associated with this Constraint
std::vector<double> cachedLambdas; // Cached lambda values of each mathematical constraint for more precise initializaton of LCP solver
public : public :
Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active); // Constructor // Constructor Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active); // Constructor // Constructor
@ -58,6 +59,8 @@ class Constraint {
virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const=0; // Compute the upperbounds values for all the mathematical constraints virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const=0; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, Vector& errorValues) const=0; // Compute the error values for all the mathematical constraints virtual void computeErrorValue(int noConstraint, Vector& errorValues) const=0; // Compute the error values for all the mathematical constraints
unsigned int getNbConstraints() const; // Return the number of mathematical constraints // Return the number of auxiliary constraints unsigned int getNbConstraints() const; // Return the number of mathematical constraints // Return the number of auxiliary constraints
double getCachedLambda(int index) const; // Get one cached lambda value
void setCachedLambda(int index, double lambda); // Set on cached lambda value
}; };
// Return the reference to the body 1 // Return the reference to the body 1
@ -80,6 +83,20 @@ inline uint Constraint::getNbConstraints() const {
return nbConstraints; return nbConstraints;
} }
// Get one previous lambda value
inline double Constraint::getCachedLambda(int index) const {
assert(index >= 0 && index < nbConstraints);
return cachedLambdas[index];
}
// Set on cached lambda value
inline void Constraint::setCachedLambda(int index, double lambda) {
assert(index >= 0 && index < nbConstraints);
cachedLambdas[index] = lambda;
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

1
src/constraint/Contact.cpp
View File

@ -153,6 +153,7 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
assert(body1); assert(body1);
assert(body2); assert(body2);
// TODO : Do we need this casting anymore
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1); RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2); RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);

10
src/constraint/Contact.h
View File

@ -31,9 +31,11 @@
#include "../body/RigidBody.h" #include "../body/RigidBody.h"
#include "../constants.h" #include "../constants.h"
#include "../mathematics/mathematics.h" #include "../mathematics/mathematics.h"
#include "../memory/MemoryPool.h"
#include <new>
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
#include <GL/freeglut.h> #include <GLUT/glut.h>
#include <GL/gl.h> #include <OpenGL/gl.h>
#endif #endif
// ReactPhysics3D namespace // ReactPhysics3D namespace
@ -82,6 +84,9 @@ class Contact : public Constraint {
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
void draw() const; // Draw the contact (for debugging) void draw() const; // Draw the contact (for debugging)
#endif #endif
//void* operator new(size_t, MemoryPool<Contact>& pool) throw(std::bad_alloc); // Overloaded new operator for customized memory allocation
//void operator delete(void* p, MemoryPool<Contact>& pool); // Overloaded delete operator for customized memory allocation
}; };
// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane // Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane
@ -143,6 +148,7 @@ inline double Contact::getPenetrationDepth() const {
return penetrationDepth; return penetrationDepth;
} }
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
// TODO : Delete this (Used to debug collision detection) // TODO : Delete this (Used to debug collision detection)
inline void Contact::draw() const { inline void Contact::draw() const {

58
src/engine/ConstraintSolver.cpp
View File

@ -183,12 +183,9 @@ void ConstraintSolver::freeMemory(bool freeBodiesMemory) {
// Notice that all the active constraints should have been evaluated first // Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices() { void ConstraintSolver::fillInMatrices() {
Constraint* constraint; Constraint* constraint;
Contact* contact;
ContactCachingInfo* contactInfo;
// For each active constraint // For each active constraint
int noConstraint = 0; int noConstraint = 0;
//uint nbAuxConstraints = 0;
for (int c=0; c<activeConstraints.size(); c++) { for (int c=0; c<activeConstraints.size(); c++) {
@ -196,7 +193,6 @@ void ConstraintSolver::fillInMatrices() {
// Fill in the J_sp matrix // Fill in the J_sp matrix
constraint->computeJacobian(noConstraint, J_sp); constraint->computeJacobian(noConstraint, J_sp);
//constraint->computeJacobian(noConstraint, J_sp);
// Fill in the body mapping matrix // Fill in the body mapping matrix
for(int i=0; i<constraint->getNbConstraints(); i++) { for(int i=0; i<constraint->getNbConstraints(); i++) {
@ -211,20 +207,9 @@ void ConstraintSolver::fillInMatrices() {
// Fill in the error vector // Fill in the error vector
constraint->computeErrorValue(noConstraint, errorValues); constraint->computeErrorValue(noConstraint, errorValues);
// Set the init lambda values // Get the cached lambda values of the constraint
contact = dynamic_cast<Contact*>(constraint);
contactInfo = NULL;
if (contact) {
// Get the lambda init value from the cache if exists
contactInfo = contactCache.getContactCachingInfo(contact);
}
for (int i=0; i<constraint->getNbConstraints(); i++) { for (int i=0; i<constraint->getNbConstraints(); i++) {
if (contactInfo) { // If the last lambda init value is in the cache lambdaInit.setValue(noConstraint + i, constraint->getCachedLambda(i));
lambdaInit.setValue(noConstraint + i, contactInfo->lambdas[i]);
}
else { // The las lambda init value was not in the cache
lambdaInit.setValue(noConstraint + i, 0.0);
}
} }
noConstraint += constraint->getNbConstraints(); noConstraint += constraint->getNbConstraints();
@ -336,43 +321,20 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
} }
} }
// Clear and Fill in the contact cache with the new lambda values // Cache the lambda values in order to reuse them in the next step
void ConstraintSolver::updateContactCache() { // to initialize the lambda vector
Contact* contact; void ConstraintSolver::cacheLambda() {
ContactCachingInfo* contactInfo;
int index;
// Clear the contact cache
contactCache.clear();
// For each active constraint // For each active constraint
int noConstraint = 0; int noConstraint = 0;
for (int c=0; c<activeConstraints.size(); c++) { for (int c=0; c<activeConstraints.size(); c++) {
index = noConstraint;
// If it's a contact constraint // For each constraint of the contact
contact = dynamic_cast<Contact*>(activeConstraints.at(c)); for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) {
if (contact) { // Get the lambda value that have just been computed
activeConstraints[c]->setCachedLambda(i, lambda.getValue(noConstraint + i));
// Get all the contact points of the contact
vector<Vector3> points;
vector<double> lambdas;
points.push_back(contact->getWorldPointOnBody1());
points.push_back(contact->getWorldPointOnBody2());
// For each constraint of the contact
for (int i=0; i<contact->getNbConstraints(); i++) {
// Get the lambda value that have just been computed
lambdas.push_back(lambda.getValue(noConstraint + i));
}
// Create a new ContactCachingInfo
contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), points, lambdas);
// Add it to the contact cache
contactCache.addContactCachingInfo(contactInfo);
} }
noConstraint += activeConstraints.at(c)->getNbConstraints(); noConstraint += activeConstraints[c]->getNbConstraints();
} }
} }

24
src/engine/ConstraintSolver.h
View File

@ -33,6 +33,7 @@
#include "PhysicsWorld.h" #include "PhysicsWorld.h"
#include <map> #include <map>
#include <set> #include <set>
#include <sys/time.h> // TODO : Remove this
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -47,7 +48,6 @@ class ConstraintSolver {
protected: protected:
PhysicsWorld* physicsWorld; // Reference to the physics world PhysicsWorld* physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver LCPSolver* lcpSolver; // LCP Solver
ContactCache contactCache; // Contact cache
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
uint nbConstraints; // Total number of constraints (with the auxiliary constraints) uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
uint nbBodies; // Current number of bodies in the physics world uint nbBodies; // Current number of bodies in the physics world
@ -90,7 +90,7 @@ class ConstraintSolver {
void computeVectorB(double dt); // Compute the vector b void computeVectorB(double dt); // Compute the vector b
void computeMatrixB_sp(); // Compute the matrix B_sp void computeMatrixB_sp(); // Compute the matrix B_sp
void computeVectorVconstraint(double dt); // Compute the vector V2 void computeVectorVconstraint(double dt); // Compute the vector V2
void updateContactCache(); // Clear and Fill in the contact cache with the new lambda values void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
void freeMemory(bool freeBodiesMemory); // Free some memory previously allocated for the constraint solver void freeMemory(bool freeBodiesMemory); // Free some memory previously allocated for the constraint solver
public: public:
@ -135,6 +135,14 @@ inline void ConstraintSolver::cleanup() {
// Solve the current LCP problem // Solve the current LCP problem
inline void ConstraintSolver::solve(double dt) { inline void ConstraintSolver::solve(double dt) {
// TODO : Remove the following timing code
timeval timeValueStart;
timeval timeValueEnd;
std::cout << "------ START (Constraint Solver) -----" << std::endl;
gettimeofday(&timeValueStart, NULL);
// Allocate memory for the matrices // Allocate memory for the matrices
initialize(); initialize();
@ -151,11 +159,19 @@ inline void ConstraintSolver::solve(double dt) {
lcpSolver->setLambdaInit(lambdaInit); lcpSolver->setLambdaInit(lambdaInit);
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda); lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// Update the contact chaching informations // Cache the lambda values in order to use them in the next step
updateContactCache(); cacheLambda();
// Compute the vector Vconstraint // Compute the vector Vconstraint
computeVectorVconstraint(dt); computeVectorVconstraint(dt);
// TODO : Remove the following timing code
std::cout << "NB constraints : " << nbConstraints << std::endl;
gettimeofday(&timeValueEnd, NULL);
long double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.tv_usec);
long double endTime = timeValueEnd.tv_sec * 1000000.0 + (timeValueEnd.tv_usec);
std::cout << "------ END (Constraint Solver) => (" << "time = " << endTime - startTime << " micro sec)-----" << std::endl;
} }
} // End of ReactPhysics3D namespace } // End of ReactPhysics3D namespace

2
src/engine/ContactCache.h
View File

@ -32,6 +32,8 @@
#include "ContactCachingInfo.h" #include "ContactCachingInfo.h"
#include "../constraint/Contact.h" #include "../constraint/Contact.h"
// TODO : Remove this class
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {

2
src/engine/ContactCachingInfo.h
View File

@ -25,6 +25,8 @@
#ifndef CONTACT_CACHING_INFO_H #ifndef CONTACT_CACHING_INFO_H
#define CONTACT_CACHING_INFO_H #define CONTACT_CACHING_INFO_H
// TODO : Remove this class
// Libraries // Libraries
#include "../shapes/BoxShape.h" #include "../shapes/BoxShape.h"

37
src/engine/PersistentContactCache.cpp
View File

@ -28,8 +28,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2) PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts)
: body1(body1), body2(body2), nbContacts(0) { : body1(body1), body2(body2), nbContacts(0), memoryPoolContacts(memoryPoolContacts) {
} }
@ -40,8 +40,22 @@ PersistentContactCache::~PersistentContactCache() {
// Add a contact in the cache // Add a contact in the cache
void PersistentContactCache::addContact(Contact* contact) { void PersistentContactCache::addContact(Contact* contact) {
int indexNewContact = nbContacts; int indexNewContact = nbContacts;
// For contact already in the cache
for (uint i=0; i<nbContacts; 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(), contacts[i]->getLocalPointOnBody1())) {
// Delete the new contact
contact->Contact::~Contact();
memoryPoolContacts.freeObject(contact);
return;
}
}
// If the contact cache is full // If the contact cache is full
if (nbContacts == MAX_CONTACTS_IN_CACHE) { if (nbContacts == MAX_CONTACTS_IN_CACHE) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
@ -60,7 +74,10 @@ void PersistentContactCache::removeContact(int index) {
assert(index >= 0 && index < nbContacts); assert(index >= 0 && index < nbContacts);
assert(nbContacts > 0); assert(nbContacts > 0);
delete contacts[index]; // Call the destructor explicitly and tell the memory pool that
// the corresponding memory block is now free
contacts[index]->Contact::~Contact();
memoryPoolContacts.freeObject(contacts[index]);
// If we don't remove the last index // If we don't remove the last index
if (index < nbContacts - 1) { if (index < nbContacts - 1) {
@ -77,15 +94,19 @@ void PersistentContactCache::removeContact(int index) {
// the contacts with a too large distance between the contact points in the plane orthogonal to the // the contacts with a too large distance between the contact points in the plane orthogonal to the
// contact normal // contact normal
void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) { void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) {
if (nbContacts == 0) return;
// Update the world coordinates and penetration depth of the contacts in the cache // Update the world coordinates and penetration depth of the contacts in the cache
for (uint i=0; i<nbContacts; i++) { for (int i=0; i<nbContacts; i++) {
contacts[i]->setWorldPointOnBody1(transform1 * contacts[i]->getLocalPointOnBody1()); contacts[i]->setWorldPointOnBody1(transform1 * contacts[i]->getLocalPointOnBody1());
contacts[i]->setWorldPointOnBody2(transform2 * contacts[i]->getLocalPointOnBody2()); contacts[i]->setWorldPointOnBody2(transform2 * contacts[i]->getLocalPointOnBody2());
contacts[i]->setPenetrationDepth((contacts[i]->getWorldPointOnBody1() - contacts[i]->getWorldPointOnBody2()).dot(contacts[i]->getNormal())); contacts[i]->setPenetrationDepth((contacts[i]->getWorldPointOnBody1() - contacts[i]->getWorldPointOnBody2()).dot(contacts[i]->getNormal()));
} }
// Remove the contacts that don't represent very well the persistent contact // Remove the contacts that don't represent very well the persistent contact
for (uint i=nbContacts-1; i>=0; i--) { for (int i=nbContacts-1; i>=0; i--) {
assert(i>= 0 && i < nbContacts);
// Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore) // Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore)
if (contacts[i]->getPenetrationDepth() <= 0.0) { if (contacts[i]->getPenetrationDepth() <= 0.0) {
removeContact(i); removeContact(i);
@ -195,7 +216,11 @@ int PersistentContactCache::getMaxArea(double area0, double area1, double area2,
// Clear the cache // Clear the cache
void PersistentContactCache::clear() { void PersistentContactCache::clear() {
for (uint i=0; i<nbContacts; i++) { for (uint i=0; i<nbContacts; i++) {
delete contacts[i];
// Call the destructor explicitly and tell the memory pool that
// the corresponding memory block is now free
contacts[i]->Contact::~Contact();
memoryPoolContacts.freeObject(contacts[i]);
} }
nbContacts = 0; nbContacts = 0;
} }

35
src/engine/PersistentContactCache.h
View File

@ -36,6 +36,8 @@ namespace reactphysics3d {
// Constants // Constants
const uint MAX_CONTACTS_IN_CACHE = 4; // Maximum number of contacts in the persistent cache const uint MAX_CONTACTS_IN_CACHE = 4; // Maximum number of contacts in the persistent cache
// TODO : Move this class in collision/ folder
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class PersistentContactCache : Class PersistentContactCache :
This class represents a cache of at most 4 contact points between This class represents a cache of at most 4 contact points between
@ -55,20 +57,43 @@ class PersistentContactCache {
Body* const body2; // Pointer to the second body Body* const body2; // Pointer to the second body
Contact* contacts[MAX_CONTACTS_IN_CACHE]; // Contacts in the cache Contact* contacts[MAX_CONTACTS_IN_CACHE]; // Contacts in the cache
uint nbContacts; // Number of contacts in the cache uint nbContacts; // Number of contacts in the cache
MemoryPool<Contact>& memoryPoolContacts; // Reference to the memory pool with the contacts
int getMaxArea(double area0, double area1, double area2, double area3) const; // Return the index of maximum area int getMaxArea(double area0, double area1, double area2, double area3) const; // Return the index of maximum area
int getIndexOfDeepestPenetration(Contact* newContact) const; // Return the index of the contact with the larger penetration depth int getIndexOfDeepestPenetration(Contact* newContact) const; // Return the index of the contact with the larger penetration depth
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; // Return the index that will be removed int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; // Return the index that will be removed
void removeContact(int index); // Remove a contact from the cache void removeContact(int index); // Remove a contact from the cache
bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; // Return true if two vectors are approximatively equal
public: public:
PersistentContactCache(Body* const body1, Body* const body2); // Constructor PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts); // Constructor
~PersistentContactCache(); // Destructor ~PersistentContactCache(); // Destructor
void addContact(Contact* contact); // Add a contact void addContact(Contact* contact); // Add a contact
void update(const Transform& transform1, const Transform& transform2); // Update the contact cache void update(const Transform& transform1, const Transform& transform2); // Update the contact cache
void clear(); // Clear the cache void clear(); // Clear the cache
uint getNbContacts() const; // Return the number of contacts in the cache
Contact* getContact(uint index) const; // Return a contact of the cache
}; };
// Return the number of contacts in the cache
inline uint PersistentContactCache::getNbContacts() const {
return nbContacts;
}
// Return a contact of the cache
inline Contact* PersistentContactCache::getContact(uint index) const {
assert(index >= 0 && index < nbContacts);
return contacts[index];
}
// Return true if two vectors are approximatively equal
inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, const Vector3& vector2) const {
const double epsilon = 0.1;
return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
approxEqual(vector1.getZ(), vector2.getZ(), epsilon));
}
} }
#endif #endif

1
src/engine/PhysicsEngine.cpp
View File

@ -57,6 +57,7 @@ void PhysicsEngine::update() {
while(timer.isPossibleToTakeStep()) { while(timer.isPossibleToTakeStep()) {
existCollision = false; existCollision = false;
// Compute the collision detection // Compute the collision detection
if (collisionDetection.computeCollisionDetection()) { if (collisionDetection.computeCollisionDetection()) {
existCollision = true; existCollision = true;

6
src/engine/PhysicsEngine.h
View File

@ -61,6 +61,7 @@ public :
void start(); // Start the physics simulation void start(); // Start the physics simulation
void stop(); // Stop the physics simulation void stop(); // Stop the physics simulation
void update(); // Update the physics simulation void update(); // Update the physics simulation
CollisionDetection& getCollisionDetection(); // TODO : DELETE THIS METHOD
}; };
// --- Inline functions --- // // --- Inline functions --- //
@ -74,6 +75,11 @@ inline void PhysicsEngine::stop() {
timer.stop(); timer.stop();
} }
// TODO : DELETE THIS METHOD
inline CollisionDetection& PhysicsEngine::getCollisionDetection() {
return collisionDetection;
}
} }
#endif #endif

35
src/engine/PhysicsWorld.cpp
View File

@ -24,6 +24,7 @@
// Libraries // Libraries
#include "PhysicsWorld.h" #include "PhysicsWorld.h"
#include "PhysicsEngine.h"
#include <algorithm> #include <algorithm>
// Namespaces // Namespaces
@ -32,17 +33,37 @@ using namespace std;
// Constructor // Constructor
PhysicsWorld::PhysicsWorld(const Vector3& gravity) PhysicsWorld::PhysicsWorld(const Vector3& gravity)
: gravity(gravity), isGravityOn(true) { : gravity(gravity), isGravityOn(true), currentBodyID(0) {
} }
// Destructor // Destructor
PhysicsWorld::~PhysicsWorld() { PhysicsWorld::~PhysicsWorld() {
// Remove and free the memory of all constraints
removeAllConstraints(); }
// Create a rigid body into the physics world
RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape) {
// TODO : Use Memory Pool memory allocation for the rigid body instead
// Create the rigid body
RigidBody* rigidBody = new RigidBody(transform, mass, inertiaTensorLocal, shape, currentBodyID);
currentBodyID++;
// Add the rigid body to the physics world and return it
addBody(rigidBody);
return rigidBody;
}
// Destroy a rigid body
void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) {
removeBody(rigidBody);
delete rigidBody;
} }
// Remove all collision contacts constraints // Remove all collision contacts constraints
// TODO : This method should be in the collision detection class
void PhysicsWorld::removeAllContactConstraints() { void PhysicsWorld::removeAllContactConstraints() {
// For all constraints // For all constraints
for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ) { for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ) {
@ -52,8 +73,7 @@ void PhysicsWorld::removeAllContactConstraints() {
// If the constraint is a contact // If the constraint is a contact
if (contact) { if (contact) {
// Delete the contact // Remove it from the constraints of the physics world
delete (*it);
it = constraints.erase(it); it = constraints.erase(it);
} }
else { else {
@ -62,11 +82,8 @@ void PhysicsWorld::removeAllContactConstraints() {
} }
} }
// Remove all constraints in the physics world and also delete them (free their memory) // Remove all constraints in the physics world
void PhysicsWorld::removeAllConstraints() { void PhysicsWorld::removeAllConstraints() {
for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ++it) {
delete *it;
}
constraints.clear(); constraints.clear();
} }

53
src/engine/PhysicsWorld.h
View File

@ -32,10 +32,12 @@
#include "../body/Body.h" #include "../body/Body.h"
#include "../constraint/Constraint.h" #include "../constraint/Constraint.h"
#include "../constraint/Contact.h" #include "../constraint/Contact.h"
#include "../memory/MemoryPool.h"
// Namespace reactphysics3d // Namespace reactphysics3d
namespace reactphysics3d { namespace reactphysics3d {
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class PhysicsWorld : Class PhysicsWorld :
This class represents the world of the This class represents the world of the
@ -49,31 +51,37 @@ class PhysicsWorld {
std::vector<Body*> addedBodies; // Added bodies since last update std::vector<Body*> addedBodies; // Added bodies since last update
std::vector<Body*> removedBodies; // Removed bodies since last update std::vector<Body*> removedBodies; // Removed bodies since last update
std::vector<Constraint*> constraints; // List that contains all the current constraints std::vector<Constraint*> constraints; // List that contains all the current constraints
Vector3 gravity; // Gravity vector of the world Vector3 gravity; // Gravity vector of the world
bool isGravityOn; // True if the gravity force is on bool isGravityOn; // True if the gravity force is on
long unsigned int currentBodyID; // Current body ID
void addBody(Body* body); // Add a body to the physics world
void removeBody(Body* body); // Remove a body from the physics world
public : public :
PhysicsWorld(const Vector3& gravity); // Constructor PhysicsWorld(const Vector3& gravity); // Constructor
virtual ~PhysicsWorld(); // Destructor virtual ~PhysicsWorld(); // Destructor
void addBody(Body* body); // Add a body to the physics world RigidBody* createRigidBody(const Transform& transform, double mass,
void removeBody(Body const* const body); // Remove a body from the physics world const Matrix3x3& inertiaTensorLocal, Shape* shape); // Create a rigid body into the physics world
void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets void destroyRigidBody(RigidBody* rigidBody); // Destroy a rigid body
Vector3 getGravity() const; // Return the gravity vector of the world void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets
bool getIsGravityOn() const; // Return if the gravity is on Vector3 getGravity() const; // Return the gravity vector of the world
void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute bool getIsGravityOn() const; // Return if the gravity is on
void addConstraint(Constraint* constraint); // Add a constraint void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
void removeConstraint(Constraint* constraint); // Remove a constraint void addConstraint(Constraint* constraint); // Add a constraint
void removeAllContactConstraints(); // Remove all collision contacts constraints void removeConstraint(Constraint* constraint); // Remove a constraint
void removeAllConstraints(); // Remove all constraints and delete them (free their memory) void removeAllContactConstraints(); // Remove all collision contacts constraints
std::vector<Constraint*>::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list void removeAllConstraints(); // Remove all constraints and delete them (free their memory)
std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list std::vector<Constraint*>::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list
std::vector<Body*>::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list
std::vector<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world std::vector<Body*>::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world
std::vector<Body*>& getAddedBodies(); // Return the added bodies since last update of the physics engine std::vector<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world
std::vector<Body*>& getRemovedBodies(); // Retrun the removed bodies since last update of the physics engine std::vector<Body*>& getAddedBodies(); // Return the added bodies since last update of the physics engine
std::vector<Body*>& getRemovedBodies(); // Retrun the removed bodies since last update of the physics engine
}; };
// Add a body to the physics world // Add a body to the physics world
inline void PhysicsWorld::addBody(Body* body) { inline void PhysicsWorld::addBody(Body* body) {
std::vector<Body*>::iterator it; std::vector<Body*>::iterator it;
@ -92,16 +100,19 @@ inline void PhysicsWorld::addBody(Body* body) {
} }
// Remove a body from the physics world // Remove a body from the physics world
inline void PhysicsWorld::removeBody(Body const* const body) { inline void PhysicsWorld::removeBody(Body* body) {
std::vector<Body*>::iterator it; std::vector<Body*>::iterator it;
assert(body); assert(body);
it = std::find(bodies.begin(), bodies.end(), body); it = std::find(bodies.begin(), bodies.end(), body);
assert(*it == body); assert(*it == body);
// Remove the body
bodies.erase(it); bodies.erase(it);
addedBodies.erase(it);
it = std::find(addedBodies.begin(), addedBodies.end(), body);
if (it != addedBodies.end()) {
addedBodies.erase(it);
}
removedBodies.push_back(*it); removedBodies.push_back(*it);
} }

1
src/mathematics/Vector3.h
View File

@ -162,6 +162,7 @@ inline bool Vector3::isParallelWith(const Vector3& vector) const {
return approxEqual(std::abs(scalarProd), length() * vector.length()); return approxEqual(std::abs(scalarProd), length() * vector.length());
} }
// Return the axis with the minimal value // Return the axis with the minimal value
inline int Vector3::getMinAxis() const { inline int Vector3::getMinAxis() const {
return (values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2)); return (values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2));

4
src/mathematics/mathematics_functions.h
View File

@ -35,9 +35,9 @@ namespace reactphysics3d {
// function to test if two real numbers are (almost) equal // function to test if two real numbers are (almost) equal
// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] // We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
inline bool approxEqual(double a, double b) { inline bool approxEqual(double a, double b, double epsilon = EPSILON) {
double difference = a - b; double difference = a - b;
return (difference < EPSILON_TEST && difference > -EPSILON_TEST); return (difference < epsilon && difference > -epsilon);
} }
} // End of ReactPhysics3D namespace } // End of ReactPhysics3D namespace

124
src/memory/MemoryPool.cpp
View File

@ -1,124 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2011 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "MemoryPool.h"
#include <cassert>
using namespace reactphysics3d;
// Constructor
// Allocate a large block of memory in order to contain
// a given number of object of the template type T
template<typename T>
MemoryPool<T>::MemoryPool(uint nbObjectsToAllocate) throw(std::bad_alloc) {
pMemoryBlock = NULL;
pAllocatedLinkedList = NULL;
pFreeLinkedList = NULL;
// Compute the total memory size that need to be allocated
memorySize = nbObjectsToAllocate * (sizeof(struct Unit) + sizeof(T));
// Allocate the whole memory block
pMemoryBlock = malloc(memorySize);
// Check that the allocation didn't fail
if (!pMemoryBlock) throw std::bad_alloc();
// For each allocated memory unit
for (uint i=0; i<nbObjectsToAllocate; i++) {
// Get the adress of a memory unit
struct Unit* currentUnit = (struct Unit*)( (char*)pMemoryBlock + i * (sizeof(T) + sizeof(struct Unit)) );
currentUnit->pPrevious = NULL;
currentUnit->pNext = pFreeLinkedList;
if (pFreeLinkedList) {
pFreeLinkedList->pPrevious = currentUnit;
}
pFreeLinkedList = currentUnit;
}
}
// Destructor
// Deallocate the block of memory that has been allocated previously
template<typename T>
MemoryPool<T>::~MemoryPool() {
// Release the whole memory block
free(pMemoryBlock);
}
// Return a pointer to an memory allocated location to store a new object
// This method does not allocate memory because it has already been done at the
// beginning but it returns a pointer to a location in the allocated block of
// memory where a new object can be stored
template<typename T>
void* MemoryPool<T>::allocateObject() {
// If no memory unit is available in the current allocated memory block
if (!pFreeLinkedList) {
// TODO : REALLOCATE MEMORY HERE
assert(false);
}
assert(pFreeLinkedList);
struct Unit* currentUnit = pFreeLinkedList;
pFreeLinkedList = currentUnit->pNext;
if (pFreeLinkedList) {
pFreeLinkedList->pPrevious = NULL;
}
currentUnit->pNext = pAllocatedLinkedList;
if (pAllocatedLinkedList) {
pAllocatedLinkedList->pPrevious = currentUnit;
}
pAllocatedLinkedList = currentUnit;
// Return a pointer to the allocated memory unit
return (void*)((char*)currentUnit + sizeof(struct Unit));
}
// Tell the pool that an object doesn't need to be store in the pool anymore
// This method does not deallocate memory because it will be done only at the
// end but it informs the memory pool that an object that was stored in the heap
// does not need to be stored anymore and therefore we can use the corresponding
// location in the pool for another object
template<typename T>
void MemoryPool<T>::freeObject(void* pObjectToFree) {
// The pointer location must be inside the memory block
assert(pMemoryBlock<pObjectToFree && pObjectToFree<(void*)((char*)pMemoryBlock + memorySize));
struct Unit* currentUnit = (struct Unit*)((char*)pObjectToFree - sizeof(struct Unit));
pAllocatedLinkedList = currentUnit->pNext;
if (pAllocatedLinkedList) {
pAllocatedLinkedList->pPrevious = NULL;
}
currentUnit->pNext = pFreeLinkedList;
if (pFreeLinkedList) {
pFreeLinkedList->pPrevious = currentUnit;
}
pFreeLinkedList = currentUnit;
}

4
src/shapes/AABB.cpp
View File

@ -26,8 +26,8 @@
#include "AABB.h" #include "AABB.h"
#include <cassert> #include <cassert>
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
#include <GL/freeglut.h> #include <GLUT/glut.h>
#include <GL/gl.h> #include <OpenGL/gl.h>
#endif #endif
using namespace reactphysics3d; using namespace reactphysics3d;

4
src/shapes/BoxShape.cpp
View File

@ -28,8 +28,8 @@
#include <cassert> #include <cassert>
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
#include <GL/freeglut.h> // TODO : Remove this in the final version #include <GLUT/glut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif #endif
using namespace reactphysics3d; using namespace reactphysics3d;

4
src/shapes/ConeShape.cpp
View File

@ -27,8 +27,8 @@
#include "ConeShape.h" #include "ConeShape.h"
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
#include <GL/freeglut.h> // TODO : Remove this in the final version #include <GLUT/glut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif #endif
using namespace reactphysics3d; using namespace reactphysics3d;

4
src/shapes/CylinderShape.cpp
View File

@ -25,8 +25,8 @@
// Libraries // Libraries
#include "CylinderShape.h" #include "CylinderShape.h"
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
#include <GL/freeglut.h> // TODO : Remove this in the final version #include <GLUT/glut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif #endif
using namespace reactphysics3d; using namespace reactphysics3d;

4
src/shapes/SphereShape.cpp
View File

@ -28,8 +28,8 @@
#include <cassert> #include <cassert>
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
#include <GL/freeglut.h> // TODO : Remove this in the final version #include <GLUT/glut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif #endif
using namespace reactphysics3d; using namespace reactphysics3d;