mirror of https://github.com/godotengine/godot
Merge pull request #100983 from mihe/jolt/only-iterate-active-bodies
Improve performance with many static/sleeping bodies when using Jolt Physics
This commit is contained in:
Rémi Verschelde
commit
1f2d535f78
|
|
@ -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();
|
||||
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -89,6 +89,8 @@ private:
|
|||
|
||||
typedef HashMap<JPH::BodyID, Overlap, BodyIDHasher> OverlapsById;
|
||||
|
||||
SelfList<JoltArea3D> call_queries_element;
|
||||
|
||||
OverlapsById bodies_by_id;
|
||||
OverlapsById areas_by_id;
|
||||
|
||||
|
|
@ -115,8 +117,13 @@ private:
|
|||
|
||||
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 @@ public:
|
|||
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
|
||||
|
|
|
|||
|
|
@ -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: {
|
||||
|
|
|
|||
|
|
@ -57,6 +57,8 @@ public:
|
|||
};
|
||||
|
||||
private:
|
||||
SelfList<JoltBody3D> call_queries_element;
|
||||
|
||||
LocalVector<RID> exceptions;
|
||||
LocalVector<Contact> contacts;
|
||||
LocalVector<JoltArea3D *> areas;
|
||||
|
|
@ -96,7 +98,7 @@ private:
|
|||
|
||||
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 @@ private:
|
|||
|
||||
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 @@ private:
|
|||
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 @@ private:
|
|||
void _exceptions_changed();
|
||||
void _axis_lock_changed();
|
||||
void _contact_reporting_changed();
|
||||
void _sleep_allowed_changed();
|
||||
|
||||
public:
|
||||
JoltBody3D();
|
||||
|
|
@ -175,8 +181,9 @@ public:
|
|||
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 @@ public:
|
|||
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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -124,8 +124,8 @@ public:
|
|||
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); }
|
||||
|
|
|
|||
|
|
@ -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<JoltBody3D *>(p_jolt_body1.GetUserData());
|
||||
const JoltBody3D *body2 = reinterpret_cast<JoltBody3D *>(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
|
||||
|
|
|
|||
|
|
@ -85,7 +85,6 @@ class JoltContactListener3D final
|
|||
};
|
||||
|
||||
HashMap<JPH::SubShapeIDPair, Manifold, ShapePairHasher> manifolds_by_shape_pair;
|
||||
HashSet<JPH::BodyID, BodyIDHasher> listening_for;
|
||||
HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_overlaps;
|
||||
HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_enters;
|
||||
HashSet<JPH::SubShapeIDPair, ShapePairHasher> 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 @@ public:
|
|||
explicit JoltContactListener3D(JoltSpace3D *p_space) :
|
||||
space(p_space) {}
|
||||
|
||||
void listen_for(JoltShapedObject3D *p_object);
|
||||
|
||||
void pre_step();
|
||||
void post_step();
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
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);
|
||||
|
||||
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<JoltShapedObject3D *>(jolt_body->GetUserData());
|
||||
|
||||
object->pre_step(p_step, *jolt_body);
|
||||
|
||||
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<JoltObject3D *>(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();
|
||||
// WARNING: The list of active bodies may have changed between `pre_step` and `post_step`.
|
||||
|
||||
for (int i = 0; i < body_count; ++i) {
|
||||
if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
|
||||
if (jolt_body->IsSoftBody()) {
|
||||
continue;
|
||||
}
|
||||
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);
|
||||
|
||||
JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
|
||||
|
||||
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<JoltObject3D *>(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;
|
||||
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();
|
||||
}
|
||||
|
||||
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<JoltBody3D *>(jolt_body->GetUserData());
|
||||
body->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();
|
||||
}
|
||||
|
||||
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<JoltArea3D *>(jolt_body->GetUserData());
|
||||
area->call_queries(*jolt_body);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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<JoltBody3D> *p_body) {
|
||||
if (!p_body->in_list()) {
|
||||
body_call_queries_list.add(p_body);
|
||||
}
|
||||
}
|
||||
|
||||
void JoltSpace3D::enqueue_call_queries(SelfList<JoltArea3D> *p_area) {
|
||||
if (!p_area->in_list()) {
|
||||
area_call_queries_list.add(p_area);
|
||||
}
|
||||
}
|
||||
|
||||
void JoltSpace3D::dequeue_call_queries(SelfList<JoltBody3D> *p_body) {
|
||||
if (p_body->in_list()) {
|
||||
body_call_queries_list.remove(p_body);
|
||||
}
|
||||
}
|
||||
|
||||
void JoltSpace3D::dequeue_call_queries(SelfList<JoltArea3D> *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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@
|
|||
#include <stdint.h>
|
||||
|
||||
class JoltArea3D;
|
||||
class JoltBody3D;
|
||||
class JoltContactListener3D;
|
||||
class JoltJoint3D;
|
||||
class JoltLayers;
|
||||
|
|
@ -55,7 +56,8 @@ class JoltObject3D;
|
|||
class JoltPhysicsDirectSpaceState3D;
|
||||
|
||||
class JoltSpace3D {
|
||||
JoltBodyWriter3D body_accessor;
|
||||
SelfList<JoltBody3D>::List body_call_queries_list;
|
||||
SelfList<JoltArea3D>::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 @@ public:
|
|||
|
||||
void try_optimize();
|
||||
|
||||
void enqueue_call_queries(SelfList<JoltBody3D> *p_body);
|
||||
void enqueue_call_queries(SelfList<JoltArea3D> *p_area);
|
||||
void dequeue_call_queries(SelfList<JoltBody3D> *p_body);
|
||||
void dequeue_call_queries(SelfList<JoltArea3D> *p_area);
|
||||
|
||||
void add_joint(JPH::Constraint *p_jolt_ref);
|
||||
void add_joint(JoltJoint3D *p_joint);
|
||||
void remove_joint(JPH::Constraint *p_jolt_ref);
|
||||
|
|
|
|||
Loading…
Reference in New Issue