Skip to content

Commit

Permalink
Merge pull request #100983 from mihe/jolt/only-iterate-active-bodies
Browse files Browse the repository at this point in the history 
Improve performance with many static/sleeping bodies when using Jolt Physics
  • Loading branch information
@akien-mga
akien-mga committed Jan 2, 2025
2 parents d28c416 + 7beaddc commit 1f2d535
Show file tree
Hide file tree
Showing 11 changed files with 193 additions and 154 deletions.
45 changes: 43 additions & 2 deletions modules/jolt_physics/objects/jolt_area_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<JPH::BodyID, Overlap> &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<JPH::BodyID, Overlap> &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();

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -270,6 +302,8 @@ void JoltArea3D::_space_changing() {
_force_bodies_exited(true);
_force_areas_exited(true);
}

_dequeue_call_queries();
}

void JoltArea3D::_space_changed() {
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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();
}
}
11 changes: 10 additions & 1 deletion modules/jolt_physics/objects/jolt_area_3d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,8 @@ class JoltArea3D final : public JoltShapedObject3D {

typedef HashMap<JPH::BodyID, Overlap, BodyIDHasher> OverlapsById;

SelfList<JoltArea3D> call_queries_element;

OverlapsById bodies_by_id;
OverlapsById areas_by_id;

Expand All @@ -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);

Expand Down Expand Up @@ -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
96 changes: 51 additions & 45 deletions modules/jolt_physics/objects/jolt_body_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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();
}
}

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -460,6 +473,7 @@ void JoltBody3D::_mode_changed() {
_update_object_layer();
_update_kinematic_transform();
_update_mass_properties();
_update_sleep_allowed();
wake_up();
}

Expand All @@ -478,6 +492,7 @@ void JoltBody3D::_space_changing() {

_destroy_joint_constraints();
_exit_all_areas();
_dequeue_call_queries();
}

void JoltBody3D::_space_changed() {
Expand All @@ -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() {
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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));
Expand All @@ -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));
Expand Down Expand Up @@ -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;
Expand All @@ -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 {
Expand Down Expand Up @@ -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 };
Expand All @@ -1218,16 +1226,14 @@ 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) {
JoltObject3D::pre_step(p_step, 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: {
Expand Down
Loading

0 comments on commit 1f2d535

Please sign in to comment.