diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 8f2c2656..057c95cd 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -25,32 +25,129 @@ // Libraries #include "CollisionBody.h" +#include "../engine/CollisionWorld.h" #include "../engine/ContactManifold.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape, - bodyindex id) - : Body(id), mType(DYNAMIC), mCollisionShape(collisionShape), mTransform(transform), - mHasMoved(false), mContactManifoldsList(NULL) { - - assert(collisionShape); +CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id) + : Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL), + mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) { mIsCollisionEnabled = true; mInterpolationFactor = 0.0; // Initialize the old transform mOldTransform = transform; - - // Initialize the AABB for broad-phase collision detection - mCollisionShape->updateAABB(mAabb, transform); } // Destructor CollisionBody::~CollisionBody() { assert(mContactManifoldsList == NULL); + + // Remove all the proxy collision shapes of the body + removeAllCollisionShapes(); +} + +// Add a collision shape to the body. +/// This methods will create a copy of the collision shape you provided inside the world and +/// return a pointer to the actual collision shape in the world. You can use this pointer to +/// remove the collision from the body. Note that when the body is destroyed, all the collision +/// shapes will also be destroyed automatically. Because an internal copy of the collision shape +/// you provided is performed, you can delete it right after calling this method. The second +/// parameter is the transformation that transform the local-space of the collision shape into +/// the local-space of the body. By default, the second parameter is the identity transform. +/// This method will return a pointer to the proxy collision shape that links the body with +/// the collision shape you have added. +const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape, + const Transform& transform) { + + // Create an internal copy of the collision shape into the world (if it does not exist yet) + CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape); + + // Create a new proxy collision shape to attach the collision shape to the body + ProxyShape* proxyShape = newCollisionShape->createProxyShape(mWorld.mMemoryAllocator, + this, transform, decimal(1.0)); + + // Add it to the list of proxy collision shapes of the body + if (mProxyCollisionShapes == NULL) { + mProxyCollisionShapes = proxyShape; + } + else { + proxyShape->mNext = mProxyCollisionShapes; + mProxyCollisionShapes = proxyShape; + } + + // Notify the collision detection about this new collision shape + mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape); + + mNbCollisionShapes++; + + // Return a pointer to the collision shape + return proxyShape; +} + +// Remove a collision shape from the body +void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) { + + ProxyShape* current = mProxyCollisionShapes; + + // If the the first proxy shape is the one to remove + if (current->getCollisionShape() == collisionShape) { + mProxyCollisionShapes = current->mNext; + mWorld.mCollisionDetection.removeProxyCollisionShape(current); + size_t sizeBytes = current->getSizeInBytes(); + current->ProxyShape::~ProxyShape(); + mWorld.mMemoryAllocator.release(current, sizeBytes); + mNbCollisionShapes--; + return; + } + + // Look for the proxy shape that contains the collision shape in parameter + while(current->mNext != NULL) { + + // If we have found the collision shape to remove + if (current->mNext->getCollisionShape() == collisionShape) { + + // Remove the proxy collision shape + ProxyShape* elementToRemove = current->mNext; + current->mNext = elementToRemove->mNext; + mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove); + size_t sizeBytes = elementToRemove->getSizeInBytes(); + elementToRemove->ProxyShape::~ProxyShape(); + mWorld.mMemoryAllocator.release(elementToRemove, sizeBytes); + mNbCollisionShapes--; + return; + } + + // Get the next element in the list + current = current->mNext; + } + + assert(mNbCollisionShapes >= 0); +} + +// Remove all the collision shapes +void CollisionBody::removeAllCollisionShapes() { + + ProxyShape* current = mProxyCollisionShapes; + + // Look for the proxy shape that contains the collision shape in parameter + while(current != NULL) { + + // Remove the proxy collision shape + ProxyShape* nextElement = current->mNext; + mWorld.mCollisionDetection.removeProxyCollisionShape(current); + current->ProxyShape::~ProxyShape(); + mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + + // Get the next element in the list + current = nextElement; + } + + mProxyCollisionShapes = NULL; } // Reset the contact manifold lists @@ -71,3 +168,18 @@ void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) assert(mContactManifoldsList == NULL); } + +// Update the broad-phase state for this body (because it has moved for instance) +void CollisionBody::updateBroadPhaseState() const { + + // For all the proxy collision shapes of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + + // Recompute the world-space AABB of the collision shape + AABB aabb; + shape->getCollisionShape()->computeAABB(aabb, mTransform); + + // Update the broad-phase state for the proxy collision shape + mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb); + } +} diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index ccc9b068..86fe32e3 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -41,6 +41,8 @@ namespace reactphysics3d { // Class declarations struct ContactManifoldListElement; +class ProxyShape; +class CollisionWorld; /// Enumeration for the type of a body /// STATIC : A static body has infinite mass, zero velocity but the position can be @@ -67,9 +69,6 @@ class CollisionBody : public Body { /// Type of body (static, kinematic or dynamic) BodyType mType; - /// Collision shape of the body - CollisionShape* mCollisionShape; - /// Position and orientation of the body Transform mTransform; @@ -82,15 +81,18 @@ class CollisionBody : public Body { /// True if the body can collide with others bodies bool mIsCollisionEnabled; - /// AABB for Broad-Phase collision detection - AABB mAabb; + /// First element of the linked list of proxy collision shapes of this body + ProxyShape* mProxyCollisionShapes; - /// True if the body has moved during the last frame - bool mHasMoved; + /// Number of collision shapes + uint mNbCollisionShapes; /// First element of the linked list of contact manifolds involving this body ContactManifoldListElement* mContactManifoldsList; + /// Reference to the world the body belongs to + CollisionWorld& mWorld; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -102,18 +104,21 @@ class CollisionBody : public Body { /// Reset the contact manifold lists void resetContactManifoldsList(MemoryAllocator& memoryAllocator); + /// Remove all the collision shapes + void removeAllCollisionShapes(); + /// Update the old transform with the current one. void updateOldTransform(); - /// Update the Axis-Aligned Bounding Box coordinates - void updateAABB(); + /// Update the broad-phase state for this body (because it has moved for instance) + void updateBroadPhaseState() const; public : // -------------------- Methods -------------------- // /// Constructor - CollisionBody(const Transform& transform, CollisionShape* collisionShape, bodyindex id); + CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id); /// Destructor virtual ~CollisionBody(); @@ -124,20 +129,18 @@ class CollisionBody : public Body { /// Set the type of the body void setType(BodyType type); - /// Return the collision shape - CollisionShape* getCollisionShape() const; - - /// Set the collision shape - void setCollisionShape(CollisionShape* collisionShape); - /// Return the current position and orientation const Transform& getTransform() const; /// Set the current position and orientation void setTransform(const Transform& transform); - /// Return the AAABB of the body - const AABB& getAABB() const; + /// Add a collision shape to the body. + const ProxyShape* addCollisionShape(const CollisionShape& collisionShape, + const Transform& transform = Transform::identity()); + + /// Remove a collision shape from the body + virtual void removeCollisionShape(const CollisionShape* collisionShape); /// Return the interpolated transform for rendering Transform getInterpolatedTransform() const; @@ -158,6 +161,7 @@ class CollisionBody : public Body { friend class DynamicsWorld; friend class CollisionDetection; + friend class BroadPhaseAlgorithm; }; // Return the type of the body @@ -168,18 +172,12 @@ inline BodyType CollisionBody::getType() const { // Set the type of the body inline void CollisionBody::setType(BodyType type) { mType = type; -} -// Return the collision shape -inline CollisionShape* CollisionBody::getCollisionShape() const { - assert(mCollisionShape); - return mCollisionShape; -} + if (mType == STATIC) { -// Set the collision shape -inline void CollisionBody::setCollisionShape(CollisionShape* collisionShape) { - assert(collisionShape); - mCollisionShape = collisionShape; + // Update the broad-phase state of the body + updateBroadPhaseState(); + } } // Return the interpolated transform for rendering @@ -201,20 +199,14 @@ inline const Transform& CollisionBody::getTransform() const { // Set the current position and orientation inline void CollisionBody::setTransform(const Transform& transform) { - // Check if the body has moved - if (mTransform != transform) { - mHasMoved = true; - } - + // Update the transform of the body mTransform = transform; + + // Update the broad-phase state of the body + updateBroadPhaseState(); } -// Return the AAABB of the body -inline const AABB& CollisionBody::getAABB() const { - return mAabb; -} - - // Return true if the body can collide with others bodies +// Return true if the body can collide with others bodies inline bool CollisionBody::isCollisionEnabled() const { return mIsCollisionEnabled; } @@ -230,13 +222,6 @@ inline void CollisionBody::updateOldTransform() { mOldTransform = mTransform; } -// Update the rigid body in order to reflect a change in the body state -inline void CollisionBody::updateAABB() { - - // Update the AABB - mCollisionShape->updateAABB(mAabb, mTransform); -} - // Return the first element of the linked list of contact manifolds involving this body inline const ContactManifoldListElement* CollisionBody::getContactManifoldsLists() const { return mContactManifoldsList; diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index bbc2de79..b36f3d83 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -27,29 +27,20 @@ #include "RigidBody.h" #include "constraint/Joint.h" #include "../collision/shapes/CollisionShape.h" +#include "../engine/CollisionWorld.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -RigidBody::RigidBody(const Transform& transform, decimal mass, CollisionShape *collisionShape, - bodyindex id) - : CollisionBody(transform, collisionShape, id), mInitMass(mass), mIsGravityEnabled(true), - mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(NULL) { - - assert(collisionShape); - - // If the mass is not positive, set it to one - if (mInitMass <= decimal(0.0)) { - mInitMass = decimal(1.0); - } - - // Compute the inertia tensor using the collision shape of the body - mCollisionShape->computeLocalInertiaTensor(mInertiaTensorLocal, mInitMass); - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); +RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) + : CollisionBody(transform, world, id), mInitMass(decimal(1.0)), + mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), + mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), + mJointsList(NULL) { // Compute the inverse mass - mMassInverse = decimal(1.0) / mass; + mMassInverse = decimal(1.0) / mInitMass; } // Destructor @@ -64,6 +55,9 @@ void RigidBody::setType(BodyType type) { CollisionBody::setType(type); + // Recompute the total mass, center of mass and inertia tensor + recomputeMassInformation(); + // If it is a static body if (mType == STATIC) { @@ -77,7 +71,8 @@ void RigidBody::setType(BodyType type) { // Reset the inverse mass and inverse inertia tensor to zero mMassInverse = decimal(0.0); - mInertiaTensorLocalInverse = Matrix3x3::zero(); + mInertiaTensorLocal.setToZero(); + mInertiaTensorLocalInverse.setToZero(); } else { // If it is a dynamic body @@ -89,24 +84,6 @@ void RigidBody::setType(BodyType type) { setIsSleeping(false); } -// Method that set the mass of the body -void RigidBody::setMass(decimal mass) { - mInitMass = mass; - - // If the mass is negative, set it to one - if (mInitMass <= decimal(0.0)) { - mInitMass = decimal(1.0); - } - - // Recompute the inverse mass - if (mType == DYNAMIC) { - mMassInverse = decimal(1.0) / mInitMass; - } - else { - mMassInverse = decimal(0.0); - } -} - // Set the local inertia tensor of the body (in body coordinates) void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { mInertiaTensorLocal = inertiaTensorLocal; @@ -148,4 +125,131 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons } } +// Add a collision shape to the body. +/// This methods will create a copy of the collision shape you provided inside the world and +/// return a pointer to the actual collision shape in the world. You can use this pointer to +/// remove the collision from the body. Note that when the body is destroyed, all the collision +/// shapes will also be destroyed automatically. Because an internal copy of the collision shape +/// you provided is performed, you can delete it right after calling this method. The second +/// parameter is the transformation that transform the local-space of the collision shape into +/// the local-space of the body. By default, the second parameter is the identity transform. +/// The third parameter is the mass of the collision shape (this will used to compute the +/// total mass of the rigid body and its inertia tensor). The mass must be positive. +const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape, + decimal mass, + const Transform& transform + ) { + + assert(mass > decimal(0.0)); + + // Create an internal copy of the collision shape into the world if it is not there yet + CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape); + + // Create a new proxy collision shape to attach the collision shape to the body + ProxyShape* proxyShape = newCollisionShape->createProxyShape(mWorld.mMemoryAllocator, + this, transform, mass); + + // Add it to the list of proxy collision shapes of the body + if (mProxyCollisionShapes == NULL) { + mProxyCollisionShapes = proxyShape; + } + else { + proxyShape->mNext = mProxyCollisionShapes; + mProxyCollisionShapes = proxyShape; + } + + // Notify the collision detection about this new collision shape + mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape); + + mNbCollisionShapes++; + + // Recompute the center of mass, total mass and inertia tensor of the body with the new + // collision shape + recomputeMassInformation(); + + // Return a pointer to the collision shape + return newCollisionShape; +} + +// Remove a collision shape from the body +void RigidBody::removeCollisionShape(const CollisionShape* collisionShape) { + + // Remove the collision shape + CollisionBody::removeCollisionShape(collisionShape); + + // Recompute the total mass, center of mass and inertia tensor + recomputeMassInformation(); +} + +// Recompute the center of mass, total mass and inertia tensor of the body using all +// the collision shapes attached to the body. +void RigidBody::recomputeMassInformation() { + + mInitMass = decimal(0.0); + mMassInverse = decimal(0.0); + mInertiaTensorLocal.setToZero(); + mInertiaTensorLocalInverse.setToZero(); + mCenterOfMassLocal.setToZero(); + + // If it is STATIC or KINEMATIC body + if (mType == STATIC || mType == KINEMATIC) { + mCenterOfMassWorld = mTransform.getPosition(); + return; + } + + assert(mType == DYNAMIC); + + // Compute the total mass of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + mInitMass += shape->getMass(); + mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); + } + + if (mInitMass > decimal(0.0)) { + mMassInverse = decimal(1.0) / mInitMass; + } + else { + mInitMass = decimal(1.0); + mMassInverse = decimal(1.0); + } + + // Compute the center of mass + const Vector3 oldCenterOfMass = mCenterOfMassWorld; + mCenterOfMassLocal *= mMassInverse; + mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + + // Compute the total mass and inertia tensor using all the collision shapes + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + + // Get the inertia tensor of the collision shape in its local-space + Matrix3x3 inertiaTensor; + shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); + + // Convert the collision shape inertia tensor into the local-space of the body + const Transform& shapeTransform = shape->getLocalToBodyTransform(); + Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); + inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); + + // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape + // center into a inertia tensor w.r.t to the body origin. + Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; + decimal offsetSquare = offset.lengthSquare(); + Matrix3x3 offsetMatrix; + offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); + offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); + offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); + offsetMatrix[0] += offset * (-offset.x); + offsetMatrix[1] += offset * (-offset.y); + offsetMatrix[2] += offset * (-offset.z); + offsetMatrix *= shape->getMass(); + + mInertiaTensorLocal += inertiaTensor + offsetMatrix; + } + + // Compute the local inverse inertia tensor + mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + + // Update the linear velocity of the center of mass + mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); +} diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 7171ee92..075c0640 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -57,6 +57,13 @@ class RigidBody : public CollisionBody { /// Intial mass of the body decimal mInitMass; + /// Center of mass of the body in local-space coordinates. + /// The center of mass can therefore be different from the body origin + Vector3 mCenterOfMassLocal; + + /// Center of mass of the body in world-space coordinates + Vector3 mCenterOfMassWorld; + /// Linear velocity of the body Vector3 mLinearVelocity; @@ -69,7 +76,8 @@ class RigidBody : public CollisionBody { /// Current external torque on the body Vector3 mExternalTorque; - /// Local inertia tensor of the body (in local-space) + /// Local inertia tensor of the body (in local-space) with respect to the + /// center of mass of the body Matrix3x3 mInertiaTensorLocal; /// Inverse of the inertia tensor of the body @@ -104,13 +112,15 @@ class RigidBody : public CollisionBody { /// Remove a joint from the joints list void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); + /// Update the transform of the body after a change of the center of mass + void updateTransformWithCenterOfMass(); + public : // -------------------- Methods -------------------- // /// Constructor - RigidBody(const Transform& transform, decimal mass, CollisionShape* collisionShape, - bodyindex id); + RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id); /// Destructor virtual ~RigidBody(); @@ -121,9 +131,6 @@ class RigidBody : public CollisionBody { /// Return the mass of the body decimal getMass() const; - /// Set the mass of the body - void setMass(decimal mass); - /// Return the linear velocity Vector3 getLinearVelocity() const; @@ -178,8 +185,8 @@ class RigidBody : public CollisionBody { /// Set the variable to know whether or not the body is sleeping virtual void setIsSleeping(bool isSleeping); - /// Apply an external force to the body at its gravity center. - void applyForceToCenter(const Vector3& force); + /// Apply an external force to the body at its center of mass. + void applyForceToCenterOfMass(const Vector3& force); /// Apply an external force to the body at a given point (in world-space coordinates). void applyForce(const Vector3& force, const Vector3& point); @@ -187,6 +194,18 @@ class RigidBody : public CollisionBody { /// Apply an external torque to the body. void applyTorque(const Vector3& torque); + /// Add a collision shape to the body. + const CollisionShape* addCollisionShape(const CollisionShape& collisionShape, + decimal mass, + const Transform& transform = Transform::identity()); + + /// Remove a collision shape from the body + virtual void removeCollisionShape(const CollisionShape* collisionShape); + + /// Recompute the center of mass, total mass and inertia tensor of the body using all + /// the collision shapes attached to the body. + void recomputeMassInformation(); + // -------------------- Friendship -------------------- // friend class DynamicsWorld; @@ -329,12 +348,12 @@ inline void RigidBody::setIsSleeping(bool isSleeping) { Body::setIsSleeping(isSleeping); } -// Apply an external force to the body at its gravity center. +// Apply an external force to the body at its center of mass. /// If the body is sleeping, calling this method will wake it up. Note that the /// force will we added to the sum of the applied forces and that this sum will be /// reset to zero at the end of each call of the DynamicsWorld::update() method. /// You can only apply a force to a dynamic body otherwise, this method will do nothing. -inline void RigidBody::applyForceToCenter(const Vector3& force) { +inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) { // If it is not a dynamic body, we do nothing if (mType != DYNAMIC) return; @@ -349,7 +368,7 @@ inline void RigidBody::applyForceToCenter(const Vector3& force) { } // Apply an external force to the body at a given point (in world-space coordinates). -/// If the point is not at the center of gravity of the body, it will also +/// If the point is not at the center of mass of the body, it will also /// generate some torque and therefore, change the angular velocity of the body. /// If the body is sleeping, calling this method will wake it up. Note that the /// force will we added to the sum of the applied forces and that this sum will be @@ -389,6 +408,13 @@ inline void RigidBody::applyTorque(const Vector3& torque) { mExternalTorque += torque; } +/// Update the transform of the body after a change of the center of mass +inline void RigidBody::updateTransformWithCenterOfMass() { + + // Translate the body according to the translation of the center of mass position + mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal); +} + } #endif diff --git a/src/collision/BroadPhasePair.h b/src/collision/BroadPhasePair.h index 4712c9e1..3b7e7e6f 100644 --- a/src/collision/BroadPhasePair.h +++ b/src/collision/BroadPhasePair.h @@ -32,6 +32,7 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +// TODO : DELETE THIS CLASS // Structure BroadPhasePair /** * This structure represents a pair of bodies @@ -79,16 +80,7 @@ struct BroadPhasePair { bool operator!=(const BroadPhasePair& broadPhasePair2) const; }; -// Return the pair of bodies index -inline bodyindexpair BroadPhasePair::computeBodiesIndexPair(CollisionBody* body1, - CollisionBody* body2) { - // 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; -} + // Return the pair of bodies index inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const { diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index c18b437f..06243de4 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -44,20 +44,16 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) - : mWorld(world), mMemoryAllocator(memoryAllocator), + : mWorld(world), mBroadPhaseAlgorithm(*this), mNarrowPhaseGJKAlgorithm(memoryAllocator), - mNarrowPhaseSphereVsSphereAlgorithm(memoryAllocator) { + mNarrowPhaseSphereVsSphereAlgorithm(memoryAllocator), + mIsCollisionShapesAdded(false) { - // Create the broad-phase algorithm that will be used (Sweep and Prune with AABB) - mBroadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this); - assert(mBroadPhaseAlgorithm != NULL); } // Destructor CollisionDetection::~CollisionDetection() { - // Delete the broad-phase algorithm - delete mBroadPhaseAlgorithm; } // Compute the collision detection @@ -77,58 +73,58 @@ void CollisionDetection::computeBroadPhase() { PROFILE("CollisionDetection::computeBroadPhase()"); - // Notify the broad-phase algorithm about the bodies that have moved since last frame - for (set::iterator it = mWorld->getBodiesBeginIterator(); - it != mWorld->getBodiesEndIterator(); it++) { + // If new collision shapes have been added to bodies + if (mIsCollisionShapesAdded) { - // If the body has moved - if ((*it)->mHasMoved) { - - // Notify the broad-phase that the body has moved - mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB()); - } - } + // Ask the broad-phase to recompute the overlapping pairs of collision + // shapes. This call can only add new overlapping pairs in the collision + // detection. + mBroadPhaseAlgorithm.computeOverlappingPairs(); + } } // Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhase() { PROFILE("CollisionDetection::computeNarrowPhase()"); - - map::iterator it; // For each possible collision pair of bodies + map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) { ContactPointInfo* contactInfo = NULL; - BroadPhasePair* pair = (*it).second; - assert(pair != NULL); + OverlappingPair* pair = it->second; - CollisionBody* const body1 = pair->body1; - CollisionBody* const body2 = pair->body2; + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); + CollisionBody* const body1 = shape1->getBody(); + CollisionBody* const body2 = shape2->getBody(); // Update the contact cache of the overlapping pair mWorld->updateOverlappingPair(pair); // Check if the two bodies are allowed to collide, otherwise, we do not test for collision - if (pair->body1->getType() != DYNAMIC && pair->body2->getType() != DYNAMIC) continue; - if (mNoCollisionPairs.count(pair->getBodiesIndexPair()) > 0) continue; + if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue; + bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); + if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; // Check if the two bodies are sleeping, if so, we do no test collision between them if (body1->isSleeping() && body2->isSleeping()) continue; // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm( - body1->getCollisionShape(), - body2->getCollisionShape()); + shape1->getCollisionShape(), + shape2->getCollisionShape()); // Notify the narrow-phase algorithm about the overlapping pair we are going to test 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(), - body2->getCollisionShape(), body2->getTransform(), + const Transform transform1 = body1->getTransform() * shape1->getLocalToBodyTransform(); + const Transform transform2 = body2->getTransform() * shape2->getLocalToBodyTransform(); + if (narrowPhaseAlgorithm.testCollision(shape1->getCollisionShape(), transform1, + shape2->getCollisionShape(), transform2, contactInfo)) { assert(contactInfo != NULL); @@ -143,20 +139,38 @@ void CollisionDetection::computeNarrowPhase() { // Delete and remove the contact info from the memory allocator contactInfo->ContactPointInfo::~ContactPointInfo(); - mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo)); + mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo)); } } } // 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(BodyPair* addedPair) { +void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { + // Compute the overlapping pair ID + overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); + + // Check if the overlapping pair already exists + if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; + + // Create the overlapping pair and add it into the set of overlapping pairs + OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, mWorld->mMemoryAllocator); + assert(newPair != NULL); + std::pair::iterator, bool> check = + mOverlappingPairs.insert(make_pair(pairID, newPair)); + assert(check.second); + + /* TODO : DELETE THIS // Get the pair of body index bodyindexpair indexPair = addedPair->getBodiesIndexPair(); + // If the overlapping pair already exists, we don't do anything + if (mOverlappingPairs.count(indexPair) > 0) return; + // Create the corresponding broad-phase pair object - BroadPhasePair* broadPhasePair = new (mMemoryAllocator.allocate(sizeof(BroadPhasePair))) + BroadPhasePair* broadPhasePair = new (mWorld->mMemoryAllocator.allocate(sizeof(BroadPhasePair))) BroadPhasePair(addedPair->body1, addedPair->body2); assert(broadPhasePair != NULL); @@ -168,23 +182,47 @@ void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(BodyPair* addedPai // Notify the world about the new broad-phase overlapping pair mWorld->notifyAddedOverlappingPair(broadPhasePair); + */ } // Allow the broadphase to notify the collision detection about a removed overlapping pair -void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(BodyPair* removedPair) { +void CollisionDetection::removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { - // Get the pair of body index - bodyindexpair indexPair = removedPair->getBodiesIndexPair(); + // Compute the overlapping pair ID + overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); - // Get the broad-phase pair - BroadPhasePair* broadPhasePair = mOverlappingPairs.find(indexPair)->second; - assert(broadPhasePair != NULL); + // If the overlapping + std::map::iterator it; + it = mOverlappingPairs.find(indexPair); + if () // Notify the world about the removed broad-phase pair - mWorld->notifyRemovedOverlappingPair(broadPhasePair); + // TODO : DELETE THIS + //mWorld->notifyRemovedOverlappingPair(broadPhasePair); // Remove the overlapping pair from the memory allocator - broadPhasePair->BroadPhasePair::~BroadPhasePair(); - mMemoryAllocator.release(broadPhasePair, sizeof(BroadPhasePair)); - mOverlappingPairs.erase(indexPair); + mOverlappingPairs.find(pairID)->second->OverlappingPair::~OverlappingPair(); + mWorld->mMemoryAllocator.release(mOverlappingPairs[indexPair], sizeof(OverlappingPair)); + mOverlappingPairs.erase(pairID); +} + +// Remove a body from the collision detection +void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { + + // Remove all the overlapping pairs involving this proxy shape + std::map::iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + if (it->second->getShape1()->getBroadPhaseID() == proxyShape->getBroadPhaseID() || + it->second->getShape2()->getBroadPhaseID() == proxyShape->getBroadPhaseID()) { + std::map::iterator itToRemove = it; + ++it; + mOverlappingPairs.erase(itToRemove); + } + else { + ++it; + } + } + + // Remove the body from the broad-phase + mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); } diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index eee6f1a2..ed209b6f 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -29,7 +29,7 @@ // Libraries #include "../body/CollisionBody.h" #include "broadphase/BroadPhaseAlgorithm.h" -#include "BroadPhasePair.h" +#include "../engine/OverlappingPair.h" #include "narrowphase/GJK/GJKAlgorithm.h" #include "narrowphase/SphereVsSphereAlgorithm.h" #include "../memory/MemoryAllocator.h" @@ -62,14 +62,11 @@ class CollisionDetection { /// Pointer to the physics world CollisionWorld* mWorld; - /// Memory allocator - MemoryAllocator& mMemoryAllocator; - /// Broad-phase overlapping pairs - std::map mOverlappingPairs; + std::map mOverlappingPairs; /// Broad-phase algorithm - BroadPhaseAlgorithm* mBroadPhaseAlgorithm; + BroadPhaseAlgorithm mBroadPhaseAlgorithm; /// Narrow-phase GJK algorithm GJKAlgorithm mNarrowPhaseGJKAlgorithm; @@ -80,6 +77,9 @@ class CollisionDetection { /// Set of pair of bodies that cannot collide between each other std::set mNoCollisionPairs; + /// True if some collision shapes have been added previously + bool mIsCollisionShapesAdded; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -95,8 +95,11 @@ class CollisionDetection { void computeNarrowPhase(); /// Select the narrow phase algorithm to use given two collision shapes - NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1, - CollisionShape* collisionShape2); + NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1, + const CollisionShape* collisionShape2); + + /// Remove an overlapping pair if it is not overlapping anymore + void removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); public : @@ -108,31 +111,32 @@ class CollisionDetection { /// Destructor ~CollisionDetection(); - /// Add a body to the collision detection - void addBody(CollisionBody* body); + /// Add a proxy collision shape to the collision detection + void addProxyCollisionShape(ProxyShape* proxyShape); - /// Remove a body from the collision detection - void removeBody(CollisionBody* body); + /// Remove a proxy collision shape from the collision detection + void removeProxyCollisionShape(ProxyShape* proxyShape); + + /// Update a proxy collision shape (that has moved for instance) + void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb); /// Add a pair of bodies that cannot collide with each other void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); /// Remove a pair of bodies that cannot collide with each other - void removeNoCollisionPair(CollisionBody *body1, CollisionBody *body2); + void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2); /// Compute the collision detection void computeCollisionDetection(); - /// Allow the broadphase to notify the collision detection about a new overlapping pair. - void broadPhaseNotifyAddedOverlappingPair(BodyPair* pair); - - /// Allow the broadphase to notify the collision detection about a removed overlapping pair - void broadPhaseNotifyRemovedOverlappingPair(BodyPair* pair); + /// Allow the broadphase to notify the collision detection about an overlapping pair. + void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); }; // Select the narrow-phase collision algorithm to use given two collision shapes inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm( - CollisionShape* collisionShape1, CollisionShape* collisionShape2) { + const CollisionShape* collisionShape1, + const CollisionShape* collisionShape2) { // Sphere vs Sphere algorithm if (collisionShape1->getType() == SPHERE && collisionShape2->getType() == SPHERE) { @@ -144,29 +148,29 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm( } // Add a body to the collision detection -inline void CollisionDetection::addBody(CollisionBody* body) { +inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape) { // Add the body to the broad-phase - mBroadPhaseAlgorithm->addObject(body, body->getAABB()); -} + mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape); -// Remove a body from the collision detection -inline void CollisionDetection::removeBody(CollisionBody* body) { - - // Remove the body from the broad-phase - mBroadPhaseAlgorithm->removeObject(body); -} + mIsCollisionShapesAdded = true; +} // Add a pair of bodies that cannot collide with each other inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1, CollisionBody* body2) { - mNoCollisionPairs.insert(BroadPhasePair::computeBodiesIndexPair(body1, body2)); + mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); } // Remove a pair of bodies that cannot collide with each other inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2) { - mNoCollisionPairs.erase(BroadPhasePair::computeBodiesIndexPair(body1, body2)); + mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); +} + +// Update a proxy collision shape (that has moved for instance) +inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb) { + mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb); } } diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index 3ab777ea..148da8a3 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -31,11 +31,223 @@ using namespace reactphysics3d; // Constructor BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) - :mPairManager(collisionDetection), mCollisionDetection(collisionDetection) { + :mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8), + mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8), + mPairManager(collisionDetection), mCollisionDetection(collisionDetection) { + // Allocate memory for the array of non-static bodies IDs + mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); + assert(mMovedShapes); + + // Allocate memory for the array of potential overlapping pairs + mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair)); + assert(mPotentialPairs); } // Destructor BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { + // Release the memory for the array of non-static bodies IDs + free(mMovedShapes); + + // Release the memory for the array of potential overlapping pairs + free(mPotentialPairs); +} + +// Add a collision shape in the array of shapes that have moved in the last simulation step +// and that need to be tested again for broad-phase overlapping. +void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) { + + // Allocate more elements in the array of bodies that have moved if necessary + if (mNbAllocatedMovedShapes == mNbMovedShapes) { + mNbAllocatedMovedShapes *= 2; + int* oldArray = mMovedShapes; + mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); + assert(mMovedShapes); + memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int)); + free(oldArray); + } + + // Store the broad-phase ID into the array of bodies that have moved + mMovedShapes[mNbMovedShapes] = broadPhaseID; + mNbMovedShapes++; +} + +// Remove a collision shape from the array of shapes that have moved in the last simulation step +// and that need to be tested again for broad-phase overlapping. +void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { + + assert(mNbNonUsedMovedShapes <= mNbMovedShapes); + + // If less than the quarter of allocated elements of the non-static bodies IDs array + // are used, we release some allocated memory + if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 && + mNbAllocatedMovedShapes > 8) { + + mNbAllocatedMovedShapes /= 2; + int* oldArray = mMovedShapes; + mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); + assert(mMovedShapes); + uint nbElements = 0; + for (uint i=0; imBroadPhaseID); +} + +// Remove a proxy collision shape from the broad-phase collision detection +void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { + + int broadPhaseID = proxyShape->mBroadPhaseID; + + // Remove the collision shape from the dynamic AABB tree + mDynamicAABBTree.removeObject(broadPhaseID); + + // Remove the collision shape into the array of bodies that have moved (or have been created) + // during the last simulation step + removeMovedCollisionShape(broadPhaseID); +} + +// Notify the broad-phase that a collision shape has moved and need to be updated +void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { + + int broadPhaseID = proxyShape->mBroadPhaseID; + + assert(broadPhaseID >= 0); + + // Update the dynamic AABB tree according to the movement of the collision shape + bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb); + + // If the collision shape has moved out of its fat AABB (and therefore has been reinserted + // into the tree). + if (hasBeenReInserted) { + + // Add the collision shape into the array of bodies that have moved (or have been created) + // during the last simulation step + addMovedCollisionShape(broadPhaseID); + } +} + +// Compute all the overlapping pairs of collision shapes +void BroadPhaseAlgorithm::computeOverlappingPairs() { + + // Reset the potential overlapping pairs + mNbPotentialPairs = 0; + + // For all collision shapes that have moved (or have been created) during the + // last simulation step + for (uint i=0; icollisionShape1ID); + ProxyShape* shape2 = mDynamicAABBTree.getCollisionShape(pair->collisionShape2ID); + + // Notify the collision detection about the overlapping pair + mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); + + // Skip the duplicate overlapping pairs + while (i < mNbPotentialPairs) { + + // Get the next pair + BroadPair* nextPair = mPotentialPairs + i; + + // If the next pair is different from the previous one, we stop skipping pairs + if (nextPair->collisionShape1ID != pair->collisionShape1ID || + nextPair->collisionShape2ID != pair->collisionShape2ID) { + break; + } + i++; + } + } + + // If the number of potential overlapping pairs is less than the quarter of allocated + // number of overlapping pairs + if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) { + + // Reduce the number of allocated potential overlapping pairs + BroadPair* oldPairs = mPotentialPairs; + mNbAllocatedPotentialPairs /= 2; + mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair)); + assert(mPotentialPairs); + memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair)); + free(oldPairs); + } +} + +// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree +void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int node2ID) { + + // If both the nodes are the same, we do not create store the overlapping pair + if (node1ID == node2ID) return; + + // If we need to allocate more memory for the array of potential overlapping pairs + if (mNbPotentialPairs == mNbAllocatedPotentialPairs) { + + // Allocate more memory for the array of potential pairs + BroadPair* oldPairs = mPotentialPairs; + mNbAllocatedPotentialPairs *= 2; + mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair)); + assert(mPotentialPairs); + memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair)); + free(oldPairs); + } + + // Add the new potential pair into the array of potential overlapping pairs + mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID); + mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); + mNbPotentialPairs++; } diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index f509d351..0cc883b8 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -38,16 +38,42 @@ namespace reactphysics3d { // Declarations class CollisionDetection; +// TODO : Check that when a kinematic or static body is manually moved, the dynamic aabb tree +// is correctly updated + +// TODO : Replace the names "body, bodies" by "collision shapes" + +// TODO : Remove the pair manager + +// TODO : RENAME THIS +// Structure BroadPair +/** + * This structure represent a potential overlapping pair during the broad-phase collision + * detection. + */ +struct BroadPair { + + // -------------------- Attributes -------------------- // + + /// Broad-phase ID of the first collision shape + int collisionShape1ID; + + /// Broad-phase ID of the second collision shape + int collisionShape2ID; + + // -------------------- Methods -------------------- // + + /// Method used to compare two pairs for sorting algorithm + static bool smallerThan(const BroadPair& pair1, const BroadPair& pair2); +}; + // Class BroadPhaseAlgorithm /** - * This class represents an algorithm the broad-phase collision detection. The - * goal of the broad-phase collision detection is to compute the pair of bodies - * that can collide. But it's important to understand that the - * broad-phase doesn't compute only body pairs that can collide but - * could also pairs of body that doesn't collide but are very close. - * The goal of the broad-phase is to remove pairs of body that cannot - * collide in order to avoid to much bodies to be tested in the - * narrow-phase. + * This class represents the broad-phase collision detection. The + * goal of the broad-phase collision detection is to compute the pairs of bodies + * that have their AABBs overlapping. Only those pairs of bodies will be tested + * later for collision during the narrow-phase collision detection. A dynamic AABB + * tree data structure is used for fast broad-phase collision detection. */ class BroadPhaseAlgorithm { @@ -58,6 +84,32 @@ class BroadPhaseAlgorithm { /// Dynamic AABB tree DynamicAABBTree mDynamicAABBTree; + /// Array with the broad-phase IDs of all collision shapes that have moved (or have been + /// created) during the last simulation step. Those are the shapes that need to be tested + /// for overlapping in the next simulation step. + int* mMovedShapes; + + /// Number of collision shapes in the array of shapes that have moved during the last + /// simulation step. + uint mNbMovedShapes; + + /// Number of allocated elements for the array of shapes that have moved during the last + /// simulation step. + uint mNbAllocatedMovedShapes; + + /// Number of non-used elements in the array of shapes that have moved during the last + /// simulation step. + uint mNbNonUsedMovedShapes; + + /// Temporary array of potential overlapping pairs (with potential duplicates) + BroadPair* mPotentialPairs; + + /// Number of potential overlapping pairs + uint mNbPotentialPairs; + + /// Number of allocated elements for the array of potential overlapping pairs + uint mNbAllocatedPotentialPairs; + /// Pair manager containing the overlapping pairs PairManager mPairManager; @@ -80,24 +132,50 @@ class BroadPhaseAlgorithm { BroadPhaseAlgorithm(CollisionDetection& collisionDetection); /// Destructor - virtual ~BroadPhaseAlgorithm(); + ~BroadPhaseAlgorithm(); - /// Notify the broad-phase about a new object in the world - virtual void addObject(CollisionBody* body, const AABB& aabb)=0; + /// Add a proxy collision shape into the broad-phase collision detection + void addProxyCollisionShape(ProxyShape* proxyShape); - /// Notify the broad-phase about an object that has been removed from the world - virtual void removeObject(CollisionBody* body)=0; + /// Remove a proxy collision shape from the broad-phase collision detection + void removeProxyCollisionShape(ProxyShape* proxyShape); - /// Notify the broad-phase that the AABB of an object has changed - virtual void updateObject(CollisionBody* body, const AABB& aabb)=0; + /// Notify the broad-phase that a collision shape has moved and need to be updated + void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); + + /// Add a collision shape in the array of shapes that have moved in the last simulation step + /// and that need to be tested again for broad-phase overlapping. + void addMovedCollisionShape(int broadPhaseID); + + /// Remove a collision shape from the array of shapes that have moved in the last simulation + /// step and that need to be tested again for broad-phase overlapping. + void removeMovedCollisionShape(int broadPhaseID); + + /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree + void notifyOverlappingPair(int node1ID, int node2ID); + + /// Compute all the overlapping pairs of collision shapes + void computeOverlappingPairs(); /// Return a pointer to the first active pair (used to iterate over the active pairs) + // TODO : DELETE THIS BodyPair* beginOverlappingPairsPointer() const; /// Return a pointer to the last active pair (used to iterate over the active pairs) + // TODO : DELETE THIS BodyPair* endOverlappingPairsPointer() const; }; +// Method used to compare two pairs for sorting algorithm +inline bool BroadPair::smallerThan(const BroadPair& pair1, const BroadPair& pair2) { + + if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true; + if (pair1.collisionShape1ID == pair2.collisionShape1ID) { + return pair1.collisionShape2ID < pair2.collisionShape2ID; + } + return false; +} + // Return a pointer to the first active pair (used to iterate over the overlapping pairs) inline BodyPair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const { return mPairManager.beginOverlappingPairsPointer(); diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index d542afcc..971bd788 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -25,6 +25,8 @@ // Libraries #include "DynamicAABBTree.h" +#include "BroadPhaseAlgorithm.h" +#include "../../memory/Stack.h" using namespace reactphysics3d; @@ -32,7 +34,7 @@ using namespace reactphysics3d; const int TreeNode::NULL_TREE_NODE = -1; // Constructor -DynamicAABBTree::DynamicAABBTree() { +DynamicAABBTree::DynamicAABBTree(BroadPhaseAlgorithm& broadPhase) : mBroadPhase(broadPhase){ mRootNodeID = TreeNode::NULL_TREE_NODE; mNbNodes = 0; @@ -92,7 +94,7 @@ int DynamicAABBTree::allocateNode() { mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE; mNodes[freeNodeID].leftChildID = TreeNode::NULL_TREE_NODE; mNodes[freeNodeID].rightChildID = TreeNode::NULL_TREE_NODE; - mNodes[freeNodeID].collisionShape = NULL; + mNodes[freeNodeID].proxyShape = NULL; mNodes[freeNodeID].height = 0; mNbNodes++; @@ -134,7 +136,7 @@ void DynamicAABBTree::releaseNode(int nodeID) { // Add an object into the tree. This method creates a new leaf node in the tree and // returns the ID of the corresponding node. -int DynamicAABBTree::addObject(CollisionShape* collisionShape, const AABB& aabb) { +void DynamicAABBTree::addObject(ProxyShape* proxyShape) { // Get the next available node (or allocate new ones if necessary) int nodeID = allocateNode(); @@ -145,7 +147,7 @@ int DynamicAABBTree::addObject(CollisionShape* collisionShape, const AABB& aabb) mNodes[nodeID].aabb.setMax(mNodes[nodeID].aabb.getMax() + gap); // Set the collision shape - mNodes[nodeID].collisionShape = collisionShape; + mNodes[nodeID].proxyShape = proxyShape; // Set the height of the node in the tree mNodes[nodeID].height = 0; @@ -153,8 +155,9 @@ int DynamicAABBTree::addObject(CollisionShape* collisionShape, const AABB& aabb) // Insert the new leaf node in the tree insertLeafNode(nodeID); - // Return the node ID - return nodeID; + // Set the broad-phase ID of the proxy shape + proxyShape->mBroadPhaseID = nodeID; + assert(nodeID >= 0); } // Remove an object from the tree @@ -172,7 +175,7 @@ void DynamicAABBTree::removeObject(int nodeID) { /// If the new AABB of the object that has moved is still inside its fat AABB, then /// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree. /// The method returns true if the object has been reinserted into the tree. -bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement) { +bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB) { assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); @@ -185,31 +188,11 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector // If the new AABB is outside the fat AABB, we remove the corresponding node removeLeafNode(nodeID); - // Compute a new fat AABB for the new AABB by taking the object displacement into account - AABB fatAABB = newAABB; + // Compute a new fat AABB for the new AABB + mNodes[nodeID].aabb = newAABB; const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP); - fatAABB.mMinCoordinates -= gap; - fatAABB.mMaxCoordinates += gap; - const Vector3 displacementGap = AABB_DISPLACEMENT_MULTIPLIER * displacement; - if (displacementGap.x < decimal(0.0)) { - fatAABB.mMinCoordinates.x += displacementGap.x; - } - else { - fatAABB.mMaxCoordinates.x += displacementGap.x; - } - if (displacementGap.y < decimal(0.0)) { - fatAABB.mMinCoordinates.y += displacementGap.y; - } - else { - fatAABB.mMaxCoordinates.y += displacementGap.y; - } - if (displacementGap.z < decimal(0.0)) { - fatAABB.mMinCoordinates.z += displacementGap.z; - } - else { - fatAABB.mMaxCoordinates.z += displacementGap.z; - } - mNodes[nodeID].aabb = fatAABB; + mNodes[nodeID].aabb.mMinCoordinates -= gap; + mNodes[nodeID].aabb.mMaxCoordinates += gap; // Reinsert the node into the tree insertLeafNode(nodeID); @@ -292,7 +275,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { int oldParentNode = mNodes[siblingNode].parentID; int newParentNode = allocateNode(); mNodes[newParentNode].parentID = oldParentNode; - mNodes[newParentNode].collisionShape = NULL; + mNodes[newParentNode].proxyShape = NULL; mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB); mNodes[newParentNode].height = mNodes[siblingNode].height + 1; @@ -552,3 +535,44 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { // If the sub-tree is balanced, return the current root node return nodeID; } + +// Report all shapes overlapping with the AABB given in parameter. +/// For each overlapping shape with the AABB given in parameter, the +/// BroadPhase::notifyOverlappingPair() method is called to store a +/// potential overlapping pair. +void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb) { + + // Create a stack with the nodes to visit + Stack stack; + stack.push(mRootNodeID); + + // While there are still nodes to visit + while(stack.getNbElements() > 0) { + + // Get the next node ID to visit + int nodeIDToVisit = stack.pop(); + + // Skip it if it is a null node + if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue; + + // Get the corresponding node + const TreeNode* nodeToVisit = mNodes + nodeIDToVisit; + + // If the AABB in parameter overlaps with the AABB of the node to visit + if (aabb.testCollision(nodeToVisit->aabb)) { + + // If the node is a leaf + if (nodeToVisit->isLeaf()) { + + // Notify the broad-phase about a new potential overlapping pair + mBroadPhase.notifyOverlappingPair(nodeID, nodeIDToVisit); + } + else { // If the node is not a leaf + + // We need to visit its children + stack.push(nodeToVisit->leftChildID); + stack.push(nodeToVisit->rightChildID); + } + } + } +} diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 542aeb8a..2488b816 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -29,11 +29,14 @@ // Libraries #include "../../configuration.h" #include "../shapes/AABB.h" -#include "../shapes/CollisionShape.h" +#include "../../body/CollisionBody.h" /// Namespace ReactPhysics3D namespace reactphysics3d { +// Declarations +class BroadPhaseAlgorithm; + // Structure TreeNode /** * This structure represents a node of the dynamic AABB tree. @@ -63,7 +66,7 @@ struct TreeNode { AABB aabb; /// Pointer to the corresponding collision shape (in case this node is a leaf) - CollisionShape* collisionShape; + ProxyShape* proxyShape; // -------------------- Methods -------------------- // @@ -85,6 +88,9 @@ class DynamicAABBTree { // -------------------- Attributes -------------------- // + /// Reference to the broad-phase + BroadPhaseAlgorithm& mBroadPhase; + /// Pointer to the memory location of the nodes of the tree TreeNode* mNodes; @@ -122,19 +128,28 @@ class DynamicAABBTree { // -------------------- Methods -------------------- // /// Constructor - DynamicAABBTree(); + DynamicAABBTree(BroadPhaseAlgorithm& broadPhase); /// Destructor ~DynamicAABBTree(); /// Add an object into the tree - int addObject(CollisionShape* collisionShape, const AABB& aabb); + void addObject(ProxyShape* proxyShape); /// Remove an object from the tree void removeObject(int nodeID); /// Update the dynamic tree after an object has moved. - bool updateObject(int nodeID, const AABB &newAABB, const Vector3 &displacement); + bool updateObject(int nodeID, const AABB& newAABB); + + /// Return the fat AABB corresponding to a given node ID + const AABB& getFatAABB(int nodeID) const; + + /// Return the collision shape of a given leaf node of the tree + ProxyShape* getCollisionShape(int nodeID) const; + + /// Report all shapes overlapping with the AABB given in parameter. + void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb); }; // Return true if the node is a leaf of the tree @@ -142,6 +157,19 @@ inline bool TreeNode::isLeaf() const { return leftChildID == NULL_TREE_NODE; } +// Return the fat AABB corresponding to a given node ID +inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const { + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + return mNodes[nodeID].aabb; +} + +// Return the collision shape of a given leaf node of the tree +inline ProxyShape* DynamicAABBTree::getCollisionShape(int nodeID) const { + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + assert(mNodes[nodeID].isLeaf()); + return mNodes[nodeID].proxyShape; +} + } #endif diff --git a/src/collision/broadphase/NoBroadPhaseAlgorithm.h b/src/collision/broadphase/NoBroadPhaseAlgorithm.h index 9c4ea7ca..d153b057 100644 --- a/src/collision/broadphase/NoBroadPhaseAlgorithm.h +++ b/src/collision/broadphase/NoBroadPhaseAlgorithm.h @@ -69,18 +69,18 @@ class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm { virtual ~NoBroadPhaseAlgorithm(); /// Notify the broad-phase about a new object in the world - virtual void addObject(CollisionBody* body, const AABB& aabb); + virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb); /// Notify the broad-phase about an object that has been removed from the world - virtual void removeObject(CollisionBody* body); + virtual void removeProxyCollisionShape(CollisionBody* body); /// Notify the broad-phase that the AABB of an object has changed - virtual void updateObject(CollisionBody* body, const AABB& aabb); + virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb); }; // Notify the broad-phase about a new object in the world -inline void NoBroadPhaseAlgorithm::addObject(CollisionBody* body, const AABB& aabb) { +inline void NoBroadPhaseAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) { // For each body that is already in the world for (std::set::iterator it = mBodies.begin(); it != mBodies.end(); ++it) { @@ -94,7 +94,7 @@ inline void NoBroadPhaseAlgorithm::addObject(CollisionBody* body, const AABB& aa } // Notify the broad-phase about an object that has been removed from the world -inline void NoBroadPhaseAlgorithm::removeObject(CollisionBody* body) { +inline void NoBroadPhaseAlgorithm::removeProxyCollisionShape(CollisionBody* body) { // For each body that is in the world for (std::set::iterator it = mBodies.begin(); it != mBodies.end(); ++it) { @@ -111,7 +111,7 @@ inline void NoBroadPhaseAlgorithm::removeObject(CollisionBody* body) { } // Notify the broad-phase that the AABB of an object has changed -inline void NoBroadPhaseAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) { +inline void NoBroadPhaseAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) { // Do nothing return; } diff --git a/src/collision/broadphase/PairManager.cpp b/src/collision/broadphase/PairManager.cpp index ad5e9fce..882920bb 100644 --- a/src/collision/broadphase/PairManager.cpp +++ b/src/collision/broadphase/PairManager.cpp @@ -104,7 +104,7 @@ BodyPair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2) { mHashTable[hashValue] = mNbOverlappingPairs++; // Notify the collision detection about this new overlapping pair - mCollisionDetection.broadPhaseNotifyAddedOverlappingPair(newPair); + mCollisionDetection.broadPhaseNotifyOverlappingPair(newPair); // Return a pointer to the new created pair return newPair; diff --git a/src/collision/broadphase/PairManager.h b/src/collision/broadphase/PairManager.h index b95ea011..cec8569f 100644 --- a/src/collision/broadphase/PairManager.h +++ b/src/collision/broadphase/PairManager.h @@ -26,6 +26,8 @@ #ifndef REACTPHYSICS3D_PAIR_MANAGER_H #define REACTPHYSICS3D_PAIR_MANAGER_H +// TODO : REMOVE THE PAIR MANAGER CLASS + // Libraries #include "../../body/CollisionBody.h" #include diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp index 142ec56f..06691573 100644 --- a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp +++ b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp @@ -81,7 +81,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(CollisionBody* body, const AABB& aabb) { +void SweepAndPruneAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) { bodyindex boxIndex; @@ -144,11 +144,11 @@ void SweepAndPruneAlgorithm::addObject(CollisionBody* body, const AABB& aabb) { // correct position in the array. This will also create the overlapping // pairs in the pair manager if the new AABB is overlapping with others // AABBs - updateObject(body, aabb); + updateProxyCollisionShape(body, aabb); } // Notify the broad-phase about an object that has been removed from the world -void SweepAndPruneAlgorithm::removeObject(CollisionBody* body) { +void SweepAndPruneAlgorithm::removeProxyCollisionShape(CollisionBody* body) { assert(mNbBoxes > 0); diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.h b/src/collision/broadphase/SweepAndPruneAlgorithm.h index a4e2594b..0c0225c4 100644 --- a/src/collision/broadphase/SweepAndPruneAlgorithm.h +++ b/src/collision/broadphase/SweepAndPruneAlgorithm.h @@ -180,13 +180,13 @@ class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm { virtual ~SweepAndPruneAlgorithm(); /// Notify the broad-phase about a new object in the world. - virtual void addObject(CollisionBody* body, const AABB& aabb); + virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb); /// Notify the broad-phase about a object that has been removed from the world - virtual void removeObject(CollisionBody* body); + virtual void removeProxyCollisionShape(CollisionBody* body); /// Notify the broad-phase that the AABB of an object has changed - virtual void updateObject(CollisionBody* body, const AABB& aabb); + virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb); }; /// Encode a floating value into a integer value in order to @@ -229,7 +229,7 @@ inline bool SweepAndPruneAlgorithm::testIntersect2D(const BoxAABB& box1, const B } // Notify the broad-phase that the AABB of an object has changed -inline void SweepAndPruneAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) { +inline void SweepAndPruneAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) { // Compute the corresponding AABB with integer coordinates AABBInt aabbInt(aabb); diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 6268da6b..0c78ddf6 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -83,9 +83,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find /// the correct penetration depth bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, - CollisionShape* collisionShape1, + ProxyShape* collisionShape1, const Transform& transform1, - CollisionShape* collisionShape2, + ProxyShape* collisionShape2, const Transform& transform2, Vector3& v, ContactPointInfo*& contactInfo) { diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index 8a03f2cb..f033ee42 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -119,9 +119,9 @@ class EPAAlgorithm { /// Compute the penetration depth with EPA algorithm. bool computePenetrationDepthAndContactPoints(const Simplex& simplex, - CollisionShape* collisionShape1, + ProxyShape* collisionShape1, const Transform& transform1, - CollisionShape* collisionShape2, + ProxyShape* collisionShape2, const Transform& transform2, Vector3& v, ContactPointInfo*& contactInfo); }; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 6ab3e959..0548b2b6 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -57,11 +57,8 @@ GJKAlgorithm::~GJKAlgorithm() { /// algorithm on the enlarged object to obtain a simplex polytope that contains the /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. -bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1, - const Transform& transform1, - CollisionShape* collisionShape2, - const Transform& transform2, - ContactPointInfo*& contactInfo) { +bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2, + ContactPointInfo*& contactInfo) { Vector3 suppA; // Support point of object A Vector3 suppB; // Support point of object B @@ -71,6 +68,12 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1, decimal vDotw; decimal prevDistSquare; + // Get the local-space to world-space transforms + const Transform transform1 = collisionShape1->getBody()->getTransform() * + collisionShape1->getLocalToBodyTransform(); + const Transform transform2 = collisionShape2->getBody()->getTransform() * + collisionShape2->getLocalToBodyTransform(); + // Transform a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) Transform body2Tobody1 = transform1.getInverse() * transform2; @@ -89,7 +92,7 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1, Simplex simplex; // Get the previous point V (last cached separating axis) - Vector3 v = mCurrentOverlappingPair->previousSeparatingAxis; + Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); // Initialize the upper bound for the square distance decimal distSquare = DECIMAL_LARGEST; @@ -110,7 +113,7 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1, if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - mCurrentOverlappingPair->previousSeparatingAxis = v; + mCurrentOverlappingPair->setCachedSeparatingAxis(v); // No intersection, we return false return false; @@ -259,9 +262,9 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1, /// assumed to intersect in the original objects (without margin). Therefore such /// a polytope must exist. Then, we give that polytope to the EPA algorithm to /// compute the correct penetration depth and contact points of the enlarged objects. -bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(CollisionShape* collisionShape1, +bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisionShape1, const Transform& transform1, - CollisionShape* collisionShape2, + ProxyShape* collisionShape2, const Transform& transform2, ContactPointInfo*& contactInfo, Vector3& v) { diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index dae18d66..e807a45b 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -74,9 +74,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); /// Compute the penetration depth for enlarged objects. - bool computePenetrationDepthForEnlargedObjects(CollisionShape* collisionShape1, + bool computePenetrationDepthForEnlargedObjects(ProxyShape* collisionShape1, const Transform& transform1, - CollisionShape* collisionShape2, + ProxyShape* collisionShape2, const Transform& transform2, ContactPointInfo*& contactInfo, Vector3& v); @@ -91,10 +91,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { ~GJKAlgorithm(); /// Return true and compute a contact info if the two bounding volumes collide. - virtual bool testCollision(CollisionShape *collisionShape1, - const Transform& transform1, - CollisionShape *collisionShape2, - const Transform& transform2, + virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2, ContactPointInfo*& contactInfo); }; diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 3dd5d086..de778e1b 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -31,7 +31,7 @@ #include "../../constraint/ContactPoint.h" #include "../broadphase/PairManager.h" #include "../../memory/MemoryAllocator.h" -#include "../BroadPhasePair.h" +#include "../../engine/OverlappingPair.h" /// Namespace ReactPhysics3D @@ -54,7 +54,7 @@ class NarrowPhaseAlgorithm { MemoryAllocator& mMemoryAllocator; /// Overlapping pair of the bodies currently tested for collision - BroadPhasePair* mCurrentOverlappingPair; + OverlappingPair* mCurrentOverlappingPair; // -------------------- Methods -------------------- // @@ -75,18 +75,15 @@ class NarrowPhaseAlgorithm { virtual ~NarrowPhaseAlgorithm(); /// Set the current overlapping pair of bodies - void setCurrentOverlappingPair(BroadPhasePair* overlappingPair); + void setCurrentOverlappingPair(OverlappingPair* overlappingPair); /// Return true and compute a contact info if the two bounding volume collide - virtual bool testCollision(CollisionShape* collisionShape1, - const Transform& transform1, - CollisionShape* collisionShape2, - const Transform& transform2, + virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2, ContactPointInfo*& contactInfo)=0; }; // Set the current overlapping pair of bodies -inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(BroadPhasePair *overlappingPair) { +inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) { mCurrentOverlappingPair = overlappingPair; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 539cb487..75aee2de 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -41,16 +41,22 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() { } -bool SphereVsSphereAlgorithm::testCollision(CollisionShape* collisionShape1, - const Transform& transform1, - CollisionShape* collisionShape2, - const Transform& transform2, +bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1, + ProxyShape* collisionShape2, ContactPointInfo*& contactInfo) { // Get the sphere collision shapes - const SphereShape* sphereShape1 = dynamic_cast(collisionShape1); - const SphereShape* sphereShape2 = dynamic_cast(collisionShape2); - + const CollisionShape* shape1 = collisionShape1->getCollisionShape(); + const CollisionShape* shape2 = collisionShape2->getCollisionShape(); + const SphereShape* sphereShape1 = dynamic_cast(shape1); + const SphereShape* sphereShape2 = dynamic_cast(shape2); + + // Get the local-space to world-space transforms + const Transform transform1 = collisionShape1->getBody()->getTransform() * + collisionShape1->getLocalToBodyTransform(); + const Transform transform2 = collisionShape2->getBody()->getTransform() * + collisionShape2->getLocalToBodyTransform(); + // Compute the distance between the centers Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index f5c6cc18..ba4e8a2d 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -63,10 +63,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { virtual ~SphereVsSphereAlgorithm(); /// Return true and compute a contact info if the two bounding volume collide - virtual bool testCollision(CollisionShape* collisionShape1, - const Transform& transform1, - CollisionShape* collisionShape2, - const Transform& transform2, + virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2, ContactPointInfo*& contactInfo); }; diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 1914f87f..e3afb84c 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -61,3 +61,15 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const 0.0, factor * (xSquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + ySquare)); } + +// Constructor +ProxyBoxShape::ProxyBoxShape(const BoxShape* shape, CollisionBody* body, + const Transform& transform, decimal mass) + :ProxyShape(body, transform, mass), mCollisionShape(shape){ + +} + +// Destructor +ProxyBoxShape::~ProxyBoxShape() { + +} diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index f53ebc70..f4a6e7ae 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -89,16 +89,69 @@ class BoxShape : public CollisionShape { virtual size_t getSizeInBytes() const; /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; /// Test equality between two box shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; + + /// Create a proxy collision shape for the collision shape + virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const; +}; + +// Class ProxyBoxShape +/** + * The proxy collision shape for a box shape. + */ +class ProxyBoxShape : public ProxyShape { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual collision shape + const BoxShape* mCollisionShape; + + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ProxyBoxShape(const ProxyBoxShape& proxyShape); + + /// Private assignment operator + ProxyBoxShape& operator=(const ProxyBoxShape& proxyShape); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ProxyBoxShape(const BoxShape* shape, CollisionBody* body, + const Transform& transform, decimal mass); + + /// Destructor + ~ProxyBoxShape(); + + /// Return the collision shape + virtual const CollisionShape* getCollisionShape() const; + + /// Return the number of bytes used by the proxy collision shape + virtual size_t getSizeInBytes() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + + /// Return the current collision shape margin + virtual decimal getMargin() const; }; // Allocate and return a copy of the object @@ -128,7 +181,7 @@ inline size_t BoxShape::getSizeInBytes() const { } // Return a local support point in a given direction with the object margin -inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) { +inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) const { assert(mMargin > 0.0); @@ -138,7 +191,7 @@ inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction } // Return a local support point in a given direction without the objec margin -inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { +inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, direction.y < 0.0 ? -mExtent.y : mExtent.y, @@ -151,6 +204,38 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const return (mExtent == otherShape.mExtent); } +// Create a proxy collision shape for the collision shape +inline ProxyShape* BoxShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const { + return new (allocator.allocate(sizeof(ProxyBoxShape))) ProxyBoxShape(this, body, + transform, mass); +} + +// Return the collision shape +inline const CollisionShape* ProxyBoxShape::getCollisionShape() const { + return mCollisionShape; +} + +// Return the number of bytes used by the proxy collision shape +inline size_t ProxyBoxShape::getSizeInBytes() const { + return sizeof(ProxyBoxShape); +} + +// Return a local support point in a given direction with the object margin +inline Vector3 ProxyBoxShape::getLocalSupportPointWithMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithMargin(direction); +} + +// Return a local support point in a given direction without the object margin +inline Vector3 ProxyBoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithoutMargin(direction); +} + +// Return the current object margin +inline decimal ProxyBoxShape::getMargin() const { + return mCollisionShape->getMargin(); +} + } #endif diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index ff1103ff..97f63828 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -55,7 +55,7 @@ CapsuleShape::~CapsuleShape() { /// Therefore, in this method, we compute the support points of both top and bottom spheres of /// the capsule and return the point with the maximum dot product with the direction vector. Note /// that the object margin is implicitly the radius and height of the capsule. -Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) { +Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) const { // If the direction vector is not the zero vector if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) { @@ -87,7 +87,7 @@ Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) { } // Return a local support point in a given direction without the object margin. -Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { +Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // If the dot product of the direction and the local Y axis (dotProduct = direction.y) // is positive @@ -123,3 +123,15 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co 0.0, Iyy, 0.0, 0.0, 0.0, IxxAndzz); } + +// Constructor +ProxyCapsuleShape::ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body, + const Transform& transform, decimal mass) + :ProxyShape(body, transform, mass), mCollisionShape(shape){ + +} + +// Destructor +ProxyCapsuleShape::~ProxyCapsuleShape() { + +} diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index a43de124..bf532369 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -86,10 +86,10 @@ class CapsuleShape : public CollisionShape { virtual size_t getSizeInBytes() const; /// Return a local support point in a given direction with the object margin. - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const; @@ -99,6 +99,59 @@ class CapsuleShape : public CollisionShape { /// Test equality between two capsule shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; + + /// Create a proxy collision shape for the collision shape + virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const; +}; + +// Class ProxyCapsuleShape +/** + * The proxy collision shape for a capsule shape. + */ +class ProxyCapsuleShape : public ProxyShape { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual collision shape + const CapsuleShape* mCollisionShape; + + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ProxyCapsuleShape(const ProxyCapsuleShape& proxyShape); + + /// Private assignment operator + ProxyCapsuleShape& operator=(const ProxyCapsuleShape& proxyShape); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body, + const Transform& transform, decimal mass); + + /// Destructor + ~ProxyCapsuleShape(); + + /// Return the collision shape + virtual const CollisionShape* getCollisionShape() const; + + /// Return the number of bytes used by the proxy collision shape + virtual size_t getSizeInBytes() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + + /// Return the current collision shape margin + virtual decimal getMargin() const; }; /// Allocate and return a copy of the object @@ -142,6 +195,38 @@ inline bool CapsuleShape::isEqualTo(const CollisionShape& otherCollisionShape) c return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight); } +// Create a proxy collision shape for the collision shape +inline ProxyShape* CapsuleShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const { + return new (allocator.allocate(sizeof(ProxyCapsuleShape))) ProxyCapsuleShape(this, body, + transform, mass); +} + +// Return the collision shape +inline const CollisionShape* ProxyCapsuleShape::getCollisionShape() const { + return mCollisionShape; +} + +// Return the number of bytes used by the proxy collision shape +inline size_t ProxyCapsuleShape::getSizeInBytes() const { + return sizeof(ProxyCapsuleShape); +} + +// Return a local support point in a given direction with the object margin +inline Vector3 ProxyCapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithMargin(direction); +} + +// Return a local support point in a given direction without the object margin +inline Vector3 ProxyCapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithoutMargin(direction); +} + +// Return the current object margin +inline decimal ProxyCapsuleShape::getMargin() const { + return mCollisionShape->getMargin(); +} + } #endif diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index e895f9bb..f7de614f 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -47,8 +47,8 @@ CollisionShape::~CollisionShape() { assert(mNbSimilarCreatedShapes == 0); } -// Update the AABB of a body using its collision shape -void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) { +// Compute the world-space AABB of the collision shape given a transform +void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { // Get the local bounds in x,y and z direction Vector3 minBounds; @@ -72,3 +72,15 @@ void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) { aabb.setMin(minCoordinates); aabb.setMax(maxCoordinates); } + +// Constructor +ProxyShape::ProxyShape(CollisionBody* body, const Transform& transform, decimal mass) + :mBody(body), mLocalToBodyTransform(transform), mMass(mass), mNext(NULL), + mBroadPhaseID(-1) { + +} + +// Destructor +ProxyShape::~ProxyShape() { + +} diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 4e6072c1..0623febe 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -32,6 +32,7 @@ #include "../../mathematics/Vector3.h" #include "../../mathematics/Matrix3x3.h" #include "AABB.h" +#include "../../memory/MemoryAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -40,7 +41,8 @@ namespace reactphysics3d { enum CollisionShapeType {BOX, SPHERE, CONE, CYLINDER, CAPSULE, CONVEX_MESH}; // Declarations -class Body; +class CollisionBody; +class ProxyShape; // Class CollisionShape /** @@ -95,20 +97,14 @@ class CollisionShape { /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const = 0; - /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction)=0; - - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction)=0; - /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; /// Return the local inertia tensor of the collision shapes virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; - /// Update the AABB of a body using its collision shape - virtual void updateAABB(AABB& aabb, const Transform& transform); + /// Compute the world-space AABB of the collision shape given a transform + virtual void computeAABB(AABB& aabb, const Transform& transform) const; /// Increment the number of similar allocated collision shapes void incrementNbSimilarCreatedShapes(); @@ -121,6 +117,93 @@ class CollisionShape { /// Test equality between two collision shapes of the same type (same derived classes). virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const=0; + + /// Create a proxy collision shape for the collision shape + virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const=0; +}; + + +// Class ProxyShape +/** + * The CollisionShape instances are supposed to be unique for memory optimization. For instance, + * consider two rigid bodies with the same sphere collision shape. In this situation, we will have + * a unique instance of SphereShape but we need to differentiate between the two instances during + * the collision detection. They do not have the same position in the world and they do not + * belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a + * rigid body with one of its collision shape. A body can have multiple proxy shapes (one for + * each collision shape attached to the body). + */ +class ProxyShape { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the parent body + CollisionBody* mBody; + + /// Local-space to parent body-space transform (does not change over time) + const Transform mLocalToBodyTransform; + + /// Mass (in kilogramms) of the corresponding collision shape + decimal mMass; + + /// Pointer to the next proxy shape of the body (linked list) + ProxyShape* mNext; + + /// Broad-phase ID (node ID in the dynamic AABB tree) + int mBroadPhaseID; + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ProxyShape(const ProxyShape& proxyShape); + + /// Private assignment operator + ProxyShape& operator=(const ProxyShape& proxyShape); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ProxyShape(CollisionBody* body, const Transform& transform, decimal mass); + + /// Destructor + ~ProxyShape(); + + /// Return the collision shape + virtual const CollisionShape* getCollisionShape() const=0; + + /// Return the number of bytes used by the proxy collision shape + virtual size_t getSizeInBytes() const=0; + + /// Return the parent body + CollisionBody* getBody() const; + + /// Return the mass of the collision shape + decimal getMass() const; + + /// Return the local to parent body transform + const Transform& getLocalToBodyTransform() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction)=0; + + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction)=0; + + /// Return the current object margin + virtual decimal getMargin() const=0; + + // -------------------- Friendship -------------------- // + + friend class OverlappingPair; + friend class CollisionBody; + friend class RigidBody; + friend class BroadPhaseAlgorithm; + friend class DynamicAABBTree; }; // Return the type of the collision shape @@ -165,6 +248,21 @@ inline bool CollisionShape::operator==(const CollisionShape& otherCollisionShape return otherCollisionShape.isEqualTo(*this); } +// Return the parent body +inline CollisionBody* ProxyShape::getBody() const { + return mBody; +} + +// Return the mass of the collision shape +inline decimal ProxyShape::getMass() const { + return mMass; +} + +// Return the local to parent body transform +inline const Transform& ProxyShape::getLocalToBodyTransform() const { + return mLocalToBodyTransform; +} + } #endif diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index b674384d..ef2b958f 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -54,7 +54,7 @@ ConeShape::~ConeShape() { } // Return a local support point in a given direction with the object margin -Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) { +Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) const { // Compute the support point without the margin Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); @@ -70,7 +70,7 @@ Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) { } // Return a local support point in a given direction without the object margin -Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { +Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { const Vector3& v = direction; decimal sinThetaTimesLengthV = mSinTheta * v.length(); @@ -92,3 +92,15 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { return supportPoint; } + +// Constructor +ProxyConeShape::ProxyConeShape(const ConeShape* shape, CollisionBody* body, + const Transform& transform, decimal mass) + :ProxyShape(body, transform, mass), mCollisionShape(shape){ + +} + +// Destructor +ProxyConeShape::~ProxyConeShape() { + +} diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index 1c4a9326..a6232127 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -94,10 +94,10 @@ class ConeShape : public CollisionShape { virtual size_t getSizeInBytes() const; /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const; @@ -107,6 +107,59 @@ class ConeShape : public CollisionShape { /// Test equality between two cone shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; + + /// Create a proxy collision shape for the collision shape + virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const; +}; + +// Class ProxyConeShape +/** + * The proxy collision shape for a cone shape. + */ +class ProxyConeShape : public ProxyShape { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual collision shape + const ConeShape* mCollisionShape; + + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ProxyConeShape(const ProxyConeShape& proxyShape); + + /// Private assignment operator + ProxyConeShape& operator=(const ProxyConeShape& proxyShape); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ProxyConeShape(const ConeShape* shape, CollisionBody* body, + const Transform& transform, decimal mass); + + /// Destructor + ~ProxyConeShape(); + + /// Return the collision shape + virtual const CollisionShape* getCollisionShape() const; + + /// Return the number of bytes used by the proxy collision shape + virtual size_t getSizeInBytes() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + + /// Return the current collision shape margin + virtual decimal getMargin() const; }; // Allocate and return a copy of the object @@ -158,6 +211,38 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight); } +// Create a proxy collision shape for the collision shape +inline ProxyShape* ConeShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const { + return new (allocator.allocate(sizeof(ProxyConeShape))) ProxyConeShape(this, body, + transform, mass); +} + +// Return the collision shape +inline const CollisionShape* ProxyConeShape::getCollisionShape() const { + return mCollisionShape; +} + +// Return the number of bytes used by the proxy collision shape +inline size_t ProxyConeShape::getSizeInBytes() const { + return sizeof(ProxyConeShape); +} + +// Return a local support point in a given direction with the object margin +inline Vector3 ProxyConeShape::getLocalSupportPointWithMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithMargin(direction); +} + +// Return a local support point in a given direction without the object margin +inline Vector3 ProxyConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithoutMargin(direction); +} + +// Return the current object margin +inline decimal ProxyConeShape::getMargin() const { + return mCollisionShape->getMargin(); +} + } #endif diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 75f819bc..a59c2b37 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin) : CollisionShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0), - mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false), mCachedSupportVertex(0) { + mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) { assert(nbVertices > 0); assert(stride > 0); assert(margin > decimal(0.0)); @@ -58,7 +58,7 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, /// the addVertex() method. ConvexMeshShape::ConvexMeshShape(decimal margin) : CollisionShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0), - mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false), mCachedSupportVertex(0) { + mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) { assert(margin > decimal(0.0)); } @@ -67,8 +67,7 @@ ConvexMeshShape::ConvexMeshShape(const ConvexMeshShape& shape) : CollisionShape(shape), mVertices(shape.mVertices), mNbVertices(shape.mNbVertices), mMinBounds(shape.mMinBounds), mMaxBounds(shape.mMaxBounds), mIsEdgesInformationUsed(shape.mIsEdgesInformationUsed), - mEdgesAdjacencyList(shape.mEdgesAdjacencyList), - mCachedSupportVertex(shape.mCachedSupportVertex) { + mEdgesAdjacencyList(shape.mEdgesAdjacencyList) { assert(mNbVertices == mVertices.size()); } @@ -79,10 +78,11 @@ ConvexMeshShape::~ConvexMeshShape() { } // Return a local support point in a given direction with the object margin -Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction) { +Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction, + uint& cachedSupportVertex) const { // Get the support point without the margin - Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); + Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedSupportVertex); // Get the unit direction vector Vector3 unitDirection = direction; @@ -103,7 +103,8 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction /// it as a start in a hill-climbing (local search) process to find the new support vertex which /// will be in most of the cases very close to the previous one. Using hill-climbing, this method /// runs in almost constant time. -Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { +Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction, + uint& cachedSupportVertex) const { assert(mNbVertices == mVertices.size()); @@ -112,7 +113,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct assert(mEdgesAdjacencyList.size() == mNbVertices); - uint maxVertex = mCachedSupportVertex; + uint maxVertex = cachedSupportVertex; decimal maxDotProduct = direction.dot(mVertices[maxVertex]); bool isOptimal; @@ -142,7 +143,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct } while(!isOptimal); // Cache the support vertex - mCachedSupportVertex = maxVertex; + cachedSupportVertex = maxVertex; // Return the support vertex return mVertices[maxVertex]; @@ -204,11 +205,7 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const if (mNbVertices != otherShape.mNbVertices) return false; - // If edges information is used, it means that a collison shape object will store - // cached data (previous support vertex) and therefore, we should not reuse the shape - // for another body. Therefore, we consider that all convex mesh shape using edges - // information are different. - if (mIsEdgesInformationUsed) return false; + if (mIsEdgesInformationUsed != otherShape.mIsEdgesInformationUsed) return false; if (mEdgesAdjacencyList.size() != otherShape.mEdgesAdjacencyList.size()) return false; @@ -226,3 +223,16 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const return true; } + +// Constructor +ProxyConvexMeshShape::ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body, + const Transform& transform, decimal mass) + :ProxyShape(body, transform, mass), mCollisionShape(shape), + mCachedSupportVertex(0) { + +} + +// Destructor +ProxyConvexMeshShape::~ProxyConvexMeshShape() { + +} diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 4649d2dd..8d4a43e9 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -77,9 +77,6 @@ class ConvexMeshShape : public CollisionShape { /// Adjacency list representing the edges of the mesh std::map > mEdgesAdjacencyList; - /// Cached support vertex index (previous support vertex) - uint mCachedSupportVertex; - // -------------------- Methods -------------------- // /// Private copy-constructor @@ -112,10 +109,12 @@ class ConvexMeshShape : public CollisionShape { virtual size_t getSizeInBytes() const; /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction, + uint& cachedSupportVertex) const; /// Return a local support point in a given direction without the object margin. - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + uint& cachedSupportVertex) const; /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const; @@ -123,7 +122,7 @@ class ConvexMeshShape : public CollisionShape { /// Return the local inertia tensor of the collision shape. virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; - /// Test equality between two cone shapes + /// Test equality between two collision shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; /// Add a vertex into the convex mesh @@ -138,6 +137,62 @@ class ConvexMeshShape : public CollisionShape { /// Set the variable to know if the edges information is used to speed up the /// collision detection void setIsEdgesInformationUsed(bool isEdgesUsed); + + /// Create a proxy collision shape for the collision shape + virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const; +}; + + +// Class ProxyConvexMeshSphape +/** + * The proxy collision shape for a convex mesh shape. + */ +class ProxyConvexMeshShape : public ProxyShape { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual collision shape + const ConvexMeshShape* mCollisionShape; + + /// Cached support vertex index (previous support vertex for hill-climbing) + uint mCachedSupportVertex; + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ProxyConvexMeshShape(const ProxyConvexMeshShape& proxyShape); + + /// Private assignment operator + ProxyConvexMeshShape& operator=(const ProxyConvexMeshShape& proxyShape); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body, + const Transform& transform, decimal mass); + + /// Destructor + ~ProxyConvexMeshShape(); + + /// Return the collision shape + virtual const CollisionShape* getCollisionShape() const; + + /// Return the number of bytes used by the proxy collision shape + virtual size_t getSizeInBytes() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + + /// Return the current collision shape margin + virtual decimal getMargin() const; }; // Allocate and return a copy of the object @@ -222,6 +277,40 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { mIsEdgesInformationUsed = isEdgesUsed; } +// Create a proxy collision shape for the collision shape +inline ProxyShape* ConvexMeshShape::createProxyShape(MemoryAllocator& allocator, + CollisionBody* body, + const Transform& transform, + decimal mass) const { + return new (allocator.allocate(sizeof(ProxyConvexMeshShape))) ProxyConvexMeshShape(this, body, + transform, mass); +} + +// Return the collision shape +inline const CollisionShape* ProxyConvexMeshShape::getCollisionShape() const { + return mCollisionShape; +} + +// Return the number of bytes used by the proxy collision shape +inline size_t ProxyConvexMeshShape::getSizeInBytes() const { + return sizeof(ProxyConvexMeshShape); +} + +// Return a local support point in a given direction with the object margin +inline Vector3 ProxyConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithMargin(direction, mCachedSupportVertex); +} + +// Return a local support point in a given direction without the object margin +inline Vector3 ProxyConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithoutMargin(direction, mCachedSupportVertex); +} + +// Return the current object margin +inline decimal ProxyConvexMeshShape::getMargin() const { + return mCollisionShape->getMargin(); +} + } #endif diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 675dd480..13f62cac 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -50,7 +50,7 @@ CylinderShape::~CylinderShape() { } // Return a local support point in a given direction with the object margin -Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) { +Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) const { // Compute the support point without the margin Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); @@ -66,7 +66,7 @@ Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) } // Return a local support point in a given direction without the object margin -Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { +Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { Vector3 supportPoint(0.0, 0.0, 0.0); decimal uDotv = direction.y; @@ -85,3 +85,15 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio return supportPoint; } + +// Constructor +ProxyCylinderShape::ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body, + const Transform& transform, decimal mass) + :ProxyShape(body, transform, mass), mCollisionShape(cylinderShape){ + +} + +// Destructor +ProxyCylinderShape::~ProxyCylinderShape() { + +} diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index 21cda294..4e697501 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -91,10 +91,10 @@ class CylinderShape : public CollisionShape { virtual size_t getSizeInBytes() const; /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const; @@ -104,6 +104,60 @@ class CylinderShape : public CollisionShape { /// Test equality between two cylinder shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; + + /// Create a proxy collision shape for the collision shape + virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const; + +}; + +// Class ProxyCylinderShape +/** + * The proxy collision shape for a cylinder shape. + */ +class ProxyCylinderShape : public ProxyShape { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual collision shape + const CylinderShape* mCollisionShape; + + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ProxyCylinderShape(const ProxyCylinderShape& proxyShape); + + /// Private assignment operator + ProxyCylinderShape& operator=(const ProxyCylinderShape& proxyShape); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body, + const Transform& transform, decimal mass); + + /// Destructor + ~ProxyCylinderShape(); + + /// Return the collision shape + virtual const CollisionShape* getCollisionShape() const; + + /// Return the number of bytes used by the proxy collision shape + virtual size_t getSizeInBytes() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + + /// Return the current collision shape margin + virtual decimal getMargin() const; }; /// Allocate and return a copy of the object @@ -155,6 +209,38 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape) return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight); } +// Create a proxy collision shape for the collision shape +inline ProxyShape* CylinderShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const { + return new (allocator.allocate(sizeof(ProxyCylinderShape))) ProxyCylinderShape(this, body, + transform, mass); +} + +// Return the collision shape +inline const CollisionShape* ProxyCylinderShape::getCollisionShape() const { + return mCollisionShape; +} + +// Return the number of bytes used by the proxy collision shape +inline size_t ProxyCylinderShape::getSizeInBytes() const { + return sizeof(ProxyCylinderShape); +} + +// Return a local support point in a given direction with the object margin +inline Vector3 ProxyCylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithMargin(direction); +} + +// Return a local support point in a given direction without the object margin +inline Vector3 ProxyCylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithoutMargin(direction); +} + +// Return the current object margin +inline decimal ProxyCylinderShape::getMargin() const { + return mCollisionShape->getMargin(); +} + } #endif diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index c6b3b3e3..f1cd1e0d 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -45,3 +45,15 @@ SphereShape::SphereShape(const SphereShape& shape) SphereShape::~SphereShape() { } + +// Constructor +ProxySphereShape::ProxySphereShape(const SphereShape* shape, CollisionBody* body, + const Transform& transform, decimal mass) + :ProxyShape(body, transform, mass), mCollisionShape(shape){ + +} + +// Destructor +ProxySphereShape::~ProxySphereShape() { + +} diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 962c0277..d1492a4a 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -78,10 +78,10 @@ class SphereShape : public CollisionShape { virtual size_t getSizeInBytes() const; /// Return a local support point in a given direction with the object margin - virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; /// Return the local bounds of the shape in x, y and z directions. virtual void getLocalBounds(Vector3& min, Vector3& max) const; @@ -90,10 +90,64 @@ class SphereShape : public CollisionShape { virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; /// Update the AABB of a body using its collision shape - virtual void updateAABB(AABB& aabb, const Transform& transform); + virtual void computeAABB(AABB& aabb, const Transform& transform); /// Test equality between two sphere shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; + + /// Create a proxy collision shape for the collision shape + virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const; +}; + + +// Class ProxySphereShape +/** + * The proxy collision shape for a sphere shape. + */ +class ProxySphereShape : public ProxyShape { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual collision shape + const SphereShape* mCollisionShape; + + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ProxySphereShape(const ProxySphereShape& proxyShape); + + /// Private assignment operator + ProxySphereShape& operator=(const ProxySphereShape& proxyShape); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ProxySphereShape(const SphereShape* shape, CollisionBody* body, + const Transform& transform, decimal mass); + + /// Destructor + ~ProxySphereShape(); + + /// Return the collision shape + virtual const CollisionShape* getCollisionShape() const; + + /// Return the number of bytes used by the proxy collision shape + virtual size_t getSizeInBytes() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + + /// Return the current collision shape margin + virtual decimal getMargin() const; }; /// Allocate and return a copy of the object @@ -112,7 +166,7 @@ inline size_t SphereShape::getSizeInBytes() const { } // Return a local support point in a given direction with the object margin -inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) { +inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) const { // If the direction vector is not the zero vector if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) { @@ -127,7 +181,7 @@ inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direct } // Return a local support point in a given direction without the object margin -inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { +inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // Return the center of the sphere (the radius is taken into account in the object margin) return Vector3(0.0, 0.0, 0.0); @@ -157,7 +211,7 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma } // Update the AABB of a body using its collision shape -inline void SphereShape::updateAABB(AABB& aabb, const Transform& transform) { +inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) { // Get the local extents in x,y and z direction Vector3 extents(mRadius, mRadius, mRadius); @@ -173,6 +227,38 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co return (mRadius == otherShape.mRadius); } +// Create a proxy collision shape for the collision shape +inline ProxyShape* SphereShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body, + const Transform& transform, decimal mass) const { + return new (allocator.allocate(sizeof(ProxySphereShape))) ProxySphereShape(this, body, + transform, mass); +} + +// Return the collision shape +inline const CollisionShape* ProxySphereShape::getCollisionShape() const { + return mCollisionShape; +} + +// Return the number of bytes used by the proxy collision shape +inline size_t ProxySphereShape::getSizeInBytes() const { + return sizeof(ProxySphereShape); +} + +// Return a local support point in a given direction with the object margin +inline Vector3 ProxySphereShape::getLocalSupportPointWithMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithMargin(direction); +} + +// Return a local support point in a given direction without the object margin +inline Vector3 ProxySphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { + return mCollisionShape->getLocalSupportPointWithoutMargin(direction); +} + +// Return the current object margin +inline decimal ProxySphereShape::getMargin() const { + return mCollisionShape->getMargin(); +} + } #endif diff --git a/src/configuration.h b/src/configuration.h index 63c5ad3e..2e011cb1 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -125,11 +125,6 @@ const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0)); /// fatten to allow the collision shape to move a little bit without triggering /// a large modification of the tree which can be costly const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1); - -/// In the dynamic AABB tree, we multiply this factor by the displacement of -/// an object that has moved to recompute a new fat AABB -const decimal AABB_DISPLACEMENT_MULTIPLIER = decimal(2.0); - } #endif diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index a85b1f9f..f4ef3d39 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -53,9 +53,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); + // Get the bodies center of mass and orientations + const Vector3& x1 = mBody1->mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -164,7 +164,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con // do not execute this method if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; - // Get the bodies positions and orientations + // Get the bodies center of mass and orientations Vector3& x1 = constraintSolverData.positions[mIndexBody1]; Vector3& x2 = constraintSolverData.positions[mIndexBody2]; Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; @@ -214,7 +214,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 + // Update the body center of mass and orientation of body 1 x1 += v1; q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index a26418fc..075b13eb 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -62,8 +62,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; // Get the bodies positions and orientations - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); + const Vector3& x1 = mBody1->mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 24d61747..aa3954d3 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -77,8 +77,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; // Get the bodies positions and orientations - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); + const Vector3& x1 = mBody1->mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 85e3ff3c..fe7ed7c3 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -76,8 +76,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; // Get the bodies positions and orientations - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); + const Vector3& x1 = mBody1-mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -679,6 +679,7 @@ void SliderJoint::enableMotor(bool isMotorEnabled) { } // Return the current translation value of the joint +// TODO : Check if we need to compare rigid body position or center of mass here decimal SliderJoint::getTranslation() const { // Get the bodies positions and orientations diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 87dfa76b..c1defb5a 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -43,27 +43,15 @@ CollisionWorld::~CollisionWorld() { assert(mBodies.empty()); } -// 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, +void CollisionWorld::notifyNewContact(const OverlappingPair *broadPhasePair, const ContactPointInfo* contactInfo) { // TODO : Implement this method } // Update the overlapping pair -inline void CollisionWorld::updateOverlappingPair(const BroadPhasePair* pair) { +inline void CollisionWorld::updateOverlappingPair(const OverlappingPair *pair) { } @@ -87,7 +75,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform, mBodies.insert(collisionBody); // Add the collision body to the collision detection - mCollisionDetection.addBody(collisionBody); + mCollisionDetection.addProxyCollisionShape(collisionBody); // Return the pointer to the rigid body return collisionBody; @@ -97,7 +85,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform, void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { // Remove the body from the collision detection - mCollisionDetection.removeBody(collisionBody); + mCollisionDetection.removeProxyCollisionShape(collisionBody); // Add the body ID to the list of free IDs mFreeBodiesIDs.push_back(collisionBody->getID()); @@ -129,7 +117,7 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() { return bodyID; } -// Create a new collision shape. +// Create a new collision shape in the world. /// First, this methods checks that the new collision shape does not exist yet in the /// world. If it already exists, we do not allocate memory for a new one but instead /// we reuse the existing one. The goal is to only allocate memory for a single diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 4d297466..1f62ed21 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -85,28 +85,22 @@ class CollisionWorld { /// Private assignment operator CollisionWorld& operator=(const CollisionWorld& world); - /// Notify the world about a new broad-phase overlapping pair - virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair); - - /// Notify the world about a removed broad-phase overlapping pair - virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); - /// Notify the world about a new narrow-phase contact - virtual void notifyNewContact(const BroadPhasePair* pair, + virtual void notifyNewContact(const OverlappingPair* pair, const ContactPointInfo* contactInfo); /// Update the overlapping pair - virtual void updateOverlappingPair(const BroadPhasePair* pair); + virtual void updateOverlappingPair(const OverlappingPair* pair); /// Return the next available body ID bodyindex computeNextAvailableBodyID(); - /// Create a new collision shape. - CollisionShape* createCollisionShape(const CollisionShape& collisionShape); - /// Remove a collision shape. void removeCollisionShape(CollisionShape* collisionShape); + /// Create a new collision shape in the world. + CollisionShape* createCollisionShape(const CollisionShape& collisionShape); + public : // -------------------- Methods -------------------- // @@ -133,6 +127,8 @@ class CollisionWorld { // -------------------- Friends -------------------- // friend class CollisionDetection; + friend class CollisionBody; + friend class RigidBody; }; // Return an iterator to the beginning of the bodies of the physics world diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 9912c4d8..dc105209 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -87,8 +87,8 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2(); // Get the position of the two bodies - Vector3 x1 = body1->getTransform().getPosition(); - Vector3 x2 = body2->getTransform().getPosition(); + const Vector3& x1 = body1->mCenterOfMassWorld; + const Vector3& x2 = body2->mCenterOfMassWorld; // Initialize the internal contact manifold structure using the external // contact manifold diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 5e39ce56..eb6b520b 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -34,6 +34,14 @@ using namespace reactphysics3d; using namespace std; +// TODO : Check if we really need to store the contact manifolds also in mContactManifolds. + +// TODO : Check how to compute the initial inertia tensor now (especially when a body has multiple +// collision shapes. + +// TODO : Check the Body::setType() function in Box2D to be sure we are not missing any update +// of the collision shapes and broad-phase modification. + // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true), @@ -128,9 +136,6 @@ void DynamicsWorld::update() { // Integrate the velocities integrateRigidBodiesVelocities(); - // Reset the movement boolean variable of each body to false - resetBodiesMovementVariable(); - // Update the timer mTimer.nextStep(); @@ -199,6 +204,9 @@ void DynamicsWorld::integrateRigidBodiesPositions() { Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) * currentOrientation * decimal(0.5) * dt; + // Update the world-space center of mass + // TODO : IMPLEMENT THIS + // Update the Transform of the body Transform newTransform(newPosition, newOrientation.getUnit()); bodies[b]->setTransform(newTransform); @@ -218,6 +226,9 @@ void DynamicsWorld::updateRigidBodiesAABB() { // If the body has moved if ((*it)->mHasMoved) { + // Update the transform of the body due to the change of its center of mass + (*it)->updateTransformWithCenterOfMass(); + // Update the AABB of the rigid body (*it)->updateAABB(); } @@ -444,7 +455,7 @@ void DynamicsWorld::solvePositionCorrection() { // Get the position/orientation of the rigid body const Transform& transform = bodies[b]->getTransform(); - mConstrainedPositions[index] = transform.getPosition(); + mConstrainedPositions[index] = bodies[b]->mCenterOfMassWorld; mConstrainedOrientations[index]= transform.getOrientation(); } @@ -463,20 +474,20 @@ void DynamicsWorld::solvePositionCorrection() { uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - // Get the new position/orientation of the body - const Vector3& newPosition = mConstrainedPositions[index]; - const Quaternion& newOrientation = mConstrainedOrientations[index]; + // Update the position of the center of mass of the body + bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index]; - // Update the Transform of the body - Transform newTransform(newPosition, newOrientation.getUnit()); - bodies[b]->setTransform(newTransform); + // Update the orientation of the body + bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit()); + + // Update the Transform of the body (using the new center of mass and new orientation) + bodies[b]->updateTransformWithCenterOfMass(); } } } // Create a rigid body into the physics world -RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass, - const CollisionShape& collisionShape) { +RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass) { // Compute the body ID bodyindex bodyID = computeNextAvailableBodyID(); @@ -484,22 +495,18 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma // Largest index cannot be used (it is used for invalid index) assert(bodyID < std::numeric_limits::max()); - // Create a collision shape for the rigid body into the world - CollisionShape* newCollisionShape = createCollisionShape(collisionShape); - // Create the rigid body RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, - mass, - newCollisionShape, - bodyID); + mass, bodyID); assert(rigidBody != NULL); // Add the rigid body to the physics world mBodies.insert(rigidBody); mRigidBodies.insert(rigidBody); + // TODO : DELETE THIS // Add the rigid body to the collision detection - mCollisionDetection.addBody(rigidBody); + //mCollisionDetection.addProxyCollisionShape(rigidBody); // Return the pointer to the rigid body return rigidBody; @@ -508,14 +515,16 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma // Destroy a rigid body and all the joints which it belongs void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { + // TODO : DELETE THIS // Remove the body from the collision detection - mCollisionDetection.removeBody(rigidBody); + //mCollisionDetection.removeProxyCollisionShape(rigidBody); // Add the body ID to the list of free IDs mFreeBodiesIDs.push_back(rigidBody->getID()); + // TODO : DELETE THIS // Remove the collision shape from the world - removeCollisionShape(rigidBody->getCollisionShape()); + //removeCollisionShape(rigidBody->getCollisionShape()); // Destroy all the joints in which the rigid body to be destroyed is involved JointListElement* element; @@ -910,33 +919,6 @@ void DynamicsWorld::updateSleepingBodies() { } } -// Notify the world about a new broad-phase overlapping pair -void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) { - - // Get the pair of body index - bodyindexpair indexPair = addedPair->getBodiesIndexPair(); - - // Add the pair into the set of overlapping pairs (if not there yet) - OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(addedPair->body1, addedPair->body2, mMemoryAllocator); - assert(newPair != NULL); - std::pair::iterator, bool> check = - mOverlappingPairs.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 allocator - mOverlappingPairs.find(indexPair)->second->OverlappingPair::~OverlappingPair(); - mMemoryAllocator.release(mOverlappingPairs[indexPair], sizeof(OverlappingPair)); - mOverlappingPairs.erase(indexPair); -} - // Notify the world about a new narrow-phase contact void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const ContactPointInfo* contactInfo) { diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 226522d7..bef110b5 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -154,6 +154,11 @@ class DynamicsWorld : public CollisionWorld { void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, Vector3 newAngVelocity); + /// Add a contact manifold to the linked list of contact manifolds of the two bodies + /// involed in the corresponding contact. + void addContactManifoldToBody(ContactManifold* contactManifold, + CollisionBody *body1, CollisionBody *body2); + /// Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); @@ -172,9 +177,6 @@ class DynamicsWorld : public CollisionWorld { /// Cleanup the constrained velocities array at each step void cleanupConstrainedVelocitiesArray(); - /// Reset the boolean movement variable of each body - void resetBodiesMovementVariable(); - /// Compute the islands of awake bodies. void computeIslands(); @@ -184,12 +186,6 @@ class DynamicsWorld : public CollisionWorld { /// Update the overlapping pair virtual void updateOverlappingPair(const BroadPhasePair* pair); - /// Notify the world about a new broad-phase overlapping pair - virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair); - - /// Notify the world about a removed broad-phase overlapping pair - virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); - /// Notify the world about a new narrow-phase contact virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo); @@ -230,8 +226,7 @@ class DynamicsWorld : public CollisionWorld { void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); /// Create a rigid body into the physics world. - RigidBody* createRigidBody(const Transform& transform, decimal mass, - const CollisionShape& collisionShape); + RigidBody* createRigidBody(const Transform& transform, decimal mass); /// Destroy a rigid body and all the joints which it belongs void destroyRigidBody(RigidBody* rigidBody); @@ -245,11 +240,6 @@ class DynamicsWorld : public CollisionWorld { /// Add the joint to the list of joints of the two bodies involved in the joint void addJointToBody(Joint* joint); - /// Add a contact manifold to the linked list of contact manifolds of the two bodies - /// involed in the corresponding contact. - void addContactManifoldToBody(ContactManifold* contactManifold, - CollisionBody *body1, CollisionBody *body2); - /// Reset all the contact manifolds linked list of each body void resetContactManifoldListsOfBodies(); @@ -369,21 +359,11 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); } -// 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)->mHasMoved = false; - } -} - // Update the overlapping pair inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) { + // TODO : CHECK WHERE TO CALL THIS METHOD + // Get the pair of body index std::pair indexPair = pair->getBodiesIndexPair(); diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 1ecfe0c6..921dba1d 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -28,15 +28,19 @@ // Libraries #include "ContactManifold.h" +#include "../collision/shapes/CollisionShape.h" /// ReactPhysics3D namespace namespace reactphysics3d { +// Type for the overlapping pair ID +typedef std::pair overlappingpairid; + // Class OverlappingPair /** - * This class represents a pair of two bodies that are overlapping + * This class represents a pair of two proxy collision shapes that are overlapping * during the broad-phase collision detection. It is created when - * the two bodies start to overlap and is destroyed when they do not + * the two proxy collision shapes start to overlap and is destroyed when they do not * overlap anymore. This class contains a contact manifold that * store all the contact points between the two bodies. */ @@ -46,11 +50,11 @@ class OverlappingPair { // -------------------- Attributes -------------------- // - /// Pointer to the first body of the contact - CollisionBody* mBody1; + /// Pointer to the first proxy collision shape + ProxyShape* mShape1; - /// Pointer to the second body of the contact - CollisionBody* mBody2; + /// Pointer to the second proxy collision shape + ProxyShape* mShape2; /// Persistent contact manifold ContactManifold mContactManifold; @@ -71,17 +75,16 @@ class OverlappingPair { // -------------------- Methods -------------------- // /// Constructor - OverlappingPair(CollisionBody* body1, CollisionBody* body2, - MemoryAllocator& memoryAllocator); + OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator); /// Destructor ~OverlappingPair(); - /// Return the pointer to first body - CollisionBody* const getBody1() const; + /// Return the pointer to first proxy collision shape + ProxyShape* const getShape1() const; /// Return the pointer to second body - CollisionBody* const getBody2() const; + ProxyShape* const getShape2() const; /// Add a contact to the contact cache void addContact(ContactPoint* contact); @@ -93,6 +96,7 @@ class OverlappingPair { Vector3 getCachedSeparatingAxis() const; /// Set the cached separating axis + // TODO : Check that this variable is correctly used allong the collision detection process void setCachedSeparatingAxis(const Vector3& axis); /// Return the number of contacts in the cache @@ -101,19 +105,25 @@ class OverlappingPair { /// Return the contact manifold ContactManifold* getContactManifold(); + /// Return the pair of bodies index + static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); + + /// Return the pair of bodies index of the pair + static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2); + // -------------------- Friendship -------------------- // friend class DynamicsWorld; }; // Return the pointer to first body -inline CollisionBody* const OverlappingPair::getBody1() const { - return mBody1; +inline ProxyShape* const OverlappingPair::getShape1() const { + return mShape1; } // Return the pointer to second body -inline CollisionBody* const OverlappingPair::getBody2() const { - return mBody2; +inline ProxyShape* const OverlappingPair::getShape2() const { + return mShape2; } // Add a contact to the contact manifold @@ -123,7 +133,7 @@ inline void OverlappingPair::addContact(ContactPoint* contact) { // Update the contact manifold inline void OverlappingPair::update() { - mContactManifold.update(mBody1->getTransform(), mBody2->getTransform()); + mContactManifold.update(mShape1->getBody()->getTransform(), mShape2->getBody()->getTransform()); } // Return the cached separating axis @@ -147,6 +157,30 @@ inline ContactManifold* OverlappingPair::getContactManifold() { return &mContactManifold; } +// Return the pair of bodies index +inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { + assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0); + + // Construct the pair of body index + overlappingpairid pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ? + std::make_pair(shape1->mBroadPhaseID, shape2->mBroadPhaseID) : + std::make_pair(shape2->mBroadPhaseID, shape1->mBroadPhaseID); + assert(pairID.first != pairID.second); + return pairID; +} + +// Return the pair of bodies index +inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body1, + CollisionBody* body2) { + + // 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; +} + } #endif diff --git a/src/memory/Stack.h b/src/memory/Stack.h new file mode 100644 index 00000000..a6c68c2b --- /dev/null +++ b/src/memory/Stack.h @@ -0,0 +1,125 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2014 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 REACTPHYSICS3D_STACK_H +#define REACTPHYSICS3D_STACK_H + +// Libraries +#include "../configuration.h" + +namespace reactphysics3d { + +// Class Stack +/** + * This class represents a simple generic stack with an initial capacity. If the number + * of elements exceeds the capacity, the heap will be used to allocated more memory. + */ +template +class Stack { + + private: + + // -------------------- Attributes -------------------- // + + /// Initial array that contains the elements of the stack + T mInitArray[capacity]; + + /// Pointer to the first element of the stack + T* mElements; + + /// Number of elements in the stack + uint mNbElements; + + /// Number of allocated elements in the stack + uint mNbAllocatedElements; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + Stack() : mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) { + + } + + /// Destructor + ~Stack() { + + // If elements have been allocated on the heap + if (mInitArray != mElements) { + + // Release the memory allocated on the heap + free(mElements); + } + } + + /// Push an element into the stack + void push(const T& element); + + /// Pop an element from the stack (remove it from the stack and return it) + T pop(); + + /// Return the number of elments in the stack + uint getNbElements() const; + +}; + +// Push an element into the stack +template +inline void Stack::push(const T& element) { + + // If we need to allocate more elements + if (mNbElements == mNbAllocatedElements) { + T* oldElements = mElements; + mNbAllocatedElements *= 2; + mElements = (T*) malloc(mNbAllocatedElements * sizeof(T)); + assert(mElements); + memcpy(mElements, oldElements, mNbElements * sizeof(T)); + if (oldElements != mInitArray) { + free(oldElements); + } + } + + mElements[mNbElements] = element; + mNbElements++; +} + +// Pop an element from the stack (remove it from the stack and return it) +template +inline T Stack::pop() { + assert(mNbElements > 0); + mNbElements--; + return mElements[mNbElements]; +} + +// Return the number of elments in the stack +template +inline uint Stack::getNbElements() const { + return mNbElements; +} + +} + +#endif