From 7beaddc9c03c723a1c374fe69c591b95d5aff501 Mon Sep 17 00:00:00 2001 From: Mikael Hermansson Date: Mon, 30 Dec 2024 16:01:31 +0100 Subject: [PATCH] Improve performance with many static/sleeping bodies when using Jolt Physics --- modules/jolt_physics/objects/jolt_area_3d.cpp | 45 ++++++- modules/jolt_physics/objects/jolt_area_3d.h | 11 +- modules/jolt_physics/objects/jolt_body_3d.cpp | 96 ++++++++------- modules/jolt_physics/objects/jolt_body_3d.h | 17 ++- .../objects/jolt_shaped_object_3d.cpp | 9 +- .../objects/jolt_soft_body_3d.cpp | 8 +- .../jolt_physics/objects/jolt_soft_body_3d.h | 4 +- .../spaces/jolt_contact_listener_3d.cpp | 28 ++--- .../spaces/jolt_contact_listener_3d.h | 7 +- modules/jolt_physics/spaces/jolt_space_3d.cpp | 112 ++++++++---------- modules/jolt_physics/spaces/jolt_space_3d.h | 10 +- 11 files changed, 193 insertions(+), 154 deletions(-) diff --git a/modules/jolt_physics/objects/jolt_area_3d.cpp b/modules/jolt_physics/objects/jolt_area_3d.cpp index 50ee2699a244..eccd7c385478 100644 --- a/modules/jolt_physics/objects/jolt_area_3d.cpp +++ b/modules/jolt_physics/objects/jolt_area_3d.cpp @@ -59,6 +59,26 @@ JPH::ObjectLayer JoltArea3D::_get_object_layer() const { return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask); } +bool JoltArea3D::_has_pending_events() const { + if (body_monitor_callback.is_valid()) { + for (const KeyValue &E : bodies_by_id) { + if (!E.value.pending_added.is_empty() || !E.value.pending_removed.is_empty()) { + return true; + } + } + } + + if (area_monitor_callback.is_valid()) { + for (const KeyValue &E : areas_by_id) { + if (!E.value.pending_added.is_empty() || !E.value.pending_removed.is_empty()) { + return true; + } + } + } + + return false; +} + void JoltArea3D::_add_to_space() { jolt_shape = build_shape(); @@ -90,6 +110,18 @@ void JoltArea3D::_add_to_space() { jolt_settings = nullptr; } +void JoltArea3D::_enqueue_call_queries() { + if (space != nullptr) { + space->enqueue_call_queries(&call_queries_element); + } +} + +void JoltArea3D::_dequeue_call_queries() { + if (space != nullptr) { + space->dequeue_call_queries(&call_queries_element); + } +} + void JoltArea3D::_add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { const JoltReadableBody3D other_jolt_body = space->read_body(p_body_id); const JoltShapedObject3D *other_object = other_jolt_body.as_shaped(); @@ -270,6 +302,8 @@ void JoltArea3D::_space_changing() { _force_bodies_exited(true); _force_areas_exited(true); } + + _dequeue_call_queries(); } void JoltArea3D::_space_changed() { @@ -304,7 +338,8 @@ void JoltArea3D::_gravity_changed() { } JoltArea3D::JoltArea3D() : - JoltShapedObject3D(OBJECT_TYPE_AREA) { + JoltShapedObject3D(OBJECT_TYPE_AREA), + call_queries_element(this) { } bool JoltArea3D::is_default_area() const { @@ -659,7 +694,13 @@ void JoltArea3D::area_exited(const JPH::BodyID &p_body_id) { overlap->shape_pairs.clear(); } -void JoltArea3D::call_queries(JPH::Body &p_jolt_body) { +void JoltArea3D::call_queries() { _flush_events(bodies_by_id, body_monitor_callback); _flush_events(areas_by_id, area_monitor_callback); } + +void JoltArea3D::post_step(float p_step, JPH::Body &p_jolt_body) { + if (_has_pending_events()) { + _enqueue_call_queries(); + } +} diff --git a/modules/jolt_physics/objects/jolt_area_3d.h b/modules/jolt_physics/objects/jolt_area_3d.h index d151a7d05ad3..18410c34cfc2 100644 --- a/modules/jolt_physics/objects/jolt_area_3d.h +++ b/modules/jolt_physics/objects/jolt_area_3d.h @@ -89,6 +89,8 @@ class JoltArea3D final : public JoltShapedObject3D { typedef HashMap OverlapsById; + SelfList call_queries_element; + OverlapsById bodies_by_id; OverlapsById areas_by_id; @@ -115,8 +117,13 @@ class JoltArea3D final : public JoltShapedObject3D { virtual JPH::EMotionType _get_motion_type() const override { return JPH::EMotionType::Kinematic; } + bool _has_pending_events() const; + virtual void _add_to_space() override; + void _enqueue_call_queries(); + void _dequeue_call_queries(); + void _add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); bool _remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); @@ -217,10 +224,12 @@ class JoltArea3D final : public JoltShapedObject3D { void body_exited(const JPH::BodyID &p_body_id, bool p_notify = true); void area_exited(const JPH::BodyID &p_body_id); - void call_queries(JPH::Body &p_jolt_body); + void call_queries(); virtual bool has_custom_center_of_mass() const override { return false; } virtual Vector3 get_center_of_mass_custom() const override { return Vector3(); } + + virtual void post_step(float p_step, JPH::Body &p_jolt_body) override; }; #endif // JOLT_AREA_3D_H diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp index 59ba42c6ca91..3768dc8d0a9c 100644 --- a/modules/jolt_physics/objects/jolt_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_body_3d.cpp @@ -128,6 +128,7 @@ void JoltBody3D::_add_to_space() { jolt_settings->mAllowDynamicOrKinematic = true; jolt_settings->mCollideKinematicVsNonDynamic = reports_all_kinematic_contacts(); jolt_settings->mUseManifoldReduction = !reports_contacts(); + jolt_settings->mAllowSleeping = is_sleep_actually_allowed(); jolt_settings->mLinearDamping = 0.0f; jolt_settings->mAngularDamping = 0.0f; jolt_settings->mMaxLinearVelocity = JoltProjectSettings::get_max_linear_velocity(); @@ -153,11 +154,19 @@ void JoltBody3D::_add_to_space() { jolt_settings = nullptr; } -void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) { - if (!p_jolt_body.IsActive()) { - return; +void JoltBody3D::_enqueue_call_queries() { + if (space != nullptr) { + space->enqueue_call_queries(&call_queries_element); + } +} + +void JoltBody3D::_dequeue_call_queries() { + if (space != nullptr) { + space->dequeue_call_queries(&call_queries_element); } +} +void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) { _update_gravity(p_jolt_body); if (!custom_integrator) { @@ -182,8 +191,6 @@ void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) { p_jolt_body.AddForce(to_jolt(constant_force)); p_jolt_body.AddTorque(to_jolt(constant_torque)); } - - sync_state = true; } void JoltBody3D::_move_kinematic(float p_step, JPH::Body &p_jolt_body) { @@ -201,27 +208,19 @@ void JoltBody3D::_move_kinematic(float p_step, JPH::Body &p_jolt_body) { } p_jolt_body.MoveKinematic(new_position, new_rotation, p_step); - - sync_state = true; -} - -void JoltBody3D::_pre_step_static(float p_step, JPH::Body &p_jolt_body) { - // Nothing to do. } void JoltBody3D::_pre_step_rigid(float p_step, JPH::Body &p_jolt_body) { _integrate_forces(p_step, p_jolt_body); + _enqueue_call_queries(); } void JoltBody3D::_pre_step_kinematic(float p_step, JPH::Body &p_jolt_body) { _update_gravity(p_jolt_body); - _move_kinematic(p_step, p_jolt_body); if (reports_contacts()) { - // This seems to emulate the behavior of Godot Physics, where kinematic bodies are set as active (and thereby - // have their state synchronized on every step) only if its max reported contacts is non-zero. - sync_state = true; + _enqueue_call_queries(); } } @@ -428,6 +427,20 @@ void JoltBody3D::_update_possible_kinematic_contacts() { } } +void JoltBody3D::_update_sleep_allowed() { + const bool value = is_sleep_actually_allowed(); + + if (!in_space()) { + jolt_settings->mAllowSleeping = value; + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->SetAllowSleeping(value); +} + void JoltBody3D::_destroy_joint_constraints() { for (JoltJoint3D *joint : joints) { joint->destroy(); @@ -460,6 +473,7 @@ void JoltBody3D::_mode_changed() { _update_object_layer(); _update_kinematic_transform(); _update_mass_properties(); + _update_sleep_allowed(); wake_up(); } @@ -478,6 +492,7 @@ void JoltBody3D::_space_changing() { _destroy_joint_constraints(); _exit_all_areas(); + _dequeue_call_queries(); } void JoltBody3D::_space_changed() { @@ -486,9 +501,8 @@ void JoltBody3D::_space_changed() { _update_kinematic_transform(); _update_group_filter(); _update_joint_constraints(); + _update_sleep_allowed(); _areas_changed(); - - sync_state = false; } void JoltBody3D::_areas_changed() { @@ -519,11 +533,18 @@ void JoltBody3D::_axis_lock_changed() { void JoltBody3D::_contact_reporting_changed() { _update_possible_kinematic_contacts(); + _update_sleep_allowed(); + wake_up(); +} + +void JoltBody3D::_sleep_allowed_changed() { + _update_sleep_allowed(); wake_up(); } JoltBody3D::JoltBody3D() : - JoltShapedObject3D(OBJECT_TYPE_BODY) { + JoltShapedObject3D(OBJECT_TYPE_BODY), + call_queries_element(this) { } JoltBody3D::~JoltBody3D() { @@ -573,7 +594,7 @@ Variant JoltBody3D::get_state(PhysicsServer3D::BodyState p_state) const { return is_sleeping(); } case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - return can_sleep(); + return is_sleep_allowed(); } default: { ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); @@ -596,7 +617,7 @@ void JoltBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_ set_is_sleeping(p_value); } break; case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - set_can_sleep(p_value); + set_is_sleep_allowed(p_value); } break; default: { ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); @@ -712,6 +733,10 @@ bool JoltBody3D::is_sleeping() const { return !body->IsActive(); } +bool JoltBody3D::is_sleep_actually_allowed() const { + return sleep_allowed && !(is_kinematic() && reports_contacts()); +} + void JoltBody3D::set_is_sleeping(bool p_enabled) { if (!in_space()) { sleep_initially = p_enabled; @@ -727,27 +752,14 @@ void JoltBody3D::set_is_sleeping(bool p_enabled) { } } -bool JoltBody3D::can_sleep() const { - if (!in_space()) { - return jolt_settings->mAllowSleeping; - } - - const JoltReadableBody3D body = space->read_body(jolt_id); - ERR_FAIL_COND_V(body.is_invalid(), false); - - return body->GetAllowSleeping(); -} - -void JoltBody3D::set_can_sleep(bool p_enabled) { - if (!in_space()) { - jolt_settings->mAllowSleeping = p_enabled; +void JoltBody3D::set_is_sleep_allowed(bool p_enabled) { + if (sleep_allowed == p_enabled) { return; } - const JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); + sleep_allowed = p_enabled; - body->SetAllowSleeping(p_enabled); + _sleep_allowed_changed(); } Basis JoltBody3D::get_principal_inertia_axes() const { @@ -1187,11 +1199,7 @@ void JoltBody3D::remove_joint(JoltJoint3D *p_joint) { _joints_changed(); } -void JoltBody3D::call_queries(JPH::Body &p_jolt_body) { - if (!sync_state) { - return; - } - +void JoltBody3D::call_queries() { if (custom_integration_callback.is_valid()) { const Variant direct_state_variant = get_direct_state(); const Variant *args[2] = { &direct_state_variant, &custom_integration_userdata }; @@ -1218,8 +1226,6 @@ void JoltBody3D::call_queries(JPH::Body &p_jolt_body) { ERR_PRINT_ONCE(vformat("Failed to call state synchronization callback for '%s'. It returned the following error: '%s'.", to_string(), Variant::get_callable_error_text(state_sync_callback, args, 1, ce))); } } - - sync_state = false; } void JoltBody3D::pre_step(float p_step, JPH::Body &p_jolt_body) { @@ -1227,7 +1233,7 @@ void JoltBody3D::pre_step(float p_step, JPH::Body &p_jolt_body) { switch (mode) { case PhysicsServer3D::BODY_MODE_STATIC: { - _pre_step_static(p_step, p_jolt_body); + // Will never happen. } break; case PhysicsServer3D::BODY_MODE_RIGID: case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { diff --git a/modules/jolt_physics/objects/jolt_body_3d.h b/modules/jolt_physics/objects/jolt_body_3d.h index dc1e258bec86..8e5a149437a3 100644 --- a/modules/jolt_physics/objects/jolt_body_3d.h +++ b/modules/jolt_physics/objects/jolt_body_3d.h @@ -57,6 +57,8 @@ class JoltBody3D final : public JoltShapedObject3D { }; private: + SelfList call_queries_element; + LocalVector exceptions; LocalVector contacts; LocalVector areas; @@ -96,7 +98,7 @@ class JoltBody3D final : public JoltShapedObject3D { uint32_t locked_axes = 0; - bool sync_state = false; + bool sleep_allowed = true; bool sleep_initially = false; bool custom_center_of_mass = false; bool custom_integrator = false; @@ -108,11 +110,13 @@ class JoltBody3D final : public JoltShapedObject3D { virtual void _add_to_space() override; + void _enqueue_call_queries(); + void _dequeue_call_queries(); + void _integrate_forces(float p_step, JPH::Body &p_jolt_body); void _move_kinematic(float p_step, JPH::Body &p_jolt_body); - void _pre_step_static(float p_step, JPH::Body &p_jolt_body); void _pre_step_rigid(float p_step, JPH::Body &p_jolt_body); void _pre_step_kinematic(float p_step, JPH::Body &p_jolt_body); @@ -128,6 +132,7 @@ class JoltBody3D final : public JoltShapedObject3D { void _update_group_filter(); void _update_joint_constraints(); void _update_possible_kinematic_contacts(); + void _update_sleep_allowed(); void _destroy_joint_constraints(); @@ -144,6 +149,7 @@ class JoltBody3D final : public JoltShapedObject3D { void _exceptions_changed(); void _axis_lock_changed(); void _contact_reporting_changed(); + void _sleep_allowed_changed(); public: JoltBody3D(); @@ -175,8 +181,9 @@ class JoltBody3D final : public JoltShapedObject3D { void put_to_sleep() { set_is_sleeping(true); } void wake_up() { set_is_sleeping(false); } - bool can_sleep() const; - void set_can_sleep(bool p_enabled); + bool is_sleep_allowed() const { return sleep_allowed; } + bool is_sleep_actually_allowed() const; + void set_is_sleep_allowed(bool p_enabled); Basis get_principal_inertia_axes() const; Vector3 get_inverse_inertia() const; @@ -238,7 +245,7 @@ class JoltBody3D final : public JoltShapedObject3D { void add_joint(JoltJoint3D *p_joint); void remove_joint(JoltJoint3D *p_joint); - void call_queries(JPH::Body &p_jolt_body); + void call_queries(); virtual void pre_step(float p_step, JPH::Body &p_jolt_body) override; diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp index 9c83718fb6e6..577e8157a53d 100644 --- a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp +++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp @@ -160,13 +160,14 @@ void JoltShapedObject3D::_update_shape() { const JoltWritableBody3D body = space->write_body(jolt_id); ERR_FAIL_COND(body.is_invalid()); - previous_jolt_shape = jolt_shape; - jolt_shape = build_shape(); - - if (jolt_shape == previous_jolt_shape) { + JPH::ShapeRefC new_shape = build_shape(); + if (new_shape == jolt_shape) { return; } + previous_jolt_shape = jolt_shape; + jolt_shape = new_shape; + space->get_body_iface().SetShape(jolt_id, jolt_shape, false, JPH::EActivation::DontActivate); _shapes_built(); diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp index c9e834fd42e2..281591af9a6c 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp @@ -438,7 +438,7 @@ void JoltSoftBody3D::set_is_sleeping(bool p_enabled) { } } -bool JoltSoftBody3D::can_sleep() const { +bool JoltSoftBody3D::is_sleep_allowed() const { if (!in_space()) { return true; } @@ -449,7 +449,7 @@ bool JoltSoftBody3D::can_sleep() const { return body->GetAllowSleeping(); } -void JoltSoftBody3D::set_can_sleep(bool p_enabled) { +void JoltSoftBody3D::set_is_sleep_allowed(bool p_enabled) { if (!in_space()) { return; } @@ -532,7 +532,7 @@ Variant JoltSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const { return is_sleeping(); } case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - return can_sleep(); + return is_sleep_allowed(); } default: { ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); @@ -555,7 +555,7 @@ void JoltSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant set_is_sleeping(p_value); } break; case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - set_can_sleep(p_value); + set_is_sleep_allowed(p_value); } break; default: { ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.h b/modules/jolt_physics/objects/jolt_soft_body_3d.h index 45e8be2e7bf4..f236310f6c69 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.h +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.h @@ -124,8 +124,8 @@ class JoltSoftBody3D final : public JoltObject3D { bool is_sleeping() const; void set_is_sleeping(bool p_enabled); - bool can_sleep() const; - void set_can_sleep(bool p_enabled); + bool is_sleep_allowed() const; + void set_is_sleep_allowed(bool p_enabled); void put_to_sleep() { set_is_sleeping(true); } void wake_up() { set_is_sleeping(false); } diff --git a/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp b/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp index 85cf08d4c921..85a29a244bdd 100644 --- a/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp +++ b/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp @@ -86,10 +86,6 @@ void JoltContactListener3D::OnSoftBodyContactAdded(const JPH::Body &p_soft_body, #endif -bool JoltContactListener3D::_is_listening_for(const JPH::Body &p_body) const { - return listening_for.has(p_body.GetID()); -} - bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) { if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) { return false; @@ -178,16 +174,19 @@ bool JoltContactListener3D::_try_apply_surface_velocities(const JPH::Body &p_jol return true; } -bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) { - if (p_body1.IsSensor() || p_body2.IsSensor()) { +bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) { + if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) { return false; } - if (!_is_listening_for(p_body1) && !_is_listening_for(p_body2)) { + const JoltBody3D *body1 = reinterpret_cast(p_jolt_body1.GetUserData()); + const JoltBody3D *body2 = reinterpret_cast(p_jolt_body2.GetUserData()); + + if (!body1->reports_contacts() && !body2->reports_contacts()) { return false; } - const JPH::SubShapeIDPair shape_pair(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2); + const JPH::SubShapeIDPair shape_pair(p_jolt_body1.GetID(), p_manifold.mSubShapeID1, p_jolt_body2.GetID(), p_manifold.mSubShapeID2); Manifold &manifold = [&]() -> Manifold & { const MutexLock write_lock(write_mutex); @@ -201,8 +200,7 @@ bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JP manifold.depth = p_manifold.mPenetrationDepth; JPH::CollisionEstimationResult collision; - - JPH::EstimateCollisionResponse(p_body1, p_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::get_bounce_velocity_threshold(), 5); + JPH::EstimateCollisionResponse(p_jolt_body1, p_jolt_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::get_bounce_velocity_threshold(), 5); for (JPH::uint i = 0; i < contact_count; ++i) { const JPH::RVec3 relative_point1 = JPH::RVec3(p_manifold.mRelativeContactPointsOn1[i]); @@ -211,8 +209,8 @@ bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JP const JPH::RVec3 world_point1 = p_manifold.mBaseOffset + relative_point1; const JPH::RVec3 world_point2 = p_manifold.mBaseOffset + relative_point2; - const JPH::Vec3 velocity1 = p_body1.GetPointVelocity(world_point1); - const JPH::Vec3 velocity2 = p_body2.GetPointVelocity(world_point2); + const JPH::Vec3 velocity1 = p_jolt_body1.GetPointVelocity(world_point1); + const JPH::Vec3 velocity2 = p_jolt_body2.GetPointVelocity(world_point2); const JPH::CollisionEstimationResult::Impulse &impulse = collision.mImpulses[i]; @@ -532,13 +530,7 @@ void JoltContactListener3D::_flush_area_exits() { area_exits.clear(); } -void JoltContactListener3D::listen_for(JoltShapedObject3D *p_object) { - listening_for.insert(p_object->get_jolt_id()); -} - void JoltContactListener3D::pre_step() { - listening_for.clear(); - #ifdef DEBUG_ENABLED debug_contact_count = 0; #endif diff --git a/modules/jolt_physics/spaces/jolt_contact_listener_3d.h b/modules/jolt_physics/spaces/jolt_contact_listener_3d.h index 26136abd83ac..41b3c9ef3bb2 100644 --- a/modules/jolt_physics/spaces/jolt_contact_listener_3d.h +++ b/modules/jolt_physics/spaces/jolt_contact_listener_3d.h @@ -85,7 +85,6 @@ class JoltContactListener3D final }; HashMap manifolds_by_shape_pair; - HashSet listening_for; HashSet area_overlaps; HashSet area_enters; HashSet area_exits; @@ -107,12 +106,10 @@ class JoltContactListener3D final virtual void OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) override; #endif - bool _is_listening_for(const JPH::Body &p_body) const; - bool _try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings); bool _try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings); bool _try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings); - bool _try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings); + bool _try_add_contacts(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings); bool _try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold); bool _try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair); bool _try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair); @@ -131,8 +128,6 @@ class JoltContactListener3D final explicit JoltContactListener3D(JoltSpace3D *p_space) : space(p_space) {} - void listen_for(JoltShapedObject3D *p_object); - void pre_step(); void post_step(); diff --git a/modules/jolt_physics/spaces/jolt_space_3d.cpp b/modules/jolt_physics/spaces/jolt_space_3d.cpp index 57d813df2b95..78bdf778f554 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.cpp +++ b/modules/jolt_physics/spaces/jolt_space_3d.cpp @@ -63,55 +63,36 @@ constexpr double DEFAULT_SOLVER_ITERATIONS = 8; } // namespace void JoltSpace3D::_pre_step(float p_step) { - body_accessor.acquire_all(); - contact_listener->pre_step(); - const int body_count = body_accessor.get_count(); - - for (int i = 0; i < body_count; ++i) { - if (JPH::Body *jolt_body = body_accessor.try_get(i)) { - if (jolt_body->IsSoftBody()) { - continue; - } - - JoltShapedObject3D *object = reinterpret_cast(jolt_body->GetUserData()); - - object->pre_step(p_step, *jolt_body); + const JPH::BodyLockInterface &lock_iface = get_lock_iface(); + const JPH::BodyID *active_rigid_bodies = physics_system->GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody); + const JPH::uint32 active_rigid_body_count = physics_system->GetNumActiveBodies(JPH::EBodyType::RigidBody); - if (object->reports_contacts()) { - contact_listener->listen_for(object); - } - } + for (JPH::uint32 i = 0; i < active_rigid_body_count; i++) { + JPH::Body *jolt_body = lock_iface.TryGetBody(active_rigid_bodies[i]); + JoltObject3D *object = reinterpret_cast(jolt_body->GetUserData()); + object->pre_step(p_step, *jolt_body); } - - body_accessor.release(); } void JoltSpace3D::_post_step(float p_step) { - body_accessor.acquire_all(); - contact_listener->post_step(); - const int body_count = body_accessor.get_count(); - - for (int i = 0; i < body_count; ++i) { - if (JPH::Body *jolt_body = body_accessor.try_get(i)) { - if (jolt_body->IsSoftBody()) { - continue; - } + // WARNING: The list of active bodies may have changed between `pre_step` and `post_step`. - JoltObject3D *object = reinterpret_cast(jolt_body->GetUserData()); + const JPH::BodyLockInterface &lock_iface = get_lock_iface(); + const JPH::BodyID *active_rigid_bodies = physics_system->GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody); + const JPH::uint32 active_rigid_body_count = physics_system->GetNumActiveBodies(JPH::EBodyType::RigidBody); - object->post_step(p_step, *jolt_body); - } + for (JPH::uint32 i = 0; i < active_rigid_body_count; i++) { + JPH::Body *jolt_body = lock_iface.TryGetBody(active_rigid_bodies[i]); + JoltObject3D *object = reinterpret_cast(jolt_body->GetUserData()); + object->post_step(p_step, *jolt_body); } - - body_accessor.release(); } JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) : - body_accessor(this), job_system(p_job_system), temp_allocator(new JoltTempAllocator()), layers(new JoltLayers()), @@ -208,44 +189,21 @@ void JoltSpace3D::step(float p_step) { _post_step(p_step); bodies_added_since_optimizing = 0; - has_stepped = true; stepping = false; } void JoltSpace3D::call_queries() { - if (!has_stepped) { - // We need to skip the first invocation of this method, because there will be pending notifications that need to - // be flushed first, which can cause weird conflicts with things like `_integrate_forces`. This happens to also - // emulate the behavior of Godot Physics, where (active) collision objects must register to have `call_queries` - // invoked, which they don't do until the physics step, which happens after this. - // - // TODO: This would be better solved by just doing what Godot Physics does with `GodotSpace*D::active_list`. - return; - } - - body_accessor.acquire_all(); - - const int body_count = body_accessor.get_count(); - - for (int i = 0; i < body_count; ++i) { - if (JPH::Body *jolt_body = body_accessor.try_get(i)) { - if (!jolt_body->IsSensor() && !jolt_body->IsSoftBody()) { - JoltBody3D *body = reinterpret_cast(jolt_body->GetUserData()); - body->call_queries(*jolt_body); - } - } + while (body_call_queries_list.first()) { + JoltBody3D *body = body_call_queries_list.first()->self(); + body_call_queries_list.remove(body_call_queries_list.first()); + body->call_queries(); } - for (int i = 0; i < body_count; ++i) { - if (JPH::Body *jolt_body = body_accessor.try_get(i)) { - if (jolt_body->IsSensor()) { - JoltArea3D *area = reinterpret_cast(jolt_body->GetUserData()); - area->call_queries(*jolt_body); - } - } + while (area_call_queries_list.first()) { + JoltArea3D *body = area_call_queries_list.first()->self(); + area_call_queries_list.remove(area_call_queries_list.first()); + body->call_queries(); } - - body_accessor.release(); } double JoltSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const { @@ -445,6 +403,30 @@ void JoltSpace3D::try_optimize() { bodies_added_since_optimizing = 0; } +void JoltSpace3D::enqueue_call_queries(SelfList *p_body) { + if (!p_body->in_list()) { + body_call_queries_list.add(p_body); + } +} + +void JoltSpace3D::enqueue_call_queries(SelfList *p_area) { + if (!p_area->in_list()) { + area_call_queries_list.add(p_area); + } +} + +void JoltSpace3D::dequeue_call_queries(SelfList *p_body) { + if (p_body->in_list()) { + body_call_queries_list.remove(p_body); + } +} + +void JoltSpace3D::dequeue_call_queries(SelfList *p_area) { + if (p_area->in_list()) { + area_call_queries_list.remove(p_area); + } +} + void JoltSpace3D::add_joint(JPH::Constraint *p_jolt_ref) { physics_system->AddConstraint(p_jolt_ref); } diff --git a/modules/jolt_physics/spaces/jolt_space_3d.h b/modules/jolt_physics/spaces/jolt_space_3d.h index eb666644eae6..989df9a49853 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.h +++ b/modules/jolt_physics/spaces/jolt_space_3d.h @@ -48,6 +48,7 @@ #include class JoltArea3D; +class JoltBody3D; class JoltContactListener3D; class JoltJoint3D; class JoltLayers; @@ -55,7 +56,8 @@ class JoltObject3D; class JoltPhysicsDirectSpaceState3D; class JoltSpace3D { - JoltBodyWriter3D body_accessor; + SelfList::List body_call_queries_list; + SelfList::List area_call_queries_list; RID rid; @@ -73,7 +75,6 @@ class JoltSpace3D { bool active = false; bool stepping = false; - bool has_stepped = false; void _pre_step(float p_step); void _post_step(float p_step); @@ -132,6 +133,11 @@ class JoltSpace3D { void try_optimize(); + void enqueue_call_queries(SelfList *p_body); + void enqueue_call_queries(SelfList *p_area); + void dequeue_call_queries(SelfList *p_body); + void dequeue_call_queries(SelfList *p_area); + void add_joint(JPH::Constraint *p_jolt_ref); void add_joint(JoltJoint3D *p_joint); void remove_joint(JPH::Constraint *p_jolt_ref);