diff --git a/src/body/Body.cpp b/src/body/Body.cpp index 9f1cda82..ffc8a482 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -31,25 +31,11 @@ using namespace reactphysics3d; // Constructor -Body::Body(const Transform& transform, CollisionShape *collisionShape, decimal mass, bodyindex id) - : collisionShape(collisionShape), mass(mass), transform(transform), isActive(true), id(id), hasMoved(false) { - assert(mass > 0.0); - assert(collisionShape); +Body::Body(bodyindex id) : id(id) { - isMotionEnabled = true; - isCollisionEnabled = true; - interpolationFactor = 0.0; - - // Initialize the old transform - oldTransform = transform; - - // Create the AABB for broad-phase collision detection - aabb = new AABB(transform, collisionShape->getLocalExtents(OBJECT_MARGIN)); } // Destructor Body::~Body() { - // Delete the AABB - delete aabb; } diff --git a/src/body/Body.h b/src/body/Body.h index ae2038d8..71c03b8c 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -29,9 +29,6 @@ // Libraries #include #include -#include "../mathematics/Transform.h" -#include "../collision/shapes/AABB.h" -#include "../collision/shapes/CollisionShape.h" #include "../configuration.h" // Namespace reactphysics3d @@ -45,49 +42,21 @@ namespace reactphysics3d { ------------------------------------------------------------------- */ class Body { + protected : - CollisionShape* collisionShape; // Collision shape of the body - decimal mass; // Mass of the body - Transform transform; // Position and orientation of the body - Transform oldTransform; // Last position and orientation of the body - decimal interpolationFactor; // Interpolation factor used for the state interpolation - bool isActive; // True if the body is active (not sleeping because of deactivation) - 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 bodyindex id; // ID of the body - bool hasMoved; // True if the body has moved during the last frame public : - Body(const Transform& transform, CollisionShape* collisionShape, decimal mass, bodyindex id); // Constructor + Body(bodyindex id); // Constructor virtual ~Body(); // Destructor bodyindex 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) - CollisionShape* getCollisionShape() const; // Return the collision shape - void setCollisionShape(CollisionShape* collisionShape); // Set the collision shape - decimal getMass() const; // Return 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 - void setIsActive(bool isActive); // Set the isActive variable - const Transform& getTransform() const; // Return the current position and orientation - void setTransform(const Transform& transform); // Set the current position and orientation - const AABB* getAABB() const; // Return the AAABB of the body - Transform getInterpolatedTransform() const; // Return the interpolated transform for rendering - void setInterpolationFactor(decimal factor); // Set the interpolation factor of the body - bool getIsMotionEnabled() const; // Return if the rigid body can move - void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move - bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies - void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value - void updateOldTransform(); // Update the old transform with the current one - 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 + bool operator!=(const Body& body2) const; // Not equal operator }; // Return the id of the body @@ -95,115 +64,6 @@ inline bodyindex Body::getID() const { 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 collision shape -inline CollisionShape *Body::getCollisionShape() const { - assert(collisionShape); - return collisionShape; -} - -// Set the collision shape -inline void Body::setCollisionShape(CollisionShape *collisionShape) { - assert(collisionShape); - this->collisionShape = collisionShape; -} - -// Method that return the mass of the body -inline decimal Body::getMass() const { - return mass; -}; - -// Method that set the mass of the body -inline void Body::setMass(decimal mass) { - this->mass = mass; -} - -// Return true if the body is active -inline bool Body::getIsActive() const { - return isActive; -} - -// Set the isActive variable -inline void Body::setIsActive(bool isActive) { - this->isActive = isActive; -} - -// Return the interpolated transform for rendering -inline Transform Body::getInterpolatedTransform() const { - return Transform::interpolateTransforms(oldTransform, transform, interpolationFactor); -} - -// Set the interpolation factor of the body -inline void Body::setInterpolationFactor(decimal factor) { - // Set the factor - interpolationFactor = factor; -} - -// Return if the rigid body can move -inline bool Body::getIsMotionEnabled() const { - return isMotionEnabled; -} - -// Set the value to true if the body can move -inline void Body::setIsMotionEnabled(bool isMotionEnabled) { - this->isMotionEnabled = isMotionEnabled; -} - -// Return the current position and orientation -inline const Transform& Body::getTransform() const { - return transform; -} - -// Set the current position and orientation -inline void Body::setTransform(const Transform& transform) { - - // Check if the body has moved - if (this->transform != transform) { - hasMoved = true; - } - - this->transform = transform; -} - -// Return the AAABB of the body -inline const AABB* Body::getAABB() const { - return aabb; -} - - // Return true if the body can collide with others bodies -inline bool Body::getIsCollisionEnabled() const { - return isCollisionEnabled; -} - -// Set the isCollisionEnabled value -inline void Body::setIsCollisionEnabled(bool isCollisionEnabled) { - this->isCollisionEnabled = isCollisionEnabled; -} - -// Update the old transform with the current one -// This is used to compute the interpolated position and orientation of the body -inline void Body::updateOldTransform() { - oldTransform = transform; -} - -// Update the rigid body in order to reflect a change in the body state -inline void Body::updateAABB() { - - // TODO : An AABB should not be updated every frame but only if the body has moved - - // Update the AABB - aabb->update(transform, collisionShape->getLocalExtents(OBJECT_MARGIN)); -} - // Smaller than operator inline bool Body::operator<(const Body& body2) const { return (id < body2.id); @@ -219,7 +79,7 @@ inline bool Body::operator==(const Body& body2) const { return (id == body2.id); } -// Equal operator +// Not equal operator inline bool Body::operator!=(const Body& body2) const { return (id != body2.id); } diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp new file mode 100644 index 00000000..3d9c1079 --- /dev/null +++ b/src/body/CollisionBody.cpp @@ -0,0 +1,55 @@ + +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + + // Libraries +#include "CollisionBody.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape, bodyindex id) + : Body(id), collisionShape(collisionShape), transform(transform), isActive(true), hasMoved(false) { + + assert(collisionShape); + + isMotionEnabled = true; + isCollisionEnabled = true; + interpolationFactor = 0.0; + + // Initialize the old transform + oldTransform = transform; + + // Create the AABB for broad-phase collision detection + aabb = new AABB(transform, collisionShape->getLocalExtents(OBJECT_MARGIN)); +} + +// Destructor +CollisionBody::~CollisionBody() { + + // Delete the AABB + delete aabb; +} diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h new file mode 100644 index 00000000..bec9df2d --- /dev/null +++ b/src/body/CollisionBody.h @@ -0,0 +1,185 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef COLLISION_BODY_H +#define COLLISION_BODY_H + +// Libraries +#include +#include +#include "Body.h" +#include "../mathematics/Transform.h" +#include "../collision/shapes/AABB.h" +#include "../collision/shapes/CollisionShape.h" +#include "../configuration.h" + +// Namespace reactphysics3d +namespace reactphysics3d { + + +/* ------------------------------------------------------------------- + Class CollisionBody : + This class represents a body that is able to collide with others + bodies. This class inherits from the Body class. + ------------------------------------------------------------------- +*/ +class CollisionBody : public Body { + + protected : + CollisionShape* collisionShape; // Collision shape of the body + Transform transform; // Position and orientation of the body + Transform oldTransform; // Last position and orientation of the body + decimal interpolationFactor; // Interpolation factor used for the state interpolation + bool isActive; // True if the body is active (not sleeping because of deactivation) + 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 + bool hasMoved; // True if the body has moved during the last frame + + public : + CollisionBody(const Transform& transform, CollisionShape* collisionShape, bodyindex id); // Constructor + virtual ~CollisionBody(); // Destructor + + 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) + CollisionShape* getCollisionShape() const; // Return the collision shape + void setCollisionShape(CollisionShape* collisionShape); // Set the collision shape + bool getIsActive() const; // Return true if the body is active + void setIsActive(bool isActive); // Set the isActive variable + const Transform& getTransform() const; // Return the current position and orientation + void setTransform(const Transform& transform); // Set the current position and orientation + const AABB* getAABB() const; // Return the AAABB of the body + Transform getInterpolatedTransform() const; // Return the interpolated transform for rendering + void setInterpolationFactor(decimal factor); // Set the interpolation factor of the body + bool getIsMotionEnabled() const; // Return if the rigid body can move + void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move + bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies + void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value + void updateOldTransform(); // Update the old transform with the current one + void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates +}; + +// Return true if the body has moved during the last frame +inline bool CollisionBody::getHasMoved() const { + return hasMoved; +} + +// Set the hasMoved variable (true if the body has moved during the last frame) +inline void CollisionBody::setHasMoved(bool hasMoved) { + this->hasMoved = hasMoved; +} + +// Return the collision shape +inline CollisionShape* CollisionBody::getCollisionShape() const { + assert(collisionShape); + return collisionShape; +} + +// Set the collision shape +inline void CollisionBody::setCollisionShape(CollisionShape* collisionShape) { + assert(collisionShape); + this->collisionShape = collisionShape; +} + +// Return true if the body is active +inline bool CollisionBody::getIsActive() const { + return isActive; +} + +// Set the isActive variable +inline void CollisionBody::setIsActive(bool isActive) { + this->isActive = isActive; +} + +// Return the interpolated transform for rendering +inline Transform CollisionBody::getInterpolatedTransform() const { + return Transform::interpolateTransforms(oldTransform, transform, interpolationFactor); +} + +// Set the interpolation factor of the body +inline void CollisionBody::setInterpolationFactor(decimal factor) { + // Set the factor + interpolationFactor = factor; +} + +// Return if the rigid body can move +inline bool CollisionBody::getIsMotionEnabled() const { + return isMotionEnabled; +} + +// Set the value to true if the body can move +inline void CollisionBody::setIsMotionEnabled(bool isMotionEnabled) { + this->isMotionEnabled = isMotionEnabled; +} + +// Return the current position and orientation +inline const Transform& CollisionBody::getTransform() const { + return transform; +} + +// Set the current position and orientation +inline void CollisionBody::setTransform(const Transform& transform) { + + // Check if the body has moved + if (this->transform != transform) { + hasMoved = true; + } + + this->transform = transform; +} + +// Return the AAABB of the body +inline const AABB* CollisionBody::getAABB() const { + return aabb; +} + + // Return true if the body can collide with others bodies +inline bool CollisionBody::getIsCollisionEnabled() const { + return isCollisionEnabled; +} + +// Set the isCollisionEnabled value +inline void CollisionBody::setIsCollisionEnabled(bool isCollisionEnabled) { + this->isCollisionEnabled = isCollisionEnabled; +} + +// Update the old transform with the current one +// This is used to compute the interpolated position and orientation of the body +inline void CollisionBody::updateOldTransform() { + oldTransform = transform; +} + +// Update the rigid body in order to reflect a change in the body state +inline void CollisionBody::updateAABB() { + + // TODO : An AABB should not be updated every frame but only if the body has moved + + // Update the AABB + aabb->update(transform, collisionShape->getLocalExtents(OBJECT_MARGIN)); +} + +} + + #endif diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 71cdc00d..d59b9f7e 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -32,8 +32,8 @@ using namespace reactphysics3d; // Constructor RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, CollisionShape *collisionShape, bodyindex id) - : Body(transform, collisionShape, mass, id), inertiaTensorLocal(inertiaTensorLocal), - inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) { + : CollisionBody(transform, collisionShape, id), inertiaTensorLocal(inertiaTensorLocal), + mass(mass), inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) { restitution = 1.0; diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 5e5a5bbc..0a3cf90a 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -28,8 +28,7 @@ // Libraries #include -#include "Body.h" -#include "../collision/shapes/CollisionShape.h" +#include "CollisionBody.h" #include "../mathematics/mathematics.h" // Namespace reactphysics3d @@ -39,11 +38,16 @@ namespace reactphysics3d { Class RigidBody : This class represents a rigid body of the physics engine. A rigid body is a non-deformable body that - has a constant mass. + has a constant mass. This class inherits from the + CollisionBody class. ------------------------------------------------------------------- */ -class RigidBody : public Body { +class RigidBody : public CollisionBody { + protected : + + // TODO : Remove the mass variable (duplicate with inverseMass) + decimal mass; // Mass of the body Vector3 linearVelocity; // Linear velocity of the body Vector3 angularVelocity; // Angular velocity of the body Vector3 externalForce; // Current external force on the body @@ -58,6 +62,8 @@ class RigidBody : public Body { CollisionShape* collisionShape, bodyindex id); // Constructor // Copy-constructor virtual ~RigidBody(); // Destructor + decimal getMass() const; // Return the mass of the body + void setMass(decimal mass); // Set the mass of the body Vector3 getLinearVelocity() const; // Return the linear velocity void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body Vector3 getAngularVelocity() const; // Return the angular velocity @@ -78,6 +84,16 @@ class RigidBody : public Body { void setRestitution(decimal restitution) throw(std::invalid_argument); // Set the restitution coefficient }; +// Method that return the mass of the body +inline decimal RigidBody::getMass() const { + return mass; +}; + +// Method that set the mass of the body +inline void RigidBody::setMass(decimal mass) { + this->mass = mass; +} + // Return the linear velocity inline Vector3 RigidBody::getLinearVelocity() const { return linearVelocity; diff --git a/src/collision/BroadPhasePair.cpp b/src/collision/BroadPhasePair.cpp new file mode 100644 index 00000000..e09b0187 --- /dev/null +++ b/src/collision/BroadPhasePair.cpp @@ -0,0 +1,40 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "BroadPhasePair.h" + +using namespace reactphysics3d; + + // Constructor +BroadPhasePair::BroadPhasePair(CollisionBody* body1, CollisionBody* body2) + : body1(body1), body2(body2), previousSeparatingAxis(Vector3(1,1,1)) { + +} + +// Destructor +BroadPhasePair::~BroadPhasePair() { + +} diff --git a/src/collision/BroadPhasePair.h b/src/collision/BroadPhasePair.h new file mode 100644 index 00000000..6f03ec1e --- /dev/null +++ b/src/collision/BroadPhasePair.h @@ -0,0 +1,89 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef BROAD_PHASE_PAIR_H +#define BROAD_PHASE_PAIR_H + +// Libraries +#include "../body/CollisionBody.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// struct BroadPhasePair +// This structure represents a pair of bodies +// during the broad-phase collision detection +struct BroadPhasePair { + + public: + CollisionBody* body1; // Pointer to the first body + CollisionBody* body2; // Pointer to the second body + Vector3 previousSeparatingAxis; // Previous cached separating axis + + BroadPhasePair(CollisionBody* body1, CollisionBody* body2); // Constructor + ~BroadPhasePair(); // Destructor + + bodyindexpair getBodiesIndexPair() const; // Return the pair of bodies index + + // Operators + bool operator<(const BroadPhasePair& broadPhasePair2) const; // Smaller than operator + bool operator>(const BroadPhasePair& broadPhasePair2) const; // Larger than operator + bool operator==(const BroadPhasePair& broadPhasePair2) const; // Equal operator + bool operator!=(const BroadPhasePair& broadPhasePair2) const; // Not equal operator +}; + +// Return the pair of bodies index +inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const { + + // Construct the pair of body index + bodyindexpair indexPair = body1->getID() < body2->getID() ? std::make_pair(body1->getID(), body2->getID()) : + std::make_pair(body2->getID(), body1->getID()); + assert(indexPair.first != indexPair.second); + return indexPair; +} + +// Smaller than operator +inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const { + return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2)); +} + +// Larger than operator +inline bool BroadPhasePair::operator>(const BroadPhasePair& broadPhasePair2) const { + return (body1 > broadPhasePair2.body1 ? true : (body2 > broadPhasePair2.body2)); +} + +// Equal operator +inline bool BroadPhasePair::operator==(const BroadPhasePair& broadPhasePair2) const { + return (body1 == broadPhasePair2.body1 && body2 == broadPhasePair2.body2); +} + +// Not equal operator +inline bool BroadPhasePair::operator!=(const BroadPhasePair& broadPhasePair2) const { + return (body1 != broadPhasePair2.body1 || body2 != broadPhasePair2.body2); +} + +} + +#endif diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index b60e3fbc..1d93568c 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -25,7 +25,7 @@ // Libraries #include "CollisionDetection.h" -#include "../engine/PhysicsWorld.h" +#include "../engine/CollisionWorld.h" #include "broadphase/SweepAndPruneAlgorithm.h" #include "broadphase/NoBroadPhaseAlgorithm.h" #include "../body/Body.h" @@ -45,7 +45,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetection::CollisionDetection(PhysicsWorld* world) +CollisionDetection::CollisionDetection(CollisionWorld* world) : world(world), narrowPhaseGJKAlgorithm(memoryPoolContactInfos), narrowPhaseSphereVsSphereAlgorithm(memoryPoolContactInfos) { @@ -56,12 +56,6 @@ CollisionDetection::CollisionDetection(PhysicsWorld* world) // 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); - } // Delete the broad-phase algorithm delete broadPhaseAlgorithm; @@ -69,9 +63,7 @@ CollisionDetection::~CollisionDetection() { // Compute the collision detection bool CollisionDetection::computeCollisionDetection() { - - world->removeAllContactConstraints(); - + // TODO : Remove this code timeval timeValueStart; timeval timeValueStop; @@ -98,7 +90,7 @@ bool CollisionDetection::computeCollisionDetection() { void CollisionDetection::computeBroadPhase() { // Notify the broad-phase algorithm about the bodies that have moved since last frame - for (set::iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) { + for (set::iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) { // If the body has moved if ((*it)->getHasMoved()) { @@ -107,32 +99,31 @@ void CollisionDetection::computeBroadPhase() { broadPhaseAlgorithm->updateObject(*it, *((*it)->getAABB())); } } - - // TODO : DELETE THIS - //std::cout << "Nb overlapping pairs : " << overlappingPairs.size() << std::endl; - //std::cout << "Nb active pairs in pair manager : " << broadPhaseAlgorithm->getNbOverlappingPairs() << std::endl; } // Compute the narrow-phase collision detection bool CollisionDetection::computeNarrowPhase() { bool collisionExists = false; - map, OverlappingPair*>::iterator it; + map::iterator it; // For each possible collision pair of bodies for (it = overlappingPairs.begin(); it != overlappingPairs.end(); it++) { - ContactInfo* contactInfo = NULL; + ContactInfo* contactInfo = 0; - Body* const body1 = (*it).second->getBody1(); - Body* const body2 = (*it).second->getBody2(); + BroadPhasePair* pair = (*it).second; + assert(pair); + + CollisionBody* const body1 = pair->body1; + CollisionBody* const body2 = pair->body2; // Update the contact cache of the overlapping pair - (*it).second->update(); + world->updateOverlappingPair(pair); // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(body1->getCollisionShape(), body2->getCollisionShape()); // Notify the narrow-phase algorithm about the overlapping pair we are going to test - narrowPhaseAlgorithm.setCurrentOverlappingPair((*it).second); + narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); // Use the narrow-phase collision detection algorithm to check if there really is a collision if (narrowPhaseAlgorithm.testCollision(body1->getCollisionShape(), body1->getTransform(), @@ -140,21 +131,12 @@ bool CollisionDetection::computeNarrowPhase() { assert(contactInfo); collisionExists = true; - // Create a new contact - Contact* contact = new (memoryPoolContacts.allocateObject()) Contact(body1, body2, contactInfo); - + // Notify the world about the new narrow-phase contact + world->notifyNewContact(pair, contactInfo); + // Delete and remove the contact info from the memory pool contactInfo->ContactInfo::~ContactInfo(); memoryPoolContactInfos.freeObject(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)); - } } } @@ -163,34 +145,37 @@ bool CollisionDetection::computeNarrowPhase() { // 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::broadPhaseNotifyAddedOverlappingPair(const BroadPhasePair* addedPair) { +void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(BodyPair* addedPair) { - // Construct the pair of body index - pair indexPair = addedPair->body1->getID() < addedPair->body2->getID() ? make_pair(addedPair->body1->getID(), addedPair->body2->getID()) : - make_pair(addedPair->body2->getID(), addedPair->body1->getID()); - assert(indexPair.first != indexPair.second); + // Get the pair of body index + bodyindexpair indexPair = addedPair->getBodiesIndexPair(); + + // Create the corresponding broad-phase pair object + BroadPhasePair* broadPhasePair = new (memoryPoolOverlappingPairs.allocateObject()) BroadPhasePair(addedPair->body1, addedPair->body2); // Add the pair into the set of overlapping pairs (if not there yet) - OverlappingPair* newPair = new (memoryPoolOverlappingPairs.allocateObject()) OverlappingPair(addedPair->body1, addedPair->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); - } + pair::iterator, bool> check = overlappingPairs.insert(make_pair(indexPair, broadPhasePair)); + assert(check.second); + + // Notify the world about the new broad-phase overlapping pair + world->notifyAddedOverlappingPair(broadPhasePair); } // Allow the broadphase to notify the collision detection about a removed overlapping pair -void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(const BroadPhasePair* removedPair) { +void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(BodyPair* removedPair) { - // Construct the pair of body index - pair indexPair = removedPair->body1->getID() < removedPair->body2->getID() ? make_pair(removedPair->body1->getID(), removedPair->body2->getID()) : - make_pair(removedPair->body2->getID(), removedPair->body1->getID()); + // Get the pair of body index + bodyindexpair indexPair = removedPair->getBodiesIndexPair(); + + // Get the broad-phase pair + BroadPhasePair* broadPhasePair = overlappingPairs[indexPair]; + assert(broadPhasePair); + + // Notify the world about the removed broad-phase pair + world->notifyRemovedOverlappingPair(broadPhasePair); // Remove the overlapping pair from the memory pool - overlappingPairs[indexPair]->OverlappingPair::~OverlappingPair(); - memoryPoolOverlappingPairs.freeObject(overlappingPairs[indexPair]); + broadPhasePair->BroadPhasePair::~BroadPhasePair(); + memoryPoolOverlappingPairs.freeObject(broadPhasePair); overlappingPairs.erase(indexPair); } diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index a2663fb7..66672fa6 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -27,9 +27,9 @@ #define COLLISION_DETECTION_H // Libraries -#include "../body/Body.h" -#include "OverlappingPair.h" +#include "../body/CollisionBody.h" #include "broadphase/BroadPhaseAlgorithm.h" +#include "BroadPhasePair.h" #include "../memory/MemoryPool.h" #include "narrowphase/GJK/GJKAlgorithm.h" #include "narrowphase/SphereVsSphereAlgorithm.h" @@ -46,7 +46,7 @@ namespace reactphysics3d { // Declarations class BroadPhaseAlgorithm; -class PhysicsWorld; +class CollisionWorld; /* ------------------------------------------------------------------- Class CollisionDetection : @@ -57,45 +57,32 @@ class PhysicsWorld; ------------------------------------------------------------------- */ class CollisionDetection { + private : - 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 + CollisionWorld* world; // Pointer to the physics world + std::map overlappingPairs; // Broad-phase overlapping pairs of bodies BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm GJKAlgorithm narrowPhaseGJKAlgorithm; // Narrow-phase GJK algorithm SphereVsSphereAlgorithm narrowPhaseSphereVsSphereAlgorithm; // Narrow-phase Sphere vs Sphere algorithm - MemoryPool memoryPoolContacts; // Memory pool for the contacts - MemoryPool memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs MemoryPool memoryPoolContactInfos; // Memory pool for the contact info - + MemoryPool memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs + void computeBroadPhase(); // Compute the broad-phase collision detection bool computeNarrowPhase(); // Compute the narrow-phase collision detection NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1, CollisionShape* collisionShape2); // Select the narrow phase algorithm to use given two collision shapes public : - CollisionDetection(PhysicsWorld* physicsWorld); // Constructor + CollisionDetection(CollisionWorld* world); // Constructor ~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(bodyindex body1ID, bodyindex body2ID); // Return an overlapping pair or null + void addBody(CollisionBody* body); // Add a body to the collision detection + void removeBody(CollisionBody* body); // Remove a body from the collision detection bool computeCollisionDetection(); // Compute the collision detection - 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 + void broadPhaseNotifyAddedOverlappingPair(BodyPair* pair); // Allow the broadphase to notify the collision detection about a new overlapping pair + void broadPhaseNotifyRemovedOverlappingPair(BodyPair* 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 -// The method returns null if the pair of bodies is not overlapping -inline OverlappingPair* CollisionDetection::getOverlappingPair(bodyindex body1ID, bodyindex 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 0; -} - // Select the narrow-phase collision algorithm to use given two collision shapes inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1, CollisionShape* collisionShape2) { @@ -109,14 +96,14 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(Coll } // Add a body to the collision detection -inline void CollisionDetection::addBody(Body* body) { +inline void CollisionDetection::addBody(CollisionBody* 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) { +inline void CollisionDetection::removeBody(CollisionBody* body) { // Remove the body from the broad-phase broadPhaseAlgorithm->removeObject(body); diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 79ae24fc..54946d7c 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include -#include "../../body/Body.h" +#include "../../body/CollisionBody.h" #include "PairManager.h" // Namespace ReactPhysics3D @@ -62,12 +62,12 @@ class BroadPhaseAlgorithm { // 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 addObject(CollisionBody* body, const AABB& aabb)=0; // Notify the broad-phase about a new object in the world + virtual void removeObject(CollisionBody* body)=0; // Notify the broad-phase about an object that has been removed from the world + virtual void updateObject(CollisionBody* body, const AABB& aabb)=0; // Notify the broad-phase that the AABB of an object has changed - BroadPhasePair* beginOverlappingPairsPointer() const; // Return a pointer to the first active pair (used to iterate over the active pairs) - BroadPhasePair* endOverlappingPairsPointer() const; // Return a pointer to the last active pair (used to iterate over the active pairs) + BodyPair* beginOverlappingPairsPointer() const; // Return a pointer to the first active pair (used to iterate over the active pairs) + BodyPair* endOverlappingPairsPointer() const; // Return a pointer to the last active pair (used to iterate over the active pairs) }; // TODO : DELETE THIS METHOD @@ -77,12 +77,12 @@ inline uint BroadPhaseAlgorithm::getNbOverlappingPairs() const { // Return a pointer to the first active pair (used to iterate over the overlapping pairs) -inline BroadPhasePair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const { +inline BodyPair* 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 { +inline BodyPair* BroadPhaseAlgorithm::endOverlappingPairsPointer() const { return pairManager.endOverlappingPairsPointer(); } diff --git a/src/collision/broadphase/NoBroadPhaseAlgorithm.h b/src/collision/broadphase/NoBroadPhaseAlgorithm.h index b4da7554..db696567 100644 --- a/src/collision/broadphase/NoBroadPhaseAlgorithm.h +++ b/src/collision/broadphase/NoBroadPhaseAlgorithm.h @@ -44,25 +44,25 @@ namespace reactphysics3d { */ class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm { protected : - std::set bodies; // All bodies of the world + std::set bodies; // All bodies of the world public : NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor virtual ~NoBroadPhaseAlgorithm(); // Destructor - virtual void addObject(Body* body, const AABB& aabb); // Notify the broad-phase about a new object in the world - virtual void removeObject(Body* body); // Notify the broad-phase about an object that has been removed from the world - virtual void updateObject(Body* body, const AABB& aabb); // Notify the broad-phase that the AABB of an object has changed + virtual void addObject(CollisionBody* body, const AABB& aabb); // Notify the broad-phase about a new object in the world + virtual void removeObject(CollisionBody* body); // Notify the broad-phase about an object that has been removed from the world + virtual void updateObject(CollisionBody* 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) { +inline void NoBroadPhaseAlgorithm::addObject(CollisionBody* 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::iterator it = bodies.begin(); it != bodies.end(); ++it) { + for (std::set::iterator it = bodies.begin(); it != bodies.end(); ++it) { // Add an overlapping pair with the new body pairManager.addPair(*it, body); @@ -73,10 +73,10 @@ inline void NoBroadPhaseAlgorithm::addObject(Body* body, const AABB& aabb) { } // Notify the broad-phase about an object that has been removed from the world -inline void NoBroadPhaseAlgorithm::removeObject(Body* body) { +inline void NoBroadPhaseAlgorithm::removeObject(CollisionBody* body) { // For each body that is in the world - for (std::set::iterator it = bodies.begin(); it != bodies.end(); ++it) { + for (std::set::iterator it = bodies.begin(); it != bodies.end(); ++it) { if ((*it)->getID() != body->getID()) { @@ -90,7 +90,7 @@ inline void NoBroadPhaseAlgorithm::removeObject(Body* body) { } // Notify the broad-phase that the AABB of an object has changed -inline void NoBroadPhaseAlgorithm::updateObject(Body* body, const AABB& aabb) { +inline void NoBroadPhaseAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) { // Do nothing return; } diff --git a/src/collision/broadphase/PairManager.cpp b/src/collision/broadphase/PairManager.cpp index 5b232270..9ad5d8e7 100644 --- a/src/collision/broadphase/PairManager.cpp +++ b/src/collision/broadphase/PairManager.cpp @@ -56,7 +56,7 @@ PairManager::~PairManager() { // that pair. If the pair to add does not already exist in the set of // overlapping pairs, it will be created and if it already exists, we only // return a pointer to that pair. -BroadPhasePair* PairManager::addPair(Body* body1, Body* body2) { +BodyPair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2) { // Sort the bodies to have the body with smallest ID first sortBodiesUsingID(body1, body2); @@ -69,7 +69,7 @@ BroadPhasePair* PairManager::addPair(Body* body1, Body* body2) { uint hashValue = computeHashBodies(id1, id2) & hashMask; // Try to find the pair in the current overlapping pairs. - BroadPhasePair* pair = findPairWithHashValue(id1, id2, hashValue); + BodyPair* pair = findPairWithHashValue(id1, id2, hashValue); // If the pair is already in the set of overlapping pairs if (pair) { @@ -93,7 +93,7 @@ BroadPhasePair* PairManager::addPair(Body* body1, Body* body2) { } // Create the new overlapping pair - BroadPhasePair* newPair = &overlappingPairs[nbOverlappingPairs]; + BodyPair* newPair = &overlappingPairs[nbOverlappingPairs]; newPair->body1 = body1; newPair->body2 = body2; @@ -119,7 +119,7 @@ bool PairManager::removePair(bodyindex id1, bodyindex id2) { const uint hashValue = computeHashBodies(id1, id2) & hashMask; // Find the pair to remove - const BroadPhasePair* pair = findPairWithHashValue(id1, id2, hashValue); + BodyPair* pair = findPairWithHashValue(id1, id2, hashValue); // If we have not found the pair if (!pair) { @@ -184,7 +184,7 @@ void PairManager::removePairWithHashValue(bodyindex id1, bodyindex id2, luint ha // now free because of the pair we want to remove // Get the last pair - const BroadPhasePair* lastPair = &overlappingPairs[indexLastPair]; + const BodyPair* lastPair = &overlappingPairs[indexLastPair]; const uint lastPairHashValue = computeHashBodies(lastPair->body1->getID(), lastPair->body2->getID()) & hashMask; // Compute the initial offset of the last pair @@ -223,7 +223,7 @@ void PairManager::removePairWithHashValue(bodyindex id1, bodyindex id2, luint ha } // Look for a pair in the set of overlapping pairs -BroadPhasePair* PairManager::lookForAPair(bodyindex id1, bodyindex id2, luint hashValue) const { +BodyPair* PairManager::lookForAPair(bodyindex id1, bodyindex id2, luint hashValue) const { // Look for the pair in the set of overlapping pairs bodyindex offset = hashTable[hashValue]; @@ -256,7 +256,7 @@ void PairManager::reallocatePairs() { } // Reallocate the overlapping pairs - BroadPhasePair* newOverlappingPairs = (BroadPhasePair*) malloc(nbElementsHashTable * sizeof(BroadPhasePair)); + BodyPair* newOverlappingPairs = (BodyPair*) malloc(nbElementsHashTable * sizeof(BodyPair)); bodyindex* newOffsetNextPair = (bodyindex*) malloc(nbElementsHashTable * sizeof(bodyindex)); assert(newOverlappingPairs); @@ -264,8 +264,8 @@ void PairManager::reallocatePairs() { // If there is already some overlapping pairs if (nbOverlappingPairs) { - // Copy the pairs to the new place - memcpy(newOverlappingPairs, overlappingPairs, nbOverlappingPairs * sizeof(BroadPhasePair)); + // Copy the pairs to the new location + memcpy(newOverlappingPairs, overlappingPairs, nbOverlappingPairs * sizeof(BodyPair)); } // Recompute the hash table with the new hash values diff --git a/src/collision/broadphase/PairManager.h b/src/collision/broadphase/PairManager.h index 23c8813c..ee39c698 100644 --- a/src/collision/broadphase/PairManager.h +++ b/src/collision/broadphase/PairManager.h @@ -27,7 +27,8 @@ #define PAIR_MANAGER_H // Libraries -#include "../../body/Body.h" +#include "../../body/CollisionBody.h" +#include // Namespace ReactPhysics3D @@ -35,15 +36,26 @@ namespace reactphysics3d { // Declaration class CollisionDetection; - -// Structure BroadPhasePair that represents a pair of bodies -// during the broad-phase collision detection -struct BroadPhasePair { - public: - Body* body1; // Pointer to the first body - Body* body2; // Pointer to the second body -}; +// struct BodyPair +// This structure represents a pair of bodies +// during the broad-phase collision detection +struct BodyPair { + + public: + CollisionBody* body1; // Pointer to the first body + CollisionBody* body2; // Pointer to the second body + + // Return the pair of bodies index + bodyindexpair getBodiesIndexPair() const { + + // Construct the pair of body index + bodyindexpair indexPair = body1->getID() < body2->getID() ? std::make_pair(body1->getID(), body2->getID()) : + std::make_pair(body2->getID(), body1->getID()); + assert(indexPair.first != indexPair.second); + return indexPair; + } +}; /* -------------------------------------------------------------------- Class PairManager : @@ -63,23 +75,23 @@ class PairManager { bodyindex* offsetNextPair; // Array that contains for each offset, the offset of the next pair with // the same hash value // for a given same hash value - BroadPhasePair* overlappingPairs; // Array that contains the currently active pairs + BodyPair* overlappingPairs; // Array that contains the currently active pairs static bodyindex INVALID_INDEX; // Invalid ID CollisionDetection& collisionDetection; // Reference to the collision detection - void sortBodiesUsingID(Body*& body1, Body*& body2) const; // Sort the bodies according to their IDs (smallest ID first) - void sortIDs(bodyindex& id1, bodyindex& id2) const; // Sort the IDs (smallest ID first) - bool isDifferentPair(const BroadPhasePair& pair1, bodyindex pair2ID1, + void sortBodiesUsingID(CollisionBody*& body1, CollisionBody*& body2) const; // Sort the bodies according to their IDs (smallest ID first) + void sortIDs(bodyindex& id1, bodyindex& id2) const; // Sort the IDs (smallest ID first) + bool isDifferentPair(const BodyPair& pair1, bodyindex pair2ID1, bodyindex pair2ID2) const; // Return true if pair1 and pair2 are the same uint computeHashBodies(uint id1, uint id2) const; // Compute the hash value of two bodies using their IDs int computeHash32Bits(int key) const; // This method returns an hash value for a 32 bits key luint computeNextPowerOfTwo(luint number) const; // Return the next power of two void reallocatePairs(); // Reallocate memory for more pairs void shrinkMemory(); // Shrink the allocated memory - bodyindex computePairOffset(const BroadPhasePair* pair) const; // Compute the offset of a given pair - BroadPhasePair* lookForAPair(bodyindex id1, bodyindex id2, + bodyindex computePairOffset(const BodyPair* pair) const; // Compute the offset of a given pair + BodyPair* lookForAPair(bodyindex id1, bodyindex id2, luint hashValue) const; // Look for a pair in the set of overlapping pairs - BroadPhasePair* findPairWithHashValue(bodyindex id1, bodyindex id2, + BodyPair* findPairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue) const; // Find a pair given two body IDs and an hash value void removePairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue, bodyindex indexPair); // Remove a pair from the set of active pair @@ -88,14 +100,14 @@ class PairManager { ~PairManager(); // Destructor bodyindex getNbOverlappingPairs() const; // Return the number of active pairs - BroadPhasePair* addPair(Body* body1, Body* body2); // Add a pair of bodies in the pair manager + BodyPair* addPair(CollisionBody* body1, CollisionBody* body2); // Add a pair of bodies in the pair manager bool removePair(bodyindex id1, bodyindex id2); // Remove a pair of bodies from the pair manager - BroadPhasePair* findPair(bodyindex id1, bodyindex id2) const; // Find a pair given two body IDs - BroadPhasePair* beginOverlappingPairsPointer() const; // Return a pointer to the first overlapping pair (used to iterate over the active pairs) - BroadPhasePair* endOverlappingPairsPointer() const; // Return a pointer to the last overlapping pair (used to iterate over the active pairs) - void registerAddedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BroadPhasePair* addedActivePair)); // Register a callback function (using a function pointer) that will be called when a new overlapping pair is added in the pair manager + BodyPair* findPair(bodyindex id1, bodyindex id2) const; // Find a pair given two body IDs + BodyPair* beginOverlappingPairsPointer() const; // Return a pointer to the first overlapping pair (used to iterate over the active pairs) + BodyPair* endOverlappingPairsPointer() const; // Return a pointer to the last overlapping pair (used to iterate over the active pairs) + void registerAddedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BodyPair* addedActivePair)); // Register a callback function (using a function pointer) that will be called when a new overlapping pair is added in the pair manager void unregisterAddedOverlappingPairCallback(); // Unregister the callback function that will be called when a new active pair is added in the pair manager - void registerRemovedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BroadPhasePair* removedActivePair)); // Register a callback function (using a function pointer) that will be called when an overlapping pair is removed from the pair manager + void registerRemovedOverlappingPairCallback(void (CollisionDetection::*callbackFunction) (const BodyPair* removedActivePair)); // Register a callback function (using a function pointer) that will be called when an overlapping pair is removed from the pair manager void unregisterRemovedOverlappingPairCallback(); // Unregister a callback function that will be called when a active pair is removed from the pair manager }; @@ -110,7 +122,7 @@ inline uint PairManager::computeHashBodies(uint id1, uint id2) const { } // Return true if pair1 and pair2 are the same -inline bool PairManager::isDifferentPair(const BroadPhasePair& pair1, bodyindex pair2ID1, bodyindex pair2ID2) const { +inline bool PairManager::isDifferentPair(const BodyPair& pair1, bodyindex pair2ID1, bodyindex pair2ID2) const { return (pair2ID1 != pair1.body1->getID() || pair2ID2 != pair1.body2->getID()); } @@ -125,13 +137,13 @@ inline luint PairManager::computeNextPowerOfTwo(luint number) const { } // Sort the bodies according to their IDs (smallest ID first) -inline void PairManager::sortBodiesUsingID(Body*& body1, Body*& body2) const { +inline void PairManager::sortBodiesUsingID(CollisionBody*& body1, CollisionBody*& body2) const { // If the ID of body1 is larger than the ID of body 2 if (body1->getID() > body2->getID()) { // Swap the two bodies pointers - Body* temp = body2; + CollisionBody* temp = body2; body2 = body1; body1 = temp; } @@ -161,7 +173,7 @@ inline int PairManager::computeHash32Bits(int key) const { } // Find a pair given two body IDs -inline BroadPhasePair* PairManager::findPair(bodyindex id1, bodyindex id2) const { +inline BodyPair* PairManager::findPair(bodyindex id1, bodyindex id2) const { // Check if the hash table has been allocated yet if (!hashTable) return 0; @@ -179,7 +191,7 @@ inline BroadPhasePair* PairManager::findPair(bodyindex id1, bodyindex id2) const // Find a pair given two body IDs and an hash value // This internal version is used to avoid computing multiple times in the // caller method -inline BroadPhasePair* PairManager::findPairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue) const { +inline BodyPair* PairManager::findPairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue) const { // Check if the hash table has been allocated yet if (!hashTable) return 0; @@ -202,19 +214,19 @@ inline void PairManager::shrinkMemory() { } // Compute the offset of a given pair in the array of overlapping pairs -inline bodyindex PairManager::computePairOffset(const BroadPhasePair* pair) const { - return ((bodyindex)((size_t(pair) - size_t(overlappingPairs))) / sizeof(BroadPhasePair)); +inline bodyindex PairManager::computePairOffset(const BodyPair* pair) const { + return ((bodyindex)((size_t(pair) - size_t(overlappingPairs))) / sizeof(BodyPair)); } // Return a pointer to the first overlapping pair (used to iterate over the overlapping pairs) or // returns 0 if there is no overlapping pairs. -inline BroadPhasePair* PairManager::beginOverlappingPairsPointer() const { +inline BodyPair* PairManager::beginOverlappingPairsPointer() const { return &overlappingPairs[0]; } // Return a pointer to the last overlapping pair (used to iterate over the overlapping pairs) or // returns 0 if there is no overlapping pairs. -inline BroadPhasePair* PairManager::endOverlappingPairsPointer() const { +inline BodyPair* PairManager::endOverlappingPairsPointer() const { if (nbOverlappingPairs > 0) { return &overlappingPairs[nbOverlappingPairs-1]; } diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp index ab05c07e..6f5623bb 100644 --- a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp +++ b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp @@ -65,7 +65,7 @@ SweepAndPruneAlgorithm::~SweepAndPruneAlgorithm() { // Notify the broad-phase about a new object in the world // This method adds the AABB of the object ion to broad-phase -void SweepAndPruneAlgorithm::addObject(Body* body, const AABB& aabb) { +void SweepAndPruneAlgorithm::addObject(CollisionBody* body, const AABB& aabb) { bodyindex boxIndex; // If the index of the first free box is valid (means that @@ -112,7 +112,7 @@ void SweepAndPruneAlgorithm::addObject(Body* body, const AABB& aabb) { } // Add the body pointer to box index mapping - mapBodyToBoxIndex.insert(pair(body, boxIndex)); + mapBodyToBoxIndex.insert(pair(body, boxIndex)); nbBoxes++; @@ -124,7 +124,7 @@ void SweepAndPruneAlgorithm::addObject(Body* body, const AABB& aabb) { } // Notify the broad-phase about a object that has been removed from the world -void SweepAndPruneAlgorithm::removeObject(Body* body) { +void SweepAndPruneAlgorithm::removeObject(CollisionBody* body) { // Call the update method with an AABB that is very far away // in order to remove all overlapping pairs from the pair manager @@ -145,7 +145,7 @@ void SweepAndPruneAlgorithm::removeObject(Body* body) { } // Notify the broad-phase that the AABB of an object has changed -void SweepAndPruneAlgorithm::updateObject(Body* body, const AABB& aabb) { +void SweepAndPruneAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) { // Compute the AABB with integer coordinates AABBInt aabbInt(aabb); diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.h b/src/collision/broadphase/SweepAndPruneAlgorithm.h index a9066d45..21b27fb5 100644 --- a/src/collision/broadphase/SweepAndPruneAlgorithm.h +++ b/src/collision/broadphase/SweepAndPruneAlgorithm.h @@ -62,7 +62,7 @@ struct BoxAABB { public: bodyindex min[3]; // Index of the three minimum end-points of the AABB over the axis X, Y and Z bodyindex 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 + CollisionBody* body; // Body that corresponds to the owner of the AABB }; // Structure AABBInt @@ -94,10 +94,10 @@ class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm { bodyindex nbBoxes; // Number of AABB boxes in the broad-phase bodyindex nbMaxBoxes; // Maximum number of boxes in the boxes array std::vector freeBoxIndices; // Indices that are not used by any boxes - std::map mapBodyToBoxIndex; // Map a body pointer to its box index + std::map 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 + void addPair(CollisionBody* body1, CollisionBody* body2); // Add an overlapping pair of AABBS bool testIntersect1DSortedAABBs(const BoxAABB& box1, const AABBInt& box2, const EndPoint* const baseEndPoint, uint axis) const; // Check for 1D box intersection bool testIntersect2D(const BoxAABB& box1, const BoxAABB& box2, @@ -107,9 +107,9 @@ class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm { SweepAndPruneAlgorithm(CollisionDetection& collisionDetection); // Constructor virtual ~SweepAndPruneAlgorithm(); // Destructor - virtual void addObject(Body* body, const AABB& aabb); // Notify the broad-phase about a new object in the world - virtual void removeObject(Body* body); // Notify the broad-phase about a object that has been removed from the world - virtual void updateObject(Body* body, const AABB& aabb); // Notify the broad-phase that the AABB of an object has changed + virtual void addObject(CollisionBody* body, const AABB& aabb); // Notify the broad-phase about a new object in the world + virtual void removeObject(CollisionBody* body); // Notify the broad-phase about a object that has been removed from the world + virtual void updateObject(CollisionBody* body, const AABB& aabb); // Notify the broad-phase that the AABB of an object has changed }; // Encode a floating value into a integer value in order to diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 18a6befa..e0450503 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -28,7 +28,6 @@ #include "Simplex.h" #include "../../../constraint/Contact.h" #include "../../../configuration.h" -#include "../../OverlappingPair.h" #include #include #include @@ -87,7 +86,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, const Tr Simplex simplex; // Get the previous point V (last cached separating axis) - Vector3 v = currentOverlappingPair->getCachedSeparatingAxis(); + Vector3 v = currentOverlappingPair->previousSeparatingAxis; // Initialize the upper bound for the square distance decimal distSquare = DECIMAL_LARGEST; @@ -107,7 +106,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, const Tr if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - currentOverlappingPair->setCachedSeparatingAxis(v); + currentOverlappingPair->previousSeparatingAxis = v; // No intersection, we return false return false; diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 04fe036e..2278b090 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -29,7 +29,10 @@ // Libraries #include "../../body/Body.h" #include "../ContactInfo.h" -#include "../OverlappingPair.h" +#include "../broadphase/PairManager.h" +#include "../../memory/MemoryPool.h" +#include "../BroadPhasePair.h" + // Namespace ReactPhysics3D namespace reactphysics3d { @@ -46,20 +49,20 @@ namespace reactphysics3d { class NarrowPhaseAlgorithm { protected : MemoryPool& memoryPoolContactInfos; // Reference to the memory pool for contact infos - OverlappingPair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision + BroadPhasePair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision public : NarrowPhaseAlgorithm(MemoryPool& memoryPool); // Constructor virtual ~NarrowPhaseAlgorithm(); // Destructor - void setCurrentOverlappingPair(OverlappingPair* overlappingPair); // Set the current overlapping pair of bodies + void setCurrentOverlappingPair(BroadPhasePair* overlappingPair); // Set the current overlapping pair of bodies virtual bool testCollision(const CollisionShape* collisionShape1, const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide }; // Set the current overlapping pair of bodies -inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) { +inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(BroadPhasePair *overlappingPair) { currentOverlappingPair = overlappingPair; } diff --git a/src/configuration.h b/src/configuration.h index ac9eefb8..fb1c00a1 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -29,6 +29,7 @@ // Libraries #include #include +#include #include "decimal.h" // Windows platform @@ -47,6 +48,7 @@ namespace reactphysics3d { typedef unsigned int uint; typedef long unsigned int luint; typedef short unsigned int bodyindex; // TODO : Replace whenever possible using the bodyindex type +typedef std::pair bodyindexpair; // Mathematical constants const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits::max(); // Minimun decimal value diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index a7c90da1..28dab4b8 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor -Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active, ConstraintType type) +Constraint::Constraint(RigidBody* const body1, RigidBody* const body2, uint nbConstraints, bool active, ConstraintType type) :body1(body1), body2(body2), active(active), nbConstraints(nbConstraints), type(type) { // Initialize the cached lambda values diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index ea97fd36..002a27d1 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -27,7 +27,7 @@ #define CONSTRAINT_H // Libraries -#include "../body/Body.h" +#include "../body/RigidBody.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace @@ -46,19 +46,19 @@ enum ConstraintType {CONTACT}; */ class Constraint { protected : - Body* const body1; // Pointer to the first body of the constraint - Body* const body2; // Pointer to the second body of the constraint + RigidBody* const body1; // Pointer to the first body of the constraint + RigidBody* const body2; // Pointer to the second body of the constraint bool active; // True if the constraint is active uint nbConstraints; // Number mathematical constraints associated with this Constraint const ConstraintType type; // Type of the constraint std::vector 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, + Constraint(RigidBody* const body1, RigidBody* const body2, uint nbConstraints, bool active, ConstraintType type); // Constructor // Constructor virtual ~Constraint(); // Destructor - Body* const getBody1() const; // Return the reference to the body 1 - Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint + RigidBody* const getBody1() const; // Return the reference to the body 1 + RigidBody* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 ConstraintType getType() const; // Return the type of the constraint virtual void computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0; // Compute the jacobian matrix for all mathematical constraints @@ -71,12 +71,12 @@ class Constraint { }; // Return the reference to the body 1 -inline Body* const Constraint::getBody1() const { +inline RigidBody* const Constraint::getBody1() const { return body1; } // Return the reference to the body 2 -inline Body* const Constraint::getBody2() const { +inline RigidBody* const Constraint::getBody2() const { return body2; } diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index 085d6390..28b4a79b 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -Contact::Contact(Body* const body1, Body* const body2, const ContactInfo* contactInfo) +Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo) : Constraint(body1, body2, 3, true, CONTACT), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth), localPointOnBody1(contactInfo->localPoint1), localPointOnBody2(contactInfo->localPoint2), worldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), worldPointOnBody2(body2->getTransform() * contactInfo->localPoint2) { @@ -170,4 +170,4 @@ void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const { errorValues[noConstraint] = errorValue; // Error value for contact constraint errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint errorValues[noConstraint + 2] = 0.0; // Error value for friction constraint -} \ No newline at end of file +} diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index b3050540..854375e1 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -62,6 +62,7 @@ namespace reactphysics3d { ------------------------------------------------------------------- */ class Contact : public Constraint { + protected : const Vector3 normal; // Normal vector of the contact (From body1 toward body2) in world space decimal penetrationDepth; // Penetration depth @@ -75,7 +76,7 @@ class Contact : public Constraint { void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane public : - Contact(Body* const body1, Body* const body2, const ContactInfo* contactInfo); // Constructor + Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); // Constructor virtual ~Contact(); // Destructor Vector3 getNormal() const; // Return the normal vector of the contact diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/CollisionWorld.cpp similarity index 54% rename from src/engine/PhysicsWorld.cpp rename to src/engine/CollisionWorld.cpp index 88571601..dd8bf48e 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -24,8 +24,7 @@ ********************************************************************************/ // Libraries -#include "PhysicsWorld.h" -#include "PhysicsEngine.h" +#include "CollisionWorld.h" #include // Namespaces @@ -33,88 +32,93 @@ using namespace reactphysics3d; using namespace std; // Constructor -PhysicsWorld::PhysicsWorld(const Vector3& gravity) - : gravity(gravity), isGravityOn(true), currentBodyID(0) { +CollisionWorld::CollisionWorld() : collisionDetection(this), currentBodyID(0) { } // Destructor -PhysicsWorld::~PhysicsWorld() { +CollisionWorld::~CollisionWorld() { } -// Create a rigid body into the physics world -RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, CollisionShape* collisionShape) { - +// Notify the world about a new broad-phase overlapping pair +void CollisionWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) { + + // TODO : Implement this method +} + +// Notify the world about a removed broad-phase overlapping pair +void CollisionWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) { + + // TODO : Implement this method +} + +// Notify the world about a new narrow-phase contact +void CollisionWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const ContactInfo* contactInfo) { + + // TODO : Implement this method +} + +// Update the overlapping pair +inline void CollisionWorld::updateOverlappingPair(const BroadPhasePair* pair) { + +} + +// Create a collision body and add it to the world +CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform, CollisionShape* collisionShape) { + + // Get the next available body ID + bodyindex bodyID = computeNextAvailableBodyID(); + + // Largest index cannot be used (it is used for invalid index) + assert(bodyID < std::numeric_limits::max()); + + // Create the collision body + CollisionBody* collisionBody = new (memoryPoolCollisionBodies.allocateObject()) CollisionBody(transform, collisionShape, bodyID); + + // Add the collision body to the world + bodies.insert(collisionBody); + + // Add the collision body to the collision detection + collisionDetection.addBody(collisionBody); + + // Return the pointer to the rigid body + return collisionBody; +} + +// Destroy a collision body +void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { + + // Remove the body from the collision detection + collisionDetection.removeBody(collisionBody); + + // Add the body ID to the list of free IDs + freeBodiesIDs.push_back(collisionBody->getID()); + + // Call the constructor of the collision body + collisionBody->CollisionBody::~CollisionBody(); + + // Remove the collision body from the list of bodies + bodies.erase(collisionBody); // TOOD : Maybe use a set to make this faster + + // Free the object from the memory pool + memoryPoolCollisionBodies.freeObject(collisionBody); +} + +// Return the next available body ID +bodyindex CollisionWorld::computeNextAvailableBodyID() { + // Compute the body ID bodyindex bodyID; - if (!freeRigidBodyIDs.empty()) { - bodyID = freeRigidBodyIDs.back(); - freeRigidBodyIDs.pop_back(); + if (!freeBodiesIDs.empty()) { + bodyID = freeBodiesIDs.back(); + freeBodiesIDs.pop_back(); } else { bodyID = currentBodyID; currentBodyID++; } - // Largest index cannot be used (it is used for invalid index) - assert(bodyID < std::numeric_limits::max()); - - // Create the rigid body - RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collisionShape, bodyID); - - // Add the rigid body to the physics world - bodies.insert(rigidBody); - rigidBodies.insert(rigidBody); - - // Add the rigid body to the collision detection - collisionDetection->addBody(rigidBody); - - // Return the pointer to the rigid body - return rigidBody; -} - -// Destroy a rigid body -void PhysicsWorld::destroyRigidBody(RigidBody* 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 - 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 - memoryPoolRigidBodies.freeObject(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(); ) { - - // Try a downcasting - Contact* contact = dynamic_cast(*it); - - // If the constraint is a contact - if (contact) { - // Remove it from the constraints of the physics world - it = constraints.erase(it); - } - else { - ++it; - } - } + return bodyID; } -// Remove all constraints in the physics world -void PhysicsWorld::removeAllConstraints() { - constraints.clear(); -} diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h new file mode 100644 index 00000000..d7b29c39 --- /dev/null +++ b/src/engine/CollisionWorld.h @@ -0,0 +1,93 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef COLLISION_WORLD_H +#define COLLISION_WORLD_H + +// Libraries +#include +#include +#include +#include "../mathematics/mathematics.h" +#include "../body/CollisionBody.h" +#include "OverlappingPair.h" +#include "../collision/CollisionDetection.h" +#include "../constraint/Constraint.h" +#include "../constraint/Contact.h" +#include "../memory/MemoryPool.h" + +// Namespace reactphysics3d +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class CollisionWorld : + This class represent a world where it is possible to move bodies + by hand and to test collision between each other. In this kind of + world, the bodies movement is not computed using the laws of physics. + ------------------------------------------------------------------- +*/ +class CollisionWorld { + + protected : + CollisionDetection collisionDetection; // Reference to the collision detection + std::set bodies; // All the bodies (rigid and soft) of the physics world + std::map, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies + bodyindex currentBodyID; // Current body ID + MemoryPool memoryPoolCollisionBodies; // Memory pool for rigid bodies memory allocation + std::vector freeBodiesIDs; // List of free ID for rigid bodies + + virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair); // Notify the world about a new broad-phase overlapping pair + virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); // Notify the world about a removed broad-phase overlapping pair + virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo); // Notify the world about a new narrow-phase contact + virtual void updateOverlappingPair(const BroadPhasePair* pair); // Update the overlapping pair + bodyindex computeNextAvailableBodyID(); // Return the next available body ID + + + public : + CollisionWorld(); // Constructor + virtual ~CollisionWorld(); // Destructor + std::set::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world + std::set::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world + CollisionBody* createCollisionBody(const Transform& transform, + CollisionShape* collisionShape); // Create a collision body + void destroyCollisionBody(CollisionBody* collisionBody); // Destroy a collision body + + // Friends + friend class CollisionDetection; +}; + +// Return an iterator to the beginning of the bodies of the physics world +inline std::set::iterator CollisionWorld::getBodiesBeginIterator() { + return bodies.begin(); +} + +// Return an iterator to the end of the bodies of the physics world +inline std::set::iterator CollisionWorld::getBodiesEndIterator() { + return bodies.end(); +} + +} // End of the ReactPhysics3D namespace + + #endif diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 4e8592d4..cd98bb64 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -25,6 +25,7 @@ // Libraries #include "ConstraintSolver.h" +#include "DynamicsWorld.h" #include "../body/RigidBody.h" using namespace reactphysics3d; @@ -32,8 +33,8 @@ using namespace std; // Constructor -ConstraintSolver::ConstraintSolver(PhysicsWorld* world) - :physicsWorld(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS), +ConstraintSolver::ConstraintSolver(DynamicsWorld* world) + :world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS), nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION), isErrorCorrectionActive(false) { @@ -53,7 +54,7 @@ void ConstraintSolver::initialize() { // For each constraint vector::iterator it; - for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); ++it) { + for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { constraint = *it; // If the constraint is active diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index f4d90f33..2f194254 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -29,12 +29,14 @@ // Libraries #include "../configuration.h" #include "../constraint/Constraint.h" -#include "PhysicsWorld.h" #include #include // ReactPhysics3D namespace namespace reactphysics3d { + +// Declarations +class DynamicsWorld; /* ------------------------------------------------------------------- @@ -64,7 +66,7 @@ namespace reactphysics3d { */ class ConstraintSolver { private: - PhysicsWorld* physicsWorld; // Reference to the physics world + DynamicsWorld* world; // Reference to the world std::vector activeConstraints; // Current active constraints in the physics world bool isErrorCorrectionActive; // True if error correction (with world order) is active uint nbIterationsLCP; // Number of iterations of the LCP solver @@ -125,7 +127,7 @@ class ConstraintSolver { void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection public: - ConstraintSolver(PhysicsWorld* world); // Constructor + ConstraintSolver(DynamicsWorld* world); // Constructor virtual ~ConstraintSolver(); // Destructor void solve(decimal dt); // Solve the current LCP problem bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint @@ -232,4 +234,4 @@ inline void ConstraintSolver::solve(decimal dt) { } // End of ReactPhysics3D namespace -#endif \ No newline at end of file +#endif diff --git a/src/engine/PhysicsEngine.cpp b/src/engine/DynamicsWorld.cpp similarity index 58% rename from src/engine/PhysicsEngine.cpp rename to src/engine/DynamicsWorld.cpp index 969d788b..8e990f29 100644 --- a/src/engine/PhysicsEngine.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -24,28 +24,32 @@ ********************************************************************************/ // Libraries -#include "PhysicsEngine.h" +#include "DynamicsWorld.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; using namespace std; // Constructor -PhysicsEngine::PhysicsEngine(PhysicsWorld* world, decimal timeStep = DEFAULT_TIMESTEP) - : world(world), collisionDetection(world), timer(timeStep), constraintSolver(world), +DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) + : CollisionWorld(), timer(timeStep), gravity(gravity), isGravityOn(true), constraintSolver(this), isDeactivationActive(DEACTIVATION_ENABLED) { - assert(world); - assert(timeStep > 0.0); - world->setCollisionDetection(&collisionDetection); + } // Destructor -PhysicsEngine::~PhysicsEngine() { +DynamicsWorld::~DynamicsWorld() { + // 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); + } } // Update the physics simulation -void PhysicsEngine::update() { +void DynamicsWorld::update() { bool existCollision = false; assert(timer.getIsRunning()); @@ -58,7 +62,10 @@ void PhysicsEngine::update() { // While the time accumulator is not empty while(timer.isPossibleToTakeStep()) { + existCollision = false; + + removeAllContactConstraints(); // Compute the collision detection if (collisionDetection.computeCollisionDetection()) { @@ -96,7 +103,7 @@ void PhysicsEngine::update() { // and orientation of each body. // This method uses the semi-implicit Euler method to update the position and // orientation of the body -void PhysicsEngine::updateAllBodiesMotion() { +void DynamicsWorld::updateAllBodiesMotion() { decimal dt = timer.getTimeStep(); Vector3 newLinearVelocity; Vector3 newAngularVelocity; @@ -104,7 +111,7 @@ void PhysicsEngine::updateAllBodiesMotion() { Vector3 angularVelocityErrorCorrection; // For each body of thephysics world - for (set::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) { + for (set::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { RigidBody* rigidBody = *it; assert(rigidBody); @@ -148,7 +155,7 @@ void PhysicsEngine::updateAllBodiesMotion() { // Update the position and orientation of a body // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // orientation of the body -void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity, +void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity, const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) { decimal dt = timer.getTimeStep(); @@ -177,14 +184,14 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con } // Compute and set the interpolation factor to all bodies -void PhysicsEngine::setInterpolationFactorToAllBodies() { +void DynamicsWorld::setInterpolationFactorToAllBodies() { // Compute the interpolation factor decimal factor = timer.computeInterpolationFactor(); assert(factor >= 0.0 && factor <= 1.0); // Set the factor to all bodies - for (set::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) { + for (set::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { RigidBody* rigidBody = dynamic_cast(*it); assert(rigidBody); @@ -194,18 +201,140 @@ void PhysicsEngine::setInterpolationFactorToAllBodies() { } // Apply the gravity force to all bodies of the physics world -void PhysicsEngine::applyGravity() { +void DynamicsWorld::applyGravity() { // For each body of the physics world - for (set::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) { + for (set::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { RigidBody* rigidBody = dynamic_cast(*it); assert(rigidBody); // If the gravity force is on - if(world->getIsGravityOn()) { + if(isGravityOn) { // Apply the current gravity force to the body - rigidBody->setExternalForce(rigidBody->getMass() * world->getGravity()); + rigidBody->setExternalForce(rigidBody->getMass() * gravity); } } } + +// Create a rigid body into the physics world +RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass, + const Matrix3x3& inertiaTensorLocal, + CollisionShape* collisionShape) { + + // Compute the body ID + bodyindex bodyID = computeNextAvailableBodyID(); + + // Largest index cannot be used (it is used for invalid index) + assert(bodyID < std::numeric_limits::max()); + + // Create the rigid body + RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collisionShape, bodyID); + + // Add the rigid body to the physics world + bodies.insert(rigidBody); + rigidBodies.insert(rigidBody); + + // Add the rigid body to the collision detection + collisionDetection.addBody(rigidBody); + + // Return the pointer to the rigid body + return rigidBody; +} + +// Destroy a rigid body +void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { + + // Remove the body from the collision detection + collisionDetection.removeBody(rigidBody); + + // Add the body ID to the list of free IDs + freeBodiesIDs.push_back(rigidBody->getID()); + + // Call the constructor of the rigid body + 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 + memoryPoolRigidBodies.freeObject(rigidBody); +} + +// Remove all collision contacts constraints +// TODO : This method should be in the collision detection class +void DynamicsWorld::removeAllContactConstraints() { + // For all constraints + for (vector::iterator it = constraints.begin(); it != constraints.end(); ) { + + // Try a downcasting + Contact* contact = dynamic_cast(*it); + + // If the constraint is a contact + if (contact) { + // Remove it from the constraints of the physics world + it = constraints.erase(it); + } + else { + ++it; + } + } +} + +// Remove all constraints in the physics world +void DynamicsWorld::removeAllConstraints() { + constraints.clear(); +} + +// Notify the world about a new broad-phase overlapping pair +void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) { + + // Get the pair of body index + std::pair indexPair = addedPair->getBodiesIndexPair(); + + // Add the pair into the set of overlapping pairs (if not there yet) + OverlappingPair* newPair = new (memoryPoolOverlappingPairs.allocateObject()) OverlappingPair(addedPair->body1, addedPair->body2, memoryPoolContacts); + std::pair, OverlappingPair*>::iterator, bool> check = overlappingPairs.insert(make_pair(indexPair, newPair)); + assert(check.second); +} + +// Notify the world about a removed broad-phase overlapping pair +void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) { + + // Get the pair of body index + std::pair indexPair = removedPair->getBodiesIndexPair(); + + // Remove the overlapping pair from the memory pool + overlappingPairs[indexPair]->OverlappingPair::~OverlappingPair(); + memoryPoolOverlappingPairs.freeObject(overlappingPairs[indexPair]); + overlappingPairs.erase(indexPair); +} + +// Notify the world about a new narrow-phase contact +void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const ContactInfo* contactInfo) { + + RigidBody* const rigidBody1 = dynamic_cast(broadPhasePair->body1); + RigidBody* const rigidBody2 = dynamic_cast(broadPhasePair->body2); + + assert(rigidBody1); + assert(rigidBody2); + + // Create a new contact + Contact* contact = new (memoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo); + assert(contact); + + // Get the corresponding overlapping pair + pair indexPair = broadPhasePair->getBodiesIndexPair(); + OverlappingPair* overlappingPair = overlappingPairs[indexPair]; + assert(overlappingPair); + + // Add the contact to the contact cache of the corresponding overlapping pair + overlappingPair->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; igetNbContacts(); i++) { + addConstraint(overlappingPair->getContact(i)); + } +} diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h new file mode 100644 index 00000000..8082bc55 --- /dev/null +++ b/src/engine/DynamicsWorld.h @@ -0,0 +1,200 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef DYNAMICS_WORLD_H +#define DYNAMICS_WORLD_H + +// Libraries +#include "CollisionWorld.h" +#include "../collision/CollisionDetection.h" +#include "ConstraintSolver.h" +#include "../body/RigidBody.h" +#include "Timer.h" +#include "../configuration.h" + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class DynamicsWorld : + This class represents a dynamics world. This class inherits from + the CollisionWorld class. In a dynamics world, bodies can collide + and their movements are simulated using the laws of physics. + ------------------------------------------------------------------- +*/ +class DynamicsWorld : public CollisionWorld { + + protected : + Timer timer; // Timer of the physics engine + ConstraintSolver constraintSolver; // Constraint solver + bool isDeactivationActive; // True if the deactivation (sleeping) of inactive bodies is enabled + std::set rigidBodies; // All the rigid bodies of the physics world + std::vector constraints; // List that contains all the current constraints + Vector3 gravity; // Gravity vector of the world + bool isGravityOn; // True if the gravity force is on + MemoryPool memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs + MemoryPool memoryPoolRigidBodies; // Memory pool for rigid bodies memory allocation + MemoryPool memoryPoolContacts; // Memory pool for the contacts + + void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations + void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity, + 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 applyGravity(); // Apply the gravity force to all bodies + void resetBodiesMovementVariable(); // Reset the boolean movement variable of each body + virtual void updateOverlappingPair(const BroadPhasePair* pair); // Update the overlapping pair + virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair); // Notify the world about a new broad-phase overlapping pair + virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); // Notify the world about a removed broad-phase overlapping pair + virtual void notifyNewContact(const BroadPhasePair* pair, + const ContactInfo* contactInfo); // Notify the world about a new narrow-phase contact + +public : + DynamicsWorld(const Vector3& gravity, decimal timeStep); // Constructor + virtual ~DynamicsWorld(); // Destructor + + Timer& getTimer() {return timer;} + void start(); // Start the physics simulation + void stop(); // Stop the physics simulation + void update(); // Update the physics simulation + void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver + void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value + RigidBody* createRigidBody(const Transform& transform, decimal mass, + const Matrix3x3& inertiaTensorLocal, CollisionShape* collisionShape); // Create a rigid body into the physics world + void destroyRigidBody(RigidBody* rigidBody); // Destroy a rigid body + 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::set::iterator getRigidBodiesBeginIterator(); // Return an iterator to the beginning of the rigid bodies of the physics world + std::set::iterator getRigidBodiesEndIterator(); // Return an iterator to the end of the rigid bodies of the physics world +}; + +// --- Inline functions --- // + +// Start the physics simulation +inline void DynamicsWorld::start() { + timer.start(); +} + +inline void DynamicsWorld::stop() { + std::cout << "Stop Simulation" << std::endl; + timer.stop(); +} + +// Set the number of iterations of the LCP solver +inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) { + constraintSolver.setNbLCPIterations(nbIterations); +} + +// Set the isErrorCorrectionActive value +inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { + constraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive); +} + +// Reset the boolean movement variable of each body +inline void DynamicsWorld::resetBodiesMovementVariable() { + + // For each rigid body + for (std::set::iterator it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); it++) { + + // Set the hasMoved variable to false + (*it)->setHasMoved(false); + } +} + +// Update the overlapping pair +inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) { + + // Get the pair of body index + std::pair indexPair = pair->getBodiesIndexPair(); + + // Get the corresponding overlapping pair + OverlappingPair* overlappingPair = overlappingPairs[indexPair]; + + // Update the contact cache of the overlapping pair + overlappingPair->update(); +} + + +// Add a constraint into the physics world +inline void DynamicsWorld::addConstraint(Constraint* constraint) { + assert(constraint != 0); + constraints.push_back(constraint); +} + +// Remove a constraint and free its memory +inline void DynamicsWorld::removeConstraint(Constraint* constraint) { + std::vector::iterator it; + + assert(constraint); + it = std::find(constraints.begin(), constraints.end(), constraint); + assert(*it == constraint); + delete *it; + constraints.erase(it); +} + +// Return the gravity vector of the world +inline Vector3 DynamicsWorld::getGravity() const { + return gravity; +} + +// Return if the gravity is on +inline bool DynamicsWorld::getIsGravityOn() const { + return isGravityOn; +} + +// Set the isGravityOn attribute +inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) { + this->isGravityOn = isGravityOn; +} + +// Return an iterator to the beginning of the bodies of the physics world +inline std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { + return rigidBodies.begin(); +} + +// Return an iterator to the end of the bodies of the physics world +inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() { + return rigidBodies.end(); +} + +// Return a start iterator on the constraint list +inline std::vector::iterator DynamicsWorld::getConstraintsBeginIterator() { + return constraints.begin(); +} + +// Return a end iterator on the constraint list +inline std::vector::iterator DynamicsWorld::getConstraintsEndIterator() { + return constraints.end(); +} + +} + +#endif diff --git a/src/collision/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp similarity index 95% rename from src/collision/OverlappingPair.cpp rename to src/engine/OverlappingPair.cpp index 035d37e0..f901404d 100644 --- a/src/collision/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor -OverlappingPair::OverlappingPair(Body* body1, Body* body2, MemoryPool& memoryPoolContacts) +OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2, MemoryPool& memoryPoolContacts) : body1(body1), body2(body2), contactsCache(body1, body2, memoryPoolContacts), cachedSeparatingAxis(1.0, 1.0, 1.0) { } diff --git a/src/collision/OverlappingPair.h b/src/engine/OverlappingPair.h similarity index 88% rename from src/collision/OverlappingPair.h rename to src/engine/OverlappingPair.h index 80248846..6fe4004f 100644 --- a/src/collision/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -43,17 +43,17 @@ namespace reactphysics3d { */ 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 + CollisionBody* const body1; // Pointer to the first body of the contact + CollisionBody* 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(CollisionBody* body1, CollisionBody* 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 + CollisionBody* const getBody1() const; // Return the pointer to first body + CollisionBody* 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 @@ -63,12 +63,12 @@ class OverlappingPair { }; // Return the pointer to first body -inline Body* const OverlappingPair::getBody1() const { +inline CollisionBody* const OverlappingPair::getBody1() const { return body1; } // Return the pointer to second body -inline Body* const OverlappingPair::getBody2() const { +inline CollisionBody* const OverlappingPair::getBody2() const { return body2; } diff --git a/src/collision/PersistentContactCache.cpp b/src/engine/PersistentContactCache.cpp similarity index 100% rename from src/collision/PersistentContactCache.cpp rename to src/engine/PersistentContactCache.cpp diff --git a/src/collision/PersistentContactCache.h b/src/engine/PersistentContactCache.h similarity index 100% rename from src/collision/PersistentContactCache.h rename to src/engine/PersistentContactCache.h diff --git a/src/engine/PhysicsEngine.h b/src/engine/PhysicsEngine.h deleted file mode 100644 index a81efe31..00000000 --- a/src/engine/PhysicsEngine.h +++ /dev/null @@ -1,107 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef PHYSICS_ENGINE_H -#define PHYSICS_ENGINE_H - -// Libraries -#include "PhysicsWorld.h" -#include "../collision/CollisionDetection.h" -#include "ConstraintSolver.h" -#include "../body/RigidBody.h" -#include "Timer.h" -#include "../configuration.h" - -// Namespace ReactPhysics3D -namespace reactphysics3d { - -/* ------------------------------------------------------------------- - Class PhysicsEngine : - This class represents the physics engine - of the library. - ------------------------------------------------------------------- -*/ -class PhysicsEngine { - private : - PhysicsWorld* world; // Pointer to the physics world of the physics engine - Timer timer; // Timer of the physics engine - CollisionDetection collisionDetection; // Collision detection - ConstraintSolver constraintSolver; // Constraint solver - 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 updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity, - 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 applyGravity(); // Apply the gravity force to all bodies - void resetBodiesMovementVariable(); // Reset the boolean movement variable of each body - -public : - PhysicsEngine(PhysicsWorld* world, decimal timeStep); // Constructor - ~PhysicsEngine(); // Destructor - - void start(); // Start the physics simulation - void stop(); // Stop the physics simulation - void update(); // Update the physics simulation - void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver - void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value - -}; - -// --- Inline functions --- // - -// Start the physics simulation -inline void PhysicsEngine::start() { - timer.start(); -} - -inline void PhysicsEngine::stop() { - timer.stop(); -} - -// Set the number of iterations of the LCP solver -inline void PhysicsEngine::setNbLCPIterations(uint nbIterations) { - constraintSolver.setNbLCPIterations(nbIterations); -} - -// Set the isErrorCorrectionActive value -inline void PhysicsEngine::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { - constraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive); -} - -// Reset the boolean movement variable of each body -inline void PhysicsEngine::resetBodiesMovementVariable() { - - // For each rigid body - for (std::set::iterator it = world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); it++) { - - // Set the hasMoved variable to false - (*it)->setHasMoved(false); - } -} - -} - -#endif diff --git a/src/engine/PhysicsWorld.h b/src/engine/PhysicsWorld.h deleted file mode 100644 index 12d28181..00000000 --- a/src/engine/PhysicsWorld.h +++ /dev/null @@ -1,155 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef PHYSICS_WORLD_H -#define PHYSICS_WORLD_H - -// Libraries -#include -#include -#include -#include "../mathematics/mathematics.h" -#include "../body/Body.h" -#include "../collision/CollisionDetection.h" -#include "../constraint/Constraint.h" -#include "../constraint/Contact.h" -#include "../memory/MemoryPool.h" - -// Namespace reactphysics3d -namespace reactphysics3d { - - -/* ------------------------------------------------------------------- - Class PhysicsWorld : - This class represents the world of the - physics engine. The physics world contains all the bodies of the physics - engine. - ------------------------------------------------------------------- -*/ -class PhysicsWorld { - protected : - CollisionDetection* collisionDetection; // Reference to the collision detection - std::set bodies; // All the bodies (rigid and soft) of the physics world - std::set rigidBodies; // All the rigid bodies of the physics world - std::vector freeRigidBodyIDs; // List of free ID for rigid bodies - std::vector constraints; // List that contains all the current constraints - Vector3 gravity; // Gravity vector of the world - bool isGravityOn; // True if the gravity force is on - bodyindex currentBodyID; // Current body ID - MemoryPool memoryPoolRigidBodies; // Memory pool for rigid bodies memory allocation - - public : - PhysicsWorld(const Vector3& gravity); // Constructor - virtual ~PhysicsWorld(); // Destructor - - RigidBody* createRigidBody(const Transform& transform, decimal mass, - const Matrix3x3& inertiaTensorLocal, CollisionShape* collisionShape); // Create a rigid body into the physics world - void destroyRigidBody(RigidBody* rigidBody); // Destroy a rigid body - 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 setCollisionDetection(CollisionDetection* collisionDetection); // Set the collision detection reference - 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::set::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world - std::set::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world - std::set::iterator getRigidBodiesBeginIterator(); // Return an iterator to the beginning of the rigid bodies of the physics world - std::set::iterator getRigidBodiesEndIterator(); // Return an iterator to the end of the rigid bodies of the physics world -}; - -// Add a constraint into the physics world -inline void PhysicsWorld::addConstraint(Constraint* constraint) { - assert(constraint != 0); - constraints.push_back(constraint); -} - -// Remove a constraint and free its memory -inline void PhysicsWorld::removeConstraint(Constraint* constraint) { - std::vector::iterator it; - - assert(constraint); - it = std::find(constraints.begin(), constraints.end(), constraint); - assert(*it == constraint); - delete *it; - constraints.erase(it); -} - -// Return the gravity vector of the world -inline Vector3 PhysicsWorld::getGravity() const { - return gravity; -} - -// Return if the gravity is on -inline bool PhysicsWorld::getIsGravityOn() const { - return isGravityOn; -} - -// Set the isGravityOn attribute -inline void PhysicsWorld::setIsGratityOn(bool 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 -inline std::vector::iterator PhysicsWorld::getConstraintsBeginIterator() { - return constraints.begin(); -} - -// Return a end iterator on the constraint list -inline std::vector::iterator PhysicsWorld::getConstraintsEndIterator() { - return constraints.end(); -} - -// Return an iterator to the beginning of the bodies of the physics world -inline std::set::iterator PhysicsWorld::getBodiesBeginIterator() { - return bodies.begin(); -} - -// Return an iterator to the end of the bodies of the physics world -inline std::set::iterator PhysicsWorld::getBodiesEndIterator() { - return bodies.end(); -} - -// Return an iterator to the beginning of the bodies of the physics world -inline std::set::iterator PhysicsWorld::getRigidBodiesBeginIterator() { - return rigidBodies.begin(); -} - -// Return an iterator to the end of the bodies of the physics world -inline std::set::iterator PhysicsWorld::getRigidBodiesEndIterator() { - return rigidBodies.end(); -} - -} // End of the ReactPhysics3D namespace - - #endif diff --git a/src/engine/Timer.h b/src/engine/Timer.h index 49d8f373..ff8d5e15 100644 --- a/src/engine/Timer.h +++ b/src/engine/Timer.h @@ -126,6 +126,7 @@ inline void Timer::start() { // Stop the timer inline void Timer::stop() { + std::cout << "Timer stop" << std::endl; isRunning = false; } diff --git a/src/memory/MemoryPool.h b/src/memory/MemoryPool.h index 49fbc182..50828054 100644 --- a/src/memory/MemoryPool.h +++ b/src/memory/MemoryPool.h @@ -65,8 +65,8 @@ class MemoryPool { static const uint NB_OBJECTS_FIRST_BLOCK; // Number of objects allocated in the first block void* pBlocks; // Pointer to the first allocated memory block - struct MemoryUnit* pAllocatedUnits; // Pointer to the first allocated memory unit - struct MemoryUnit* pFreeUnits; // Pointer to the first free memory unit + MemoryUnit* pAllocatedUnits; // Pointer to the first allocated memory unit + MemoryUnit* pFreeUnits; // Pointer to the first free memory unit uint currentNbObjects; // Current number of objects in the pool uint capacity; // Current maximum number of objects that can be allocated in the pool uint nbObjectsNextBlock; // Number of objects to allocate in the next block @@ -110,8 +110,8 @@ MemoryPool::~MemoryPool() { assert(currentNbObjects == 0); // Release all the allocated memory blocks - struct MemoryBlock* currentBlock = (struct MemoryBlock*) pBlocks; - while(currentBlock != 0) { + MemoryBlock* currentBlock = (MemoryBlock*) pBlocks; + while(currentBlock) { MemoryBlock* tempBlock = currentBlock->pNext; free(currentBlock); currentBlock = tempBlock; @@ -134,7 +134,7 @@ void* MemoryPool::allocateObject() { assert(currentNbObjects < capacity); assert(pFreeUnits); - struct MemoryUnit* currentUnit = pFreeUnits; + MemoryUnit* currentUnit = pFreeUnits; pFreeUnits = currentUnit->pNext; if (pFreeUnits) { pFreeUnits->pPrevious = 0; @@ -149,7 +149,7 @@ void* MemoryPool::allocateObject() { currentNbObjects++; // Return a pointer to the allocated memory unit - return (void*)((char*)currentUnit + sizeof(struct MemoryUnit)); + return (void*)((char*)currentUnit + sizeof(MemoryUnit)); } // Tell the pool that an object does not need to be stored in the pool anymore @@ -163,7 +163,7 @@ void MemoryPool::freeObject(void* pObjectToFree) { // The pointer location must be inside the memory block //assert(pBlockspNext; if (pAllocatedUnits) { pAllocatedUnits->pPrevious = 0; @@ -185,9 +185,9 @@ template void MemoryPool::allocateMemory() { // Compute the size of the new - size_t sizeBlock = nbObjectsNextBlock * (sizeof(struct MemoryUnit) + sizeof(T)); + size_t sizeBlock = nbObjectsNextBlock * (sizeof(MemoryUnit) + sizeof(T)); - struct MemoryBlock* tempBlocks = (struct MemoryBlock*) pBlocks; + MemoryBlock* tempBlocks = (MemoryBlock*) pBlocks; // Allocate a new memory block pBlocks = malloc(sizeBlock); @@ -195,14 +195,14 @@ void MemoryPool::allocateMemory() { // Check that the allocation didn't fail if (!pBlocks) throw std::bad_alloc(); - struct MemoryBlock* block = (struct MemoryBlock*) pBlocks; + MemoryBlock* block = (MemoryBlock*) pBlocks; block->pNext = tempBlocks; // For each allocated memory unit in the new block for (uint i=0; ipPrevious = 0; currentUnit->pNext = pFreeUnits; diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index a6be63f4..74eb32e7 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -37,9 +37,10 @@ // Libraries #include "configuration.h" #include "mathematics/mathematics.h" +#include "body/CollisionBody.h" #include "body/RigidBody.h" -#include "engine/PhysicsWorld.h" -#include "engine/PhysicsEngine.h" +#include "engine/DynamicsWorld.h" +#include "engine/CollisionWorld.h" #include "collision/shapes/CollisionShape.h" #include "collision/shapes/BoxShape.h" #include "collision/shapes/SphereShape.h"