From 4049f6ae3bc75f41ebca405a14c43981ca3ef4c6 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Tue, 18 Oct 2011 22:03:05 +0000 Subject: [PATCH] Add persistent contact caching in collision detection git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@443 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/body/Body.cpp | 4 +- src/body/Body.h | 12 +- src/body/RigidBody.cpp | 4 +- src/body/RigidBody.h | 22 +- src/collision/BroadPhaseAlgorithm.cpp | 3 +- src/collision/BroadPhaseAlgorithm.h | 15 +- src/collision/CollisionDetection.cpp | 270 ++++++++---------------- src/collision/CollisionDetection.h | 59 ++++-- src/collision/GJK/GJKAlgorithm.cpp | 15 +- src/collision/GJK/GJKAlgorithm.h | 4 +- src/collision/NarrowPhaseAlgorithm.cpp | 3 +- src/collision/NarrowPhaseAlgorithm.h | 10 +- src/collision/NoBroadPhaseAlgorithm.cpp | 58 +++-- src/collision/NoBroadPhaseAlgorithm.h | 9 +- src/collision/OverlappingPair.cpp | 40 ++++ src/collision/OverlappingPair.h | 102 +++++++++ src/collision/SAPAlgorithm.cpp | 41 ++-- src/collision/SAPAlgorithm.h | 14 +- src/constants.h | 7 +- src/constraint/Constraint.cpp | 6 +- src/constraint/Constraint.h | 25 ++- src/constraint/Contact.cpp | 1 + src/constraint/Contact.h | 10 +- src/engine/ConstraintSolver.cpp | 58 +---- src/engine/ConstraintSolver.h | 26 ++- src/engine/ContactCache.h | 2 + src/engine/ContactCachingInfo.h | 2 + src/engine/PersistentContactCache.cpp | 45 +++- src/engine/PersistentContactCache.h | 35 ++- src/engine/PhysicsEngine.cpp | 3 +- src/engine/PhysicsEngine.h | 6 + src/engine/PhysicsWorld.cpp | 35 ++- src/engine/PhysicsWorld.h | 59 +++--- src/mathematics/Vector3.h | 5 +- src/mathematics/mathematics_functions.h | 4 +- src/memory/MemoryPool.cpp | 124 ----------- src/shapes/AABB.cpp | 4 +- src/shapes/BoxShape.cpp | 4 +- src/shapes/ConeShape.cpp | 4 +- src/shapes/CylinderShape.cpp | 4 +- src/shapes/SphereShape.cpp | 4 +- 41 files changed, 608 insertions(+), 550 deletions(-) create mode 100644 src/collision/OverlappingPair.cpp create mode 100644 src/collision/OverlappingPair.h delete mode 100644 src/memory/MemoryPool.cpp 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 addedBodies, std::vector removedBodies, - std::vector >& 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 bodies)=0; // Notify the broad-phase algorithm about new bodies in the physics world + virtual void notifyRemovedBodies(std::vector 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 #include +#include +#include // 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, 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 >::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, OverlappingPair*>::iterator it; // For each possible collision pair of bodies - for (unsigned int i=0; igetBody1(); + 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; iaddConstraint(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(contactInfo->body1->getNarrowBoundingVolume()); - const OBB* const obb2 = dynamic_cast(contactInfo->body2->getNarrowBoundingVolume()); - Vector3D normal = contactInfo->normal; - double penetrationDepth = contactInfo->penetrationDepth; - - const vector obb1ExtremePoints = obb1->getExtremeVertices(normal); - const vector 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 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 edge = projectPointsOntoPlane(obb1ExtremePoints, obb2ExtremePoints[0], normal); - - // Clip the edge of OBB1 using the face of OBB2 - vector 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 edge = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal); - - // Clip the edge of OBB2 using the face of OBB1 - vector 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 faceOBB2 = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal); - - // Clip the face of OBB2 using the face of OBB1 - vector 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 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, 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 +#include +#include +#include + +// 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 > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) - std::vector 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, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies + std::set > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step + std::set > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step + BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm + NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm + MemoryPool memoryPoolContacts; // Memory pool for the contacts + MemoryPool 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 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 addedBodies, std::vector removedBodies, - std::vector >& possibleCollisionPairs) { - // Add the new bodies - for (std::vector::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::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::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) { - for (std::vector::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::iterator it1 = bodies.begin(); it1 != bodies.end(); it1++) { + for (vector::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 addedBodies) { + // Add the new bodies + for (vector::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 removedBodies) { + // Remove the bodies to be removed + for (vector::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 bodies; // All bodies of the engine public : - NoBroadPhaseAlgorithm(); // Constructor - virtual ~NoBroadPhaseAlgorithm(); // Destructor + NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor + virtual ~NoBroadPhaseAlgorithm(); // Destructor - virtual void computePossibleCollisionPairs(std::vector addedBodies, std::vector removedBodies, - std::vector >& possibleCollisionPairs); // Compute the possible collision pairs of bodies + virtual void computePossibleCollisionPairs(); // Compute the possible collision pairs of bodies + virtual void notifyAddedBodies(std::vector bodies); // Notify the broad-phase algorithm about new bodies in the physics world + virtual void notifyRemovedBodies(std::vector 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& 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& 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 // 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 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 bodies) { vector::iterator elemToRemove; const AABB* aabb; @@ -58,8 +61,9 @@ void SAPAlgorithm::removeBodiesAABB(vector bodies) { } } -// Add the AABB representation of a given body in the sortedAABBs set -void SAPAlgorithm::addBodiesAABB(vector 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 bodies) { const AABB* aabb; for (vector::iterator it = bodies.begin(); it != bodies.end(); ++it) { @@ -70,32 +74,21 @@ void SAPAlgorithm::addBodiesAABB(vector 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 addedBodies, vector removedBodies, - vector >& 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::iterator it; // Iterator on the sortedAABBs set vector::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 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 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 bodies); // Remove the AABB representation of a given set of bodies from the sortedAABBs set - void addBodiesAABB(std::vector 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 addedBodies, std::vector removedBodies, - std::vector >& possibleCollisionPairs); // Compute the possible collision pairs of bodies + virtual void computePossibleCollisionPairs(); // Compute the possible collision pairs of bodies + virtual void notifyAddedBodies(std::vector bodies); // Notify the broad-phase algorithm about new bodies in the physics world + virtual void notifyRemovedBodies(std::vector 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 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(body1); RigidBody* rigidBody2 = dynamic_cast(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 #ifdef VISUAL_DEBUG - #include - #include + #include + #include #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& pool) throw(std::bad_alloc); // Overloaded new operator for customized memory allocation + //void operator delete(void* p, MemoryPool& 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; ccomputeJacobian(noConstraint, J_sp); - //constraint->computeJacobian(noConstraint, J_sp); // Fill in the body mapping matrix for(int i=0; igetNbConstraints(); 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(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; igetNbConstraints(); 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.at(c)); - if (contact) { - - // Get all the contact points of the contact - vector points; - vector lambdas; - points.push_back(contact->getWorldPointOnBody1()); - points.push_back(contact->getWorldPointOnBody2()); - - // For each constraint of the contact - for (int i=0; igetNbConstraints(); 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; igetNbConstraints(); 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 #include +#include // 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 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& 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; igetLocalPointOnBody1(), 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; isetWorldPointOnBody1(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; iContact::~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& 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& 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 // 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::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::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 addedBodies; // Added bodies since last update std::vector removedBodies; // Removed bodies since last update std::vector 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::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list - std::vector::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list - std::vector::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world - std::vector::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world - std::vector& getAddedBodies(); // Return the added bodies since last update of the physics engine - std::vector& 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::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list + std::vector::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list + std::vector::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world + std::vector::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world + std::vector& getAddedBodies(); // Return the added bodies since last update of the physics engine + std::vector& 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::iterator it; - +inline void PhysicsWorld::removeBody(Body* body) { + std::vector::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 - -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 -MemoryPool::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; ipPrevious = NULL; - currentUnit->pNext = pFreeLinkedList; - - if (pFreeLinkedList) { - pFreeLinkedList->pPrevious = currentUnit; - } - - pFreeLinkedList = currentUnit; - } - -} - -// Destructor -// Deallocate the block of memory that has been allocated previously -template -MemoryPool::~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 -void* MemoryPool::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 -void MemoryPool::freeObject(void* pObjectToFree) { - // The pointer location must be inside the memory block - assert(pMemoryBlockpNext; - 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 #ifdef VISUAL_DEBUG - #include - #include + #include + #include #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 #ifdef VISUAL_DEBUG - #include // TODO : Remove this in the final version - #include // TODO : Remove this in the final version + #include // TODO : Remove this in the final version + #include // 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 // TODO : Remove this in the final version - #include // TODO : Remove this in the final version + #include // TODO : Remove this in the final version + #include // 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 // TODO : Remove this in the final version - #include // TODO : Remove this in the final version + #include // TODO : Remove this in the final version + #include // 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 #ifdef VISUAL_DEBUG - #include // TODO : Remove this in the final version - #include // TODO : Remove this in the final version + #include // TODO : Remove this in the final version + #include // TODO : Remove this in the final version #endif using namespace reactphysics3d;