/******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * * Copyright (c) 2010-2022 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * * In no event will the authors be held liable for any damages arising from the * * use of this software. * * * * Permission is granted to anyone to use this software for any purpose, * * including commercial applications, and to alter it and redistribute it * * freely, subject to the following restrictions: * * * * 1. The origin of this software must not be misrepresented; you must not claim * * that you wrote the original software. If you use this software in a * * product, an acknowledgment in the product documentation would be * * appreciated but is not required. * * * * 2. Altered source versions must be plainly marked as such, and must not be * * misrepresented as being the original software. * * * * 3. This notice may not be removed or altered from any source distribution. * * * ********************************************************************************/ // Libraries #include #include #include #include #include #include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor BroadPhaseSystem::BroadPhaseSystem(CollisionDetectionSystem& collisionDetection, ColliderComponents& collidersComponents, TransformComponents& transformComponents, RigidBodyComponents& rigidBodyComponents) :mDynamicAABBTree(collisionDetection.getMemoryManager().getHeapAllocator(), DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE), mCollidersComponents(collidersComponents), mTransformsComponents(transformComponents), mRigidBodyComponents(rigidBodyComponents), mMovedShapes(collisionDetection.getMemoryManager().getHeapAllocator()), mCollisionDetection(collisionDetection) { #ifdef IS_RP3D_PROFILING_ENABLED mProfiler = nullptr; #endif } // Return true if the two broad-phase collision shapes are overlapping bool BroadPhaseSystem::testOverlappingShapes(int32 shape1BroadPhaseId, int32 shape2BroadPhaseId) const { RP3D_PROFILE("BroadPhaseSystem::testOverlappingShapes()", mProfiler); assert(shape1BroadPhaseId != -1 && shape2BroadPhaseId != -1); // Get the two AABBs of the collision shapes const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1BroadPhaseId); const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2BroadPhaseId); // Check if the two AABBs are overlapping return aabb1.testCollision(aabb2); } // Ray casting method void BroadPhaseSystem::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { RP3D_PROFILE("BroadPhaseSystem::raycast()", mProfiler); BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); // Compute the inverse ray direction const Vector3 rayDirection = ray.point2 - ray.point1; const Vector3 rayDirectionInverse(decimal(1.0) / rayDirection.x, decimal(1.0) / rayDirection.y, decimal(1.0) / rayDirection.z); mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); } // Add a collider into the broad-phase collision detection void BroadPhaseSystem::addCollider(Collider* collider, const AABB& aabb) { assert(collider->getBroadPhaseId() == -1); // Add the collision shape into the dynamic AABB tree and get its broad-phase ID int nodeId = mDynamicAABBTree.addObject(aabb, collider); // Set the broad-phase ID of the collider mCollidersComponents.setBroadPhaseId(collider->getEntity(), nodeId); // Add the collision shape into the array of bodies that have moved (or have been created) // during the last simulation step addMovedCollider(collider->getBroadPhaseId(), collider); } // Remove a collider from the broad-phase collision detection void BroadPhaseSystem::removeCollider(Collider* collider) { assert(collider->getBroadPhaseId() != -1); int broadPhaseID = collider->getBroadPhaseId(); mCollidersComponents.setBroadPhaseId(collider->getEntity(), -1); // Remove the collision shape from the dynamic AABB tree mDynamicAABBTree.removeObject(broadPhaseID); // Remove the collision shape into the array of shapes that have moved (or have been created) // during the last simulation step removeMovedCollider(broadPhaseID); } // Update the broad-phase state of a single collider void BroadPhaseSystem::updateCollider(Entity colliderEntity) { assert(mCollidersComponents.mMapEntityToComponentIndex.containsKey(colliderEntity)); // Get the index of the collider component in the array uint32 index = mCollidersComponents.mMapEntityToComponentIndex[colliderEntity]; // Update the collider component updateCollidersComponents(index, 1); } // Update the broad-phase state of all the enabled colliders void BroadPhaseSystem::updateColliders() { RP3D_PROFILE("BroadPhaseSystem::updateColliders()", mProfiler); // Update all the enabled collider components if (mCollidersComponents.getNbEnabledComponents() > 0) { updateCollidersComponents(0, mCollidersComponents.getNbEnabledComponents()); } } // Notify the broad-phase that a collision shape has moved and need to be updated void BroadPhaseSystem::updateColliderInternal(int32 broadPhaseId, Collider* collider, const AABB& aabb, bool forceReInsert) { assert(broadPhaseId >= 0); // Update the dynamic AABB tree according to the movement of the collision shape bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseId, aabb, forceReInsert); // If the collision shape has moved out of its fat AABB (and therefore has been reinserted // into the tree). if (hasBeenReInserted) { // Add the collision shape into the array of shapes that have moved (or have been created) // during the last simulation step addMovedCollider(broadPhaseId, collider); } } // Update the broad-phase state of some colliders components void BroadPhaseSystem::updateCollidersComponents(uint32 startIndex, uint32 nbItems) { RP3D_PROFILE("BroadPhaseSystem::updateCollidersComponents()", mProfiler); assert(nbItems > 0); assert(startIndex < mCollidersComponents.getNbComponents()); assert(startIndex + nbItems <= mCollidersComponents.getNbComponents()); // Make sure we do not update disabled components startIndex = std::min(startIndex, mCollidersComponents.getNbEnabledComponents()); uint32 endIndex = std::min(startIndex + nbItems, mCollidersComponents.getNbEnabledComponents()); nbItems = endIndex - startIndex; // For each collider component to update for (uint32 i = startIndex; i < startIndex + nbItems; i++) { const int32 broadPhaseId = mCollidersComponents.mBroadPhaseIds[i]; if (broadPhaseId != -1) { const Entity& bodyEntity = mCollidersComponents.mBodiesEntities[i]; const Transform& transform = mTransformsComponents.getTransform(bodyEntity); // Recompute the world-space AABB of the collision shape AABB aabb; mCollidersComponents.mCollisionShapes[i]->computeAABB(aabb, transform * mCollidersComponents.mLocalToBodyTransforms[i]); // If the size of the collision shape has been changed by the user, // we need to reset the broad-phase AABB to its new size const bool forceReInsert = mCollidersComponents.mHasCollisionShapeChangedSize[i]; // Update the broad-phase state of the collider updateColliderInternal(broadPhaseId, mCollidersComponents.mColliders[i], aabb, forceReInsert); mCollidersComponents.mHasCollisionShapeChangedSize[i] = false; } } } // Add a collider in the array of colliders that have moved in the last simulation step // and that need to be tested again for broad-phase overlapping. void BroadPhaseSystem::addMovedCollider(int broadPhaseID, Collider* collider) { assert(broadPhaseID != -1); // Store the broad-phase ID into the array of shapes that have moved mMovedShapes.add(broadPhaseID); // Notify that the overlapping pairs where this shape is involved need to be tested for overlap mCollisionDetection.notifyOverlappingPairsToTestOverlap(collider); } // Compute all the overlapping pairs of collision shapes void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager, Array>& overlappingNodes) { RP3D_PROFILE("BroadPhaseSystem::computeOverlappingPairs()", mProfiler); // Get the array of the colliders that have moved or have been created in the last frame Array shapesToTest = mMovedShapes.toArray(memoryManager.getHeapAllocator()); // Ask the dynamic AABB tree to report all collision shapes that overlap with the shapes to test mDynamicAABBTree.reportAllShapesOverlappingWithShapes(shapesToTest, 0, static_cast(shapesToTest.size()), overlappingNodes); // Reset the array of collision shapes that have move (or have been created) during the // last simulation step mMovedShapes.clear(); } // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { mOverlappingNodes.add(nodeId); } // Called for a broad-phase shape that has to be tested for raycast decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { decimal hitFraction = decimal(-1.0); // Get the collider from the node Collider* collider = static_cast(mDynamicAABBTree.getNodeDataPointer(nodeId)); // Check if the raycast filtering mask allows raycast against this shape if ((mRaycastWithCategoryMaskBits & collider->getCollisionCategoryBits()) != 0) { // Ask the collision detection to perform a ray cast test against // the collider of this node because the ray is overlapping // with the shape in the broad-phase hitFraction = mRaycastTest.raycastAgainstShape(collider, ray); } return hitFraction; }