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

Implementation of the array-based Sweep-And-Prune broad-phase collision detection algorithm from Pierre Terdiman

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@466 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2012-07-25 21:31:57 +00:00
parent b013204b3f
commit cd47b3e617
28 changed files with 843 additions and 331 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/body/Body.cpp
View File

@ -32,7 +32,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
Body::Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id) Body::Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id)
: collider(collider), mass(mass), transform(transform), isActive(true), id(id) { : collider(collider), mass(mass), transform(transform), isActive(true), id(id), hasMoved(false) {
assert(mass > 0.0); assert(mass > 0.0);
assert(collider); assert(collider);

53
src/body/Body.h
View File

@ -40,7 +40,7 @@ namespace reactphysics3d {
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class Body : Class Body :
This class is an abstract class to represent body of the physics This class is an abstract class to represent a body of the physics
engine. engine.
------------------------------------------------------------------- -------------------------------------------------------------------
*/ */
@ -56,14 +56,17 @@ class Body {
bool isCollisionEnabled; // True if the body can collide with others bodies bool isCollisionEnabled; // True if the body can collide with others bodies
AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection
luint id; // ID of the body luint id; // ID of the body
bool hasMoved; // True if the body has moved during the last frame
public : public :
Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id); // Constructor Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id); // Constructor
virtual ~Body(); // Destructor virtual ~Body(); // Destructor
luint getID() const; // Return the id of the body luint getID() const; // Return the id of the body
bool getHasMoved() const; // Return true if the body has moved during the last frame
void setHasMoved(bool hasMoved); // Set the hasMoved variable (true if the body has moved during the last frame)
Collider* getCollider() const; // Return the collision collider Collider* getCollider() const; // Return the collision collider
void setCollider(Collider* collider); // Set the collision collider void setCollider(Collider* collider); // Set the collision collider
decimal getMass() const; // Return the mass of the body decimal getMass() const; // Return the mass of the body
void setMass(decimal mass); // Set the mass of the body void setMass(decimal mass); // Set the mass of the body
bool getIsActive() const; // Return true if the body is active bool getIsActive() const; // Return true if the body is active
@ -79,12 +82,28 @@ class Body {
void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value
void updateOldTransform(); // Update the old transform with the current one void updateOldTransform(); // Update the old transform with the current one
void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates
// Operators
bool operator<(const Body& body2) const; // Smaller than operator
bool operator>(const Body& body2) const; // Larger than operator
bool operator==(const Body& body2) const; // Equal operator
bool operator!=(const Body& body2) const; // Equal operator
}; };
// Return the id of the body // Return the id of the body
inline luint Body::getID() const { inline luint Body::getID() const {
return id; return id;
} }
// Return true if the body has moved during the last frame
inline bool Body::getHasMoved() const {
return hasMoved;
}
// Set the hasMoved variable (true if the body has moved during the last frame)
inline void Body::setHasMoved(bool hasMoved) {
this->hasMoved = hasMoved;
}
// Return the collider // Return the collider
inline Collider* Body::getCollider() const { inline Collider* Body::getCollider() const {
@ -146,6 +165,12 @@ inline const Transform& Body::getTransform() const {
// Set the current position and orientation // Set the current position and orientation
inline void Body::setTransform(const Transform& transform) { inline void Body::setTransform(const Transform& transform) {
// Check if the body has moved
if (this->transform != transform) {
hasMoved = true;
}
this->transform = transform; this->transform = transform;
} }
@ -172,10 +197,32 @@ inline void Body::updateOldTransform() {
// Update the rigid body in order to reflect a change in the body state // Update the rigid body in order to reflect a change in the body state
inline void Body::updateAABB() { inline void Body::updateAABB() {
// TODO : An AABB should not be updated every frame but only if the body has moved
// Update the AABB // Update the AABB
aabb->update(transform, collider->getLocalExtents(OBJECT_MARGIN)); aabb->update(transform, collider->getLocalExtents(OBJECT_MARGIN));
} }
// Smaller than operator
inline bool Body::operator<(const Body& body2) const {
return (id < body2.id);
}
// Larger than operator
inline bool Body::operator>(const Body& body2) const {
return (id > body2.id);
}
// Equal operator
inline bool Body::operator==(const Body& body2) const {
return (id == body2.id);
}
// Equal operator
inline bool Body::operator!=(const Body& body2) const {
return (id != body2.id);
}
} }

6
src/colliders/AABB.cpp
View File

@ -49,6 +49,12 @@ AABB::AABB() : bodyPointer(0) {
} }
// Constructor
AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates, Body* bodyPointer)
:minCoordinates(minCoordinates), maxCoordinates(maxCoordinates), bodyPointer(bodyPointer) {
}
// Constructor // Constructor
AABB::AABB(const Transform& transform, const Vector3& extents) : bodyPointer(0) { AABB::AABB(const Transform& transform, const Vector3& extents) : bodyPointer(0) {
update(transform, extents); update(transform, extents);

19
src/colliders/AABB.h
View File

@ -39,8 +39,8 @@ class Body;
Class AABB : Class AABB :
This class represents a bounding volume of type "Axis Aligned This class represents a bounding volume of type "Axis Aligned
Bounding Box". It's a box where all the edges are always aligned Bounding Box". It's a box where all the edges are always aligned
with the world coordinate system. The AABB is defined by a center with the world coordinate system. The AABB is defined by the
point and three extent size in the x,y and z directions. minimum and maximum world coordinates of the three axis.
------------------------------------------------------------------- -------------------------------------------------------------------
*/ */
class AABB { class AABB {
@ -50,13 +50,14 @@ class AABB {
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable) Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
public : public :
AABB(); // Constructor AABB(); // Constructor
AABB(const Transform& transform, const Vector3& extents); // Constructor AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates, Body* bodyPointer); // Constructor
virtual ~AABB(); // Destructor AABB(const Transform& transform, const Vector3& extents); // Constructor
virtual ~AABB(); // Destructor
Vector3 getCenter() const; // Return the center point Vector3 getCenter() const; // Return the center point
const Vector3& getMinCoordinates() const; // Return the minimum coordinates of the AABB const Vector3& getMin() const; // Return the minimum coordinates of the AABB
const Vector3& getMaxCoordinates() const; // Return the maximum coordinates of the AABB const Vector3& getMax() const; // Return the maximum coordinates of the AABB
Body* getBodyPointer() const; // Return a pointer to the owner body Body* getBodyPointer() const; // Return a pointer to the owner body
void setBodyPointer(Body* bodyPointer); // Set the body pointer void setBodyPointer(Body* bodyPointer); // Set the body pointer
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
@ -72,12 +73,12 @@ inline Vector3 AABB::getCenter() const {
} }
// Return the minimum coordinates of the AABB // Return the minimum coordinates of the AABB
inline const Vector3& AABB::getMinCoordinates() const { inline const Vector3& AABB::getMin() const {
return minCoordinates; return minCoordinates;
} }
// Return the maximum coordinates of the AABB // Return the maximum coordinates of the AABB
inline const Vector3& AABB::getMaxCoordinates() const { inline const Vector3& AABB::getMax() const {
return maxCoordinates; return maxCoordinates;
} }

94
src/collision/CollisionDetection.cpp
View File

@ -25,7 +25,9 @@
// Libraries // Libraries
#include "CollisionDetection.h" #include "CollisionDetection.h"
#include "../engine/PhysicsWorld.h"
#include "broadphase/SweepAndPruneAlgorithm.h" #include "broadphase/SweepAndPruneAlgorithm.h"
#include "broadphase/NoBroadPhaseAlgorithm.h"
#include "../body/Body.h" #include "../body/Body.h"
#include "../colliders/BoxCollider.h" #include "../colliders/BoxCollider.h"
#include "../body/RigidBody.h" #include "../body/RigidBody.h"
@ -34,6 +36,9 @@
#include <complex> #include <complex>
#include <set> #include <set>
#include <utility> #include <utility>
#include <utility>
#include <sys/time.h> // TODO : Delete this
#include <iostream> // TODO : Delete this
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
@ -42,11 +47,12 @@ using namespace std;
// Constructor // Constructor
CollisionDetection::CollisionDetection(PhysicsWorld* world) CollisionDetection::CollisionDetection(PhysicsWorld* world)
: world(world), memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS), : world(world), memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS),
memoryPoolContactInfos(NB_MAX_CONTACTS), narrowPhaseGJKAlgorithm(*this, memoryPoolContactInfos), narrowPhaseGJKAlgorithm(memoryPoolContactInfos), memoryPoolContactInfos(NB_MAX_CONTACTS),
narrowPhaseSphereVsSphereAlgorithm(*this, memoryPoolContactInfos) { narrowPhaseSphereVsSphereAlgorithm(memoryPoolContactInfos) {
// Create the broad-phase algorithm that will be used (Sweep and Prune with AABB) // Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
broadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this); broadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this);
assert(broadPhaseAlgorithm);
} }
// Destructor // Destructor
@ -57,16 +63,31 @@ CollisionDetection::~CollisionDetection() {
(*it).second->OverlappingPair::~OverlappingPair(); (*it).second->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject((*it).second); memoryPoolOverlappingPairs.freeObject((*it).second);
} }
// Delete the broad-phase algorithm
delete broadPhaseAlgorithm;
} }
// Compute the collision detection // Compute the collision detection
bool CollisionDetection::computeCollisionDetection() { bool CollisionDetection::computeCollisionDetection() {
world->removeAllContactConstraints(); world->removeAllContactConstraints();
// TODO : Remove this code
timeval timeValueStart;
timeval timeValueStop;
gettimeofday(&timeValueStart, NULL);
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// TODO : Remove this code
gettimeofday(&timeValueStop, NULL);
double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.tv_usec);
double stopTime = timeValueStop.tv_sec * 1000000.0 + (timeValueStop.tv_usec);
double deltaTime = stopTime - startTime;
printf("Broadphase time : %f micro sec \n", deltaTime);
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
bool collisionExists = computeNarrowPhase(); bool collisionExists = computeNarrowPhase();
@ -77,28 +98,20 @@ bool CollisionDetection::computeCollisionDetection() {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() { void CollisionDetection::computeBroadPhase() {
// Notify the broad-phase algorithm about new and removed bodies in the physics world // Notify the broad-phase algorithm about the bodies that have moved since last frame
broadPhaseAlgorithm->notifyAddedBodies(world->getAddedRigidBodies()); for (set<Body*>::iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
broadPhaseAlgorithm->notifyRemovedBodies(world->getRemovedRigidBodies());
// Clear the set of the overlapping pairs in the current step
currentStepOverlappingPairs.clear();
// Execute the broad-phase collision algorithm in order to compute the overlapping pairs of bodies // If the body has moved
broadPhaseAlgorithm->computePossibleCollisionPairs(); if ((*it)->getHasMoved()) {
// Notify the broad-phase that the body has moved
broadPhaseAlgorithm->updateObject(*it, *((*it)->getAABB()));
}
}
// At this point, the pairs in the set lastStepOverlappingPairs contains // TODO : DELETE THIS
// only the pairs that are not overlapping anymore. Therefore, we can std::cout << "Nb overlapping pairs : " << overlappingPairs.size() << std::endl;
// remove them from the overlapping pairs map std::cout << "Nb active pairs in pair manager : " << broadPhaseAlgorithm->getNbOverlappingPairs() << std::endl;
for (set<pair<luint, luint> >::iterator it = lastStepOverlappingPairs.begin(); it != lastStepOverlappingPairs.end(); it++) {
// Remove the overlapping pair from the memory pool
overlappingPairs[(*it)]->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject(overlappingPairs[(*it)]);
overlappingPairs.erase(*it);
}
// The current overlapping pairs become the last step overlapping pairs
lastStepOverlappingPairs = currentStepOverlappingPairs;
} }
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
@ -129,7 +142,7 @@ bool CollisionDetection::computeNarrowPhase() {
collisionExists = true; collisionExists = true;
// Create a new contact // Create a new contact
Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(body1, body2, contactInfo); Contact* contact = new (memoryPoolContacts.allocateObject()) Contact(body1, body2, contactInfo);
// Delete and remove the contact info from the memory pool // Delete and remove the contact info from the memory pool
contactInfo->ContactInfo::~ContactInfo(); contactInfo->ContactInfo::~ContactInfo();
@ -149,26 +162,18 @@ bool CollisionDetection::computeNarrowPhase() {
return collisionExists; return collisionExists;
} }
// Allow the broadphase to notify the collision detection about an overlapping pair // Allow the broadphase to notify the collision detection about an overlapping pair
// This method is called by a broad-phase collision detection algorithm // This method is called by a broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(Body* body1, Body* body2) { void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
std::cout << "New overlapping pair : id0=" << addedPair->body1->getID() << ", id=" << addedPair->body2->getID() << std::endl;
// Construct the pair of body index // Construct the pair of body index
pair<luint, luint> indexPair = body1->getID() < body2->getID() ? make_pair(body1->getID(), body2->getID()) : pair<luint, luint> indexPair = addedPair->body1->getID() < addedPair->body2->getID() ? make_pair(addedPair->body1->getID(), addedPair->body2->getID()) :
make_pair(body2->getID(), body1->getID()); make_pair(addedPair->body2->getID(), addedPair->body1->getID());
assert(indexPair.first != indexPair.second); 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) // Add the pair into the set of overlapping pairs (if not there yet)
OverlappingPair* newPair = new (memoryPoolOverlappingPairs.allocateObject()) OverlappingPair(body1, body2, memoryPoolContacts); OverlappingPair* newPair = new (memoryPoolOverlappingPairs.allocateObject()) OverlappingPair(addedPair->body1, addedPair->body2, memoryPoolContacts);
pair<map<pair<luint, luint>, OverlappingPair*>::iterator, bool> check = overlappingPairs.insert(make_pair(indexPair, newPair)); 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 the overlapping pair was already in the set of overlapping pair
@ -178,3 +183,16 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(Body* body1, Body* body
memoryPoolOverlappingPairs.freeObject(newPair); memoryPoolOverlappingPairs.freeObject(newPair);
} }
} }
// Allow the broadphase to notify the collision detection about a removed overlapping pair
void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
// Construct the pair of body index
pair<luint, luint> indexPair = removedPair->body1->getID() < removedPair->body2->getID() ? make_pair(removedPair->body1->getID(), removedPair->body2->getID()) :
make_pair(removedPair->body2->getID(), removedPair->body1->getID());
// Remove the overlapping pair from the memory pool
overlappingPairs[indexPair]->OverlappingPair::~OverlappingPair();
memoryPoolOverlappingPairs.freeObject(overlappingPairs[indexPair]);
overlappingPairs.erase(indexPair);
}

