diff --git a/modules/jolt_physics/objects/jolt_area_3d.cpp b/modules/jolt_physics/objects/jolt_area_3d.cpp index eccd7c3854..eeaeca0faa 100644 --- a/modules/jolt_physics/objects/jolt_area_3d.cpp +++ b/modules/jolt_physics/objects/jolt_area_3d.cpp @@ -80,7 +80,7 @@ bool JoltArea3D::_has_pending_events() const { } void JoltArea3D::_add_to_space() { - jolt_shape = build_shape(); + jolt_shape = build_shapes(true); JPH::CollisionGroup::GroupID group_id = 0; JPH::CollisionGroup::SubGroupID sub_group_id = 0; @@ -97,7 +97,7 @@ void JoltArea3D::_add_to_space() { jolt_settings->mCollideKinematicVsNonDynamic = true; } - jolt_settings->SetShape(build_shape()); + jolt_settings->SetShape(jolt_shape); const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings); if (new_jolt_id.IsInvalid()) { diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp index 3768dc8d0a..d1a40e7b10 100644 --- a/modules/jolt_physics/objects/jolt_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_body_3d.cpp @@ -114,7 +114,7 @@ JPH::EMotionType JoltBody3D::_get_motion_type() const { } void JoltBody3D::_add_to_space() { - jolt_shape = build_shape(); + jolt_shape = build_shapes(true); JPH::CollisionGroup::GroupID group_id = 0; JPH::CollisionGroup::SubGroupID sub_group_id = 0; @@ -477,8 +477,8 @@ void JoltBody3D::_mode_changed() { wake_up(); } -void JoltBody3D::_shapes_built() { - JoltShapedObject3D::_shapes_built(); +void JoltBody3D::_shapes_committed() { + JoltShapedObject3D::_shapes_committed(); _update_mass_properties(); _update_joint_constraints(); diff --git a/modules/jolt_physics/objects/jolt_body_3d.h b/modules/jolt_physics/objects/jolt_body_3d.h index 8e5a149437..4f5d4006ed 100644 --- a/modules/jolt_physics/objects/jolt_body_3d.h +++ b/modules/jolt_physics/objects/jolt_body_3d.h @@ -139,7 +139,7 @@ private: void _exit_all_areas(); void _mode_changed(); - virtual void _shapes_built() override; + virtual void _shapes_committed() override; virtual void _space_changing() override; virtual void _space_changed() override; void _areas_changed(); diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp index 577e8157a5..6ceb77733b 100644 --- a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp +++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp @@ -36,6 +36,7 @@ #include "../spaces/jolt_space_3d.h" #include "Jolt/Physics/Collision/Shape/EmptyShape.h" +#include "Jolt/Physics/Collision/Shape/MutableCompoundShape.h" #include "Jolt/Physics/Collision/Shape/StaticCompoundShape.h" bool JoltShapedObject3D::_is_big() const { @@ -43,7 +44,7 @@ bool JoltShapedObject3D::_is_big() const { return get_aabb().get_longest_axis_size() >= 1000.0f; } -JPH::ShapeRefC JoltShapedObject3D::_try_build_shape() { +JPH::ShapeRefC JoltShapedObject3D::_try_build_shape(bool p_optimize_compound) { int built_shapes = 0; for (JoltShapeInstance3D &shape : shapes) { @@ -56,7 +57,7 @@ JPH::ShapeRefC JoltShapedObject3D::_try_build_shape() { return nullptr; } - JPH::ShapeRefC result = built_shapes == 1 ? _try_build_single_shape() : _try_build_compound_shape(); + JPH::ShapeRefC result = built_shapes == 1 ? _try_build_single_shape() : _try_build_compound_shape(p_optimize_compound); if (unlikely(result == nullptr)) { return nullptr; } @@ -106,8 +107,12 @@ JPH::ShapeRefC JoltShapedObject3D::_try_build_single_shape() { return nullptr; } -JPH::ShapeRefC JoltShapedObject3D::_try_build_compound_shape() { - JPH::StaticCompoundShapeSettings compound_shape_settings; +JPH::ShapeRefC JoltShapedObject3D::_try_build_compound_shape(bool p_optimize) { + JPH::StaticCompoundShapeSettings static_compound_shape_settings; + JPH::MutableCompoundShapeSettings mutable_compound_shape_settings; + JPH::CompoundShapeSettings *compound_shape_settings = p_optimize ? static_cast(&static_compound_shape_settings) : static_cast(&mutable_compound_shape_settings); + + compound_shape_settings->mSubShapes.reserve((size_t)shapes.size()); for (int shape_index = 0; shape_index < (int)shapes.size(); ++shape_index) { const JoltShapeInstance3D &sub_shape = shapes[shape_index]; @@ -122,27 +127,41 @@ JPH::ShapeRefC JoltShapedObject3D::_try_build_compound_shape() { const Transform3D sub_shape_transform = sub_shape.get_transform_unscaled(); if (sub_shape_scale != Vector3(1, 1, 1)) { - JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d in body '%s'.", shape_index, to_string())); + JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d for body '%s'.", shape_index, to_string())); jolt_sub_shape = JoltShape3D::with_scale(jolt_sub_shape, sub_shape_scale); } - compound_shape_settings.AddShape(to_jolt(sub_shape_transform.origin), to_jolt(sub_shape_transform.basis), jolt_sub_shape); + compound_shape_settings->AddShape(to_jolt(sub_shape_transform.origin), to_jolt(sub_shape_transform.basis), jolt_sub_shape); } - const JPH::ShapeSettings::ShapeResult shape_result = compound_shape_settings.Create(); - ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to create compound shape with sub-shape count '%d'. It returned the following error: '%s'.", (int)compound_shape_settings.mSubShapes.size(), to_godot(shape_result.GetError()))); + const JPH::ShapeSettings::ShapeResult shape_result = p_optimize ? static_compound_shape_settings.Create(space->get_temp_allocator()) : mutable_compound_shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to create compound shape for body '%s'. It returned the following error: '%s'.", to_string(), to_godot(shape_result.GetError()))); return shape_result.Get(); } +void JoltShapedObject3D::_enqueue_needs_optimization() { + if (!needs_optimization_element.in_list()) { + space->enqueue_needs_optimization(&needs_optimization_element); + } +} + +void JoltShapedObject3D::_dequeue_needs_optimization() { + if (needs_optimization_element.in_list()) { + space->dequeue_needs_optimization(&needs_optimization_element); + } +} + void JoltShapedObject3D::_shapes_changed() { - _update_shape(); + commit_shapes(false); _update_object_layer(); } void JoltShapedObject3D::_space_changing() { JoltObject3D::_space_changing(); + _dequeue_needs_optimization(); + if (space != nullptr) { const JoltWritableBody3D body = space->write_body(jolt_id); ERR_FAIL_COND(body.is_invalid()); @@ -151,30 +170,9 @@ void JoltShapedObject3D::_space_changing() { } } -void JoltShapedObject3D::_update_shape() { - if (!in_space()) { - _shapes_built(); - return; - } - - const JoltWritableBody3D body = space->write_body(jolt_id); - ERR_FAIL_COND(body.is_invalid()); - - 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(); -} - JoltShapedObject3D::JoltShapedObject3D(ObjectType p_object_type) : - JoltObject3D(p_object_type) { + JoltObject3D(p_object_type), + needs_optimization_element(this) { jolt_settings->mAllowSleeping = true; jolt_settings->mFriction = 1.0f; jolt_settings->mRestitution = 0.0f; @@ -286,8 +284,8 @@ AABB JoltShapedObject3D::get_aabb() const { return get_transform_scaled().xform(result); } -JPH::ShapeRefC JoltShapedObject3D::build_shape() { - JPH::ShapeRefC new_shape = _try_build_shape(); +JPH::ShapeRefC JoltShapedObject3D::build_shapes(bool p_optimize_compound) { + JPH::ShapeRefC new_shape = _try_build_shape(p_optimize_compound); if (new_shape == nullptr) { if (has_custom_center_of_mass()) { @@ -300,6 +298,34 @@ JPH::ShapeRefC JoltShapedObject3D::build_shape() { return new_shape; } +void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) { + if (!in_space()) { + _shapes_committed(); + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + JPH::ShapeRefC new_shape = build_shapes(p_optimize_compound); + 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); + + if (!p_optimize_compound && jolt_shape->GetType() == JPH::EShapeType::Compound) { + _enqueue_needs_optimization(); + } else { + _dequeue_needs_optimization(); + } + + _shapes_committed(); +} + void JoltShapedObject3D::add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled) { JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed when adding shape at index %d to physics body '%s'.", shapes.size(), to_string())); diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.h b/modules/jolt_physics/objects/jolt_shaped_object_3d.h index 9632746287..fee9f5f6b0 100644 --- a/modules/jolt_physics/objects/jolt_shaped_object_3d.h +++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.h @@ -33,6 +33,8 @@ #include "jolt_object_3d.h" +#include "core/templates/self_list.h" + #include "Jolt/Jolt.h" #include "Jolt/Physics/Body/Body.h" @@ -42,6 +44,8 @@ class JoltShapedObject3D : public JoltObject3D { friend class JoltShape3D; protected: + SelfList needs_optimization_element; + Vector3 scale = Vector3(1, 1, 1); JPH::ShapeRefC jolt_shape; @@ -53,16 +57,17 @@ protected: bool _is_big() const; - JPH::ShapeRefC _try_build_shape(); + JPH::ShapeRefC _try_build_shape(bool p_optimize_compound); JPH::ShapeRefC _try_build_single_shape(); - JPH::ShapeRefC _try_build_compound_shape(); + JPH::ShapeRefC _try_build_compound_shape(bool p_optimize); + + void _enqueue_needs_optimization(); + void _dequeue_needs_optimization(); virtual void _shapes_changed(); - virtual void _shapes_built() {} + virtual void _shapes_committed() {} virtual void _space_changing() override; - void _update_shape(); - public: explicit JoltShapedObject3D(ObjectType p_object_type); virtual ~JoltShapedObject3D() override; @@ -86,7 +91,9 @@ public: virtual bool has_custom_center_of_mass() const = 0; virtual Vector3 get_center_of_mass_custom() const = 0; - JPH::ShapeRefC build_shape(); + JPH::ShapeRefC build_shapes(bool p_optimize_compound); + + void commit_shapes(bool p_optimize_compound); const JPH::Shape *get_jolt_shape() const { return jolt_shape; } const JPH::Shape *get_previous_jolt_shape() const { return previous_jolt_shape; } diff --git a/modules/jolt_physics/spaces/jolt_space_3d.cpp b/modules/jolt_physics/spaces/jolt_space_3d.cpp index 78bdf778f5..06afe72a1d 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.cpp +++ b/modules/jolt_physics/spaces/jolt_space_3d.cpp @@ -63,6 +63,12 @@ constexpr double DEFAULT_SOLVER_ITERATIONS = 8; } // namespace void JoltSpace3D::_pre_step(float p_step) { + while (needs_optimization_list.first()) { + JoltShapedObject3D *object = needs_optimization_list.first()->self(); + needs_optimization_list.remove(needs_optimization_list.first()); + object->commit_shapes(true); + } + contact_listener->pre_step(); const JPH::BodyLockInterface &lock_iface = get_lock_iface(); @@ -427,6 +433,18 @@ void JoltSpace3D::dequeue_call_queries(SelfList *p_area) { } } +void JoltSpace3D::enqueue_needs_optimization(SelfList *p_object) { + if (!p_object->in_list()) { + needs_optimization_list.add(p_object); + } +} + +void JoltSpace3D::dequeue_needs_optimization(SelfList *p_object) { + if (p_object->in_list()) { + needs_optimization_list.remove(p_object); + } +} + 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 989df9a498..c12b5c5f0f 100644 --- a/modules/jolt_physics/spaces/jolt_space_3d.h +++ b/modules/jolt_physics/spaces/jolt_space_3d.h @@ -54,10 +54,12 @@ class JoltJoint3D; class JoltLayers; class JoltObject3D; class JoltPhysicsDirectSpaceState3D; +class JoltShapedObject3D; class JoltSpace3D { SelfList::List body_call_queries_list; SelfList::List area_call_queries_list; + SelfList::List needs_optimization_list; RID rid; @@ -100,6 +102,8 @@ public: JPH::PhysicsSystem &get_physics_system() const { return *physics_system; } + JPH::TempAllocator &get_temp_allocator() const { return *temp_allocator; } + JPH::BodyInterface &get_body_iface(); const JPH::BodyInterface &get_body_iface() const; const JPH::BodyLockInterface &get_lock_iface() const; @@ -138,6 +142,9 @@ public: void dequeue_call_queries(SelfList *p_body); void dequeue_call_queries(SelfList *p_area); + void enqueue_needs_optimization(SelfList *p_object); + void dequeue_needs_optimization(SelfList *p_object); + void add_joint(JPH::Constraint *p_jolt_ref); void add_joint(JoltJoint3D *p_joint); void remove_joint(JPH::Constraint *p_jolt_ref);