From 89f9a23d9eed26a5bb4717498322c32d5e9d70d8 Mon Sep 17 00:00:00 2001 From: Mikael Hermansson Date: Thu, 12 Jun 2025 13:54:44 +0200 Subject: [PATCH] Batch the adding of Jolt Physics bodies Co-authored-by: Jorrit Rouwe --- modules/jolt_physics/objects/jolt_area_3d.cpp | 10 +- modules/jolt_physics/objects/jolt_body_3d.cpp | 10 +- .../jolt_physics/objects/jolt_object_3d.cpp | 2 +- .../objects/jolt_soft_body_3d.cpp | 24 ++- .../jolt_physics/objects/jolt_soft_body_3d.h | 1 + .../jolt_physics_direct_space_state_3d.cpp | 18 +- modules/jolt_physics/spaces/jolt_space_3d.cpp | 77 +++++--- modules/jolt_physics/spaces/jolt_space_3d.h | 17 +- .../Physics/Collision/BroadPhase/QuadTree.cpp | 167 +++++++++++------- 9 files changed, 199 insertions(+), 127 deletions(-) diff --git a/modules/jolt_physics/objects/jolt_area_3d.cpp b/modules/jolt_physics/objects/jolt_area_3d.cpp index c0326e7fbf9..e87a6e4cbc6 100644 --- a/modules/jolt_physics/objects/jolt_area_3d.cpp +++ b/modules/jolt_physics/objects/jolt_area_3d.cpp @@ -80,7 +80,7 @@ void JoltArea3D::_add_to_space() { jolt_settings->SetShape(jolt_shape); - JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings, _should_sleep()); + JPH::Body *new_jolt_body = space->add_object(*this, *jolt_settings, _should_sleep()); if (new_jolt_body == nullptr) { return; } @@ -214,15 +214,11 @@ void JoltArea3D::_remove_all_overlaps() { } void JoltArea3D::_update_sleeping() { - if (space == nullptr) { + if (!in_space()) { return; } - if (_should_sleep()) { - space->get_body_iface().DeactivateBody(jolt_body->GetID()); - } else { - space->get_body_iface().ActivateBody(jolt_body->GetID()); - } + space->set_is_object_sleeping(jolt_body->GetID(), _should_sleep()); } void JoltArea3D::_update_group_filter() { diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp index 872513e6747..1d040e971bc 100644 --- a/modules/jolt_physics/objects/jolt_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_body_3d.cpp @@ -144,7 +144,7 @@ void JoltBody3D::_add_to_space() { jolt_settings->SetShape(jolt_shape); - JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings, sleep_initially); + JPH::Body *new_jolt_body = space->add_object(*this, *jolt_settings, sleep_initially); if (new_jolt_body == nullptr) { return; } @@ -706,11 +706,7 @@ void JoltBody3D::set_is_sleeping(bool p_enabled) { if (!in_space()) { sleep_initially = p_enabled; } else { - if (p_enabled) { - space->get_body_iface().DeactivateBody(jolt_body->GetID()); - } else { - space->get_body_iface().ActivateBody(jolt_body->GetID()); - } + space->set_is_object_sleeping(jolt_body->GetID(), p_enabled); } } @@ -1180,7 +1176,7 @@ bool JoltBody3D::is_ccd_enabled() const { if (!in_space()) { return jolt_settings->mMotionQuality == JPH::EMotionQuality::LinearCast; } else { - return space->get_body_iface().GetMotionQuality(jolt_body->GetID()) == JPH::EMotionQuality::LinearCast; + return !is_static() && jolt_body->GetMotionProperties()->GetMotionQuality() == JPH::EMotionQuality::LinearCast; } } diff --git a/modules/jolt_physics/objects/jolt_object_3d.cpp b/modules/jolt_physics/objects/jolt_object_3d.cpp index d737cb3b2dd..b32fee419f9 100644 --- a/modules/jolt_physics/objects/jolt_object_3d.cpp +++ b/modules/jolt_physics/objects/jolt_object_3d.cpp @@ -41,7 +41,7 @@ void JoltObject3D::_remove_from_space() { return; } - space->remove_body(jolt_body->GetID()); + space->remove_object(jolt_body->GetID()); jolt_body = nullptr; } diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp index 089e66c2d26..7cc0b69e64c 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp @@ -110,7 +110,7 @@ void JoltSoftBody3D::_add_to_space() { jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id); jolt_settings->mMaxLinearVelocity = JoltProjectSettings::max_linear_velocity; - JPH::Body *new_jolt_body = space->add_soft_body(*this, *jolt_settings); + JPH::Body *new_jolt_body = space->add_object(*this, *jolt_settings); if (new_jolt_body == nullptr) { return; } @@ -326,6 +326,10 @@ void JoltSoftBody3D::_exceptions_changed() { _update_group_filter(); } +void JoltSoftBody3D::_motion_changed() { + wake_up(); +} + JoltSoftBody3D::JoltSoftBody3D() : JoltObject3D(OBJECT_TYPE_SOFT_BODY) { jolt_settings->mRestitution = 0.0f; @@ -410,7 +414,7 @@ void JoltSoftBody3D::apply_vertex_impulse(int p_index, const Vector3 &p_impulse) physics_vertex.mVelocity += to_jolt(p_impulse) * physics_vertex.mInvMass; - wake_up(); + _motion_changed(); } void JoltSoftBody3D::apply_vertex_force(int p_index, const Vector3 &p_force) { @@ -421,7 +425,6 @@ void JoltSoftBody3D::apply_vertex_force(int p_index, const Vector3 &p_force) { void JoltSoftBody3D::apply_central_impulse(const Vector3 &p_impulse) { ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); - ERR_FAIL_NULL(shared); JPH::SoftBodyMotionProperties &motion_properties = static_cast(*jolt_body->GetMotionPropertiesUnchecked()); JPH::Array &physics_vertices = motion_properties.GetVertices(); @@ -434,16 +437,15 @@ void JoltSoftBody3D::apply_central_impulse(const Vector3 &p_impulse) { } } - wake_up(); + _motion_changed(); } void JoltSoftBody3D::apply_central_force(const Vector3 &p_force) { ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); - ERR_FAIL_NULL(shared); - JPH::BodyInterface &body_iface = space->get_body_iface(); + jolt_body->AddForce(to_jolt(p_force)); - body_iface.AddForce(jolt_body->GetID(), to_jolt(p_force), JPH::EActivation::Activate); + _motion_changed(); } void JoltSoftBody3D::set_is_sleeping(bool p_enabled) { @@ -451,13 +453,7 @@ void JoltSoftBody3D::set_is_sleeping(bool p_enabled) { return; } - JPH::BodyInterface &body_iface = space->get_body_iface(); - - if (p_enabled) { - body_iface.DeactivateBody(jolt_body->GetID()); - } else { - body_iface.ActivateBody(jolt_body->GetID()); - } + space->set_is_object_sleeping(jolt_body->GetID(), p_enabled); } bool JoltSoftBody3D::is_sleep_allowed() const { diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.h b/modules/jolt_physics/objects/jolt_soft_body_3d.h index e1e49074474..860eb75b725 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.h +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.h @@ -95,6 +95,7 @@ class JoltSoftBody3D final : public JoltObject3D { void _pins_changed(); void _vertices_changed(); void _exceptions_changed(); + void _motion_changed(); public: JoltSoftBody3D(); diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp index ca18ced8929..36ad136c737 100644 --- a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp +++ b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp @@ -464,7 +464,7 @@ JoltPhysicsDirectSpaceState3D::JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_spac bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) { ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_ray must not be called while the physics space is being stepped."); - space->try_optimize(); + space->flush_pending_objects(); const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude, p_parameters.pick_ray); @@ -531,7 +531,7 @@ int JoltPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_para return 0; } - space->try_optimize(); + space->flush_pending_objects(); const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude); JoltQueryCollectorAnyMulti collector(p_result_max); @@ -569,7 +569,7 @@ int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_para return 0; } - space->try_optimize(); + space->flush_pending_objects(); JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); ERR_FAIL_NULL_V(shape, 0); @@ -623,7 +623,7 @@ bool JoltPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_paramet ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "cast_motion must not be called while the physics space is being stepped."); ERR_FAIL_COND_V_MSG(r_info != nullptr, false, "Providing rest info as part of cast_motion is not supported when using Jolt Physics."); - space->try_optimize(); + space->flush_pending_objects(); JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); ERR_FAIL_NULL_V(shape, false); @@ -659,7 +659,7 @@ bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_param return false; } - space->try_optimize(); + space->flush_pending_objects(); JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); ERR_FAIL_NULL_V(shape, false); @@ -728,7 +728,7 @@ bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_param bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) { ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "get_rest_info must not be called while the physics space is being stepped."); - space->try_optimize(); + space->flush_pending_objects(); JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); ERR_FAIL_NULL_V(shape, false); @@ -785,7 +785,7 @@ bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameter Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const { ERR_FAIL_COND_V_MSG(space->is_stepping(), Vector3(), "get_closest_point_to_object_volume must not be called while the physics space is being stepped."); - space->try_optimize(); + space->flush_pending_objects(); JoltPhysicsServer3D *physics_server = JoltPhysicsServer3D::get_singleton(); JoltObject3D *object = physics_server->get_area(p_object); @@ -861,6 +861,8 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_ bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const { ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "body_test_motion (maybe from move_and_slide?) must not be called while the physics space is being stepped."); + space->flush_pending_objects(); + const float margin = MAX((float)p_parameters.margin, 0.0001f); const int max_collisions = MIN(p_parameters.max_collisions, 32); @@ -870,8 +872,6 @@ bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, c Vector3 scale; JoltMath::decompose(transform, scale); - space->try_optimize(); - Vector3 recovery; const bool recovered = _body_motion_recover(p_body, transform, margin, p_parameters.exclude_bodies, p_parameters.exclude_objects, recovery); diff --git a/modules/jolt_physics/spaces/jolt_space_3d.cpp b/modules/jolt_physics/spaces/jolt_space_3d.cpp index cfe197cb094..69b73ea6f7f 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.cpp +++ b/modules/jolt_physics/spaces/jolt_space_3d.cpp @@ -65,6 +65,8 @@ constexpr double SPACE_DEFAULT_SOLVER_ITERATIONS = 8; } // namespace void JoltSpace3D::_pre_step(float p_step) { + flush_pending_objects(); + while (needs_optimization_list.first()) { JoltShapedObject3D *object = needs_optimization_list.first()->self(); needs_optimization_list.remove(needs_optimization_list.first()); @@ -202,7 +204,6 @@ void JoltSpace3D::step(float p_step) { _post_step(p_step); - bodies_added_since_optimizing = 0; stepping = false; } @@ -385,7 +386,7 @@ void JoltSpace3D::set_default_area(JoltArea3D *p_area) { } } -JPH::Body *JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) { +JPH::Body *JoltSpace3D::add_object(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) { JPH::BodyInterface &body_iface = get_body_iface(); JPH::Body *jolt_body = body_iface.CreateBody(p_settings); if (unlikely(jolt_body == nullptr)) { @@ -397,13 +398,16 @@ JPH::Body *JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH:: return nullptr; } - body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate); - bodies_added_since_optimizing += 1; + if (p_sleeping) { + pending_objects_sleeping.push_back(jolt_body->GetID()); + } else { + pending_objects_awake.push_back(jolt_body->GetID()); + } return jolt_body; } -JPH::Body *JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) { +JPH::Body *JoltSpace3D::add_object(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) { JPH::BodyInterface &body_iface = get_body_iface(); JPH::Body *jolt_body = body_iface.CreateSoftBody(p_settings); if (unlikely(jolt_body == nullptr)) { @@ -415,33 +419,66 @@ JPH::Body *JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::S return nullptr; } - body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate); - bodies_added_since_optimizing += 1; + if (p_sleeping) { + pending_objects_sleeping.push_back(jolt_body->GetID()); + } else { + pending_objects_awake.push_back(jolt_body->GetID()); + } return jolt_body; } -void JoltSpace3D::remove_body(const JPH::BodyID &p_body_id) { +void JoltSpace3D::remove_object(const JPH::BodyID &p_jolt_id) { JPH::BodyInterface &body_iface = get_body_iface(); - body_iface.RemoveBody(p_body_id); - body_iface.DestroyBody(p_body_id); + if (!pending_objects_sleeping.erase_unordered(p_jolt_id) && !pending_objects_awake.erase_unordered(p_jolt_id)) { + body_iface.RemoveBody(p_jolt_id); + } + + body_iface.DestroyBody(p_jolt_id); } -void JoltSpace3D::try_optimize() { - // This makes assumptions about the underlying acceleration structure of Jolt's broad-phase, which currently uses a - // quadtree, and which gets walked with a fixed-size node stack of 128. This means that when the quadtree is - // completely unbalanced, as is the case if we add bodies one-by-one without ever stepping the simulation, like in - // the editor viewport, we would exceed this stack size (resulting in an incomplete search) as soon as we perform a - // physics query after having added somewhere in the order of 128 * 3 bodies. We leave a hefty margin just in case. - - if (likely(bodies_added_since_optimizing < 128)) { +void JoltSpace3D::flush_pending_objects() { + if (pending_objects_sleeping.is_empty() && pending_objects_awake.is_empty()) { return; } - physics_system->OptimizeBroadPhase(); + // We only care about locking within this method, because it's called when performing queries, which aren't covered by `PhysicsServer3DWrapMT`. + MutexLock pending_objects_lock(pending_objects_mutex); - bodies_added_since_optimizing = 0; + JPH::BodyInterface &body_iface = get_body_iface(); + + if (!pending_objects_sleeping.is_empty()) { + JPH::BodyInterface::AddState add_state = body_iface.AddBodiesPrepare(pending_objects_sleeping.ptr(), pending_objects_sleeping.size()); + body_iface.AddBodiesFinalize(pending_objects_sleeping.ptr(), pending_objects_sleeping.size(), add_state, JPH::EActivation::DontActivate); + pending_objects_sleeping.reset(); + } + + if (!pending_objects_awake.is_empty()) { + JPH::BodyInterface::AddState add_state = body_iface.AddBodiesPrepare(pending_objects_awake.ptr(), pending_objects_awake.size()); + body_iface.AddBodiesFinalize(pending_objects_awake.ptr(), pending_objects_awake.size(), add_state, JPH::EActivation::Activate); + pending_objects_awake.reset(); + } +} + +void JoltSpace3D::set_is_object_sleeping(const JPH::BodyID &p_jolt_id, bool p_enable) { + if (p_enable) { + if (pending_objects_awake.erase_unordered(p_jolt_id)) { + pending_objects_sleeping.push_back(p_jolt_id); + } else if (pending_objects_sleeping.has(p_jolt_id)) { + // Do nothing. + } else { + get_body_iface().DeactivateBody(p_jolt_id); + } + } else { + if (pending_objects_sleeping.erase_unordered(p_jolt_id)) { + pending_objects_awake.push_back(p_jolt_id); + } else if (pending_objects_awake.has(p_jolt_id)) { + // Do nothing. + } else { + get_body_iface().ActivateBody(p_jolt_id); + } + } } void JoltSpace3D::enqueue_call_queries(SelfList *p_body) { diff --git a/modules/jolt_physics/spaces/jolt_space_3d.h b/modules/jolt_physics/spaces/jolt_space_3d.h index 22601338120..b33a20de36c 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.h +++ b/modules/jolt_physics/spaces/jolt_space_3d.h @@ -53,11 +53,16 @@ class JoltShapedObject3D; class JoltSoftBody3D; class JoltSpace3D { + Mutex pending_objects_mutex; + SelfList::List body_call_queries_list; SelfList::List area_call_queries_list; SelfList::List shapes_changed_list; SelfList::List needs_optimization_list; + LocalVector pending_objects_sleeping; + LocalVector pending_objects_awake; + RID rid; JPH::JobSystem *job_system = nullptr; @@ -70,8 +75,6 @@ class JoltSpace3D { float last_step = 0.0f; - int bodies_added_since_optimizing = 0; - bool active = false; bool stepping = false; @@ -125,12 +128,12 @@ public: float get_last_step() const { return last_step; } - JPH::Body *add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false); - JPH::Body *add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false); + JPH::Body *add_object(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false); + JPH::Body *add_object(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false); + void remove_object(const JPH::BodyID &p_jolt_id); + void flush_pending_objects(); - void remove_body(const JPH::BodyID &p_body_id); - - void try_optimize(); + void set_is_object_sleeping(const JPH::BodyID &p_jolt_id, bool p_enable); void enqueue_call_queries(SelfList *p_body); void enqueue_call_queries(SelfList *p_area); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp index b78b8da1a96..6966b31c521 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp @@ -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 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> 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> 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> 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> 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> 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> node_stack; + node_stack.reserve(cStackSize); node_stack.push_back(inRoot); JPH_ASSERT(inRoot.IsValid()); do