33
src/collision/CollisionDetection.h
View File

@ -29,7 +29,7 @@
// Libraries // Libraries
#include "../body/Body.h" #include "../body/Body.h"
#include "OverlappingPair.h" #include "OverlappingPair.h"
#include "../engine/PhysicsWorld.h" #include "broadphase/BroadPhaseAlgorithm.h"
#include "../memory/MemoryPool.h" #include "../memory/MemoryPool.h"
#include "narrowphase/GJK/GJKAlgorithm.h" #include "narrowphase/GJK/GJKAlgorithm.h"
#include "narrowphase/SphereVsSphereAlgorithm.h" #include "narrowphase/SphereVsSphereAlgorithm.h"
@ -38,6 +38,7 @@
#include <map> #include <map>
#include <set> #include <set>
#include <utility> #include <utility>
#include <iostream> // TODO : Delete this
// ReactPhysics3D namespace // ReactPhysics3D namespace
@ -45,7 +46,8 @@ namespace reactphysics3d {
// Declarations // Declarations
class BroadPhaseAlgorithm; class BroadPhaseAlgorithm;
class PhysicsWorld;
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class CollisionDetection : Class CollisionDetection :
This class computes the collision detection algorithms. We first This class computes the collision detection algorithms. We first
@ -70,15 +72,18 @@ class CollisionDetection {
void computeBroadPhase(); // Compute the broad-phase collision detection void computeBroadPhase(); // Compute the broad-phase collision detection
bool computeNarrowPhase(); // Compute the narrow-phase collision detection bool computeNarrowPhase(); // Compute the narrow-phase collision detection
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(Collider* collider1, NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(Collider* collider1,
Collider* collider2); // Select the narrow phase algorithm to use given two colliders Collider* collider2); // Select the narrow phase algorithm to use given two colliders
public : public :
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor ~CollisionDetection(); // Destructor
void addBody(Body* body); // Add a body to the collision detection
void removeBody(Body* body); // Remove a body from the collision detection
OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null
bool computeCollisionDetection(); // Compute the collision detection bool computeCollisionDetection(); // Compute the collision detection
void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair void broadPhaseNotifyAddedOverlappingPair(const BroadPhasePair* pair); // Allow the broadphase to notify the collision detection about a new overlapping pair
void broadPhaseNotifyRemovedOverlappingPair(const BroadPhasePair* pair); // Allow the broadphase to notify the collision detection about a removed overlapping pair
}; };
// Return an overlapping pair of bodies according to the given bodies ID // Return an overlapping pair of bodies according to the given bodies ID
@ -88,7 +93,7 @@ inline OverlappingPair* CollisionDetection::getOverlappingPair(luint body1ID, lu
if (overlappingPairs.count(pair) == 1) { if (overlappingPairs.count(pair) == 1) {
return overlappingPairs[pair]; return overlappingPairs[pair];
} }
return NULL; return 0;
} }
// Select the narrow-phase collision algorithm to use given two colliders // Select the narrow-phase collision algorithm to use given two colliders
@ -101,7 +106,21 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(Coll
else { // GJK algorithm else { // GJK algorithm
return narrowPhaseGJKAlgorithm; return narrowPhaseGJKAlgorithm;
} }
} }
// Add a body to the collision detection
inline void CollisionDetection::addBody(Body* body) {
// Add the body to the broad-phase
broadPhaseAlgorithm->addObject(body, *(body->getAABB()));
}
// Remove a body from the collision detection
inline void CollisionDetection::removeBody(Body* body) {
// Remove the body from the broad-phase
broadPhaseAlgorithm->removeObject(body);
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

2
src/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

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

34
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -28,7 +28,8 @@
// Libraries // Libraries
#include <vector> #include <vector>
#include "../../body/RigidBody.h" #include "../../body/Body.h"
#include "PairManager.h"
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
@ -51,17 +52,40 @@ class CollisionDetection;
*/ */
class BroadPhaseAlgorithm { class BroadPhaseAlgorithm {
protected : protected :
CollisionDetection& collisionDetection; // Reference to the collision detection object PairManager pairManager; // Pair manager that contains the active pairs of bodies
CollisionDetection& collisionDetection; // Reference to the collision detection object
public : public :
BroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor BroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~BroadPhaseAlgorithm(); // Destructor virtual ~BroadPhaseAlgorithm(); // Destructor
// TODO : DELETE THIS METHOD
uint getNbOverlappingPairs() const;
virtual void addObject(Body* body, const AABB& aabb)=0; // Notify the broad-phase about a new object in the world
virtual void removeObject(Body* body)=0; // Notify the broad-phase about an object that has been removed from the world
virtual void updateObject(Body* body, const AABB& aabb)=0; // Notify the broad-phase that the AABB of an object has changed
virtual void computePossibleCollisionPairs()=0; // Compute the possible collision pairs of bodies BroadPhasePair* beginOverlappingPairsPointer() const; // Return a pointer to the first active pair (used to iterate over the active pairs)
virtual void notifyAddedBodies(std::vector<RigidBody*> bodies)=0; // Notify the broad-phase algorithm about new bodies in the physics world BroadPhasePair* endOverlappingPairsPointer() const; // Return a pointer to the last active pair (used to iterate over the active pairs)
virtual void notifyRemovedBodies(std::vector<RigidBody*> bodies)=0; // Notify the broad-phase algorithm about removed bodies in the physics world
}; };
// TODO : DELETE THIS METHOD
inline uint BroadPhaseAlgorithm::getNbOverlappingPairs() const {
return pairManager.getNbOverlappingPairs();
}
// Return a pointer to the first active pair (used to iterate over the overlapping pairs)
inline BroadPhasePair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const {
return pairManager.beginOverlappingPairsPointer();
}
// Return a pointer to the last active pair (used to iterate over the overlapping pairs)
inline BroadPhasePair* BroadPhaseAlgorithm::endOverlappingPairsPointer() const {
return pairManager.endOverlappingPairsPointer();
}
} // End of reactphysics3d namespace } // End of reactphysics3d namespace
#endif #endif

