Merge pull request #119976 from almic/add-jolt-constraint-priority

Add `solver_priority` to Jolt constraint priority

doc/classes/Joint3D.xml

@@ -30,8 +30,10 @@
 			If left empty and [member node_a] is set, the body is attached to a fixed [StaticBody3D] without collision shapes.
 		</member>
 		<member name="solver_priority" type="int" setter="set_solver_priority" getter="get_solver_priority" default="1">
-			The priority used to define which solver is executed first for multiple joints. The lower the value, the higher the priority.
-			[b]Note:[/b] Only supported when using GodotPhysics3D. This property is ignored when using Jolt Physics.
+			The priority specifies how accurately a joint is solved. Generally, higher values improve accuracy. This has very different implementations between Godot Physics and Jolt Physics:
+			[b]Godot Physics:[/b] [i]Values above 1 have a performance impact[/i]. Joint is solved [code]max(1, solver_priority) * iterations[/code] times. A value of [code]4[/code] would solve the same joint [i]4x additional times per physics step[/i].
+			[b]Jolt Physics:[/b] Aside from sorting the joints, there is no performance impact with higher values. Joints with [i]high[/i] priorities are solved [i]later[/i]. A value of [code]4[/code] would solve [i]after priorities of 0, 1, 2, and 3[/i]. Later joints have the final say between the two bodies they connect.
+			Negative values are not allowed and will be silently [code]max(0, solver_priority)[/code] when set.
 		</member>
 	</members>
 </class>

modules/godot_physics_3d/godot_constraint_3d.h

@@ -67,7 +67,7 @@ public:
 	virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const { return nullptr; }
 	virtual int get_soft_body_count() const { return 0; }
 
-	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority = MAX(0, p_priority); }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
 	_FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; }

modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp

@@ -364,8 +364,8 @@ void JoltConeTwistJoint3D::rebuild() {
 
 	space->add_joint(this);
 
-	_update_enabled();
-	_update_iterations();
+	_update_joint();
+
 	_update_swing_motor_state();
 	_update_twist_motor_state();
 	_update_motor_velocity();

modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp

@@ -714,8 +714,7 @@ void JoltGeneric6DOFJoint3D::rebuild() {
 
 	space->add_joint(this);
 
-	_update_enabled();
-	_update_iterations();
+	_update_joint();
 
 	_update_limit_spring_parameters(AXIS_LINEAR_X);
 	_update_limit_spring_parameters(AXIS_LINEAR_Y);

modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp

@@ -412,8 +412,8 @@ void JoltHingeJoint3D::rebuild() {
 
 	space->add_joint(this);
 
-	_update_enabled();
-	_update_iterations();
+	_update_joint();
+
 	_update_motor_state();
 	_update_motor_velocity();
 	_update_motor_limit();

modules/jolt_physics/joints/jolt_joint_3d.cpp

@@ -36,12 +36,6 @@
 #include "../objects/jolt_body_3d.h"
 #include "../spaces/jolt_space_3d.h"
 
-namespace {
-
-constexpr int JOINT_DEFAULT_SOLVER_PRIORITY = 1;
-
-} // namespace
-
 void JoltJoint3D::_shift_reference_frames(const Vector3 &p_linear_shift, const Vector3 &p_angular_shift, Transform3D &r_shifted_ref_a, Transform3D &r_shifted_ref_b) {
 	Vector3 origin_a = local_ref_a.origin;
 	Vector3 origin_b = local_ref_b.origin;
@@ -89,6 +83,18 @@ void JoltJoint3D::_update_iterations() {
 	}
 }
 
+void JoltJoint3D::_update_priority() {
+	if (jolt_ref != nullptr) {
+		jolt_ref->SetConstraintPriority((JPH::uint)solver_priority);
+	}
+}
+
+void JoltJoint3D::_update_joint() {
+	_update_enabled();
+	_update_iterations();
+	_update_priority();
+}
+
 void JoltJoint3D::_enabled_changed() {
 	_update_enabled();
 	_wake_up_bodies();
@@ -173,14 +179,16 @@ void JoltJoint3D::set_enabled(bool p_enabled) {
 	_enabled_changed();
 }
 
-int JoltJoint3D::get_solver_priority() const {
-	return JOINT_DEFAULT_SOLVER_PRIORITY;
-}
-
 void JoltJoint3D::set_solver_priority(int p_priority) {
-	if (p_priority != JOINT_DEFAULT_SOLVER_PRIORITY) {
-		WARN_PRINT(vformat("Joint solver priority is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+	p_priority = MAX(0, p_priority);
+
+	if (solver_priority == p_priority) {
+		return;
 	}
+
+	solver_priority = p_priority;
+
+	_update_priority();
 }
 
 void JoltJoint3D::set_solver_velocity_iterations(int p_iterations) {

modules/jolt_physics/joints/jolt_joint_3d.h

@@ -46,6 +46,7 @@ protected:
 
 	int velocity_iterations = 0;
 	int position_iterations = 0;
+	int solver_priority = 1;
 
 	JPH::Ref<JPH::Constraint> jolt_ref;
 
@@ -63,6 +64,9 @@ protected:
 
 	void _update_enabled();
 	void _update_iterations();
+	void _update_priority();
+
+	void _update_joint();
 
 	void _enabled_changed();
 	void _iterations_changed();
@@ -86,7 +90,7 @@ public:
 	bool is_enabled() const { return enabled; }
 	void set_enabled(bool p_enabled);
 
-	int get_solver_priority() const;
+	int get_solver_priority() const { return solver_priority; }
 	void set_solver_priority(int p_priority);
 
 	int get_solver_velocity_iterations() const { return velocity_iterations; }

modules/jolt_physics/joints/jolt_pin_joint_3d.cpp

@@ -155,6 +155,5 @@ void JoltPinJoint3D::rebuild() {
 
 	space->add_joint(this);
 
-	_update_enabled();
-	_update_iterations();
+	_update_joint();
 }

modules/jolt_physics/joints/jolt_slider_joint_3d.cpp

@@ -525,8 +525,8 @@ void JoltSliderJoint3D::rebuild() {
 
 	space->add_joint(this);
 
-	_update_enabled();
-	_update_iterations();
+	_update_joint();
+
 	_update_motor_state();
 	_update_motor_velocity();
 	_update_motor_limit();