diff --git a/src/body/Body.cpp b/src/body/Body.cpp
index fba08e3a..f6d34c5f 100644
--- a/src/body/Body.cpp
+++ b/src/body/Body.cpp
@@ -30,8 +30,8 @@
using namespace reactphysics3d;
// Constructor
-Body::Body(const Transform& transform, Shape* shape, double mass)
- : shape(shape), transform(transform), mass(mass) {
+Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned int id)
+ : shape(shape), transform(transform), mass(mass), id(id) {
assert(mass > 0.0);
assert(shape);
diff --git a/src/body/Body.h b/src/body/Body.h
index 7ac925b4..beeea03b 100644
--- a/src/body/Body.h
+++ b/src/body/Body.h
@@ -31,6 +31,7 @@
#include "../mathematics/Transform.h"
#include "../shapes/AABB.h"
#include "../shapes/Shape.h"
+#include "../constants.h"
// Namespace reactphysics3d
namespace reactphysics3d {
@@ -52,11 +53,13 @@ class Body {
bool isMotionEnabled; // True if the body is able to move
bool isCollisionEnabled; // True if the body can collide with others bodies
AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection
+ luint id; // ID of the body
public :
- Body(const Transform& transform, Shape* shape, double mass); // Constructor
- virtual ~Body(); // Destructor
+ Body(const Transform& transform, Shape* shape, double mass, long unsigned int id); // Constructor
+ virtual ~Body(); // Destructor
+ luint getID() const; // Return the id of the body
Shape* getShape() const; // Return the collision shape
void setShape(Shape* shape); // Set the collision shape
double getMass() const; // Return the mass of the body
@@ -74,6 +77,11 @@ class Body {
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
inline Shape* Body::getShape() const {
assert(shape);
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index bf10b9c6..73f40a93 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -30,8 +30,8 @@
using namespace reactphysics3d;
// Constructor
- RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape)
- : Body(transform, shape, mass), inertiaTensorLocal(inertiaTensorLocal),
+ RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape, long unsigned id)
+ : Body(transform, shape, mass, id), inertiaTensorLocal(inertiaTensorLocal),
inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) {
restitution = 1.0;
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 9cf944f0..6be9408d 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -43,19 +43,19 @@ namespace reactphysics3d {
*/
class RigidBody : public Body {
protected :
- Vector3 linearVelocity; // Linear velocity of the body
- Vector3 angularVelocity; // Angular velocity of the body
- Vector3 externalForce; // Current external force on the body
- Vector3 externalTorque; // Current external torque on the body
- Matrix3x3 inertiaTensorLocal; // Local 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 restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
+ Vector3 linearVelocity; // Linear velocity of the body
+ Vector3 angularVelocity; // Angular velocity of the body
+ Vector3 externalForce; // Current external force on the body
+ Vector3 externalTorque; // Current external torque on the body
+ Matrix3x3 inertiaTensorLocal; // Local 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 restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
public :
- RigidBody(const Transform& transform, double mass,
- const Matrix3x3& inertiaTensorLocal, Shape* shape); // Constructor // Copy-constructor
- virtual ~RigidBody(); // Destructor
+ RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal,
+ Shape* shape, long unsigned int id); // Constructor // Copy-constructor
+ virtual ~RigidBody(); // Destructor
Vector3 getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body
diff --git a/src/collision/BroadPhaseAlgorithm.cpp b/src/collision/BroadPhaseAlgorithm.cpp
index 08253311..b16ef61e 100644
--- a/src/collision/BroadPhaseAlgorithm.cpp
+++ b/src/collision/BroadPhaseAlgorithm.cpp
@@ -29,7 +29,8 @@
using namespace reactphysics3d;
// Constructor
-BroadPhaseAlgorithm::BroadPhaseAlgorithm() {
+BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
+ :collisionDetection(collisionDetection) {
}
diff --git a/src/collision/BroadPhaseAlgorithm.h b/src/collision/BroadPhaseAlgorithm.h
index 9d88ac2b..531b5a85 100644
--- a/src/collision/BroadPhaseAlgorithm.h
+++ b/src/collision/BroadPhaseAlgorithm.h
@@ -32,6 +32,9 @@
// Namespace ReactPhysics3D
namespace reactphysics3d {
+// Declarations
+class CollisionDetection;
+
/* --------------------------------------------------------------------
Class BroadPhaseAlgorithm :
This class is an abstract class that represents an algorithm
@@ -47,13 +50,15 @@ namespace reactphysics3d {
*/
class BroadPhaseAlgorithm {
protected :
-
+ CollisionDetection& collisionDetection; // Reference to the collision detection object
+
public :
- BroadPhaseAlgorithm(); // Constructor
- virtual ~BroadPhaseAlgorithm(); // Destructor
+ BroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
+ virtual ~BroadPhaseAlgorithm(); // Destructor
- virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
- std::vector<std::pair<Body*, Body*> >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies
+ virtual void computePossibleCollisionPairs()=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
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 96c89479..c9164421 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -30,232 +30,144 @@
#include "../body/Body.h"
#include "../shapes/BoxShape.h"
#include "../body/RigidBody.h"
+#include "../constants.h"
#include <cassert>
#include <complex>
+#include <set>
+#include <utility>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
using namespace std;
// Constructor
-CollisionDetection::CollisionDetection(PhysicsWorld* world) {
- this->world = world;
-
+CollisionDetection::CollisionDetection(PhysicsWorld* 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)
- broadPhaseAlgorithm = new SAPAlgorithm();
+ broadPhaseAlgorithm = new SAPAlgorithm(*this);
// 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
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
bool CollisionDetection::computeCollisionDetection() {
-
+
world->removeAllContactConstraints();
- possibleCollisionPairs.clear();
- contactInfos.clear();
// Compute the broad-phase collision detection
computeBroadPhase();
// Compute the narrow-phase collision detection
- computeNarrowPhase();
-
- // Compute all the new contacts
- computeAllContacts();
-
+ bool collisionExists = computeNarrowPhase();
+
// Return true if at least one contact has been found
- return (contactInfos.size() > 0);
+ return collisionExists;
}
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() {
- // Clear the set of possible colliding pairs of bodies
- possibleCollisionPairs.clear();
+ // Notify the broad-phase algorithm about new and removed bodies in the physics world
+ broadPhaseAlgorithm->notifyAddedBodies(world->getAddedBodies());
+ broadPhaseAlgorithm->notifyRemovedBodies(world->getRemovedBodies());
+
+ // Clear the set of the overlapping pairs in the current step
+ currentStepOverlappingPairs.clear();
- // Compute the set of possible collision pairs of bodies
- broadPhaseAlgorithm->computePossibleCollisionPairs(world->getAddedBodies(), world->getRemovedBodies(), possibleCollisionPairs);
+ // 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
-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 (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
+ for (it = overlappingPairs.begin(); it != overlappingPairs.end(); it++) {
ContactInfo* contactInfo = NULL;
- Body* const body1 = possibleCollisionPairs.at(i).first;
- Body* const body2 = possibleCollisionPairs.at(i).second;
+ Body* const body1 = (*it).second->getBody1();
+ 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
if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(),
body2->getShape(), body2->getTransform(), contactInfo)) {
assert(contactInfo);
+ collisionExists = true;
- // Add the contact info the current list of collision informations
- contactInfos.push_back(contactInfo);
+ // Create a new contact
+ 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));
+ }
}
}
-}
-
-// 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
- computeContactGJK(contactInfo); // TODO : Call computeContactGJK() here
- }
-}
-
-// 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));
- }
+ return collisionExists;
}
-*/
+
+// Allow the broadphase to notify the collision detection about an overlapping pair
+// 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);
+ }
+}
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 461fff4e..1fa103fe 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -26,45 +26,66 @@
#define COLLISION_DETECTION_H
// Libraries
-#include "BroadPhaseAlgorithm.h"
-#include "NarrowPhaseAlgorithm.h"
#include "../body/Body.h"
+#include "OverlappingPair.h"
#include "../engine/PhysicsWorld.h"
+#include "../memory/MemoryPool.h"
#include "ContactInfo.h"
#include <vector>
+#include <map>
+#include <set>
+#include <utility>
+
+// TODO : Add a broadphase and a narrowphase folder in the src/collision/ folder
// ReactPhysics3D namespace
namespace reactphysics3d {
+// Declarations
+class BroadPhaseAlgorithm;
+class NarrowPhaseAlgorithm;
+
/* -------------------------------------------------------------------
Class CollisionDetection :
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
collision contacts between bodies.
-------------------------------------------------------------------
*/
class CollisionDetection {
private :
- PhysicsWorld* world; // Pointer to the physics world
- std::vector<std::pair<Body*, Body*> > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
- std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
- BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
-
- void computeBroadPhase(); // Compute the broad-phase collision detection
- void 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)
-
+ PhysicsWorld* world; // Pointer to the physics world
+ std::map<std::pair<luint, luint>, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies
+ std::set<std::pair<luint, luint> > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step
+ std::set<std::pair<luint, luint> > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step
+ 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
+ bool computeNarrowPhase(); // Compute the narrow-phase collision detection
+
public :
- CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
- ~CollisionDetection(); // Destructor
-
- bool computeCollisionDetection(); // Compute the collision detection
+ CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
+ ~CollisionDetection(); // Destructor
+
+ 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
#endif
diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp
index d9bfdfc9..9b3c9a15 100644
--- a/src/collision/GJK/GJKAlgorithm.cpp
+++ b/src/collision/GJK/GJKAlgorithm.cpp
@@ -38,7 +38,8 @@
using namespace reactphysics3d;
// 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
// the correct penetration depth and contact points between the enlarged objects.
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);
@@ -86,11 +88,10 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Create a simplex set
Simplex simplex;
- // Get the last point V (last separating axis)
- // TODO : Implement frame coherence. For each pair of body, store
- // the last separating axis and use it to initialize the v vector
- Vector3 v(1.0, 1.0, 1.0);
-
+ // Get the previous point V (last cached separating axis)
+ OverlappingPair* overlappingPair = collisionDetection.getOverlappingPair(body1->getID(), body2->getID());
+ Vector3 v = (overlappingPair) ? overlappingPair->getCachedSeparatingAxis() : Vector3(1.0, 1.0, 1.0);
+
// Initialize the upper bound for the square distance
double distSquare = DBL_MAX;
diff --git a/src/collision/GJK/GJKAlgorithm.h b/src/collision/GJK/GJKAlgorithm.h
index 82035e97..b79aa149 100644
--- a/src/collision/GJK/GJKAlgorithm.h
+++ b/src/collision/GJK/GJKAlgorithm.h
@@ -66,8 +66,8 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects
public :
- GJKAlgorithm(); // Constructor
- ~GJKAlgorithm(); // Destructor
+ GJKAlgorithm(CollisionDetection& collisionDetection); // Constructor
+ ~GJKAlgorithm(); // Destructor
virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
diff --git a/src/collision/NarrowPhaseAlgorithm.cpp b/src/collision/NarrowPhaseAlgorithm.cpp
index 47493c7e..8aa5a803 100644
--- a/src/collision/NarrowPhaseAlgorithm.cpp
+++ b/src/collision/NarrowPhaseAlgorithm.cpp
@@ -29,7 +29,8 @@
using namespace reactphysics3d;
// Constructor
-NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() {
+NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(CollisionDetection& collisionDetection)
+ :collisionDetection(collisionDetection) {
}
diff --git a/src/collision/NarrowPhaseAlgorithm.h b/src/collision/NarrowPhaseAlgorithm.h
index 9d48ab18..0341e7fd 100644
--- a/src/collision/NarrowPhaseAlgorithm.h
+++ b/src/collision/NarrowPhaseAlgorithm.h
@@ -28,6 +28,7 @@
// Libraries
#include "../body/Body.h"
#include "ContactInfo.h"
+#include "CollisionDetection.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
@@ -41,11 +42,12 @@ namespace reactphysics3d {
-------------------------------------------------------------------
*/
class NarrowPhaseAlgorithm {
- private :
-
+ protected :
+ CollisionDetection& collisionDetection; // Reference to the collision detection object
+
public :
- NarrowPhaseAlgorithm(); // Constructor
- virtual ~NarrowPhaseAlgorithm(); // Destructor
+ NarrowPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
+ virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
diff --git a/src/collision/NoBroadPhaseAlgorithm.cpp b/src/collision/NoBroadPhaseAlgorithm.cpp
index c2b36632..391e4ae4 100644
--- a/src/collision/NoBroadPhaseAlgorithm.cpp
+++ b/src/collision/NoBroadPhaseAlgorithm.cpp
@@ -24,12 +24,15 @@
// Libraries
#include "NoBroadPhaseAlgorithm.h"
+#include "CollisionDetection.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
+using namespace std;
// Constructor
-NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm() {
+NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection)
+ : BroadPhaseAlgorithm(collisionDetection) {
}
@@ -38,37 +41,30 @@ NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
}
-// Compute the possible collision pairs of bodies
-// The arguments "addedBodies" and "removedBodies" are respectively the set
-// 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);
- }
+// Compute the possible collision pairs of bodies. This broad-phase algorithm
+// doesn't do anything
- // Remove the bodies to be removed
- for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
- bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
- }
-
- // 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));
- }
- }
+void NoBroadPhaseAlgorithm::computePossibleCollisionPairs() {
+ // For each pair of bodies
+ 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);
}
}
}
+
+// 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));
+ }
+}
diff --git a/src/collision/NoBroadPhaseAlgorithm.h b/src/collision/NoBroadPhaseAlgorithm.h
index 74b6f4c5..cd4e5b09 100644
--- a/src/collision/NoBroadPhaseAlgorithm.h
+++ b/src/collision/NoBroadPhaseAlgorithm.h
@@ -44,11 +44,12 @@ class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
std::vector<Body*> bodies; // All bodies of the engine
public :
- NoBroadPhaseAlgorithm(); // Constructor
- virtual ~NoBroadPhaseAlgorithm(); // Destructor
+ NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
+ virtual ~NoBroadPhaseAlgorithm(); // Destructor
- virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
- std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
+ virtual void computePossibleCollisionPairs(); // 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
diff --git a/src/collision/OverlappingPair.cpp b/src/collision/OverlappingPair.cpp
new file mode 100644
index 00000000..9a4ca3c6
--- /dev/null
+++ b/src/collision/OverlappingPair.cpp
@@ -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
+}
diff --git a/src/collision/OverlappingPair.h b/src/collision/OverlappingPair.h
new file mode 100644
index 00000000..cf2ccf25
--- /dev/null
+++ b/src/collision/OverlappingPair.h
@@ -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
+
diff --git a/src/collision/SAPAlgorithm.cpp b/src/collision/SAPAlgorithm.cpp
index 4408c735..f531f9c0 100644
--- a/src/collision/SAPAlgorithm.cpp
+++ b/src/collision/SAPAlgorithm.cpp
@@ -24,6 +24,7 @@
// Libraries
#include "SAPAlgorithm.h"
+#include "CollisionDetection.h"
#include <algorithm>
// Namespaces
@@ -34,7 +35,8 @@ using namespace std;
unsigned short int SAPAlgorithm::sortAxis = 0;
// 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
-void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
+// Notify the broad-phase algorithm about new bodies in the physics world
+// 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;
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
-void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) {
+// Notify the broad-phase algorithm about new bodies in the physics world
+// This method adds the AABB representation of a given body in the sortedAABBs set
+void SAPAlgorithm::notifyAddedBodies(vector<Body*> bodies) {
const AABB* aabb;
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
-// The arguments "addedBodies" and "removedBodies" are respectively the set
-// 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.
-void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vector<Body*> removedBodies,
- vector<pair<Body*, Body*> >& possibleCollisionPairs) {
+// This method computes the possible collision pairs of bodies and notify
+// the collision detection object about overlapping pairs using the
+// broadPhaseNotifyOverlappingPair() method from the CollisionDetection class
+void SAPAlgorithm::computePossibleCollisionPairs() {
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 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 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
int i;
const Body* body; // Body pointer on the body corresponding to an AABB
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(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
- 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
if ((*it2)->getMinCoordinates()[sortAxis] > (*it)->getMaxCoordinates()[sortAxis]) {
break;
@@ -131,8 +124,8 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
// Test if both AABBs overlap
if (body->getIsCollisionEnabled() && (*it)->testCollision(*(*it2))) {
- // Add the current pair of AABBs into the possibleCollisionPairs set
- possibleCollisionPairs.push_back(make_pair((*it)->getBodyPointer(), (*it2)->getBodyPointer()));
+ // Notify the collision detection object about this current overlapping pair of bodies
+ collisionDetection.broadPhaseNotifyOverlappingPair((*it)->getBodyPointer(), (*it2)->getBodyPointer());
}
}
}
diff --git a/src/collision/SAPAlgorithm.h b/src/collision/SAPAlgorithm.h
index 462b9f51..83d0ae8d 100644
--- a/src/collision/SAPAlgorithm.h
+++ b/src/collision/SAPAlgorithm.h
@@ -29,6 +29,8 @@
#include "BroadPhaseAlgorithm.h"
#include "../shapes/AABB.h"
+// TODO : Rename this class SweepAndPruneAlgorithm
+
// 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 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 :
- SAPAlgorithm(); // Constructor
- virtual ~SAPAlgorithm(); // Destructor
+ SAPAlgorithm(CollisionDetection& collisionDetection); // Constructor
+ virtual ~SAPAlgorithm(); // Destructor
- virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
- std::vector<std::pair<Body*, Body*> >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
+ virtual void computePossibleCollisionPairs(); // 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
diff --git a/src/constants.h b/src/constants.h
index c6489bf7..5ed8771c 100644
--- a/src/constants.h
+++ b/src/constants.h
@@ -31,6 +31,7 @@
// Type definitions
typedef unsigned int uint;
+typedef long unsigned int luint;
// Mathematical constants
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
// 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
// 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
-// TODO : Change this number
-const int NB_MAX_CONTACTS = 10000; // Maximum number of contacts (for memory pool allocation)
+const int NB_MAX_CONTACTS = 100000; // 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
const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp
index 76c6e03f..a2776ddd 100644
--- a/src/constraint/Constraint.cpp
+++ b/src/constraint/Constraint.cpp
@@ -31,7 +31,11 @@ using namespace reactphysics3d;
// Constructor
Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active)
: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
diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h
index eacf6b1e..071b9cca 100644
--- a/src/constraint/Constraint.h
+++ b/src/constraint/Constraint.h
@@ -42,10 +42,11 @@ namespace reactphysics3d {
*/
class Constraint {
protected :
- Body* const body1; // Pointer to the first body of the constraint
- Body* const body2; // Pointer to the second body of the constraint
- bool active; // True if the constraint is active
- uint nbConstraints; // Number mathematical constraints associated with this Constraint
+ Body* const body1; // Pointer to the first body of the constraint
+ Body* const body2; // Pointer to the second body of the constraint
+ bool active; // True if the constraint is active
+ 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 :
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 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
+ 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
@@ -80,6 +83,20 @@ inline uint Constraint::getNbConstraints() const {
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
#endif
diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp
index ee0ebbd7..e458a185 100644
--- a/src/constraint/Contact.cpp
+++ b/src/constraint/Contact.cpp
@@ -153,6 +153,7 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
assert(body1);
assert(body2);
+ // TODO : Do we need this casting anymore
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h
index b9501edc..d00348df 100644
--- a/src/constraint/Contact.h
+++ b/src/constraint/Contact.h
@@ -31,9 +31,11 @@
#include "../body/RigidBody.h"
#include "../constants.h"
#include "../mathematics/mathematics.h"
+#include "../memory/MemoryPool.h"
+#include <new>
#ifdef VISUAL_DEBUG
- #include <GL/freeglut.h>
- #include <GL/gl.h>
+ #include <GLUT/glut.h>
+ #include <OpenGL/gl.h>
#endif
// ReactPhysics3D namespace
@@ -82,6 +84,9 @@ class Contact : public Constraint {
#ifdef VISUAL_DEBUG
void draw() const; // Draw the contact (for debugging)
#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
@@ -143,6 +148,7 @@ inline double Contact::getPenetrationDepth() const {
return penetrationDepth;
}
+
#ifdef VISUAL_DEBUG
// TODO : Delete this (Used to debug collision detection)
inline void Contact::draw() const {
diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index 78ae40f4..003ba8e7 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -183,12 +183,9 @@ void ConstraintSolver::freeMemory(bool freeBodiesMemory) {
// Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices() {
Constraint* constraint;
- Contact* contact;
- ContactCachingInfo* contactInfo;
// For each active constraint
int noConstraint = 0;
- //uint nbAuxConstraints = 0;
for (int c=0; c<activeConstraints.size(); c++) {
@@ -196,7 +193,6 @@ void ConstraintSolver::fillInMatrices() {
// Fill in the J_sp matrix
constraint->computeJacobian(noConstraint, J_sp);
- //constraint->computeJacobian(noConstraint, J_sp);
// Fill in the body mapping matrix
for(int i=0; i<constraint->getNbConstraints(); i++) {
@@ -211,20 +207,9 @@ void ConstraintSolver::fillInMatrices() {
// Fill in the error vector
constraint->computeErrorValue(noConstraint, errorValues);
- // Set the init lambda values
- contact = dynamic_cast<Contact*>(constraint);
- contactInfo = NULL;
- if (contact) {
- // Get the lambda init value from the cache if exists
- contactInfo = contactCache.getContactCachingInfo(contact);
- }
+ // Get the cached lambda values of the constraint
for (int i=0; i<constraint->getNbConstraints(); i++) {
- if (contactInfo) { // If the last lambda init value is in the cache
- lambdaInit.setValue(noConstraint + i, contactInfo->lambdas[i]);
- }
- else { // The las lambda init value was not in the cache
- lambdaInit.setValue(noConstraint + i, 0.0);
- }
+ lambdaInit.setValue(noConstraint + i, constraint->getCachedLambda(i));
}
noConstraint += constraint->getNbConstraints();
@@ -336,43 +321,20 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
}
}
-// Clear and Fill in the contact cache with the new lambda values
-void ConstraintSolver::updateContactCache() {
- Contact* contact;
- ContactCachingInfo* contactInfo;
- int index;
-
- // Clear the contact cache
- contactCache.clear();
+// Cache the lambda values in order to reuse them in the next step
+// to initialize the lambda vector
+void ConstraintSolver::cacheLambda() {
// For each active constraint
int noConstraint = 0;
for (int c=0; c<activeConstraints.size(); c++) {
- index = noConstraint;
- // If it's a contact constraint
- contact = dynamic_cast<Contact*>(activeConstraints.at(c));
- if (contact) {
-
- // 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);
+ // For each constraint of the contact
+ for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) {
+ // Get the lambda value that have just been computed
+ activeConstraints[c]->setCachedLambda(i, lambda.getValue(noConstraint + i));
}
- noConstraint += activeConstraints.at(c)->getNbConstraints();
+ noConstraint += activeConstraints[c]->getNbConstraints();
}
}
diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h
index 46c0f5c0..e0e159ab 100644
--- a/src/engine/ConstraintSolver.h
+++ b/src/engine/ConstraintSolver.h
@@ -33,6 +33,7 @@
#include "PhysicsWorld.h"
#include <map>
#include <set>
+#include <sys/time.h> // TODO : Remove this
// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -47,7 +48,6 @@ class ConstraintSolver {
protected:
PhysicsWorld* physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver
- ContactCache contactCache; // Contact cache
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
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 computeMatrixB_sp(); // Compute the matrix B_sp
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
public:
@@ -135,6 +135,14 @@ inline void ConstraintSolver::cleanup() {
// Solve the current LCP problem
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
initialize();
@@ -151,11 +159,19 @@ inline void ConstraintSolver::solve(double dt) {
lcpSolver->setLambdaInit(lambdaInit);
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
- // Update the contact chaching informations
- updateContactCache();
-
+ // Cache the lambda values in order to use them in the next step
+ cacheLambda();
+
// Compute the vector Vconstraint
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
diff --git a/src/engine/ContactCache.h b/src/engine/ContactCache.h
index 1c4cfc15..72fe9213 100644
--- a/src/engine/ContactCache.h
+++ b/src/engine/ContactCache.h
@@ -32,6 +32,8 @@
#include "ContactCachingInfo.h"
#include "../constraint/Contact.h"
+// TODO : Remove this class
+
// ReactPhysics3D namespace
namespace reactphysics3d {
diff --git a/src/engine/ContactCachingInfo.h b/src/engine/ContactCachingInfo.h
index 019080a6..3ad8b93a 100644
--- a/src/engine/ContactCachingInfo.h
+++ b/src/engine/ContactCachingInfo.h
@@ -25,6 +25,8 @@
#ifndef CONTACT_CACHING_INFO_H
#define CONTACT_CACHING_INFO_H
+// TODO : Remove this class
+
// Libraries
#include "../shapes/BoxShape.h"
diff --git a/src/engine/PersistentContactCache.cpp b/src/engine/PersistentContactCache.cpp
index 64bdce1c..71e82a5c 100644
--- a/src/engine/PersistentContactCache.cpp
+++ b/src/engine/PersistentContactCache.cpp
@@ -28,8 +28,8 @@
using namespace reactphysics3d;
// Constructor
-PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2)
- : body1(body1), body2(body2), nbContacts(0) {
+PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts)
+ : body1(body1), body2(body2), nbContacts(0), memoryPoolContacts(memoryPoolContacts) {
}
@@ -40,8 +40,22 @@ PersistentContactCache::~PersistentContactCache() {
// Add a contact in the cache
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 (nbContacts == MAX_CONTACTS_IN_CACHE) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
@@ -59,9 +73,12 @@ void PersistentContactCache::addContact(Contact* contact) {
void PersistentContactCache::removeContact(int index) {
assert(index >= 0 && index < nbContacts);
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 (index < nbContacts - 1) {
contacts[index] = contacts[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
// contact normal
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
- for (uint i=0; i<nbContacts; i++) {
+ for (int i=0; i<nbContacts; i++) {
contacts[i]->setWorldPointOnBody1(transform1 * contacts[i]->getLocalPointOnBody1());
contacts[i]->setWorldPointOnBody2(transform2 * contacts[i]->getLocalPointOnBody2());
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
- 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)
if (contacts[i]->getPenetrationDepth() <= 0.0) {
removeContact(i);
@@ -100,7 +121,7 @@ void PersistentContactCache::update(const Transform& transform1, const Transform
removeContact(i);
}
}
- }
+ }
}
// Return the index of the contact with the larger penetration depth. This
@@ -195,7 +216,11 @@ int PersistentContactCache::getMaxArea(double area0, double area1, double area2,
// Clear the cache
void PersistentContactCache::clear() {
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;
}
\ No newline at end of file
diff --git a/src/engine/PersistentContactCache.h b/src/engine/PersistentContactCache.h
index 8c2763a9..f62bede0 100644
--- a/src/engine/PersistentContactCache.h
+++ b/src/engine/PersistentContactCache.h
@@ -36,6 +36,8 @@ namespace reactphysics3d {
// Constants
const uint MAX_CONTACTS_IN_CACHE = 4; // Maximum number of contacts in the persistent cache
+// TODO : Move this class in collision/ folder
+
/* -------------------------------------------------------------------
Class PersistentContactCache :
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
Contact* contacts[MAX_CONTACTS_IN_CACHE]; // 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 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
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:
- PersistentContactCache(Body* const body1, Body* const body2); // Constructor
- ~PersistentContactCache(); // Destructor
- void addContact(Contact* contact); // Add a contact
- void update(const Transform& transform1, const Transform& transform2); // Update the contact cache
- void clear(); // Clear the cache
+ PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts); // Constructor
+ ~PersistentContactCache(); // Destructor
+ void addContact(Contact* contact); // Add a contact
+ void update(const Transform& transform1, const Transform& transform2); // Update the contact 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
diff --git a/src/engine/PhysicsEngine.cpp b/src/engine/PhysicsEngine.cpp
index 7744a9b8..d83f2999 100644
--- a/src/engine/PhysicsEngine.cpp
+++ b/src/engine/PhysicsEngine.cpp
@@ -46,7 +46,7 @@ void PhysicsEngine::update() {
bool existCollision = false;
assert(timer.getIsRunning());
-
+
// Compute the time since the last update() call and update the timer
timer.update();
@@ -57,6 +57,7 @@ void PhysicsEngine::update() {
while(timer.isPossibleToTakeStep()) {
existCollision = false;
+
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
existCollision = true;
diff --git a/src/engine/PhysicsEngine.h b/src/engine/PhysicsEngine.h
index 00690209..d461f07b 100644
--- a/src/engine/PhysicsEngine.h
+++ b/src/engine/PhysicsEngine.h
@@ -61,6 +61,7 @@ public :
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update(); // Update the physics simulation
+ CollisionDetection& getCollisionDetection(); // TODO : DELETE THIS METHOD
};
// --- Inline functions --- //
@@ -74,6 +75,11 @@ inline void PhysicsEngine::stop() {
timer.stop();
}
+// TODO : DELETE THIS METHOD
+inline CollisionDetection& PhysicsEngine::getCollisionDetection() {
+ return collisionDetection;
+}
+
}
#endif
diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp
index c6fc619b..6645d4b1 100644
--- a/src/engine/PhysicsWorld.cpp
+++ b/src/engine/PhysicsWorld.cpp
@@ -24,6 +24,7 @@
// Libraries
#include "PhysicsWorld.h"
+#include "PhysicsEngine.h"
#include <algorithm>
// Namespaces
@@ -32,17 +33,37 @@ using namespace std;
// Constructor
PhysicsWorld::PhysicsWorld(const Vector3& gravity)
- : gravity(gravity), isGravityOn(true) {
+ : gravity(gravity), isGravityOn(true), currentBodyID(0) {
}
// Destructor
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
+// TODO : This method should be in the collision detection class
void PhysicsWorld::removeAllContactConstraints() {
// For all constraints
for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ) {
@@ -52,8 +73,7 @@ void PhysicsWorld::removeAllContactConstraints() {
// If the constraint is a contact
if (contact) {
- // Delete the contact
- delete (*it);
+ // Remove it from the constraints of the physics world
it = constraints.erase(it);
}
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() {
- for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ++it) {
- delete *it;
- }
constraints.clear();
}
diff --git a/src/engine/PhysicsWorld.h b/src/engine/PhysicsWorld.h
index 33f4e283..f775fd42 100644
--- a/src/engine/PhysicsWorld.h
+++ b/src/engine/PhysicsWorld.h
@@ -32,9 +32,11 @@
#include "../body/Body.h"
#include "../constraint/Constraint.h"
#include "../constraint/Contact.h"
+#include "../memory/MemoryPool.h"
// Namespace reactphysics3d
namespace reactphysics3d {
+
/* -------------------------------------------------------------------
Class PhysicsWorld :
@@ -49,30 +51,36 @@ class PhysicsWorld {
std::vector<Body*> addedBodies; // Added bodies since last update
std::vector<Body*> removedBodies; // Removed bodies since last update
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
+ 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 :
PhysicsWorld(const Vector3& gravity); // Constructor
virtual ~PhysicsWorld(); // Destructor
- void addBody(Body* body); // Add a body to the physics world
- void removeBody(Body const* const body); // Remove a body from the physics world
- void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets
- Vector3 getGravity() const; // Return the gravity vector of the world
- bool getIsGravityOn() const; // Return if the gravity is on
- void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
- void addConstraint(Constraint* constraint); // Add a constraint
- void removeConstraint(Constraint* constraint); // Remove a constraint
- void removeAllContactConstraints(); // Remove all collision contacts constraints
- void removeAllConstraints(); // Remove all constraints and delete them (free their memory)
- std::vector<Constraint*>::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list
- std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end 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<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end 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*>& getRemovedBodies(); // Retrun the removed bodies since last update of the physics engine
+ RigidBody* createRigidBody(const Transform& transform, double mass,
+ const Matrix3x3& inertiaTensorLocal, Shape* shape); // Create a rigid body into the physics world
+ void destroyRigidBody(RigidBody* rigidBody); // Destroy a rigid body
+ void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets
+ Vector3 getGravity() const; // Return the gravity vector of the world
+ bool getIsGravityOn() const; // Return if the gravity is on
+ void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
+ void addConstraint(Constraint* constraint); // Add a constraint
+ void removeConstraint(Constraint* constraint); // Remove a constraint
+ void removeAllContactConstraints(); // Remove all collision contacts constraints
+ void removeAllConstraints(); // Remove all constraints and delete them (free their memory)
+ std::vector<Constraint*>::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list
+ std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end 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<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end 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*>& getRemovedBodies(); // Retrun the removed bodies since last update of the physics engine
};
+
// Add a body to the physics world
inline void PhysicsWorld::addBody(Body* body) {
@@ -81,7 +89,7 @@ inline void PhysicsWorld::addBody(Body* body) {
assert(body);
it = std::find(bodies.begin(), bodies.end(), body);
assert(it == bodies.end());
-
+
// The body isn't already in the bodyList, therefore we add it to the list
bodies.push_back(body);
addedBodies.push_back(body);
@@ -92,16 +100,19 @@ inline void PhysicsWorld::addBody(Body* body) {
}
// Remove a body from the physics world
-inline void PhysicsWorld::removeBody(Body const* const body) {
- std::vector<Body*>::iterator it;
-
+inline void PhysicsWorld::removeBody(Body* body) {
+ std::vector<Body*>::iterator it;
+
assert(body);
it = std::find(bodies.begin(), bodies.end(), body);
assert(*it == body);
-
- // Remove the body
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);
}
diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h
index 1fe138e0..aa9357c9 100644
--- a/src/mathematics/Vector3.h
+++ b/src/mathematics/Vector3.h
@@ -71,7 +71,7 @@ class Vector3 {
int getMinAxis() const; // Return the axis with the minimal value
int getMaxAxis() const; // Return the axis with the maximal value
bool isParallelWith(const Vector3& vector) const; // Return true if two vectors are parallel
-
+
// --- Overloaded operators --- //
bool operator== (const Vector3& vector) const; // Overloaded operator for the equality condition
bool operator!= (const Vector3& vector) const; // Overloaded operator for the is different condition
@@ -160,7 +160,8 @@ inline Vector3 Vector3::getAbsoluteVector() const {
inline bool Vector3::isParallelWith(const Vector3& vector) const {
double scalarProd = this->dot(vector);
return approxEqual(std::abs(scalarProd), length() * vector.length());
-}
+}
+
// Return the axis with the minimal value
inline int Vector3::getMinAxis() const {
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 3b71abfd..72ab3c09 100644
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -35,9 +35,9 @@ namespace reactphysics3d {
// 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]
-inline bool approxEqual(double a, double b) {
+inline bool approxEqual(double a, double b, double epsilon = EPSILON) {
double difference = a - b;
- return (difference < EPSILON_TEST && difference > -EPSILON_TEST);
+ return (difference < epsilon && difference > -epsilon);
}
} // End of ReactPhysics3D namespace
diff --git a/src/memory/MemoryPool.cpp b/src/memory/MemoryPool.cpp
deleted file mode 100644
index f2a97c9d..00000000
--- a/src/memory/MemoryPool.cpp
+++ /dev/null
@@ -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;
-}
diff --git a/src/shapes/AABB.cpp b/src/shapes/AABB.cpp
index e5375654..d722f5ae 100644
--- a/src/shapes/AABB.cpp
+++ b/src/shapes/AABB.cpp
@@ -26,8 +26,8 @@
#include "AABB.h"
#include <cassert>
#ifdef VISUAL_DEBUG
- #include <GL/freeglut.h>
- #include <GL/gl.h>
+ #include <GLUT/glut.h>
+ #include <OpenGL/gl.h>
#endif
using namespace reactphysics3d;
diff --git a/src/shapes/BoxShape.cpp b/src/shapes/BoxShape.cpp
index 427c32e3..e9252cc4 100644
--- a/src/shapes/BoxShape.cpp
+++ b/src/shapes/BoxShape.cpp
@@ -28,8 +28,8 @@
#include <cassert>
#ifdef VISUAL_DEBUG
- #include <GL/freeglut.h> // TODO : Remove this in the final version
- #include <GL/gl.h> // TODO : Remove this in the final version
+ #include <GLUT/glut.h> // TODO : Remove this in the final version
+ #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif
using namespace reactphysics3d;
diff --git a/src/shapes/ConeShape.cpp b/src/shapes/ConeShape.cpp
index 6e5410d0..d6f3b3a6 100644
--- a/src/shapes/ConeShape.cpp
+++ b/src/shapes/ConeShape.cpp
@@ -27,8 +27,8 @@
#include "ConeShape.h"
#ifdef VISUAL_DEBUG
- #include <GL/freeglut.h> // TODO : Remove this in the final version
- #include <GL/gl.h> // TODO : Remove this in the final version
+ #include <GLUT/glut.h> // TODO : Remove this in the final version
+ #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif
using namespace reactphysics3d;
diff --git a/src/shapes/CylinderShape.cpp b/src/shapes/CylinderShape.cpp
index 2a5019b1..aff9833f 100644
--- a/src/shapes/CylinderShape.cpp
+++ b/src/shapes/CylinderShape.cpp
@@ -25,8 +25,8 @@
// Libraries
#include "CylinderShape.h"
#ifdef VISUAL_DEBUG
- #include <GL/freeglut.h> // TODO : Remove this in the final version
- #include <GL/gl.h> // TODO : Remove this in the final version
+ #include <GLUT/glut.h> // TODO : Remove this in the final version
+ #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif
using namespace reactphysics3d;
diff --git a/src/shapes/SphereShape.cpp b/src/shapes/SphereShape.cpp
index 70e0b64a..01626c2d 100644
--- a/src/shapes/SphereShape.cpp
+++ b/src/shapes/SphereShape.cpp
@@ -28,8 +28,8 @@
#include <cassert>
#ifdef VISUAL_DEBUG
- #include <GL/freeglut.h> // TODO : Remove this in the final version
- #include <GL/gl.h> // TODO : Remove this in the final version
+ #include <GLUT/glut.h> // TODO : Remove this in the final version
+ #include <OpenGL/gl.h> // TODO : Remove this in the final version
#endif
using namespace reactphysics3d;