28
src/collision/broadphase/NoBroadPhaseAlgorithm.cpp
View File

@ -41,31 +41,3 @@ NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm(CollisionDetection& collisionDetect
NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() { NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
} }
// Compute the possible collision pairs of bodies. This broad-phase algorithm
// doesn't do anything
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));
}
}

52
src/collision/broadphase/NoBroadPhaseAlgorithm.h
View File

@ -29,6 +29,8 @@
// Libraries // Libraries
#include "BroadPhaseAlgorithm.h" #include "BroadPhaseAlgorithm.h"
#include <algorithm> #include <algorithm>
#include <set>
#include <iostream>
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
@ -42,17 +44,57 @@ namespace reactphysics3d {
*/ */
class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm { class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
protected : protected :
std::vector<Body*> bodies; // All bodies of the engine std::set<Body*> bodies; // All bodies of the world
public : public :
NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~NoBroadPhaseAlgorithm(); // Destructor virtual ~NoBroadPhaseAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(); // Compute the possible collision pairs of bodies virtual void addObject(Body* body, const AABB& aabb); // Notify the broad-phase about a new object in the world
virtual void notifyAddedBodies(std::vector<Body*> bodies); // Notify the broad-phase algorithm about new bodies in the physics world virtual void removeObject(Body* body); // Notify the broad-phase about an object that has been removed from the world
virtual void notifyRemovedBodies(std::vector<Body*> bodies); // Notify the broad-phase algorithm about removed bodies in the physics world virtual void updateObject(Body* body, const AABB& aabb); // Notify the broad-phase that the AABB of an object has changed
}; };
// Notify the broad-phase about a new object in the world
inline void NoBroadPhaseAlgorithm::addObject(Body* body, const AABB& aabb) {
std::cout << "New body in broadphase with id=" << body->getID() << std::endl;
// For each body that is already in the world
for (std::set<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
// Add an overlapping pair with the new body
pairManager.addPair(*it, body);
}
// Add the new body into the list of bodies
bodies.insert(body);
}
// Notify the broad-phase about an object that has been removed from the world
inline void NoBroadPhaseAlgorithm::removeObject(Body* body) {
// For each body that is in the world
for (std::set<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
if ((*it)->getID() != body->getID()) {
// Remove the overlapping pair with the new body
pairManager.removePair((*it)->getID(), body->getID());
}
}
// Remove the body from the broad-phase
bodies.erase(body);
}
// Notify the broad-phase that the AABB of an object has changed
inline void NoBroadPhaseAlgorithm::updateObject(Body* body, const AABB& aabb) {
// Do nothing
return;
}
} // End of reactphysics3d namespace } // End of reactphysics3d namespace
#endif #endif

469
src/collision/broadphase/SweepAndPruneAlgorithm.cpp
View File

