Andrew/redot-engine
1
0
Fork 0
mirror of https://github.com/Redot-Engine/redot-engine.git synced 2025-12-06 07:17:42 -05:00
Code Issues Packages Projects Releases Wiki Activity

Batch the adding of Jolt Physics bodies

Browse Source 
Co-authored-by: Jorrit Rouwe <jrouwe@gmail.com>
This commit is contained in:
Mikael Hermansson
2025-06-12 13:54:44 +02:00
parent 1218a16de5
commit 89f9a23d9e
9 changed files with 199 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp vendored
View File

@@ -122,6 +122,18 @@ bool QuadTree::Node::EncapsulateChildBounds(int inChildIndex, const AABox &inBou
const AABox QuadTree::cInvalidBounds(Vec3::sReplicate(cLargeFloat), Vec3::sReplicate(-cLargeFloat));
static inline void sQuadTreePerformanceWarning()
{
#ifdef JPH_ENABLE_ASSERTS
static atomic<bool> triggered_report { false };
bool expected = false;
if (triggered_report.compare_exchange_strong(expected, true))
Trace("QuadTree: Performance warning: Stack full!\n"
"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
#endif
}
void QuadTree::GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const
{
uint32 body_location = inTracking[inBodyID.GetIndex()].mBodyLocation;
@@ -293,14 +305,17 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
NodeID *cur_node_id = node_ids;
// Collect all bodies
NodeID node_stack[cStackSize];
node_stack[0] = root_node.GetNodeID();
JPH_ASSERT(node_stack[0].IsValid());
int top = 0;
Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack;
node_stack.reserve(cStackSize);
node_stack.push_back(root_node.GetNodeID());
JPH_ASSERT(node_stack.front().IsValid());
do
{
// Pop node from stack
NodeID node_id = node_stack.back();
node_stack.pop_back();
// Check if node is a body
NodeID node_id = node_stack[top];
if (node_id.IsBody())
{
// Validate that we're still in the right layer
@@ -330,31 +345,14 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
// Node is changed, recurse and get all children
for (NodeID child_node_id : node.mChildNodeID)
if (child_node_id.IsValid())
{
if (top < cStackSize)
{
node_stack[top] = child_node_id;
top++;
}
else
{
JPH_ASSERT(false, "Stack full!\n"
"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
// Falling back to adding the node as a whole
*cur_node_id = child_node_id;
++cur_node_id;
}
}
node_stack.push_back(child_node_id);
// Mark node to be freed
mAllocator->AddObjectToBatch(mFreeNodeBatch, node_idx);
}
}
--top;
}
while (top >= 0);
while (!node_stack.empty());
// Check that our book keeping matches
uint32 num_node_ids = uint32(cur_node_id - node_ids);
@@ -988,7 +986,9 @@ JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter,
uint64 start = GetProcessorTickCount();
#endif // JPH_TRACK_BROADPHASE_STATS
NodeID node_stack[cStackSize];
Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack_array;
node_stack_array.resize(cStackSize);
NodeID *node_stack = node_stack_array.data();
node_stack[0] = root_node.GetNodeID();
int top = 0;
do
@@ -1027,33 +1027,34 @@ JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter,
{
JPH_IF_TRACK_BROADPHASE_STATS(++nodes_visited;)
// Check if stack can hold more nodes
if (top + 4 < cStackSize)
// Ensure there is space on the stack (falls back to heap if there isn't)
if (top + 4 >= (int)node_stack_array.size())
{
// Process normal node
const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
// Load bounds of 4 children
Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX);
Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY);
Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ);
Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX);
Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY);
Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ);
// Load ids for 4 children
UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
// Check which sub nodes to visit
int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top);
child_ids.StoreInt4((uint32 *)&node_stack[top]);
top += num_results;
sQuadTreePerformanceWarning();
node_stack_array.resize(node_stack_array.size() << 1);
node_stack = node_stack_array.data();
ioVisitor.OnStackResized(node_stack_array.size());
}
else
JPH_ASSERT(false, "Stack full!\n"
"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
// Process normal node
const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
// Load bounds of 4 children
Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX);
Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY);
Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ);
Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX);
Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY);
Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ);
// Load ids for 4 children
UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
// Check which sub nodes to visit
int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top);
child_ids.StoreInt4((uint32 *)&node_stack[top]);
top += num_results;
}
// Fetch next node until we find one that the visitor wants to see
@@ -1092,6 +1093,7 @@ void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector,
mInvDirection(inRay.mDirection),
mCollector(ioCollector)
{
mFractionStack.resize(cStackSize);
mFractionStack[0] = -1;
}
@@ -1125,11 +1127,17 @@ void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector,
mCollector.AddHit(result);
}
/// Called when the stack is resized, this allows us to resize the fraction stack to match the new stack size
JPH_INLINE void OnStackResized(size_t inNewStackSize)
{
mFractionStack.resize(inNewStackSize);
}
private:
Vec3 mOrigin;
RayInvDirection mInvDirection;
RayCastBodyCollector & mCollector;
float mFractionStack[cStackSize];
Array<float, STLLocalAllocator<float, cStackSize>> mFractionStack;
};
Visitor visitor(inRay, ioCollector);
@@ -1175,6 +1183,12 @@ void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCol
mCollector.AddHit(inBodyID);
}
/// Called when the stack is resized
JPH_INLINE void OnStackResized([[maybe_unused]] size_t inNewStackSize) const
{
// Nothing to do
}
private:
const AABox & mBox;
CollideShapeBodyCollector & mCollector;
@@ -1226,7 +1240,13 @@ void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyC
mCollector.AddHit(inBodyID);
}
private:
/// Called when the stack is resized
JPH_INLINE void OnStackResized([[maybe_unused]] size_t inNewStackSize) const
{
// Nothing to do
}
private:
Vec4 mCenterX;
Vec4 mCenterY;
Vec4 mCenterZ;
@@ -1277,6 +1297,12 @@ void QuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollec
mCollector.AddHit(inBodyID);
}
/// Called when the stack is resized
JPH_INLINE void OnStackResized([[maybe_unused]] size_t inNewStackSize) const
{
// Nothing to do
}
private:
Vec3 mPoint;
CollideShapeBodyCollector & mCollector;
@@ -1325,6 +1351,12 @@ void QuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyColl
mCollector.AddHit(inBodyID);
}
/// Called when the stack is resized
JPH_INLINE void OnStackResized([[maybe_unused]] size_t inNewStackSize) const
{
// Nothing to do
}
private:
OrientedBox mBox;
CollideShapeBodyCollector & mCollector;
@@ -1346,6 +1378,7 @@ void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioColle
mInvDirection(inBox.mDirection),
mCollector(ioCollector)
{
mFractionStack.resize(cStackSize);
mFractionStack[0] = -1;
}
@@ -1383,12 +1416,18 @@ void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioColle
mCollector.AddHit(result);
}
/// Called when the stack is resized, this allows us to resize the fraction stack to match the new stack size
JPH_INLINE void OnStackResized(size_t inNewStackSize)
{
mFractionStack.resize(inNewStackSize);
}
private:
Vec3 mOrigin;
Vec3 mExtent;
RayInvDirection mInvDirection;
CastShapeBodyCollector & mCollector;
float mFractionStack[cStackSize];
Array<float, STLLocalAllocator<float, cStackSize>> mFractionStack;
};
Visitor visitor(inBox, ioCollector);
@@ -1406,7 +1445,9 @@ void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inAc
JPH_ASSERT(inActiveBodies != nullptr);
JPH_ASSERT(inNumActiveBodies > 0);
NodeID node_stack[cStackSize];
Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack_array;
node_stack_array.resize(cStackSize);
NodeID *node_stack = node_stack_array.data();
// Loop over all active bodies
for (int b1 = 0; b1 < inNumActiveBodies; ++b1)
@@ -1468,16 +1509,17 @@ void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inAc
// Sort so that overlaps are first
child_ids = UVec4::sSort4True(overlap, child_ids);
// Push them onto the stack
if (top + 4 < cStackSize)
// Ensure there is space on the stack (falls back to heap if there isn't)
if (top + 4 >= (int)node_stack_array.size())
{
child_ids.StoreInt4((uint32 *)&node_stack[top]);
top += num_results;
sQuadTreePerformanceWarning();
node_stack_array.resize(node_stack_array.size() << 1);
node_stack = node_stack_array.data();
}
else
JPH_ASSERT(false, "Stack full!\n"
"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
// Push them onto the stack
child_ids.StoreInt4((uint32 *)&node_stack[top]);
top += num_results;
}
}
--top;
@@ -1597,6 +1639,7 @@ void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) cons
// Iterate the entire tree
Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack;
node_stack.reserve(cStackSize);
node_stack.push_back(inRoot);
JPH_ASSERT(inRoot.IsValid());
do