252 lines
11 KiB
C++
252 lines
11 KiB
C++
/********************************************************************************
|
|
* 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 <reactphysics3d/systems/BroadPhaseSystem.h>
|
|
#include <reactphysics3d/systems/CollisionDetectionSystem.h>
|
|
#include <reactphysics3d/utils/Profiler.h>
|
|
#include <reactphysics3d/collision/RaycastInfo.h>
|
|
#include <reactphysics3d/memory/MemoryManager.h>
|
|
#include <reactphysics3d/engine/PhysicsWorld.h>
|
|
|
|
// 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<Pair<int32, int32>>& overlappingNodes) {
|
|
|
|
RP3D_PROFILE("BroadPhaseSystem::computeOverlappingPairs()", mProfiler);
|
|
|
|
// Get the array of the colliders that have moved or have been created in the last frame
|
|
Array<int32> 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<uint32>(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<Collider*>(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;
|
|
}
|