@ -26,120 +26,415 @@
// Libraries // Libraries
#include "SweepAndPruneAlgorithm.h" #include "SweepAndPruneAlgorithm.h"
#include "../CollisionDetection.h" #include "../CollisionDetection.h"
#include <algorithm> #include "PairManager.h"
#include <climits>
// Namespaces // Namespaces
using namespace reactphysics3d; using namespace reactphysics3d;
using namespace std; using namespace std;
// Initialize the static attributes // Constructor of AABBInt
unsigned short int SweepAndPruneAlgorithm::sortAxis = 0; AABBInt::AABBInt(const AABB& aabb) {
for (int axis=0; axis<3; axis++) {
min[axis] = encodeFloatIntoInteger(aabb.getMin()[axis]);
max[axis] = encodeFloatIntoInteger(aabb.getMax()[axis]);
}
}
// Constructor // Constructor
SweepAndPruneAlgorithm::SweepAndPruneAlgorithm(CollisionDetection& collisionDetection) SweepAndPruneAlgorithm::SweepAndPruneAlgorithm(CollisionDetection& collisionDetection)
:BroadPhaseAlgorithm(collisionDetection) { :BroadPhaseAlgorithm(collisionDetection) {
boxes = 0;
endPoints[0] = 0;
endPoints[1] = 0;
endPoints[2] = 0;
nbBoxes = 0;
nbMaxBoxes = 0;
} }
// Destructor // Destructor
SweepAndPruneAlgorithm::~SweepAndPruneAlgorithm() { SweepAndPruneAlgorithm::~SweepAndPruneAlgorithm() {
delete[] boxes;
delete[] endPoints[0];
delete[] endPoints[1];
delete[] endPoints[2];
} }
// Notify the broad-phase algorithm about new bodies in the physics world // Notify the broad-phase about a new object in the world
// This method removes the AABB representation of a given set of bodies from the sortedAABBs set // This method adds the AABB of the object ion to broad-phase
void SweepAndPruneAlgorithm::notifyRemovedBodies(vector<RigidBody*> bodies) { void SweepAndPruneAlgorithm::addObject(Body* body, const AABB& aabb) {
vector<const AABB*>::iterator elemToRemove; luint boxIndex;
const AABB* aabb;
// Removed the AABB of the bodies that have been removed
for (vector<RigidBody*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
aabb = (*it)->getAABB();
assert(aabb);
elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), aabb);
assert((*elemToRemove) == aabb);
sortedAABBs.erase(elemToRemove);
}
}
// 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 SweepAndPruneAlgorithm::notifyAddedBodies(vector<RigidBody*> bodies) {
const AABB* aabb;
for (vector<RigidBody*>::iterator it = bodies.begin(); it != bodies.end(); ++it) { // If the index of the first free box is valid (means that
aabb = 0; // there is a bucket in the middle of the array that doesn't
aabb = (*it)->getAABB(); // contain a box anymore because it has been removed)
assert(aabb); if (!freeBoxIndices.empty()) {
sortedAABBs.push_back(aabb); boxIndex = freeBoxIndices.back();
freeBoxIndices.pop_back();
} }
} else {
// If the array boxes and end-points arrays are full
// This method computes the possible collision pairs of bodies and notify if (nbBoxes == nbMaxBoxes) {
// the collision detection object about overlapping pairs using the // Resize the arrays to make them larger
// broadPhaseNotifyOverlappingPair() method from the CollisionDetection class resizeArrays();
void SweepAndPruneAlgorithm::computePossibleCollisionPairs() {
decimal variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis
decimal esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis
decimal 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
decimal 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
// Sort the set of AABBs
sort(sortedAABBs.begin(), sortedAABBs.end(), compareAABBs);
// Sweep the sorted set of AABBs
for (vector<const AABB*>::iterator it = sortedAABBs.begin(); it != sortedAABBs.end(); ++it) {
// If the collision of the AABB's corresponding body is disabled
if (!(*it)->getBodyPointer()->getIsCollisionEnabled()) {
// Go to the next AABB to test
continue;
} }
boxIndex = nbBoxes;
}
// Move the maximum limit end-point two elements further
// at the end-points array in all three axis
const luint nbSentinels = 2;
const luint indexLimitEndPoint = 2 * nbBoxes + nbSentinels - 1;
for (int axis=0; axis<3; axis++) {
EndPoint* maxLimitEndPoint = &endPoints[axis][indexLimitEndPoint];
assert(endPoints[axis][0].boxID == INVALID_INDEX && endPoints[axis][0].isMin == true);
assert(maxLimitEndPoint->boxID == INVALID_INDEX && maxLimitEndPoint->isMin == false);
EndPoint* newMaxLimitEndPoint = &endPoints[axis][indexLimitEndPoint + 2];
newMaxLimitEndPoint->setValues(maxLimitEndPoint->boxID, maxLimitEndPoint->isMin, maxLimitEndPoint->value);
}
// Create a new box
BoxAABB* box = &boxes[boxIndex];
box->body = body;
const uint minEndPointValue = encodeFloatIntoInteger(DECIMAL_LARGEST - 2.0);
const uint maxEndPointValue = encodeFloatIntoInteger(DECIMAL_LARGEST - 1.0);
for (uint axis=0; axis<3; axis++) {
box->min[axis] = indexLimitEndPoint;
box->max[axis] = indexLimitEndPoint + 1;
EndPoint* minimumEndPoint = &endPoints[axis][box->min[axis]];
minimumEndPoint->setValues(body->getID(), true, minEndPointValue);
EndPoint* maximumEndPoint = &endPoints[axis][box->max[axis]];
maximumEndPoint->setValues(body->getID(), false, maxEndPointValue);
}
// Add the body pointer to box index mapping
mapBodyToBoxIndex.insert(pair<Body*, luint>(body, boxIndex));
nbBoxes++;
// Center of the current AABB // Call the update method to put the end-points of the new AABB at the
center3D = (*it)->getCenter(); // correct position in the array. This will also create the overlapping
center[0] = center3D.getX(); // pairs in the pair manager if the new AABB is overlapping with others
center[1] = center3D.getY(); // AABBs
center[2] = center3D.getZ(); updateObject(body, aabb);
}
// Update the esperance and esperanceSquare values to compute the variance // Notify the broad-phase about a object that has been removed from the world
for (i=0; i<3; i++) { void SweepAndPruneAlgorithm::removeObject(Body* body) {
esperance[i] += center[i];
esperanceSquare[i] += center[i] * center[i];
}
// Test collision against all possible overlapping AABBs following the current one // Call the update method with an AABB that is very far away
for (it2 = it + 1; it2 != sortedAABBs.end(); it2++) { // in order to remove all overlapping pairs from the pair manager
// Stop when the tested AABBs are beyond the end of the current AABB const decimal max = DECIMAL_LARGEST;
if ((*it2)->getMinCoordinates()[sortAxis] > (*it)->getMaxCoordinates()[sortAxis]) { const Vector3 maxVector(max, max, max);
break; const AABB aabb(maxVector, maxVector, body);
updateObject(body, aabb);
// Get the corresponding box
luint boxIndex = mapBodyToBoxIndex[body];
BoxAABB* box = &boxes[boxIndex];
// Add the box index into the list of free indices
freeBoxIndices.push_back(boxIndex);
mapBodyToBoxIndex.erase(body);
nbBoxes--;
}
// Notify the broad-phase that the AABB of an object has changed
void SweepAndPruneAlgorithm::updateObject(Body* body, const AABB& aabb) {
// Compute the AABB with integer coordinates
AABBInt aabbInt(aabb);
// Get the corresponding box
luint boxIndex = mapBodyToBoxIndex[body];
BoxAABB* box = &boxes[boxIndex];
// Current axis
for (uint axis=0; axis<3; axis++) {
// Get the two others axis
const uint otherAxis1 = (1 << axis) & 3;
const uint otherAxis2 = (1 << otherAxis1) & 3;
// Get the starting end-point of the current axis
EndPoint* startEndPointsCurrentAxis = endPoints[axis];
// -------- Update the minimum end-point ------------//
EndPoint* currentMinEndPoint = &startEndPointsCurrentAxis[box->min[axis]];
assert(currentMinEndPoint->isMin);
// Get the minimum value of the AABB on the current axis
uint limit = aabbInt.min[axis];
// If the minimum value of the AABB is smaller
// than the current minimum endpoint
if (limit < currentMinEndPoint->value) {
currentMinEndPoint->value = limit;
// The minimum end-point is moving left
EndPoint savedEndPoint = *currentMinEndPoint;
luint indexEndPoint = (size_t(currentMinEndPoint) - size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
while ((--currentMinEndPoint)->value > limit) {
BoxAABB* id1 = &boxes[currentMinEndPoint->boxID];
const bool isMin = currentMinEndPoint->isMin;
// If it's a maximum end-point
if (!isMin) {
// The minimum end-point is moving to the left and
// passed a maximum end-point. Thus, the boxes start
// overlapping on the current axis. Therefore we test
// for box intersection
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
testIntersect1DSortedAABBs(*id1, aabbInt, startEndPointsCurrentAxis, axis)) {
// Add an overlapping pair to the pair manager
pairManager.addPair(body, id1->body);
}
}
id1->max[axis] = indexEndPoint--;
}
else {
id1->min[axis] = indexEndPoint--;
}
*(currentMinEndPoint+1) = *currentMinEndPoint;
} }
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
boxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
boxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
body = (*it2)->getBodyPointer(); startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
else if (limit > currentMinEndPoint->value) { // The minimum of the box has moved to the right
currentMinEndPoint->value = limit;
// The minimum en-point is moving right
EndPoint savedEndPoint = *currentMinEndPoint;
luint indexEndPoint = (size_t(currentMinEndPoint) -size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
// For each end-point between the current position of the minimum
// end-point and the new position of the minimum end-point
while ((++currentMinEndPoint)->value < limit) {
BoxAABB* id1 = &boxes[currentMinEndPoint->boxID];
const bool isMin = currentMinEndPoint->isMin;
// If it's a maximum end-point
if (!isMin) {
// The minimum end-point is moving to the right and
// passed a maximum end-point. Thus, the boxes stop
// overlapping on the current axis.
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
// Remove the pair from the pair manager
pairManager.removePair(body->getID(), id1->body->getID());
}
}
id1->max[axis] = indexEndPoint++;
}
else {
id1->min[axis] = indexEndPoint++;
}
*(currentMinEndPoint-1) = *currentMinEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
boxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
boxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
// Test if both AABBs overlap startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
if (body->getIsCollisionEnabled() && (*it)->testCollision(*(*it2))) { }
// Notify the collision detection object about this current overlapping pair of bodies }
collisionDetection.broadPhaseNotifyOverlappingPair((*it)->getBodyPointer(), (*it2)->getBodyPointer());
// ------- Update the maximum end-point ------------ //
EndPoint* currentMaxEndPoint = &startEndPointsCurrentAxis[box->max[axis]];
assert(!currentMaxEndPoint->isMin);
// Get the maximum value of the AABB on the current axis
limit = aabbInt.max[axis];
// If the new maximum value of the AABB is larger
// than the current maximum end-point value. It means
// that the AABB is moving to the right.
if (limit > currentMaxEndPoint->value) {
currentMaxEndPoint->value = limit;
EndPoint savedEndPoint = *currentMaxEndPoint;
luint indexEndPoint = (size_t(currentMaxEndPoint) -size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
while ((++currentMaxEndPoint)->value < limit) {
// Get the next end-point
BoxAABB* id1 = &boxes[currentMaxEndPoint->boxID];
const bool isMin = currentMaxEndPoint->isMin;
// If it's a maximum end-point
if (isMin) {
// The maximum end-point is moving to the right and
// passed a minimum end-point. Thus, the boxes start
// overlapping on the current axis. Therefore we test
// for box intersection
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
testIntersect1DSortedAABBs(*id1, aabbInt, startEndPointsCurrentAxis,axis)) {
// Add an overlapping pair to the pair manager
pairManager.addPair(body, id1->body);
}
}
id1->min[axis] = indexEndPoint++;
}
else {
id1->max[axis] = indexEndPoint++;
}
*(currentMaxEndPoint-1) = *currentMaxEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
boxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
boxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
else if (limit < currentMaxEndPoint->value) { // If the AABB is moving to the left
currentMaxEndPoint->value = limit;
EndPoint savedEndPoint = *currentMaxEndPoint;
luint indexEndPoint = (size_t(currentMaxEndPoint) -size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
// For each end-point between the current position of the maximum
// end-point and the new position of the maximum end-point
while ((--currentMaxEndPoint)->value > limit) {
BoxAABB* id1 = &boxes[currentMaxEndPoint->boxID];
const bool isMin = currentMaxEndPoint->isMin;
// If it's a minimum end-point
if (isMin) {
// The maximum end-point is moving to the right and
// passed a minimum end-point. Thus, the boxes stop
// overlapping on the current axis.
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
// Remove the pair from the pair manager
pairManager.removePair(body->getID(), id1->body->getID());
}
}
id1->min[axis] = indexEndPoint--;
}
else {
id1->max[axis] = indexEndPoint--;
}
*(currentMaxEndPoint+1) = *currentMaxEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
boxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
boxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
} }
} }
} }
// Compute the variance of the distribution of the AABBs on the three x,y and z axis
for (i=0; i<3; i++) {
variance[i] = esperanceSquare[i] - esperance[i] * esperance[i] / nbAABBs;
}
// Update the sorted axis according to the axis with the largest variance
sortAxis = 0;
if (variance[1] > variance[0]) sortAxis = 1;
if (variance[2] > variance[sortAxis]) sortAxis = 2;
} }
// Resize the boxes and end-points arrays when it is full
void SweepAndPruneAlgorithm::resizeArrays() {
// New number of boxes in the array
const luint nbSentinels = 2;
const luint newNbMaxBoxes = nbMaxBoxes ? 2 * nbMaxBoxes : 100;
const luint nbEndPoints = nbBoxes * 2 + nbSentinels;
const luint newNbEndPoints = newNbMaxBoxes * 2 + nbSentinels;
// Allocate memory for the new boxes and end-points arrays
BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
assert(newBoxesArray);
assert(newEndPointsXArray);
assert(newEndPointsYArray);
assert(newEndPointsZArray);
// If the arrays were not empty before
if (nbBoxes > 0) {
// Copy the data in the old arrays into the new one
memcpy(newBoxesArray, boxes, sizeof(BoxAABB) * nbBoxes);
size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
memcpy(newEndPointsXArray, endPoints[0], nbBytesNewEndPoints);
memcpy(newEndPointsYArray, endPoints[1], nbBytesNewEndPoints);
memcpy(newEndPointsZArray, endPoints[2], nbBytesNewEndPoints);
}
else { // If the arrays were empty
// Add the limits endpoints (sentinels) into the array
const uint min = encodeFloatIntoInteger(DECIMAL_SMALLEST);
const uint max = encodeFloatIntoInteger(DECIMAL_LARGEST);
newEndPointsXArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsXArray[1].setValues(INVALID_INDEX, false, max);
newEndPointsYArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsYArray[1].setValues(INVALID_INDEX, false, max);
newEndPointsZArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsZArray[1].setValues(INVALID_INDEX, false, max);
}
// Delete the old arrays
delete[] boxes;
delete[] endPoints[0];
delete[] endPoints[1];
delete[] endPoints[2];
// Assign the pointer to the new arrays
boxes = newBoxesArray;
endPoints[0] = newEndPointsXArray;
endPoints[1] = newEndPointsYArray;
endPoints[2] = newEndPointsZArray;
nbMaxBoxes = newNbMaxBoxes;
}

