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

10
modules/jolt_physics/objects/jolt_area_3d.cpp
View File

@@ -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() {

10
modules/jolt_physics/objects/jolt_body_3d.cpp
View File

@@ -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;
}
}

2
modules/jolt_physics/objects/jolt_object_3d.cpp
View File

@@ -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;
}

24
modules/jolt_physics/objects/jolt_soft_body_3d.cpp
View File

@@ -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<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
JPH::Array<JPH::SoftBodyVertex> &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 {

1
modules/jolt_physics/objects/jolt_soft_body_3d.h
View File

@@ -95,6 +95,7 @@ class JoltSoftBody3D final : public JoltObject3D {
void _pins_changed();
void _vertices_changed();
void _exceptions_changed();
void _motion_changed();
public:
JoltSoftBody3D();

18
modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
View File

@@ -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<JPH::CollidePointCollector, 32> 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);

77
modules/jolt_physics/spaces/jolt_space_3d.cpp
View File

@@ -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<JoltBody3D> *p_body) {

17
modules/jolt_physics/spaces/jolt_space_3d.h
View File

@@ -53,11 +53,16 @@ class JoltShapedObject3D;
class JoltSoftBody3D;
class JoltSpace3D {
Mutex pending_objects_mutex;
SelfList<JoltBody3D>::List body_call_queries_list;
SelfList<JoltArea3D>::List area_call_queries_list;
SelfList<JoltShapedObject3D>::List shapes_changed_list;
SelfList<JoltShapedObject3D>::List needs_optimization_list;
LocalVector<JPH::BodyID> pending_objects_sleeping;
LocalVector<JPH::BodyID> 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<JoltBody3D> *p_body);
void enqueue_call_queries(SelfList<JoltArea3D> *p_area);

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