Improve performance of changing compound shapes when using Jolt Physics

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

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();

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();

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<JPH::CompoundShapeSettings *>(&static_compound_shape_settings) : static_cast<JPH::CompoundShapeSettings *>(&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()));
 

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

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<JoltArea3D> *p_area) {
 	}
 }
 
+void JoltSpace3D::enqueue_needs_optimization(SelfList<JoltShapedObject3D> *p_object) {
+	if (!p_object->in_list()) {
+		needs_optimization_list.add(p_object);
+	}
+}
+
+void JoltSpace3D::dequeue_needs_optimization(SelfList<JoltShapedObject3D> *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);
 }

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<JoltBody3D>::List body_call_queries_list;
 	SelfList<JoltArea3D>::List area_call_queries_list;
+	SelfList<JoltShapedObject3D>::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<JoltBody3D> *p_body);
 	void dequeue_call_queries(SelfList<JoltArea3D> *p_area);
 
+	void enqueue_needs_optimization(SelfList<JoltShapedObject3D> *p_object);
+	void dequeue_needs_optimization(SelfList<JoltShapedObject3D> *p_object);
+
 	void add_joint(JPH::Constraint *p_jolt_ref);
 	void add_joint(JoltJoint3D *p_joint);
 	void remove_joint(JPH::Constraint *p_jolt_ref);