diff --git a/CMakeLists.txt b/CMakeLists.txt index 87a02105..b3d37ed1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,7 +80,6 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/GJK/GJKAlgorithm.h" "src/collision/narrowphase/GJK/GJKAlgorithm.cpp" "src/collision/narrowphase/NarrowPhaseAlgorithm.h" - "src/collision/narrowphase/NarrowPhaseAlgorithm.cpp" "src/collision/narrowphase/SphereVsSphereAlgorithm.h" "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h" @@ -121,7 +120,7 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/TriangleMesh.cpp" "src/collision/CollisionDetection.h" "src/collision/CollisionDetection.cpp" - "src/collision/CollisionShapeInfo.h" + "src/collision/NarrowPhaseInfo.h" "src/collision/ContactManifold.h" "src/collision/ContactManifold.cpp" "src/collision/ContactManifoldSet.h" @@ -173,11 +172,13 @@ SET (REACTPHYSICS3D_SOURCES "src/mathematics/Vector3.h" "src/mathematics/Ray.h" "src/mathematics/Vector3.cpp" + "src/memory/Allocator.h" "src/memory/PoolAllocator.h" "src/memory/PoolAllocator.cpp" "src/memory/SingleFrameAllocator.h" "src/memory/SingleFrameAllocator.cpp" - "src/memory/Stack.h" + "src/containers/Stack.h" + "src/containers/LinkedList.h" ) # Create the library diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index bd5c7818..2d5101b9 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -153,6 +153,9 @@ class CollisionBody : public Body { /// Raycast method with feedback information bool raycast(const Ray& ray, RaycastInfo& raycastInfo); + /// Test if the collision body overlaps with a given AABB + bool testAABBOverlap(const AABB& worldAABB) const; + /// Compute and return the AABB of the body by merging all proxy shapes AABBs AABB getAABB() const; @@ -301,6 +304,15 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const { return mTransform.getOrientation().getInverse() * worldVector; } +/// Test if the collision body overlaps with a given AABB +/** +* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap +* @return True if the given AABB overlaps with the AABB of the collision body +*/ +inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const { + return worldAABB.testCollision(getAABB()); +} + } #endif diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index e2599fca..c199859c 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -35,15 +35,16 @@ #include #include #include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator) - : mMemoryAllocator(memoryAllocator), - mWorld(world), mBroadPhaseAlgorithm(*this), +CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator) + : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator), + mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { // Set the default collision dispatch configuration @@ -60,27 +61,20 @@ void CollisionDetection::computeCollisionDetection() { // Compute the broad-phase collision detection computeBroadPhase(); + + // Compute the middle-phase collision detection + computeMiddlePhase(); // Compute the narrow-phase collision detection - computeNarrowPhase(); -} - -// Compute the collision detection -void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - - // Compute the broad-phase collision detection - computeBroadPhase(); - - // Delete all the contact points in the currently overlapping pairs - clearContactPoints(); - - // Compute the narrow-phase collision detection among given sets of shapes - computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); + computeNarrowPhase(); + + // Reset the linked list of narrow-phase info + mNarrowPhaseInfoList = nullptr; } +// TODO : Remove this method // Report collision between two sets of shapes +/* void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, const std::set& shapes1, const std::set& shapes2) { @@ -126,13 +120,9 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac ContactPoint* contactPoint = manifold->getContactPoint(i); // Create the contact info object for the contact - ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), - manifold->getShape1()->getCollisionShape(), - manifold->getShape2()->getCollisionShape(), - contactPoint->getNormal(), - contactPoint->getPenetrationDepth(), - contactPoint->getLocalPointOnBody1(), - contactPoint->getLocalPointOnBody2()); + ContactPointInfo contactInfo; + contactInfo.init(contactPoint->getNormal(), contactPoint->getPenetrationDepth(), + contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2()); // Notify the collision callback about this new contact if (callback != nullptr) callback->notifyContact(contactInfo); @@ -140,6 +130,7 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac } } } +*/ // Compute the broad-phase collision detection void CollisionDetection::computeBroadPhase() { @@ -152,18 +143,18 @@ void CollisionDetection::computeBroadPhase() { // 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(); + mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator); } } -// Compute the narrow-phase collision detection -void CollisionDetection::computeNarrowPhase() { +// Compute the middle-phase collision detection +void CollisionDetection::computeMiddlePhase() { - PROFILE("CollisionDetection::computeNarrowPhase()"); + PROFILE("CollisionDetection::computeMiddlePhase()"); // Clear the set of overlapping pairs in narrow-phase contact mContactOverlappingPairs.clear(); - + // For each possible collision pair of bodies map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { @@ -199,7 +190,7 @@ void CollisionDetection::computeNarrowPhase() { CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body2 = shape2->getBody(); - + // Update the contact cache of the overlapping pair pair->update(); @@ -211,37 +202,154 @@ void CollisionDetection::computeNarrowPhase() { // Check if the bodies are in the set of bodies that cannot collide between each other bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - - // Select the narrow phase algorithm to use according to the two collision shapes + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + + // If both shapes are convex + if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) + NarrowPhaseInfo(pair, shape1->getCollisionShape(), + shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), + shape2->getCachedCollisionData()); + mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; + + } + // Concave vs Convex algorithm + else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || + (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + + NarrowPhaseInfo* narrowPhaseInfo = nullptr; + computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo); + + // Add all the narrow-phase info object reported by the callback into the + // list of all the narrow-phase info object + while (narrowPhaseInfo != nullptr) { + NarrowPhaseInfo* head = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = narrowPhaseInfo; + mNarrowPhaseInfoList->next = head; + + narrowPhaseInfo = narrowPhaseInfo->next; + } + } + // Concave vs Concave shape + else { + // Not handled + continue; + } + } +} + +// Compute the concave vs convex middle-phase algorithm for a given pair of bodies +void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator, + NarrowPhaseInfo** firstNarrowPhaseInfo) { + + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); + + ProxyShape* convexProxyShape; + ProxyShape* concaveProxyShape; + const ConvexShape* convexShape; + const ConcaveShape* concaveShape; + + // Collision shape 1 is convex, collision shape 2 is concave + if (shape1->getCollisionShape()->isConvex()) { + convexProxyShape = shape1; + convexShape = static_cast(shape1->getCollisionShape()); + concaveProxyShape = shape2; + concaveShape = static_cast(shape2->getCollisionShape()); + } + else { // Collision shape 2 is convex, collision shape 1 is concave + convexProxyShape = shape2; + convexShape = static_cast(shape2->getCollisionShape()); + concaveProxyShape = shape1; + concaveShape = static_cast(shape1->getCollisionShape()); + } + + // Set the parameters of the callback object + MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape, + concaveShape, allocator); + + // Compute the convex shape AABB in the local-space of the convex shape + AABB aabb; + convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); + + // TODO : Implement smooth concave mesh collision somewhere + + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape->testAllTriangles(middlePhaseCallback, aabb); + + // Add all the narrow-phase info object reported by the callback into the + // list of all the narrow-phase info object + *firstNarrowPhaseInfo = middlePhaseCallback.narrowPhaseInfoList; +} + +// Compute the narrow-phase collision detection +void CollisionDetection::computeNarrowPhase() { + + PROFILE("CollisionDetection::computeNarrowPhase()"); + + const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; + while (currentNarrowPhaseInfo != nullptr) { + + // Select the narrow phase algorithm to use according to the two collision shapes + const CollisionShapeType shape1Type = currentNarrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = currentNarrowPhaseInfo->collisionShape2->getType(); const int shape1Index = static_cast(shape1Type); const int shape2Index = static_cast(shape2Type); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; - // If there is no collision algorithm between those two kinds of shapes - if (narrowPhaseAlgorithm == nullptr) continue; + // If there is no collision algorithm between those two kinds of shapes, skip it + if (narrowPhaseAlgorithm != nullptr) { - // 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 a collision occurs, the + // notifyContact() callback method will be called. + ContactPointInfo contactPointInfo; + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactPointInfo)) { - // Create the CollisionShapeInfo objects - CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), - pair, shape1->getCachedCollisionData()); - CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), - pair, shape2->getCachedCollisionData()); - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this); + // If it is the first contact since the pairs are overlapping + if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) { + + // Trigger a callback event + if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactPointInfo); + } + + // Create a new contact + ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint))) + ContactPoint(contactPointInfo); + + contact->updateWorldContactPoints(currentNarrowPhaseInfo->shape1ToWorldTransform, + currentNarrowPhaseInfo->shape2ToWorldTransform); + + // Add the contact to the contact manifold set of the corresponding overlapping pair + currentNarrowPhaseInfo->overlappingPair->addContact(contact); + + // Add the overlapping pair into the set of pairs in contact during narrow-phase + overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(), + currentNarrowPhaseInfo->overlappingPair->getShape2()); + mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair; + + // Trigger a callback event for the new contact + if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactPointInfo); + } + } + + currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; } // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); } +// TODO : Remove this method // Compute the narrow-phase collision detection +/* void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, const std::set& shapes1, const std::set& shapes2) { @@ -326,9 +434,6 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call // If there is no collision algorithm between those two kinds of shapes if (narrowPhaseAlgorithm == nullptr) continue; - // Notify the narrow-phase algorithm about the overlapping pair we are going to test - narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); - // Create the CollisionShapeInfo objects CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), pair, shape1->getCachedCollisionData()); @@ -345,6 +450,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); } +*/ // Allow the broadphase to notify the collision detection about an overlapping pair. /// This method is called by the broad-phase collision detection algorithm @@ -352,9 +458,6 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // If the two proxy collision shapes are from the same body, skip it - if (shape1->getBody()->getID() == shape2->getBody()->getID()) return; - // Check if the collision filtering allows collision between the two shapes if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; @@ -412,45 +515,6 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); } -// Called by a narrow-phase collision algorithm when a new contact has been found -void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { - - // If it is the first contact since the pairs are overlapping - if (overlappingPair->getNbContactPoints() == 0) { - - // Trigger a callback event - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactInfo); - } - - // Create a new contact - createContact(overlappingPair, contactInfo); - - // Trigger a callback event for the new contact - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactInfo); -} - -// Create a new contact -void CollisionDetection::createContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { - - PROFILE("CollisionDetection::createContact()"); - - // Create a new contact - ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint))) - ContactPoint(contactInfo); - - contact->updateWorldContactPoints(overlappingPair->getShape1()->getLocalToWorldTransform(), - overlappingPair->getShape2()->getLocalToWorldTransform()); - - // Add the contact to the contact manifold set of the corresponding overlapping pair - overlappingPair->addContact(contact); - - // Add the overlapping pair into the set of pairs in contact during narrow-phase - overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), - overlappingPair->getShape2()); - mContactOverlappingPairs[pairId] = overlappingPair; -} - void CollisionDetection::addAllContactManifoldsToBodies() { // For each overlapping pairs in contact during the narrow-phase @@ -508,6 +572,464 @@ void CollisionDetection::clearContactPoints() { } } +// Compute the middle-phase collision detection between two proxy shapes +NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2) { + + // Create a temporary overlapping pair + OverlappingPair pair(shape1, shape2, 0, mMemoryAllocator); + + // ------------------------------------------------------- + + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + + NarrowPhaseInfo* narrowPhaseInfo = nullptr; + + // If both shapes are convex + if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(&pair, shape1->getCollisionShape(), + shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), + shape2->getCachedCollisionData()); + + } + // Concave vs Convex algorithm + else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || + (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + + // Run the middle-phase collision detection algorithm to find the triangles of the concave + // shape we need to use during the narrow-phase collision detection + computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo); + } + + return nullptr; +} + +// Report all the bodies that overlap with the aabb in parameter +void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, + unsigned short categoryMaskBits) { + assert(overlapCallback != nullptr); + + std::unordered_set reportedBodies; + + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryAllocator); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); + + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { + + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + + CollisionBody* overlapBody = proxyShape->getBody(); + + // If the proxy shape is from a body that we have not already reported collision + if (reportedBodies.find(overlapBody->getID()) == reportedBodies.end()) { + + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + + // Add the body into the set of reported bodies + reportedBodies.insert(overlapBody->getID()); + + // Notify the overlap to the user + overlapCallback->notifyOverlap(overlapBody); + } + } + + // Go to the next overlapping proxy shape + element = element->next; + } +} + +// Return true if two bodies overlap +bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) { + + // For each proxy shape proxy shape of the first body + ProxyShape* body1ProxyShape = body1->getProxyShapesList(); + while (body1ProxyShape != nullptr) { + + AABB aabb1 = body1ProxyShape->getWorldAABB(); + + // For each proxy shape of the second body + ProxyShape* body2ProxyShape = body2->getProxyShapesList(); + while (body2ProxyShape != nullptr) { + + AABB aabb2 = body2ProxyShape->getWorldAABB(); + + // Test if the AABBs of the two proxy shapes overlap + if (aabb1.testCollision(aabb2)) { + + const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape); + + bool isColliding = false; + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + // If we have not found a collision yet + if (!isColliding) { + + // Select the narrow phase algorithm to use according to the two collision shapes + const int shape1Index = static_cast(shape1Type); + const int shape2Index = static_cast(shape2Type); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + ContactPointInfo contactPointInfo; + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo); + } + } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Release the allocated memory + mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } + + // Return if we have found a narrow-phase collision + if (isColliding) return true; + } + + // Go to the next proxy shape + body2ProxyShape = body2ProxyShape->getNext(); + } + + // Go to the next proxy shape + body1ProxyShape = body1ProxyShape->getNext(); + } + + // No overlap has been found + return false; +} + +// Report all the bodies that overlap with the body in parameter +void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, + unsigned short categoryMaskBits) { + + assert(overlapCallback != nullptr); + + std::unordered_set reportedBodies; + + // For each proxy shape proxy shape of the body + ProxyShape* bodyProxyShape = body->getProxyShapesList(); + while (bodyProxyShape != nullptr) { + + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryAllocator); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + + const bodyindex bodyId = body->getID(); + + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { + + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + + // If the proxy shape is from a body that we have not already reported collision and the + // two proxy collision shapes are not from the same body + if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() && + proxyShape->getBody()->getID() != bodyId) { + + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + + const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape); + + bool isColliding = false; + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + // If we have not found a collision yet + if (!isColliding) { + + // Select the narrow phase algorithm to use according to the two collision shapes + const int shape1Index = static_cast(shape1Type); + const int shape2Index = static_cast(shape2Type); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + ContactPointInfo contactPointInfo; + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo); + } + } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Release the allocated memory + mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } + + // Return if we have found a narrow-phase collision + if (isColliding) { + + CollisionBody* overlapBody = proxyShape->getBody(); + + // Add the body into the set of reported bodies + reportedBodies.insert(overlapBody->getID()); + + // Notify the overlap to the user + overlapCallback->notifyOverlap(overlapBody); + } + } + } + + // Go to the next overlapping proxy shape + element = element->next; + } + + // Go to the next proxy shape + bodyProxyShape = bodyProxyShape->getNext(); + } +} + +// Test and report collisions between two bodies +void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* collisionCallback) { + + assert(collisionCallback != nullptr); + + // For each proxy shape proxy shape of the first body + ProxyShape* body1ProxyShape = body1->getProxyShapesList(); + while (body1ProxyShape != nullptr) { + + AABB aabb1 = body1ProxyShape->getWorldAABB(); + + // For each proxy shape of the second body + ProxyShape* body2ProxyShape = body2->getProxyShapesList(); + while (body2ProxyShape != nullptr) { + + AABB aabb2 = body2ProxyShape->getWorldAABB(); + + // Test if the AABBs of the two proxy shapes overlap + if (aabb1.testCollision(aabb2)) { + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape); + + const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + // Select the narrow phase algorithm to use according to the two collision shapes + const int shape1Index = static_cast(shape1Type); + const int shape2Index = static_cast(shape2Type); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + ContactPointInfo contactPointInfo; + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + + CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body1, body2, + body1ProxyShape, body2ProxyShape); + + // Report the contact to the user + collisionCallback->notifyContact(collisionInfo); + } + } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Release the allocated memory + mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } + } + + // Go to the next proxy shape + body2ProxyShape = body2ProxyShape->getNext(); + } + + // Go to the next proxy shape + body1ProxyShape = body1ProxyShape->getNext(); + } +} + +// Test and report collisions between a body and all the others bodies of the world +void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) { + + assert(callback != nullptr); + + // For each proxy shape proxy shape of the body + ProxyShape* bodyProxyShape = body->getProxyShapesList(); + while (bodyProxyShape != nullptr) { + + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryAllocator); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + + const bodyindex bodyId = body->getID(); + + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { + + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + + // If the two proxy collision shapes are not from the same body + if (proxyShape->getBody()->getID() != bodyId) { + + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + + const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape); + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + // Select the narrow phase algorithm to use according to the two collision shapes + const int shape1Index = static_cast(shape1Type); + const int shape2Index = static_cast(shape2Type); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + ContactPointInfo contactPointInfo; + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + + CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body, + proxyShape->getBody(), bodyProxyShape, + proxyShape); + + // Report the contact to the user + callback->notifyContact(collisionInfo); + } + } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Release the allocated memory + mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } + } + } + + // Go to the next overlapping proxy shape + element = element->next; + } + + // Go to the next proxy shape + bodyProxyShape = bodyProxyShape->getNext(); + } +} + +// Test and report collisions between all shapes of the world +void CollisionDetection::testCollision(CollisionCallback* callback) { + + assert(callback != nullptr); + + // Compute the broad-phase collision detection + computeBroadPhase(); + + // For each possible collision pair of bodies + map::iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + + OverlappingPair* pair = it->second; + + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); + + // Check if the collision filtering allows collision between the two shapes and + // that the two shapes are still overlapping. + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0) && + mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(shape1, shape2); + + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + // Select the narrow phase algorithm to use according to the two collision shapes + const int shape1Index = static_cast(shape1Type); + const int shape2Index = static_cast(shape2Type); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + ContactPointInfo contactPointInfo; + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + + CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, shape1->getBody(), + shape2->getBody(), shape1, shape2); + + // Report the contact to the user + callback->notifyContact(collisionInfo); + } + } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Release the allocated memory + mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } + } + } +} + // Fill-in the collision detection matrix void CollisionDetection::fillInCollisionMatrix() { @@ -528,9 +1050,3 @@ EventListener* CollisionDetection::getWorldEventListener() { PoolAllocator& CollisionDetection::getWorldMemoryAllocator() { return mWorld->mPoolAllocator; } - -// Called by a narrow-phase collision algorithm when a new contact has been found -void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { - mCollisionCallback->notifyContact(contactInfo); -} diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 0fe486c4..3bb27366 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -33,6 +33,7 @@ #include "engine/EventListener.h" #include "narrowphase/DefaultCollisionDispatch.h" #include "memory/PoolAllocator.h" +#include "memory/SingleFrameAllocator.h" #include "constraint/ContactPoint.h" #include #include @@ -46,29 +47,7 @@ namespace reactphysics3d { class BroadPhaseAlgorithm; class CollisionWorld; class CollisionCallback; - -// Class TestCollisionBetweenShapesCallback -class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { - - private: - - CollisionCallback* mCollisionCallback; - - public: - - // Constructor - TestCollisionBetweenShapesCallback(CollisionCallback* callback) - : mCollisionCallback(callback) { - - } - - // Destructor - virtual ~TestCollisionBetweenShapesCallback() { } - - // Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) override; -}; +class OverlapCallback; // Class CollisionDetection /** @@ -77,7 +56,7 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { * collide and then we run a narrow-phase algorithm to compute the * collision contacts between bodies. */ -class CollisionDetection : public NarrowPhaseCallback { +class CollisionDetection { private : @@ -95,9 +74,15 @@ class CollisionDetection : public NarrowPhaseCallback { /// Reference to the memory allocator PoolAllocator& mMemoryAllocator; + /// Reference to the single frame memory allocator + SingleFrameAllocator& mSingleFrameAllocator; + /// Pointer to the physics world CollisionWorld* mWorld; + /// Pointer to the first narrow-phase info of the linked list + NarrowPhaseInfo* mNarrowPhaseInfoList; + /// Broad-phase overlapping pairs std::map mOverlappingPairs; @@ -111,6 +96,7 @@ class CollisionDetection : public NarrowPhaseCallback { // TODO : Delete this GJKAlgorithm mNarrowPhaseGJKAlgorithm; + // TODO : Maybe delete this set (what is the purpose ?) /// Set of pair of bodies that cannot collide between each other std::set mNoCollisionPairs; @@ -122,6 +108,9 @@ class CollisionDetection : public NarrowPhaseCallback { /// Compute the broad-phase collision detection void computeBroadPhase(); + /// Compute the middle-phase collision detection + void computeMiddlePhase(); + /// Compute the narrow-phase collision detection void computeNarrowPhase(); @@ -137,13 +126,20 @@ class CollisionDetection : public NarrowPhaseCallback { /// Add all the contact manifold of colliding pairs to their bodies void addAllContactManifoldsToBodies(); + + /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies + void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator, + NarrowPhaseInfo** firstNarrowPhaseInfo); + + /// Compute the middle-phase collision detection between two proxy shapes + NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2); public : // -------------------- Methods -------------------- // /// Constructor - CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator); + CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator); /// Destructor ~CollisionDetection() = default; @@ -183,35 +179,42 @@ class CollisionDetection : public NarrowPhaseCallback { /// Compute the collision detection void computeCollisionDetection(); - /// Compute the collision detection - void testCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2); - + // TODO : Remove this method /// Report collision between two sets of shapes - void reportCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) ; + //void reportCollisionBetweenShapes(CollisionCallback* callback, + // const std::set& shapes1, + // const std::set& shapes2) ; /// Ray casting method void raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const; - /// Test if the AABBs of two bodies overlap - bool testAABBOverlap(const CollisionBody* body1, - const CollisionBody* body2) const; + /// Report all the bodies that overlap with the aabb in parameter + void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF); - /// Test if the AABBs of two proxy shapes overlap - bool testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const; + /// Return true if two bodies overlap + bool testOverlap(CollisionBody* body1, CollisionBody* body2); + + /// Report all the bodies that overlap with the body in parameter + void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF); + + /// Test and report collisions between two bodies + void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback); + + /// Test and report collisions between a body and all the others bodies of the world + void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF); + + /// Test and report collisions between all shapes of the world + void testCollision(CollisionCallback* callback); /// Allow the broadphase to notify the collision detection about an overlapping pair. void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); + // TODO : Remove this method /// Compute the narrow-phase collision detection - void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2); + //void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, + // const std::set& shapes1, + // const std::set& shapes2); /// Return a pointer to the world CollisionWorld* getWorld(); @@ -222,12 +225,6 @@ class CollisionDetection : public NarrowPhaseCallback { /// Return a reference to the world memory allocator PoolAllocator& getWorldMemoryAllocator(); - /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override; - - /// Create a new contact - void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); - // -------------------- Friendship -------------------- // friend class DynamicsWorld; @@ -244,8 +241,6 @@ inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(Collision inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { mCollisionDispatch = collisionDispatch; - mCollisionDispatch->init(this, &mMemoryAllocator); - // Fill-in the collision matrix with the new algorithms to use fillInCollisionMatrix(); } @@ -299,18 +294,6 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback, mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); } -// Test if the AABBs of two proxy shapes overlap -inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const { - - // If one of the shape's body is not active, we return no overlap - if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { - return false; - } - - return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); -} - // Return a pointer to the world inline CollisionWorld* CollisionDetection::getWorld() { return mWorld; diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 26f3a66f..a7dc9d2e 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -33,7 +33,7 @@ ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, PoolAllocator& memoryAllocator, int nbMaxManifolds) : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), mShape2(shape2), mMemoryAllocator(memoryAllocator) { - assert(nbMaxManifolds >= 1); + } // Destructor diff --git a/src/collision/CollisionShapeInfo.h b/src/collision/NarrowPhaseInfo.h similarity index 59% rename from src/collision/CollisionShapeInfo.h rename to src/collision/NarrowPhaseInfo.h index 2e1d9504..b97a71be 100644 --- a/src/collision/CollisionShapeInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_COLLISION_SHAPE_INFO_H -#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H +#ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_H +#define REACTPHYSICS3D_NARROW_PHASE_INFO_H // Libraries #include "shapes/CollisionShape.h" @@ -34,37 +34,48 @@ namespace reactphysics3d { class OverlappingPair; -// Class CollisionShapeInfo +// Class NarrowPhaseInfo /** * This structure regroups different things about a collision shape. This is * used to pass information about a collision shape to a collision algorithm. */ -struct CollisionShapeInfo { +struct NarrowPhaseInfo { public: /// Broadphase overlapping pair OverlappingPair* overlappingPair; - /// Proxy shape - ProxyShape* proxyShape; + /// Pointer to the first collision shape to test collision with + const CollisionShape* collisionShape1; - /// Pointer to the collision shape - const CollisionShape* collisionShape; + /// Pointer to the second collision shape to test collision with + const CollisionShape* collisionShape2; - /// Transform that maps from collision shape local-space to world-space - Transform shapeToWorldTransform; + /// Transform that maps from collision shape 1 local-space to world-space + Transform shape1ToWorldTransform; + + /// Transform that maps from collision shape 2 local-space to world-space + Transform shape2ToWorldTransform; /// Cached collision data of the proxy shape - void** cachedCollisionData; + // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 + void** cachedCollisionData1; + + /// Cached collision data of the proxy shape + // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 + void** cachedCollisionData2; + + /// Pointer to the next element in the linked list + NarrowPhaseInfo* next; /// Constructor - CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape, - const Transform& shapeLocalToWorldTransform, OverlappingPair* pair, - void** cachedData) - : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape), - shapeToWorldTransform(shapeLocalToWorldTransform), - cachedCollisionData(cachedData) { + NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, + const CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, void** cachedData1, void** cachedData2) + : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), + shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), + cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) { } }; diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index d7c81621..45c0add2 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -126,6 +126,12 @@ class ProxyShape { /// Return the local to world transform const Transform getLocalToWorldTransform() const; + /// Return the AABB of the proxy shape in world-space + const AABB getWorldAABB() const; + + /// Test if the proxy shape overlaps with a given AABB + bool testAABBOverlap(const AABB& worldAABB) const; + /// Return true if a point is inside the collision shape bool testPointInside(const Vector3& worldPoint); @@ -176,7 +182,7 @@ class ProxyShape { }; // Return the pointer to the cached collision data -inline void** ProxyShape::getCachedCollisionData() { +inline void** ProxyShape::getCachedCollisionData() { return &mCachedCollisionData; } @@ -249,6 +255,16 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const { return mBody->mTransform * mLocalToBodyTransform; } +// Return the AABB of the proxy shape in world-space +/** + * @return The AABB of the proxy shape in world-space + */ +inline const AABB ProxyShape::getWorldAABB() const { + AABB aabb; + mCollisionShape->computeAABB(aabb, getLocalToWorldTransform()); + return aabb; +} + // Return the next proxy shape in the linked list of proxy shapes /** * @return Pointer to the next proxy shape in the linked list of proxy shapes @@ -320,6 +336,15 @@ inline void ProxyShape::setLocalScaling(const Vector3& scaling) { mBody->updateProxyShapeInBroadPhase(this, true); } +/// Test if the proxy shape overlaps with a given AABB +/** +* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap +* @return True if the given AABB overlaps with the AABB of the collision body +*/ +inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const { + return worldAABB.testCollision(getWorldAABB()); +} + } #endif diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index 781273e5..bc03f44d 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -162,12 +162,25 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons } } +void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb, + LinkedList& overlappingNodes) const { + + AABBOverlapCallback callback(overlappingNodes); + + // Ask the dynamic AABB tree to report all collision shapes that overlap with this AABB + mDynamicAABBTree.reportAllShapesOverlappingWithAABB(aabb, callback); +} + // Compute all the overlapping pairs of collision shapes -void BroadPhaseAlgorithm::computeOverlappingPairs() { +void BroadPhaseAlgorithm::computeOverlappingPairs(Allocator& allocator) { + + // TODO : Try to see if we can allocate potential pairs in single frame allocator // Reset the potential overlapping pairs mNbPotentialPairs = 0; + LinkedList overlappingNodes(allocator); + // For all collision shapes that have moved (or have been created) during the // last simulation step for (uint i=0; i(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID)); ProxyShape* shape2 = static_cast(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); - // Notify the collision detection about the overlapping pair - mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); + // If the two proxy collision shapes are from the same body, skip it + if (shape1->getBody()->getID() != shape2->getBody()->getID()) { + + // Notify the collision detection about the overlapping pair + mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); + } // Skip the duplicate overlapping pairs while (i < mNbPotentialPairs) { @@ -241,34 +264,41 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { } // Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree -void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) { +void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedList& overlappingNodes) { - // If both the nodes are the same, we do not create store the overlapping pair - if (node1ID == node2ID) return; + // For each overlapping node in the linked list + LinkedList::ListElement* elem = overlappingNodes.getListHead(); + while (elem != nullptr) { - // If we need to allocate more memory for the array of potential overlapping pairs - if (mNbPotentialPairs == mNbAllocatedPotentialPairs) { + // If both the nodes are the same, we do not create store the overlapping pair + if (referenceNodeId != elem->data) { - // Allocate more memory for the array of potential pairs - BroadPhasePair* oldPairs = mPotentialPairs; - mNbAllocatedPotentialPairs *= 2; - mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); - assert(mPotentialPairs); - memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); - free(oldPairs); + // 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 + BroadPhasePair* oldPairs = mPotentialPairs; + mNbAllocatedPotentialPairs *= 2; + mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); + assert(mPotentialPairs); + memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); + free(oldPairs); + } + + // Add the new potential pair into the array of potential overlapping pairs + mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(referenceNodeId, elem->data); + mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(referenceNodeId, elem->data); + mNbPotentialPairs++; + } + + elem = elem->next; } - - // 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++; } // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { - - mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); + mOverlappingNodes.insert(nodeId); } // Called for a broad-phase shape that has to be tested for raycast diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 45c8acfb..8d2cda35 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -32,6 +32,7 @@ #include "collision/ProxyShape.h" #include "DynamicAABBTree.h" #include "engine/Profiler.h" +#include "containers/LinkedList.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -66,15 +67,13 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { private: - BroadPhaseAlgorithm& mBroadPhaseAlgorithm; - - int mReferenceNodeId; - public: + LinkedList& mOverlappingNodes; + // Constructor - AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId) - : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) { + AABBOverlapCallback(LinkedList& overlappingNodes) + : mOverlappingNodes(overlappingNodes) { } @@ -197,15 +196,24 @@ class BroadPhaseAlgorithm { /// 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 notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2); + /// Add potential overlapping pairs in the dynamic AABB tree + void addOverlappingNodes(int broadPhaseId1, const LinkedList& overlappingNodes); + + /// Report all the shapes that are overlapping with a given AABB + void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList& overlappingNodes) const; /// Compute all the overlapping pairs of collision shapes - void computeOverlappingPairs(); + void computeOverlappingPairs(Allocator& allocator); + + /// Return the proxy shape corresponding to the broad-phase node id in parameter + ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const; /// Return true if the two broad-phase collision shapes are overlapping bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const; + /// Return the fat AABB of a given broad-phase shape + const AABB& getFatAABB(int broadPhaseId) const; + /// Ray casting method void raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const; @@ -232,6 +240,11 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, return aabb1.testCollision(aabb2); } +// Return the fat AABB of a given broad-phase shape +inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const { + return mDynamicAABBTree.getFatAABB(broadPhaseId); +} + // Ray casting method inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { @@ -243,6 +256,11 @@ inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTes mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); } +// Return the proxy shape corresponding to the broad-phase node id in parameter +inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPhaseId) const { + return static_cast(mDynamicAABBTree.getNodeDataPointer(broadPhaseId)); +} + } #endif diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index 7efd86ac..f829c000 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -26,7 +26,7 @@ // Libraries #include "DynamicAABBTree.h" #include "BroadPhaseAlgorithm.h" -#include "memory/Stack.h" +#include "containers/Stack.h" #include "engine/Profiler.h" using namespace reactphysics3d; diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 6e027721..230ebbb3 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -49,11 +49,6 @@ class CollisionDispatch { /// Destructor virtual ~CollisionDispatch() = default; - /// Initialize the collision dispatch configuration - virtual void init(CollisionDetection* collisionDetection, - PoolAllocator* memoryAllocator) { - - } /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index 1e3c86c6..c0da7611 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -33,94 +33,111 @@ using namespace reactphysics3d; -// Return true and compute a contact info if the two bounding volumes collide -void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) { - - ProxyShape* convexProxyShape; - ProxyShape* concaveProxyShape; - const ConvexShape* convexShape; - const ConcaveShape* concaveShape; - - // Collision shape 1 is convex, collision shape 2 is concave - if (shape1Info.collisionShape->isConvex()) { - convexProxyShape = shape1Info.proxyShape; - convexShape = static_cast(shape1Info.collisionShape); - concaveProxyShape = shape2Info.proxyShape; - concaveShape = static_cast(shape2Info.collisionShape); - } - else { // Collision shape 2 is convex, collision shape 1 is concave - convexProxyShape = shape2Info.proxyShape; - convexShape = static_cast(shape2Info.collisionShape); - concaveProxyShape = shape1Info.proxyShape; - concaveShape = static_cast(shape1Info.collisionShape); - } - - // Set the parameters of the callback object - ConvexVsTriangleCallback convexVsTriangleCallback; - convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); - convexVsTriangleCallback.setConvexShape(convexShape); - convexVsTriangleCallback.setConcaveShape(concaveShape); - convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); - convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); - - // Compute the convex shape AABB in the local-space of the convex shape - AABB aabb; - convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); - - // If smooth mesh collision is enabled for the concave mesh - if (concaveShape->getIsSmoothMeshCollisionEnabled()) { - - std::vector contactPoints; - - SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); - - convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); - - // Call the convex vs triangle callback for each triangle of the concave shape - concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); - - // Run the smooth mesh collision algorithm - processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); - } - else { - - convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); - - // Call the convex vs triangle callback for each triangle of the concave shape - concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); - } -} - -// Test collision between a triangle and the convex mesh shape -void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { +// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) +void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints) { // Create a triangle collision shape decimal margin = mConcaveShape->getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) + TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - // Select the collision algorithm to use between the triangle and the convex shape - NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), - mConvexShape->getType()); - - // If there is no collision algorithm between those two kinds of shapes - if (algo == nullptr) return; - - // Notify the narrow-phase algorithm about the overlapping pair we are going to test - algo->setCurrentOverlappingPair(mOverlappingPair); - - // Create the CollisionShapeInfo objects - CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(), - mOverlappingPair, mConvexProxyShape->getCachedCollisionData()); - CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape, - mConcaveProxyShape->getLocalToWorldTransform(), - mOverlappingPair, mConcaveProxyShape->getCachedCollisionData()); - - // Use the collision algorithm to test collision between the triangle and the other convex shape - algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback); + // Create a narrow phase info for the narrow-phase collision detection + NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; + narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo))) + NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(), + triangleShape, mConvexProxyShape->getLocalToWorldTransform(), + mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(), + mConcaveProxyShape->getCachedCollisionData()); + narrowPhaseInfoList->next = firstNarrowPhaseInfo; } +// Return true and compute a contact info if the two bounding volumes collide +void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + NarrowPhaseCallback* narrowPhaseCallback) { + +// ProxyShape* convexProxyShape; +// ProxyShape* concaveProxyShape; +// const ConvexShape* convexShape; +// const ConcaveShape* concaveShape; + +// // Collision shape 1 is convex, collision shape 2 is concave +// if (shape1Info.collisionShape->isConvex()) { +// convexProxyShape = shape1Info.proxyShape; +// convexShape = static_cast(shape1Info.collisionShape); +// concaveProxyShape = shape2Info.proxyShape; +// concaveShape = static_cast(shape2Info.collisionShape); +// } +// else { // Collision shape 2 is convex, collision shape 1 is concave +// convexProxyShape = shape2Info.proxyShape; +// convexShape = static_cast(shape2Info.collisionShape); +// concaveProxyShape = shape1Info.proxyShape; +// concaveShape = static_cast(shape1Info.collisionShape); +// } + +// // Set the parameters of the callback object +// ConvexVsTriangleCallback convexVsTriangleCallback; +// convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); +// convexVsTriangleCallback.setConvexShape(convexShape); +// convexVsTriangleCallback.setConcaveShape(concaveShape); +// convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); +// convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); + +// // Compute the convex shape AABB in the local-space of the convex shape +// AABB aabb; +// convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); + +// // If smooth mesh collision is enabled for the concave mesh +// if (concaveShape->getIsSmoothMeshCollisionEnabled()) { + +// std::vector contactPoints; + +// SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); + +// convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); + +// // Call the convex vs triangle callback for each triangle of the concave shape +// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); + +// // Run the smooth mesh collision algorithm +// processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); +// } +// else { + +// convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); + +// // Call the convex vs triangle callback for each triangle of the concave shape +// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); +// } +} + +//// Test collision between a triangle and the convex mesh shape +//void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { + +// // Create a triangle collision shape +// decimal margin = mConcaveShape->getTriangleMargin(); +// TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + +// // Select the collision algorithm to use between the triangle and the convex shape +// NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), +// mConvexShape->getType()); + +// // If there is no collision algorithm between those two kinds of shapes +// if (algo == nullptr) return; + +// // Notify the narrow-phase algorithm about the overlapping pair we are going to test +// algo->setCurrentOverlappingPair(mOverlappingPair); + +// // Create the CollisionShapeInfo objects +// CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(), +// mOverlappingPair, mConvexProxyShape->getCachedCollisionData()); +// CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape, +// mConcaveProxyShape->getLocalToWorldTransform(), +// mOverlappingPair, mConcaveProxyShape->getCachedCollisionData()); + +// // Use the collision algorithm to test collision between the triangle and the other convex shape +// algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback); +//} + // Process the concave triangle mesh collision using the smooth mesh collision algorithm described // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision // issue with some internal edges. @@ -246,36 +263,37 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi return false; } -// Called by a narrow-phase collision algorithm when a new contact has been found -void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { - Vector3 triangleVertices[3]; - bool isFirstShapeTriangle; - // If the collision shape 1 is the triangle - if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) { - assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE); +//// Called by a narrow-phase collision algorithm when a new contact has been found +//void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, +// const ContactPointInfo& contactInfo) { +// Vector3 triangleVertices[3]; +// bool isFirstShapeTriangle; - const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape1); - triangleVertices[0] = triangleShape->getVertex(0); - triangleVertices[1] = triangleShape->getVertex(1); - triangleVertices[2] = triangleShape->getVertex(2); +// // If the collision shape 1 is the triangle +// if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) { +// assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE); - isFirstShapeTriangle = true; - } - else { // If the collision shape 2 is the triangle - assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE); +// const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape1); +// triangleVertices[0] = triangleShape->getVertex(0); +// triangleVertices[1] = triangleShape->getVertex(1); +// triangleVertices[2] = triangleShape->getVertex(2); - const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape2); - triangleVertices[0] = triangleShape->getVertex(0); - triangleVertices[1] = triangleShape->getVertex(1); - triangleVertices[2] = triangleShape->getVertex(2); +// isFirstShapeTriangle = true; +// } +// else { // If the collision shape 2 is the triangle +// assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE); - isFirstShapeTriangle = false; - } - SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); +// const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape2); +// triangleVertices[0] = triangleShape->getVertex(0); +// triangleVertices[1] = triangleShape->getVertex(1); +// triangleVertices[2] = triangleShape->getVertex(2); - // Add the narrow-phase contact into the list of contact to process for - // smooth mesh collision - mContactPoints.push_back(smoothContactInfo); -} +// isFirstShapeTriangle = false; +// } +// SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); + +// // Add the narrow-phase contact into the list of contact to process for +// // smooth mesh collision +// mContactPoints.push_back(smoothContactInfo); +//} diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h index bcd4ef8f..531615da 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h @@ -30,6 +30,7 @@ #include "NarrowPhaseAlgorithm.h" #include "collision/shapes/ConvexShape.h" #include "collision/shapes/ConcaveShape.h" +#include "memory/SingleFrameAllocator.h" #include /// Namespace ReactPhysics3D @@ -37,73 +38,43 @@ namespace reactphysics3d { // Class ConvexVsTriangleCallback /** - * This class is used to encapsulate a callback method for - * collision detection between the triangle of a concave mesh shape - * and a convex shape. + * This class is used to report a collision between the triangle + * of a concave mesh shape and a convex shape during the + * middle-phase algorithm */ -class ConvexVsTriangleCallback : public TriangleCallback { +class MiddlePhaseTriangleCallback : public TriangleCallback { protected: - /// Pointer to the collision detection object - CollisionDetection* mCollisionDetection; - - /// Narrow-phase collision callback - NarrowPhaseCallback* mNarrowPhaseCallback; - - /// Convex collision shape to test collision with - const ConvexShape* mConvexShape; - - /// Concave collision shape - const ConcaveShape* mConcaveShape; - - /// Proxy shape of the convex collision shape - ProxyShape* mConvexProxyShape; - - /// Proxy shape of the concave collision shape - ProxyShape* mConcaveProxyShape; - /// Broadphase overlapping pair OverlappingPair* mOverlappingPair; - /// Used to sort ContactPointInfos according to their penetration depth - static bool contactsDepthCompare(const ContactPointInfo& contact1, - const ContactPointInfo& contact2); + /// Pointer to the concave proxy shape + ProxyShape* mConcaveProxyShape; + + /// Pointer to the convex proxy shape + ProxyShape* mConvexProxyShape; + + /// Pointer to the concave collision shape + const ConcaveShape* mConcaveShape; + + /// Reference to the single-frame memory allocator + Allocator& mAllocator; public: - /// Destructor - virtual ~ConvexVsTriangleCallback() override = default; + /// Pointer to the first element of the linked-list of narrow-phase info + NarrowPhaseInfo* narrowPhaseInfoList; - /// Set the collision detection pointer - void setCollisionDetection(CollisionDetection* collisionDetection) { - mCollisionDetection = collisionDetection; - } + /// Constructor + MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, + ProxyShape* concaveProxyShape, + ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, + Allocator& allocator) + :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), + mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), + mAllocator(allocator), narrowPhaseInfoList(nullptr) { - /// Set the narrow-phase collision callback - void setNarrowPhaseCallback(NarrowPhaseCallback* callback) { - mNarrowPhaseCallback = callback; - } - - /// Set the convex collision shape to test collision with - void setConvexShape(const ConvexShape* convexShape) { - mConvexShape = convexShape; - } - - /// Set the concave collision shape - void setConcaveShape(const ConcaveShape* concaveShape) { - mConcaveShape = concaveShape; - } - - /// Set the broadphase overlapping pair - void setOverlappingPair(OverlappingPair* overlappingPair) { - mOverlappingPair = overlappingPair; - } - - /// Set the proxy shapes of the two collision shapes - void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) { - mConvexProxyShape = convexProxyShape; - mConcaveProxyShape = concaveProxyShape; } /// Test collision between a triangle and the convex mesh shape @@ -148,13 +119,14 @@ struct ContactsDepthCompare { // return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; //} +// TODO : Delete this // Class SmoothCollisionNarrowPhaseCallback /** * This class is used as a narrow-phase callback to get narrow-phase contacts * of the concave triangle mesh to temporary store them in order to be used in * the smooth mesh collision algorithm if this one is enabled. */ -class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback { +class SmoothCollisionNarrowPhaseCallback { private: @@ -169,13 +141,9 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback { } - - /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) override; - }; +// TODO : Delete this // Class ConcaveVsConvexAlgorithm /** * This class is used to compute the narrow-phase collision detection @@ -183,7 +151,7 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback { * to use the GJK collision detection algorithm to compute the collision between * the convex shape and each of the triangles in the concave shape. */ -class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { +class ConcaveVsConvexAlgorithm { protected : @@ -212,7 +180,7 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { ConcaveVsConvexAlgorithm() = default; /// Destructor - virtual ~ConcaveVsConvexAlgorithm() override = default; + ~ConcaveVsConvexAlgorithm() = default; /// Private copy-constructor ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm) = delete; @@ -221,9 +189,8 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) override; + void testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + NarrowPhaseCallback* narrowPhaseCallback); }; // Add a triangle vertex into the set of processed triangles diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index f6812d74..35b1e50b 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -29,16 +29,6 @@ using namespace reactphysics3d; -/// Initialize the collision dispatch configuration -void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection, - PoolAllocator* memoryAllocator) { - - // Initialize the collision algorithms - mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator); - mGJKAlgorithm.init(collisionDetection, memoryAllocator); - mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator); -} - // Select and return the narrow-phase collision detection algorithm to // use between two types of collision shapes. NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) { @@ -46,19 +36,14 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t CollisionShapeType shape1Type = static_cast(type1); CollisionShapeType shape2Type = static_cast(type2); - // Sphere vs Sphere algorithm - if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { - return &mSphereVsSphereAlgorithm; - } - // Concave vs Convex algorithm - else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { - return &mConcaveVsConvexAlgorithm; - } // Convex vs Convex algorithm (GJK algorithm) - else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { + if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { return &mGJKAlgorithm; } + // Sphere vs Sphere algorithm + else if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { + return &mSphereVsSphereAlgorithm; + } else { return nullptr; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 5dd07bf2..f62a6564 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -47,9 +47,6 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Sphere vs Sphere collision algorithm SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; - /// Concave vs Convex collision algorithm - ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm; - /// GJK Algorithm GJKAlgorithm mGJKAlgorithm; @@ -61,10 +58,6 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Destructor virtual ~DefaultCollisionDispatch() override = default; - /// Initialize the collision dispatch configuration - virtual void init(CollisionDetection* collisionDetection, - PoolAllocator* memoryAllocator) override; - /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override; diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 6fc19680..8779c33e 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -74,25 +74,22 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, /// the correct penetration depth. This method returns true if the EPA penetration /// depth computation has succeeded and false it has failed. bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, - CollisionShapeInfo shape1Info, - const Transform& transform1, - CollisionShapeInfo shape2Info, - const Transform& transform2, + const NarrowPhaseInfo* narrowPhaseInfo, Vector3& v, - NarrowPhaseCallback* narrowPhaseCallback) { + ContactPointInfo& contactPointInfo) { PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); decimal gjkPenDepthSquare = v.lengthSquare(); - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(narrowPhaseInfo->collisionShape1->isConvex()); + assert(narrowPhaseInfo->collisionShape2->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); - const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); + const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); + const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; - void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; + void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates @@ -103,12 +100,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& // 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; + Transform body2Tobody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform; // Matrix that transform a direction from local // space of body 1 into local space of body 2 - Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * - transform1.getOrientation(); + Quaternion rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getInverse() * + narrowPhaseInfo->shape1ToWorldTransform.getOrientation(); // Get the simplex computed previously by the GJK algorithm int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); @@ -416,7 +413,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); // Compute the contact info - v = transform1.getOrientation() * triangle->getClosestPoint(); + v = narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * triangle->getClosestPoint(); Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3 normal = v.getUnit(); @@ -430,10 +427,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) { // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); - - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + contactPointInfo.init(normal, penetrationDepth, pALocal, pBLocal); return true; } diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index 05a78eea..1d241540 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -29,7 +29,7 @@ // Libraries #include "collision/narrowphase/GJK/VoronoiSimplex.h" #include "collision/shapes/CollisionShape.h" -#include "collision/CollisionShapeInfo.h" +#include "collision/NarrowPhaseInfo.h" #include "constraint/ContactPoint.h" #include "collision/narrowphase/NarrowPhaseAlgorithm.h" #include "mathematics/mathematics.h" @@ -87,9 +87,6 @@ class EPAAlgorithm { // -------------------- Attributes -------------------- // - /// Reference to the memory allocator - PoolAllocator* mMemoryAllocator; - /// Triangle comparison operator TriangleComparison mTriangleComparison; @@ -119,17 +116,11 @@ class EPAAlgorithm { /// Deleted assignment operator EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete; - /// Initalize the algorithm - void init(PoolAllocator* memoryAllocator); - /// Compute the penetration depth with EPA algorithm. bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, - CollisionShapeInfo shape1Info, - const Transform& transform1, - CollisionShapeInfo shape2Info, - const Transform& transform2, + const NarrowPhaseInfo* narrowPhaseInfo, Vector3& v, - NarrowPhaseCallback* narrowPhaseCallback); + ContactPointInfo &contactPointInfo); }; // Add a triangle face in the candidate triangle heap in the EPA algorithm @@ -150,11 +141,6 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** } } -// Initalize the algorithm -inline void EPAAlgorithm::init(PoolAllocator* memoryAllocator) { - mMemoryAllocator = memoryAllocator; -} - } #endif diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index b7916a53..dec43865 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -36,11 +36,6 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -// Constructor -GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() { - -} - // Compute a contact info if the two collision shapes collide. /// This method implements the Hybrid Technique for computing the penetration depth by /// running the GJK algorithm on original objects (without margin). If the shapes intersect @@ -50,9 +45,8 @@ GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() { /// 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. -void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) { +bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) { PROFILE("GJKAlgorithm::testCollision()"); @@ -65,20 +59,20 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, decimal prevDistSquare; bool contactFound = false; - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(narrowPhaseInfo->collisionShape1->isConvex()); + assert(narrowPhaseInfo->collisionShape2->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); - const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); + const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); + const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; - void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; + void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); // Get the local-space to world-space transforms - const Transform transform1 = shape1Info.shapeToWorldTransform; - const Transform transform2 = shape2Info.shapeToWorldTransform; + const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; // 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) @@ -92,13 +86,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // Initialize the margin (sum of margins of both objects) decimal margin = shape1->getMargin() + shape2->getMargin(); decimal marginSquare = margin * margin; - assert(margin > 0.0); + assert(margin > decimal(0.0)); // Create a simplex set VoronoiSimplex simplex; // Get the previous point V (last cached separating axis) - Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); + Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis(); // Initialize the upper bound for the square distance decimal distSquare = DECIMAL_LARGEST; @@ -116,13 +110,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, vDotw = v.dot(w); // If the enlarge objects (with margins) do not intersect - if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { + if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - mCurrentOverlappingPair->setCachedSeparatingAxis(v); + narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v); // No intersection, we return - return; + return false; } // If the objects intersect only in the margins @@ -188,8 +182,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // again but on the enlarged objects to compute a simplex polytope that contains // the origin. Then, we give that simplex polytope to the EPA algorithm to compute // the correct penetration depth and contact points between the enlarged objects. - isEPAResultValid = computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, - transform2, narrowPhaseCallback, v); + isEPAResultValid = computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v); } if ((contactFound || !isEPAResultValid) && distSquare > MACHINE_EPSILON) { @@ -200,7 +193,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // Project those two points on the margins to have the closest points of both // object with the margins decimal dist = std::sqrt(distSquare); - assert(dist > 0.0); + assert(dist > decimal(0.0)); pA = (pA - (shape1->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); @@ -209,21 +202,22 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, decimal penetrationDepth = margin - dist; // If the penetration depth is negative (due too numerical errors), there is no contact - if (penetrationDepth <= 0.0) { - return; + if (penetrationDepth <= decimal(0.0)) { + return false; } // Do not generate a contact point with zero normal length if (normal.lengthSquare() < MACHINE_EPSILON) { - return; + return false; } // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); + contactPointInfo.init(normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + return true; } + + return false; } /// This method runs the GJK algorithm on the two enlarged objects (with margin) @@ -231,11 +225,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, /// 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(const CollisionShapeInfo& shape1Info, - const Transform& transform1, - const CollisionShapeInfo& shape2Info, - const Transform& transform2, - NarrowPhaseCallback* narrowPhaseCallback, +bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo, Vector3& v) { PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); @@ -247,24 +238,24 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap decimal distSquare = DECIMAL_LARGEST; decimal prevDistSquare; - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(narrowPhaseInfo->collisionShape1->isConvex()); + assert(narrowPhaseInfo->collisionShape2->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); - const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); + const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); + const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; - void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; + void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; // 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; + Transform body2ToBody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform; // Matrix that transform a direction from local space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * - transform1.getOrientation().getMatrix(); + Matrix3x3 rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getMatrix().getTranspose() * + narrowPhaseInfo->shape1ToWorldTransform.getOrientation().getMatrix(); do { // Compute the support points for the enlarged object A and B @@ -277,7 +268,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap vDotw = v.dot(w); // If the enlarge objects do not intersect - if (vDotw > 0.0) { + if (vDotw > decimal(0.0)) { // No intersection, we return return false; @@ -308,9 +299,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap // Give the simplex computed with GJK algorithm to the EPA algorithm // which will compute the correct penetration depth and contact points // between the two enlarged objects - return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, - transform1, shape2Info, transform2, - v, narrowPhaseCallback); + return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, narrowPhaseInfo, v, contactPointInfo); } // Use the GJK Algorithm to find if a point is inside a convex collision shape @@ -378,7 +367,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS return true; } - // Ray casting algorithm agains a convex collision shape using the GJK Algorithm /// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in /// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection". diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index b6cce5c2..e0dad891 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -69,11 +69,8 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { // -------------------- Methods -------------------- // /// Compute the penetration depth for enlarged objects. - bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, - const Transform& transform1, - const CollisionShapeInfo& shape2Info, - const Transform& transform2, - NarrowPhaseCallback* narrowPhaseCallback, + bool computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo, Vector3& v); public : @@ -81,7 +78,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { // -------------------- Methods -------------------- // /// Constructor - GJKAlgorithm(); + GJKAlgorithm() = default; /// Destructor ~GJKAlgorithm() = default; @@ -92,14 +89,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete; - /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, - PoolAllocator* memoryAllocator) override; - /// Compute a contact info if the two bounding volumes collide. - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) override; + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; /// Use the GJK Algorithm to find if a point is inside a convex collision shape bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); @@ -108,13 +100,6 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); }; -// Initalize the algorithm -inline void GJKAlgorithm::init(CollisionDetection* collisionDetection, - PoolAllocator* memoryAllocator) { - NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator); - mAlgoEPA.init(memoryAllocator); -} - } #endif diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 0759d138..f7f68907 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -31,7 +31,7 @@ #include "constraint/ContactPoint.h" #include "memory/PoolAllocator.h" #include "engine/OverlappingPair.h" -#include "collision/CollisionShapeInfo.h" +#include "collision/NarrowPhaseInfo.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -67,21 +67,12 @@ class NarrowPhaseAlgorithm { // -------------------- Attributes -------------------- // - /// Pointer to the collision detection object - CollisionDetection* mCollisionDetection; - - /// Pointer to the memory allocator - PoolAllocator* mMemoryAllocator; - - /// Overlapping pair of the bodies currently tested for collision - OverlappingPair* mCurrentOverlappingPair; - public : // -------------------- Methods -------------------- // /// Constructor - NarrowPhaseAlgorithm(); + NarrowPhaseAlgorithm() = default; /// Destructor virtual ~NarrowPhaseAlgorithm() = default; @@ -92,23 +83,10 @@ class NarrowPhaseAlgorithm { /// Deleted assignment operator NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; - /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator); - - /// Set the current overlapping pair of bodies - void setCurrentOverlappingPair(OverlappingPair* overlappingPair); - /// Compute a contact info if the two bounding volume collide - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback)=0; + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo)=0; }; -// Set the current overlapping pair of bodies -inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) { - mCurrentOverlappingPair = overlappingPair; -} - } #endif diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 9d2c4baa..6b28372d 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -30,17 +30,16 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) { +bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) { // Get the sphere collision shapes - const SphereShape* sphereShape1 = static_cast(shape1Info.collisionShape); - const SphereShape* sphereShape2 = static_cast(shape2Info.collisionShape); + const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); + const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); // Get the local-space to world-space transforms - const Transform& transform1 = shape1Info.shapeToWorldTransform; - const Transform& transform2 = shape2Info.shapeToWorldTransform; + const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; // Compute the distance between the centers Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); @@ -60,11 +59,11 @@ void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); + contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth, + intersectionOnBody1, intersectionOnBody2); - // Notify about the new contact - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + return true; } + + return false; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index 525c55d5..24886f7f 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -61,9 +61,8 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) override; + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; }; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 994f5426..4028ed8f 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -28,7 +28,7 @@ // Libraries #include "body/CollisionBody.h" -#include "collision/CollisionShapeInfo.h" +#include "collision/NarrowPhaseInfo.h" #include "configuration.h" #include "mathematics/mathematics.h" #include "configuration.h" @@ -53,20 +53,6 @@ struct ContactPointInfo { // -------------------- Attributes -------------------- // - // TODO : Check if we really need the shape1, shape2, collisionShape1 and collisionShape2 fields - - /// First proxy shape of the contact - ProxyShape* shape1; - - /// Second proxy shape of the contact - ProxyShape* shape2; - - /// First collision shape - const CollisionShape* collisionShape1; - - /// Second collision shape - const CollisionShape* collisionShape2; - /// Normalized normal vector of the collision contact in world space Vector3 normal; @@ -82,13 +68,18 @@ struct ContactPointInfo { // -------------------- Methods -------------------- // /// Constructor - ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1, - const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth, - const Vector3& localPoint1, const Vector3& localPoint2) - : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2), - normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), - localPoint2(localPoint2) { + ContactPointInfo() = default; + /// Destructor + ~ContactPointInfo() = default; + + /// Initialize the contact point info + void init(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) { + normal = contactNormal; + penetrationDepth = penDepth; + localPoint1 = localPt1; + localPoint2 = localPt2; } }; diff --git a/src/containers/LinkedList.h b/src/containers/LinkedList.h new file mode 100644 index 00000000..281c134c --- /dev/null +++ b/src/containers/LinkedList.h @@ -0,0 +1,124 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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_LINKED_LIST_H +#define REACTPHYSICS3D_LINKED_LIST_H + +// Libraries +#include "memory/Allocator.h" + +namespace reactphysics3d { + +// Class LinkedList +/** + * This class represents a simple generic linked list. + */ +template +class LinkedList { + + public: + + /// Element of the linked list + struct ListElement { + + /// Data of the list element + T data; + + /// Pointer to the next element of the list + ListElement* next; + + /// Constructor + ListElement(T elementData, ListElement* nextElement) + : data(elementData), next(nextElement) { + + } + }; + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the first element of the list + ListElement* mListHead; + + /// Memory allocator used to allocate the list elements + Allocator& mAllocator; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + LinkedList(Allocator& allocator) : mListHead(nullptr), mAllocator(allocator) { + + } + + /// Destructor + ~LinkedList() { + reset(); + } + + /// Return the first element of the list + ListElement* getListHead() const; + + /// Insert an element at the beginning of the linked list + void insert(const T& data); + + /// Remove all the elements of the list + void reset(); + +}; + +// Return the first element of the list +template +inline typename LinkedList::ListElement* LinkedList::getListHead() const { + return mListHead; +} + +// Insert an element at the beginning of the linked list +template +inline void LinkedList::insert(const T& data) { + ListElement* element = new (mAllocator.allocate(sizeof(ListElement))) ListElement(data, mListHead); + mListHead = element; +} + +// Remove all the elements of the list +template +inline void LinkedList::reset() { + + // Release all the list elements + ListElement* element = mListHead; + while (element != nullptr) { + ListElement* nextElement = element->next; + mAllocator.release(element, sizeof(ListElement)); + element = nextElement; + } + + mListHead = nullptr; +} + +} + +#endif diff --git a/src/memory/Stack.h b/src/containers/Stack.h similarity index 100% rename from src/memory/Stack.h rename to src/containers/Stack.h diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index b1b004ec..d160021c 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -33,7 +33,8 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mCollisionDetection(this, mPoolAllocator), mCurrentBodyID(0), + : mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES), + mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0), mEventListener(nullptr) { } @@ -148,119 +149,18 @@ bool CollisionWorld::testAABBOverlap(const CollisionBody* body1, return body1AABB.testCollision(body2AABB); } -// Test and report collisions between a given shape and all the others -// shapes of the world. +// Report all the bodies that overlap with the aabb in parameter /** - * @param shape Pointer to the proxy shape to test - * @param callback Pointer to the object with the callback method + * @param aabb AABB used to test for overlap + * @param overlapCallback Pointer to the callback class to report overlap + * @param categoryMaskBits bits mask used to filter the bodies to test overlap with */ -void CollisionWorld::testCollision(const ProxyShape* shape, - CollisionCallback* callback) { - - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); - - // Create the sets of shapes - std::set shapes; - shapes.insert(shape->mBroadPhaseID); - std::set emptySet; - - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); +inline void CollisionWorld::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) { + mCollisionDetection.testAABBOverlap(aabb, overlapCallback, categoryMaskBits); } -// Test and report collisions between two given shapes -/** - * @param shape1 Pointer to the first proxy shape to test - * @param shape2 Pointer to the second proxy shape to test - * @param callback Pointer to the object with the callback method - */ -void CollisionWorld::testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback) { - - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); - - // Create the sets of shapes - std::set shapes1; - shapes1.insert(shape1->mBroadPhaseID); - std::set shapes2; - shapes2.insert(shape2->mBroadPhaseID); - - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); -} - -// Test and report collisions between a body and all the others bodies of the -// world -/** - * @param body Pointer to the first body to test - * @param callback Pointer to the object with the callback method - */ -void CollisionWorld::testCollision(const CollisionBody* body, - CollisionCallback* callback) { - - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); - - // Create the sets of shapes - std::set shapes1; - - // For each shape of the body - for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } - - std::set emptySet; - - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); -} - -// Test and report collisions between two bodies -/** - * @param body1 Pointer to the first body to test - * @param body2 Pointer to the second body to test - * @param callback Pointer to the object with the callback method - */ -void CollisionWorld::testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback) { - - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); - - // Create the sets of shapes - std::set shapes1; - for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } - - std::set shapes2; - for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr; - shape = shape->getNext()) { - shapes2.insert(shape->mBroadPhaseID); - } - - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); -} - -// Test and report collisions between all shapes of the world -/** - * @param callback Pointer to the object with the callback method - */ -void CollisionWorld::testCollision(CollisionCallback* callback) { - - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); - - std::set emptySet; - - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); +// Return true if two bodies overlap +bool CollisionWorld::testOverlap(CollisionBody* body1, CollisionBody* body2) { + return mCollisionDetection.testOverlap(body1, body2); } diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index d04acb4e..dc162716 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -47,6 +47,7 @@ namespace reactphysics3d { // Declarations class CollisionCallback; +class OverlapCallback; // Class CollisionWorld /** @@ -60,6 +61,12 @@ class CollisionWorld { // -------------------- Attributes -------------------- // + /// Pool Memory allocator + PoolAllocator mPoolAllocator; + + /// Single frame Memory allocator + SingleFrameAllocator mSingleFrameAllocator; + /// Reference to the collision detection CollisionDetection mCollisionDetection; @@ -72,9 +79,6 @@ class CollisionWorld { /// List of free ID for rigid bodies std::vector mFreeBodiesIDs; - /// Pool Memory allocator - PoolAllocator mPoolAllocator; - /// Pointer to an event listener object EventListener* mEventListener; @@ -125,32 +129,23 @@ class CollisionWorld { bool testAABBOverlap(const CollisionBody* body1, const CollisionBody* body2) const; - /// Test if the AABBs of two proxy shapes overlap - bool testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const; + /// Report all the bodies that overlap with the AABB in parameter + void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF); - /// Test and report collisions between a given shape and all the others - /// shapes of the world - virtual void testCollision(const ProxyShape* shape, - CollisionCallback* callback); + /// Return true if two bodies overlap + bool testOverlap(CollisionBody* body1, CollisionBody* body2); - /// Test and report collisions between two given shapes - virtual void testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback); - - /// Test and report collisions between a body and all the others bodies of the - /// world - virtual void testCollision(const CollisionBody* body, - CollisionCallback* callback); + /// Report all the bodies that overlap with the body in parameter + void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF); /// Test and report collisions between two bodies - virtual void testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback); + void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback); + + /// Test and report collisions between a body and all the others bodies of the world + void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF); /// Test and report collisions between all shapes of the world - virtual void testCollision(CollisionCallback* callback); + void testCollision(CollisionCallback* callback); // -------------------- Friendship -------------------- // @@ -200,36 +195,100 @@ inline void CollisionWorld::raycast(const Ray& ray, mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); } -// Test if the AABBs of two proxy shapes overlap +// Test and report collisions between two bodies /** - * @param shape1 Pointer to the first proxy shape to test - * @param shape2 Pointer to the second proxy shape to test - * @return + * @param body1 Pointer to the first body to test + * @param body2 Pointer to the second body to test + * @param callback Pointer to the object with the callback method */ -inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const { +inline void CollisionWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback) { + mCollisionDetection.testCollision(body1, body2, callback); +} - return mCollisionDetection.testAABBOverlap(shape1, shape2); +// Test and report collisions between a body and all the others bodies of the world +/** + * @param body Pointer to the body against which we need to test collision + * @param callback Pointer to the object with the callback method to report contacts + * @param categoryMaskBits Bits mask corresponding to the category of bodies we need to test collision with + */ +inline void CollisionWorld::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) { + mCollisionDetection.testCollision(body, callback, categoryMaskBits); +} + +// Test and report collisions between all bodies of the world +/** + * @param callback Pointer to the object with the callback method to report contacts + */ +inline void CollisionWorld::testCollision(CollisionCallback* callback) { + mCollisionDetection.testCollision(callback); +} + +// Report all the bodies that overlap with the body in parameter +/** + * @param body Pointer to the collision body to test overlap with + * @param overlapCallback Pointer to the callback class to report overlap + * @param categoryMaskBits bits mask used to filter the bodies to test overlap with + */ +inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) { + mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits); } // Class CollisionCallback /** * This class can be used to register a callback for collision test queries. * You should implement your own class inherited from this one and implement - * the notifyRaycastHit() method. This method will be called for each ProxyShape - * that is hit by the ray. + * the notifyContact() method. This method will called each time a contact + * point is reported. */ class CollisionCallback { public: + struct CollisionCallbackInfo { + + public: + const ContactPointInfo& contactPoint; + CollisionBody* body1; + CollisionBody* body2; + const ProxyShape* proxyShape1; + const ProxyShape* proxyShape2; + + // Constructor + CollisionCallbackInfo(const ContactPointInfo& point, CollisionBody* b1, CollisionBody* b2, + const ProxyShape* proxShape1, const ProxyShape* proxShape2) : + contactPoint(point), body1(b1), body2(b2), + proxyShape1(proxShape1), proxyShape2(proxShape2) { + + } + }; + /// Destructor virtual ~CollisionCallback() { } - /// This method will be called for contact - virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0; + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0; +}; + +// Class OverlapCallback +/** + * This class can be used to register a callback for collision overlap queries. + * You should implement your own class inherited from this one and implement + * the notifyOverlap() method. This method will called each time a contact + * point is reported. + */ +class OverlapCallback { + + public: + + /// Destructor + virtual ~OverlapCallback() { + + } + + /// This method will be called for each reported overlapping bodies + virtual void notifyOverlap(CollisionBody* collisionBody)=0; }; } diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index d52348f8..4f7bd15a 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -40,7 +40,6 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES), mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator), mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), @@ -829,117 +828,6 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { } } -// Test and report collisions between a given shape and all the others -// shapes of the world. -/// This method should be called after calling the -/// DynamicsWorld::update() method that will compute the collisions. -/** - * @param shape Pointer to the proxy shape to test - * @param callback Pointer to the object with the callback method - */ -void DynamicsWorld::testCollision(const ProxyShape* shape, - CollisionCallback* callback) { - - // Create the sets of shapes - std::set shapes; - shapes.insert(shape->mBroadPhaseID); - std::set emptySet; - - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); -} - -// Test and report collisions between two given shapes. -/// This method should be called after calling the -/// DynamicsWorld::update() method that will compute the collisions. -/** - * @param shape1 Pointer to the first proxy shape to test - * @param shape2 Pointer to the second proxy shape to test - * @param callback Pointer to the object with the callback method - */ -void DynamicsWorld::testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback) { - - // Create the sets of shapes - std::set shapes1; - shapes1.insert(shape1->mBroadPhaseID); - std::set shapes2; - shapes2.insert(shape2->mBroadPhaseID); - - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); -} - -// Test and report collisions between a body and all the others bodies of the -// world. -/// This method should be called after calling the -/// DynamicsWorld::update() method that will compute the collisions. -/** - * @param body Pointer to the first body to test - * @param callback Pointer to the object with the callback method - */ -void DynamicsWorld::testCollision(const CollisionBody* body, - CollisionCallback* callback) { - - // Create the sets of shapes - std::set shapes1; - - // For each shape of the body - for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } - - std::set emptySet; - - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); -} - -// Test and report collisions between two bodies. -/// This method should be called after calling the -/// DynamicsWorld::update() method that will compute the collisions. -/** - * @param body1 Pointer to the first body to test - * @param body2 Pointer to the second body to test - * @param callback Pointer to the object with the callback method - */ -void DynamicsWorld::testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback) { - - // Create the sets of shapes - std::set shapes1; - for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } - - std::set shapes2; - for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr; - shape = shape->getNext()) { - shapes2.insert(shape->mBroadPhaseID); - } - - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); -} - -// Test and report collisions between all shapes of the world. -/// This method should be called after calling the -/// DynamicsWorld::update() method that will compute the collisions. -/** - * @param callback Pointer to the object with the callback method - */ -void DynamicsWorld::testCollision(CollisionCallback* callback) { - - std::set emptySet; - - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); -} - /// Return the list of all contacts of the world std::vector DynamicsWorld::getContactsList() const { diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 746e8255..b632dcf2 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -50,9 +50,6 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Attributes -------------------- // - /// Single frame Memory allocator - SingleFrameAllocator mSingleFrameAllocator; - /// Contact solver ContactSolver mContactSolver; @@ -267,29 +264,6 @@ class DynamicsWorld : public CollisionWorld { /// Set an event listener object to receive events callbacks. void setEventListener(EventListener* eventListener); - /// Test and report collisions between a given shape and all the others - /// shapes of the world - virtual void testCollision(const ProxyShape* shape, - CollisionCallback* callback) override; - - /// Test and report collisions between two given shapes - virtual void testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback) override; - - /// Test and report collisions between a body and all - /// the others bodies of the world - virtual void testCollision(const CollisionBody* body, - CollisionCallback* callback) override; - - /// Test and report collisions between two bodies - virtual void testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback) override; - - /// Test and report collisions between all shapes of the world - virtual void testCollision(CollisionCallback* callback) override; - /// Return the list of all contacts of the world std::vector getContactsList() const; diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/src/memory/Allocator.h similarity index 75% rename from src/collision/narrowphase/NarrowPhaseAlgorithm.cpp rename to src/memory/Allocator.h index 782f8d6f..f7b9ac73 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp +++ b/src/memory/Allocator.h @@ -23,20 +23,34 @@ * * ********************************************************************************/ +#ifndef REACTPHYSICS3D_ALLOCATOR_H +#define REACTPHYSICS3D_ALLOCATOR_H + // Libraries -#include "NarrowPhaseAlgorithm.h" +#include -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; +/// ReactPhysics3D namespace +namespace reactphysics3d { -// Constructor -NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() - : mMemoryAllocator(nullptr), mCurrentOverlappingPair(nullptr) { +// Class Allocator +/** + * Abstract class with the basic interface of all the derived memory allocators + */ +class Allocator { + + public: + + /// Destructor + virtual ~Allocator() = default; + + /// Allocate memory of a given size (in bytes) and return a pointer to the + /// allocated memory. + virtual void* allocate(size_t size)=0; + + /// Release previously allocated memory. + virtual void release(void* pointer, size_t size)=0; +}; } -// Initalize the algorithm -void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator) { - mCollisionDetection = collisionDetection; - mMemoryAllocator = memoryAllocator; -} +#endif diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h index 2af762ca..71e11714 100644 --- a/src/memory/PoolAllocator.h +++ b/src/memory/PoolAllocator.h @@ -27,8 +27,8 @@ #define REACTPHYSICS3D_POOL_ALLOCATOR_H // Libraries -#include #include "configuration.h" +#include "Allocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -40,7 +40,7 @@ namespace reactphysics3d { * efficiently. This implementation is inspired by the small block allocator * described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp */ -class PoolAllocator { +class PoolAllocator : public Allocator { private : @@ -129,14 +129,14 @@ class PoolAllocator { PoolAllocator(); /// Destructor - ~PoolAllocator(); + virtual ~PoolAllocator() override; /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. - void* allocate(size_t size); + virtual void* allocate(size_t size) override; /// Release previously allocated memory. - void release(void* pointer, size_t size); + virtual void release(void* pointer, size_t size) override; }; diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h index e3151f0a..cc030daa 100644 --- a/src/memory/SingleFrameAllocator.h +++ b/src/memory/SingleFrameAllocator.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H // Libraries -#include +#include "Allocator.h" #include "configuration.h" /// ReactPhysics3D namespace @@ -38,7 +38,7 @@ namespace reactphysics3d { * This class represent a memory allocator used to efficiently allocate * memory on the heap that is used during a single frame. */ -class SingleFrameAllocator { +class SingleFrameAllocator : public Allocator { private : @@ -61,13 +61,18 @@ class SingleFrameAllocator { SingleFrameAllocator(size_t totalSizeBytes); /// Destructor - ~SingleFrameAllocator(); + virtual ~SingleFrameAllocator() override; /// Allocate memory of a given size (in bytes) - void* allocate(size_t size); + virtual void* allocate(size_t size) override; + + /// Release previously allocated memory. + virtual void release(void* pointer, size_t size) override { } /// Reset the marker of the current allocated memory - void reset(); + virtual void reset(); + + };