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;
// 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);

12
src/body/Body.h
View File

@ -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);

4
src/body/RigidBody.cpp
View File

@ -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;

22
src/body/RigidBody.h
View File

@ -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

3
src/collision/BroadPhaseAlgorithm.cpp
View File

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

15
src/collision/BroadPhaseAlgorithm.h
View File

@ -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

270
src/collision/CollisionDetection.cpp
View File

@ -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);
}
}

59
src/collision/CollisionDetection.h
View File

@ -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

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

@ -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;

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
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,

3
src/collision/NarrowPhaseAlgorithm.cpp
View File

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

10
src/collision/NarrowPhaseAlgorithm.h
View File

@ -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,

58
src/collision/NoBroadPhaseAlgorithm.cpp
View File

@ -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));
}
}

9
src/collision/NoBroadPhaseAlgorithm.h
View File

@ -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

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
#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());
}
}
}

14
src/collision/SAPAlgorithm.h
View File

@ -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

7
src/constants.h
View File

@ -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

6
src/constraint/Constraint.cpp
View File

@ -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

25
src/constraint/Constraint.h
View File

@ -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

1
src/constraint/Contact.cpp
View File

@ -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);

10
src/constraint/Contact.h
View File

@ -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 {

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
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();
}
}

26
src/engine/ConstraintSolver.h
View File

@ -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

2
src/engine/ContactCache.h
View File

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

2
src/engine/ContactCachingInfo.h
View File

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

45
src/engine/PersistentContactCache.cpp
View File

@ -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;
}

35
src/engine/PersistentContactCache.h
View File

@ -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

3
src/engine/PhysicsEngine.cpp
View File

@ -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;

6
src/engine/PhysicsEngine.h
View File

@ -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

35
src/engine/PhysicsWorld.cpp
View File

@ -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();
}

59
src/engine/PhysicsWorld.h
View File

@ -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);
}

5
src/mathematics/Vector3.h
View File

@ -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 {

4
src/mathematics/mathematics_functions.h
View File

@ -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

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 <cassert>
#ifdef VISUAL_DEBUG
#include <GL/freeglut.h>
#include <GL/gl.h>
#include <GLUT/glut.h>
#include <OpenGL/gl.h>
#endif
using namespace reactphysics3d;

4
src/shapes/BoxShape.cpp
View File

@ -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;

4
src/shapes/ConeShape.cpp
View File

@ -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;

4
src/shapes/CylinderShape.cpp
View File

@ -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;

4
src/shapes/SphereShape.cpp
View File

@ -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;