136
src/collision/broadphase/SweepAndPruneAlgorithm.h
View File

@ -29,53 +29,127 @@
// Libraries // Libraries
#include "BroadPhaseAlgorithm.h" #include "BroadPhaseAlgorithm.h"
#include "../../colliders/AABB.h" #include "../../colliders/AABB.h"
#include <map>
#include <vector>
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
// EndPoint structure that represent an end-point of an AABB
// on one of the three x,y or z axis
struct EndPoint {
public:
// TODO : Use uint here
luint boxID; // ID of the AABB box corresponding to this end-point
bool isMin; // True if the end-point is a minimum end-point of a box
uint value; // Value (one dimension coordinate) of the end-point
void setValues(luint boxID, bool isMin, uint value); // Set the values of the endpoint
};
// Set the values of the endpoint
inline void EndPoint::setValues(luint boxID, bool isMin, uint value) {
this->boxID = boxID;
this->isMin = isMin;
this->value = value;
}
// Structure BoxAABB that represents an AABB in the
// Sweep-And-Prune algorithm
struct BoxAABB {
public:
luint min[3]; // Index of the three minimum end-points of the AABB over the axis X, Y and Z
luint max[3]; // Index of the three maximum end-points of the AABB over the axis X, Y and Z
Body* body; // Body that corresponds to the owner of the AABB
};
// Structure AABBInt
// Axis-Aligned Bounding box with integer coordinates
struct AABBInt {
public:
uint min[3]; // Minimum values on the three axis
uint max[3]; // Maximum values on the three axis
AABBInt(const AABB& aabb); // Constructor
};
// TODO : Use uint instead of luint here
// TODO : Rename the methods and variable to my way
// TODO : Try to replace the Body* pointer by the body ID everywhere in order that
// this class can be used in a more generic way
/* -------------------------------------------------------------------- /* --------------------------------------------------------------------
Class SAPAlgorithm : Class SweepAndPruneAlgorithm :
This class implements the Sweep and Prune (SAP) broad-phase This class implements the Sweep-And-Prune (SAP) broad-phase
algorithm. This algorithm uses the AABB bounding-volume of the collision detection algorithm. This class implements an
bodies and keep a sorted representation of the intervals of the array-based implementation of the algorithm from Pierre Terdiman
bodies' AABB on the three x. y and z axis. Given this sorted that is described here : www.codercorner.com/SAP.pdf.
representation, we can efficiently compute the set of possible
colliding pairs of bodies. At each broad-phase computation, we
should sort the AABB according to the axis (x, y or z) with the
largest variance of the AABBs positions in order that the sorted
AABB's set does not change a lot between each computations. To do
so, we compute at each time the variance of each axis and select
the axis (sortAxis) with the largest variance for the next
broad-phase computation.
-------------------------------------------------------------------- --------------------------------------------------------------------
*/ */
class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm { class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm {
protected : protected :
std::vector<const AABB*> sortedAABBs; // Sorted set of AABB of the bodies on one of the x.y or z axis static const luint INVALID_INDEX = ULONG_MAX; // Invalid array index
static unsigned short int sortAxis; // Current sorting axis (0 for x, 1 for y, 2 for z axis)
BoxAABB* boxes; // Array that contains all the AABB boxes of the broad-phase
static bool compareAABBs(const AABB* a, const AABB* b); // Static method that compare two AABBs (in order to sort them) EndPoint* endPoints[3]; // Array of end-points on the three axis
luint nbBoxes; // Number of AABB boxes in the broad-phase
luint nbMaxBoxes; // Maximum number of boxes in the boxes array
std::vector<luint> freeBoxIndices; // Indices that are not used by any boxes
std::map<Body*, luint> mapBodyToBoxIndex; // Map a body pointer to its box index
void resizeArrays(); // Resize the boxes and end-points arrays when it's full
void addPair(Body* body1, Body* body2); // Add an overlapping pair of AABBS
bool testIntersect1DSortedAABBs(const BoxAABB& box1, const AABBInt& box2,
const EndPoint* const baseEndPoint, luint axis) const; // Check for 1D box intersection
bool testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
luint axis1, luint axis2) const; // Check for 2D box intersection
public : public :
SweepAndPruneAlgorithm(CollisionDetection& collisionDetection); // Constructor SweepAndPruneAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~SweepAndPruneAlgorithm(); // Destructor virtual ~SweepAndPruneAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(); // Compute the possible collision pairs of bodies virtual void addObject(Body* body, const AABB& aabb); // Notify the broad-phase about a new object in the world
virtual void notifyAddedBodies(std::vector<RigidBody*> bodies); // Notify the broad-phase algorithm about new bodies in the physics world virtual void removeObject(Body* body); // Notify the broad-phase about a object that has been removed from the world
virtual void notifyRemovedBodies(std::vector<RigidBody*> bodies); // Notify the broad-phase algorithm about removed bodies in the physics world virtual void updateObject(Body* body, const AABB& aabb); // Notify the broad-phase that the AABB of an object has changed
}; };
// Static method that compare two AABBs. This method will be used to compare to AABBs // TODO : ADD Documentation for this method
// in order to sort them with the sort() function to obtain the sortedAABBs set. // Encode a floating value into a integer value
// This method must return true if the AABB "a" goes before the AABB "b". We inline uint encodeFloatIntoInteger(float number) {
// consider that "a" goes before "b" if the minimum value of "a" on the current uint intNumber = (uint&) number;
// sorting axis (sortAxis) is smaller than the minimum value of "b" on this same
// axis. // If it's a negative number
inline bool SweepAndPruneAlgorithm::compareAABBs(const AABB* a, const AABB* b) { if(intNumber & 0x80000000)
return (a->getMinCoordinates()[sortAxis] < b->getMinCoordinates()[sortAxis]); intNumber = ~intNumber;
} else { // If it is a positive number
intNumber |= 0x80000000;
}
return intNumber;
}
// Check for 1D box intersection between two boxes that are sorted on the
// given axis. Therefore, only one test is necessary here. We know that the
// minimum of box1 cannot be larger that the maximum of box2 on the axis.
inline bool SweepAndPruneAlgorithm::testIntersect1DSortedAABBs(const BoxAABB& box1, const AABBInt& box2,
const EndPoint* const endPointsArray, luint axis) const {
return !(endPointsArray[box1.max[axis]].value < box2.min[axis]);
}
// Check for 2D box intersection. This method is used when we know
// that two boxes already overlap on one axis and when want to test
// if they also overlap on the two others axis.
inline bool SweepAndPruneAlgorithm::testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
luint axis1, luint axis2) const {
return !(box2.max[axis1] < box1.min[axis1] || box1.max[axis1] < box2.min[axis1] ||
box2.max[axis2] < box1.min[axis2] || box1.max[axis2] < box2.min[axis2]);
}
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

22
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -222,10 +222,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap // Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
break; break;
} }
@ -288,12 +288,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
link(EdgeEPA(face5, 1), EdgeEPA(face3, 2)); link(EdgeEPA(face5, 1), EdgeEPA(face3, 2));
// Add the candidate faces in the heap // Add the candidate faces in the heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face4, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face4, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face5, triangleHeap, nbTriangles, DECIMAL_MAX); addFaceCandidate(face5, triangleHeap, nbTriangles, DECIMAL_LARGEST);
nbVertices = 5; nbVertices = 5;
} }
@ -308,7 +308,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
} }
TriangleEPA* triangle = 0; TriangleEPA* triangle = 0;
decimal upperBoundSquarePenDepth = DECIMAL_MAX; decimal upperBoundSquarePenDepth = DECIMAL_LARGEST;
do { do {
triangle = triangleHeap[0]; triangle = triangleHeap[0];

9
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -29,7 +29,6 @@
#include "../../../constraint/Contact.h" #include "../../../constraint/Contact.h"
#include "../../../configuration.h" #include "../../../configuration.h"
#include "../../OverlappingPair.h" #include "../../OverlappingPair.h"
#include "../../CollisionDetection.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
#include <cfloat> #include <cfloat>
@ -41,8 +40,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
GJKAlgorithm::GJKAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPoolContactInfos) GJKAlgorithm::GJKAlgorithm(MemoryPool<ContactInfo>& memoryPoolContactInfos)
:NarrowPhaseAlgorithm(collisionDetection, memoryPoolContactInfos), algoEPA(memoryPoolContactInfos) { :NarrowPhaseAlgorithm(memoryPoolContactInfos), algoEPA(memoryPoolContactInfos) {
} }
@ -91,7 +90,7 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
Vector3 v = currentOverlappingPair->getCachedSeparatingAxis(); Vector3 v = currentOverlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_MAX; decimal distSquare = DECIMAL_LARGEST;
do { do {
@ -252,7 +251,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Collider* con
Vector3 suppB; Vector3 suppB;
Vector3 w; Vector3 w;
decimal vDotw; decimal vDotw;
decimal distSquare = DECIMAL_MAX; decimal distSquare = DECIMAL_LARGEST;
decimal prevDistSquare; decimal prevDistSquare;
// Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1) // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1)

3
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -66,8 +66,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects
public : public :
GJKAlgorithm(CollisionDetection& collisionDetection, GJKAlgorithm(MemoryPool<ContactInfo>& memoryPoolContactInfos); // Constructor
MemoryPool<ContactInfo>& memoryPoolContactInfos); // Constructor
~GJKAlgorithm(); // Destructor ~GJKAlgorithm(); // Destructor
virtual bool testCollision(const Collider* collider1, const Transform& transform1, virtual bool testCollision(const Collider* collider1, const Transform& transform1,

2
src/collision/narrowphase/GJK/Simplex.cpp
View File

@ -327,7 +327,7 @@ bool Simplex::computeClosestPoint(Vector3& v) {
// Backup the closest point // Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3& v) { void Simplex::backupClosestPointInSimplex(Vector3& v) {
decimal minDistSquare = DECIMAL_MAX; decimal minDistSquare = DECIMAL_LARGEST;
Bits bit; Bits bit;
for (bit = allBits; bit != 0x0; bit--) { for (bit = allBits; bit != 0x0; bit--) {

4
src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
View File

@ -30,8 +30,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPool) NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(MemoryPool<ContactInfo>& memoryPool)
:collisionDetection(collisionDetection), memoryPoolContactInfos(memoryPool), currentOverlappingPair(0) { :memoryPoolContactInfos(memoryPool), currentOverlappingPair(0) {
} }

7
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -34,8 +34,6 @@
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
// Class declarations
class CollisionDetection;
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class NarrowPhaseAlgorithm : Class NarrowPhaseAlgorithm :
@ -47,13 +45,12 @@ class CollisionDetection;
*/ */
class NarrowPhaseAlgorithm { class NarrowPhaseAlgorithm {
protected : protected :
CollisionDetection& collisionDetection; // Reference to the collision detection object
MemoryPool<ContactInfo>& memoryPoolContactInfos; // Reference to the memory pool for contact infos MemoryPool<ContactInfo>& memoryPoolContactInfos; // Reference to the memory pool for contact infos
OverlappingPair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision OverlappingPair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision
public : public :
NarrowPhaseAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPool); // Constructor NarrowPhaseAlgorithm(MemoryPool<ContactInfo>& memoryPool); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor virtual ~NarrowPhaseAlgorithm(); // Destructor
void setCurrentOverlappingPair(OverlappingPair* overlappingPair); // Set the current overlapping pair of bodies void setCurrentOverlappingPair(OverlappingPair* overlappingPair); // Set the current overlapping pair of bodies
virtual bool testCollision(const Collider* collider1, const Transform& transform1, virtual bool testCollision(const Collider* collider1, const Transform& transform1,

4
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -31,8 +31,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPoolContactInfos) SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(MemoryPool<ContactInfo>& memoryPoolContactInfos)
:NarrowPhaseAlgorithm(collisionDetection, memoryPoolContactInfos) { :NarrowPhaseAlgorithm(memoryPoolContactInfos) {
} }

3
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -45,8 +45,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected : protected :
public : public :
SphereVsSphereAlgorithm(CollisionDetection& collisionDetection, SphereVsSphereAlgorithm(MemoryPool<ContactInfo>& memoryPoolContactInfos); // Constructor
MemoryPool<ContactInfo>& memoryPoolContactInfos); // Constructor
virtual ~SphereVsSphereAlgorithm(); // Destructor virtual ~SphereVsSphereAlgorithm(); // Destructor
virtual bool testCollision(const Collider* collider1, const Transform& transform1, virtual bool testCollision(const Collider* collider1, const Transform& transform1,

5
src/configuration.h
View File

@ -43,10 +43,11 @@
// Type definitions // Type definitions
typedef unsigned int uint; typedef unsigned int uint;
typedef long unsigned int luint; typedef long unsigned int luint;
typedef short unsigned int bodyindex; // TODO : Replace whenever possible using the bodyindex type
// Mathematical constants // Mathematical constants
const reactphysics3d::decimal DECIMAL_MIN = std::numeric_limits<reactphysics3d::decimal>::min(); // Minimun decimal value const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits<reactphysics3d::decimal>::max(); // Minimun decimal value
const reactphysics3d::decimal DECIMAL_MAX = std::numeric_limits<reactphysics3d::decimal>::max(); // Maximum decimal value const reactphysics3d::decimal DECIMAL_LARGEST = std::numeric_limits<reactphysics3d::decimal>::max(); // Maximum decimal value
const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits<reactphysics3d::decimal>::epsilon(); // Machine epsilon const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits<reactphysics3d::decimal>::epsilon(); // Machine epsilon
const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<reactphysics3d::decimal>::infinity(); // Infinity const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<reactphysics3d::decimal>::infinity(); // Infinity
const reactphysics3d::decimal PI = 3.14159265; // Pi constant const reactphysics3d::decimal PI = 3.14159265; // Pi constant

2
src/decimal.h
View File

@ -31,7 +31,7 @@ namespace reactphysics3d {
#if defined(DOUBLE_PRECISION_ENABLED) // If we are compiling for double precision #if defined(DOUBLE_PRECISION_ENABLED) // If we are compiling for double precision
typedef double decimal; typedef double decimal;
#else // If we are compiling for single precision #else // If we are compiling for single precision
typedef float decimal; typedef float decimal;
#endif #endif

15
src/engine/PhysicsEngine.cpp
View File

@ -32,10 +32,11 @@ using namespace std;
// Constructor // Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, decimal timeStep = DEFAULT_TIMESTEP) PhysicsEngine::PhysicsEngine(PhysicsWorld* world, decimal timeStep = DEFAULT_TIMESTEP)
: world(world), timer(timeStep), collisionDetection(world), constraintSolver(world), : world(world), collisionDetection(world), timer(timeStep), constraintSolver(world),
isDeactivationActive(DEACTIVATION_ENABLED) { isDeactivationActive(DEACTIVATION_ENABLED) {
assert(world); assert(world);
assert(timeStep > 0.0); assert(timeStep > 0.0);
world->setCollisionDetection(&collisionDetection);
} }
// Destructor // Destructor
@ -70,6 +71,9 @@ void PhysicsEngine::update() {
// Update the timer // Update the timer
timer.nextStep(); timer.nextStep();
// Reset the movement boolean variable of each body to false
resetBodiesMovementVariable();
// Update the position and orientation of each body // Update the position and orientation of each body
updateAllBodiesMotion(); updateAllBodiesMotion();
@ -77,9 +81,6 @@ void PhysicsEngine::update() {
if (existCollision) { if (existCollision) {
constraintSolver.cleanup(); constraintSolver.cleanup();
} }
// Clear the added and removed bodies from last update() method call
world->clearAddedAndRemovedBodies();
} }
// Compute and set the interpolation factor to all the bodies // Compute and set the interpolation factor to all the bodies
@ -103,7 +104,7 @@ void PhysicsEngine::updateAllBodiesMotion() {
Vector3 angularVelocityErrorCorrection; Vector3 angularVelocityErrorCorrection;
// For each body of thephysics world // For each body of thephysics world
for (vector<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) { for (set<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) {
RigidBody* rigidBody = *it; RigidBody* rigidBody = *it;
assert(rigidBody); assert(rigidBody);
@ -183,7 +184,7 @@ void PhysicsEngine::setInterpolationFactorToAllBodies() {
assert(factor >= 0.0 && factor <= 1.0); assert(factor >= 0.0 && factor <= 1.0);
// Set the factor to all bodies // Set the factor to all bodies
for (vector<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) { for (set<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it); RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody); assert(rigidBody);
@ -196,7 +197,7 @@ void PhysicsEngine::setInterpolationFactorToAllBodies() {
void PhysicsEngine::applyGravity() { void PhysicsEngine::applyGravity() {
// For each body of the physics world // For each body of the physics world
for (vector<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) { for (set<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it); RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody); assert(rigidBody);

12
src/engine/PhysicsEngine.h
View File

@ -50,11 +50,13 @@ class PhysicsEngine {
CollisionDetection collisionDetection; // Collision detection CollisionDetection collisionDetection; // Collision detection
ConstraintSolver constraintSolver; // Constraint solver ConstraintSolver constraintSolver; // Constraint solver
bool isDeactivationActive; // True if the deactivation (sleeping) of inactive bodies is enabled bool isDeactivationActive; // True if the deactivation (sleeping) of inactive bodies is enabled
void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity, void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection); // Update the position and orientation of a body const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection); // Update the position and orientation of a body
void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies
void applyGravity(); // Apply the gravity force to all bodies void applyGravity(); // Apply the gravity force to all bodies
void resetBodiesMovementVariable(); // Reset the boolean movement variable of each body
public : public :
PhysicsEngine(PhysicsWorld* world, decimal timeStep); // Constructor PhysicsEngine(PhysicsWorld* world, decimal timeStep); // Constructor
@ -89,6 +91,16 @@ inline void PhysicsEngine::setIsErrorCorrectionActive(bool isErrorCorrectionActi
constraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive); constraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive);
} }
// Reset the boolean movement variable of each body
inline void PhysicsEngine::resetBodiesMovementVariable() {
// For each rigid body
for (std::set<RigidBody*>::iterator it = world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); it++) {
// Set the hasMoved variable to false
(*it)->setHasMoved(false);
}
}
} }

37
src/engine/PhysicsWorld.cpp
View File

@ -35,7 +35,6 @@ using namespace std;
// Constructor // Constructor
PhysicsWorld::PhysicsWorld(const Vector3& gravity) PhysicsWorld::PhysicsWorld(const Vector3& gravity)
: gravity(gravity), isGravityOn(true), currentBodyID(0), memoryPoolRigidBodies(NB_MAX_BODIES) { : gravity(gravity), isGravityOn(true), currentBodyID(0), memoryPoolRigidBodies(NB_MAX_BODIES) {
} }
// Destructor // Destructor
@ -46,23 +45,47 @@ PhysicsWorld::~PhysicsWorld() {
// Create a rigid body into the physics world // Create a rigid body into the physics world
RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider) { RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider) {
// Compute the body ID
luint bodyID;
if (!freeRigidBodyIDs.empty()) {
bodyID = freeRigidBodyIDs.back();
freeRigidBodyIDs.pop_back();
}
else {
bodyID = currentBodyID;
currentBodyID++;
}
// Create the rigid body // Create the rigid body
RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collider, currentBodyID); RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collider, bodyID);
currentBodyID++; // Add the rigid body to the physics world
bodies.insert(rigidBody);
// Add the rigid body to the physics world and return it rigidBodies.insert(rigidBody);
addRigidBody(rigidBody);
// Add the rigid body to the collision detection
collisionDetection->addBody(rigidBody);
// Return the pointer to the rigid body
return rigidBody; return rigidBody;
} }
// Destroy a rigid body // Destroy a rigid body
void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) { void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) {
removeRigidBody(rigidBody);
// Remove the body from the collision detection
collisionDetection->removeBody(rigidBody);
// Add the body ID to the list of free IDs
freeRigidBodyIDs.push_back(rigidBody->getID());
// Call the constructor of the rigid body // Call the constructor of the rigid body
rigidBody->RigidBody::~RigidBody(); rigidBody->RigidBody::~RigidBody();
// Remove the rigid body from the list of rigid bodies
bodies.erase(rigidBody); // TOOD : Maybe use a set to make this faster
rigidBodies.erase(rigidBody); // TOOD : Maybe use a set to make this faster
// Free the object from the memory pool // Free the object from the memory pool
memoryPoolRigidBodies.freeObject(rigidBody); memoryPoolRigidBodies.freeObject(rigidBody);
} }

96
src/engine/PhysicsWorld.h
View File

@ -28,9 +28,11 @@
// Libraries // Libraries
#include <vector> #include <vector>
#include <set>
#include <algorithm> #include <algorithm>
#include "../mathematics/mathematics.h" #include "../mathematics/mathematics.h"
#include "../body/Body.h" #include "../body/Body.h"
#include "../collision/CollisionDetection.h"
#include "../constraint/Constraint.h" #include "../constraint/Constraint.h"
#include "../constraint/Contact.h" #include "../constraint/Contact.h"
#include "../memory/MemoryPool.h" #include "../memory/MemoryPool.h"
@ -48,17 +50,15 @@ namespace reactphysics3d {
*/ */
class PhysicsWorld { class PhysicsWorld {
protected : protected :
std::vector<RigidBody*> rigidBodies; // All the rigid bodies of the physics world CollisionDetection* collisionDetection; // Reference to the collision detection
std::vector<RigidBody*> addedBodies; // Added bodies since last update std::set<Body*> bodies; // All the bodies (rigid and soft) of the physics world
std::vector<RigidBody*> removedBodies; // Removed bodies since last update std::set<RigidBody*> rigidBodies; // All the rigid bodies of the physics world
std::vector<luint> freeRigidBodyIDs; // List of free ID for rigid bodies
std::vector<Constraint*> constraints; // List that contains all the current constraints std::vector<Constraint*> constraints; // List that contains all the current constraints
Vector3 gravity; // Gravity vector of the world Vector3 gravity; // Gravity vector of the world
bool isGravityOn; // True if the gravity force is on bool isGravityOn; // True if the gravity force is on
long unsigned int currentBodyID; // Current body ID luint currentBodyID; // Current body ID
MemoryPool<RigidBody> memoryPoolRigidBodies; // Memory pool for rigid bodies memory allocation MemoryPool<RigidBody> memoryPoolRigidBodies; // Memory pool for rigid bodies memory allocation
void addRigidBody(RigidBody* body); // Add a body to the physics world
void removeRigidBody(RigidBody* body); // Remove a body from the physics world
public : public :
PhysicsWorld(const Vector3& gravity); // Constructor PhysicsWorld(const Vector3& gravity); // Constructor
@ -67,56 +67,21 @@ class PhysicsWorld {
RigidBody* createRigidBody(const Transform& transform, decimal mass, RigidBody* createRigidBody(const Transform& transform, decimal mass,
const Matrix3x3& inertiaTensorLocal, Collider* collider); // Create a rigid body into the physics world const Matrix3x3& inertiaTensorLocal, Collider* collider); // Create a rigid body into the physics world
void destroyRigidBody(RigidBody* rigidBody); // Destroy a rigid body 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 Vector3 getGravity() const; // Return the gravity vector of the world
bool getIsGravityOn() const; // Return if the gravity is on bool getIsGravityOn() const; // Return if the gravity is on
void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
void setCollisionDetection(CollisionDetection* collisionDetection); // Set the collision detection reference
void addConstraint(Constraint* constraint); // Add a constraint void addConstraint(Constraint* constraint); // Add a constraint
void removeConstraint(Constraint* constraint); // Remove a constraint void removeConstraint(Constraint* constraint); // Remove a constraint
void removeAllContactConstraints(); // Remove all collision contacts constraints void removeAllContactConstraints(); // Remove all collision contacts constraints
void removeAllConstraints(); // Remove all constraints and delete them (free their memory) 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 getConstraintsBeginIterator(); // Return a start iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list
std::vector<RigidBody*>::iterator getRigidBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world std::set<Body*>::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world
std::vector<RigidBody*>::iterator getRigidBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world std::set<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world
std::vector<RigidBody*>& getAddedRigidBodies(); // Return the added bodies since last update of the physics engine std::set<RigidBody*>::iterator getRigidBodiesBeginIterator(); // Return an iterator to the beginning of the rigid bodies of the physics world
std::vector<RigidBody*>& getRemovedRigidBodies(); // Retrun the removed bodies since last update of the physics engine std::set<RigidBody*>::iterator getRigidBodiesEndIterator(); // Return an iterator to the end of the rigid bodies of the physics world
}; };
// Add a body to the physics world
inline void PhysicsWorld::addRigidBody(RigidBody* body) {
std::vector<RigidBody*>::iterator it;
assert(body);
it = std::find(rigidBodies.begin(), rigidBodies.end(), body);
assert(it == rigidBodies.end());
// The body isn't already in the bodyList, therefore we add it to the list
rigidBodies.push_back(body);
addedBodies.push_back(body);
it = std::find(removedBodies.begin(), removedBodies.end(), body);
if (it != removedBodies.end()) {
removedBodies.erase(it);
}
}
// Remove a body from the physics world
inline void PhysicsWorld::removeRigidBody(RigidBody* body) {
std::vector<RigidBody*>::iterator it;
assert(body);
it = std::find(rigidBodies.begin(), rigidBodies.end(), body);
assert(*it == body);
rigidBodies.erase(it);
it = std::find(addedBodies.begin(), addedBodies.end(), body);
if (it != addedBodies.end()) {
addedBodies.erase(it);
}
removedBodies.push_back(*it);
}
// Add a constraint into the physics world // Add a constraint into the physics world
inline void PhysicsWorld::addConstraint(Constraint* constraint) { inline void PhysicsWorld::addConstraint(Constraint* constraint) {
@ -135,12 +100,6 @@ inline void PhysicsWorld::removeConstraint(Constraint* constraint) {
constraints.erase(it); constraints.erase(it);
} }
// Clear the addedBodies and removedBodies sets
inline void PhysicsWorld::clearAddedAndRemovedBodies() {
addedBodies.clear();
removedBodies.clear();
}
// Return the gravity vector of the world // Return the gravity vector of the world
inline Vector3 PhysicsWorld::getGravity() const { inline Vector3 PhysicsWorld::getGravity() const {
return gravity; return gravity;
@ -156,6 +115,11 @@ inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) {
this->isGravityOn = isGravityOn; this->isGravityOn = isGravityOn;
} }
// Set the collision detection reference
inline void PhysicsWorld::setCollisionDetection(CollisionDetection* collisionDetection) {
this->collisionDetection = collisionDetection;
}
// Return a start iterator on the constraint list // Return a start iterator on the constraint list
inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsBeginIterator() { inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsBeginIterator() {
return constraints.begin(); return constraints.begin();
@ -167,25 +131,25 @@ inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsEndIterato
} }
// Return an iterator to the beginning of the bodies of the physics world // Return an iterator to the beginning of the bodies of the physics world
inline std::vector<RigidBody*>::iterator PhysicsWorld::getRigidBodiesBeginIterator() { inline std::set<Body*>::iterator PhysicsWorld::getBodiesBeginIterator() {
return bodies.begin();
}
// Return an iterator to the end of the bodies of the physics world
inline std::set<Body*>::iterator PhysicsWorld::getBodiesEndIterator() {
return bodies.end();
}
// Return an iterator to the beginning of the bodies of the physics world
inline std::set<RigidBody*>::iterator PhysicsWorld::getRigidBodiesBeginIterator() {
return rigidBodies.begin(); return rigidBodies.begin();
} }
// Return an iterator to the end of the bodies of the physics world // Return an iterator to the end of the bodies of the physics world
inline std::vector<RigidBody*>::iterator PhysicsWorld::getRigidBodiesEndIterator() { inline std::set<RigidBody*>::iterator PhysicsWorld::getRigidBodiesEndIterator() {
return rigidBodies.end(); return rigidBodies.end();
} }
// Return the added bodies since last update of the physics engine
inline std::vector<RigidBody*>& PhysicsWorld::getAddedRigidBodies() {
return addedBodies;
}
// Retrun the removed bodies since last update of the physics engine
inline std::vector<RigidBody*>& PhysicsWorld::getRemovedRigidBodies() {
return removedBodies;
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

18
src/mathematics/Transform.h
View File

@ -42,8 +42,8 @@ namespace reactphysics3d {
*/ */
class Transform { class Transform {
private : private :
Vector3 position; // Position Vector3 position; // Position
Quaternion orientation; // Orientation Quaternion orientation; // Orientation
public : public :
Transform(); // Constructor Transform(); // Constructor
@ -65,6 +65,8 @@ class Transform {
Vector3 operator*(const Vector3& vector) const; // Return the transformed vector Vector3 operator*(const Vector3& vector) const; // Return the transformed vector
Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one
bool operator==(const Transform& transform2) const; // Return true if the two transforms are equal
bool operator!=(const Transform& transform2) const; // Return true if the two transforms are different
}; };
// Return the position of the transform // Return the position of the transform
@ -91,7 +93,7 @@ inline void Transform::setOrientation(const Quaternion& orientation) {
inline void Transform::setToIdentity() { inline void Transform::setToIdentity() {
position = Vector3(0.0, 0.0, 0.0); position = Vector3(0.0, 0.0, 0.0);
orientation = Quaternion::identity(); orientation = Quaternion::identity();
} }
// Set the transform from an OpenGL transform matrix // Set the transform from an OpenGL transform matrix
inline void Transform::setFromOpenGL(decimal* openglMatrix) { inline void Transform::setFromOpenGL(decimal* openglMatrix) {
@ -135,6 +137,16 @@ inline Transform Transform::operator*(const Transform& transform2) const {
return Transform(position + orientation.getMatrix() * transform2.position, orientation * transform2.orientation); return Transform(position + orientation.getMatrix() * transform2.position, orientation * transform2.orientation);
} }
// Return true if the two transforms are equal
inline bool Transform::operator==(const Transform& transform2) const {
return (position == transform2.position) && (orientation == transform2.orientation);
}
// Return true if the two transforms are different
inline bool Transform::operator!=(const Transform& transform2) const {
return !(*this == transform2);
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

7
src/memory/MemoryPool.h
View File

@ -36,6 +36,13 @@
// TODO : Check that casting is done correctly in this class using // TODO : Check that casting is done correctly in this class using
// C++ cast operator like reinterpret_cast<>, ... // C++ cast operator like reinterpret_cast<>, ...
// TODO : Rename the variable "maxNbObjects" into "capacity"
// TODO : Replace uint by luint in this class
// TODO : Make the memory pool to start without allocated memory and to grow
// by using several blocks instead of only one.
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {