Add collision weight to PhysicsBody for penetrations must be avoided

Co-authored-by: Juan Linietsky <reduzio@gmail.com>

30 files changed, 213 insertions, 2 deletions

diff --git a/doc/classes/CollisionObject2D.xml b/doc/classes/CollisionObject2D.xml
index 4353d97597..832f47e2bb 100644
--- a/doc/classes/CollisionObject2D.xml
+++ b/doc/classes/CollisionObject2D.xml
@@ -206,6 +206,9 @@
 			The physics layers this CollisionObject2D scans. Collision objects can scan one or more of 32 different layers. See also [member collision_layer].
 			[b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=$DOCS_URL/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information.
 		</member>
+		<member name="collision_priority" type="float" setter="set_collision_priority" getter="get_collision_priority" default="1.0">
+			The priority used to solve colliding when occurring penetration. The higher the priority is, the lower the penetration into the object will be. This can for example be used to prevent the player from breaking through the boundaries of a level.
+		</member>
 		<member name="disable_mode" type="int" setter="set_disable_mode" getter="get_disable_mode" enum="CollisionObject2D.DisableMode" default="0">
 			Defines the behavior in physics when [member Node.process_mode] is set to [constant Node.PROCESS_MODE_DISABLED]. See [enum DisableMode] for more details about the different modes.
 		</member>
diff --git a/doc/classes/CollisionObject3D.xml b/doc/classes/CollisionObject3D.xml
index f9e28824df..04ccf3fc62 100644
--- a/doc/classes/CollisionObject3D.xml
+++ b/doc/classes/CollisionObject3D.xml
@@ -177,6 +177,9 @@
 			The physics layers this CollisionObject3D [b]scans[/b]. Collision objects can scan one or more of 32 different layers. See also [member collision_layer].
 			[b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=$DOCS_URL/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information.
 		</member>
+		<member name="collision_priority" type="float" setter="set_collision_priority" getter="get_collision_priority" default="1.0">
+			The priority used to solve colliding when occurring penetration. The higher the priority is, the lower the penetration into the object will be. This can for example be used to prevent the player from breaking through the boundaries of a level.
+		</member>
 		<member name="disable_mode" type="int" setter="set_disable_mode" getter="get_disable_mode" enum="CollisionObject3D.DisableMode" default="0">
 			Defines the behavior in physics when [member Node.process_mode] is set to [constant Node.PROCESS_MODE_DISABLED]. See [enum DisableMode] for more details about the different modes.
 		</member>
diff --git a/doc/classes/PhysicsServer2D.xml b/doc/classes/PhysicsServer2D.xml
index d7b5f24f4a..921c2ecf7e 100644
--- a/doc/classes/PhysicsServer2D.xml
+++ b/doc/classes/PhysicsServer2D.xml
@@ -358,6 +358,13 @@
 				Returns the physics layer or layers a body can collide with.
 			</description>
 		</method>
+		<method name="body_get_collision_priority" qualifiers="const">
+			<return type="float" />
+			<param index="0" name="body" type="RID" />
+			<description>
+				Returns the body's collision priority.
+			</description>
+		</method>
 		<method name="body_get_constant_force" qualifiers="const">
 			<return type="Vector2" />
 			<param index="0" name="body" type="RID" />
@@ -509,6 +516,14 @@
 				Sets the physics layer or layers a body can collide with.
 			</description>
 		</method>
+		<method name="body_set_collision_priority">
+			<return type="void" />
+			<param index="0" name="body" type="RID" />
+			<param index="1" name="priority" type="float" />
+			<description>
+				Sets the body's collision priority.
+			</description>
+		</method>
 		<method name="body_set_constant_force">
 			<return type="void" />
 			<param index="0" name="body" type="RID" />
diff --git a/doc/classes/PhysicsServer3D.xml b/doc/classes/PhysicsServer3D.xml
index c5456f7536..ac9dfbf3f8 100644
--- a/doc/classes/PhysicsServer3D.xml
+++ b/doc/classes/PhysicsServer3D.xml
@@ -338,6 +338,13 @@
 				Returns the physics layer or layers a body can collide with.
 			</description>
 		</method>
+		<method name="body_get_collision_priority" qualifiers="const">
+			<return type="float" />
+			<param index="0" name="body" type="RID" />
+			<description>
+				Returns the body's collision priority.
+			</description>
+		</method>
 		<method name="body_get_constant_force" qualifiers="const">
 			<return type="Vector3" />
 			<param index="0" name="body" type="RID" />
@@ -505,6 +512,14 @@
 				Sets the physics layer or layers a body can collide with.
 			</description>
 		</method>
+		<method name="body_set_collision_priority">
+			<return type="void" />
+			<param index="0" name="body" type="RID" />
+			<param index="1" name="priority" type="float" />
+			<description>
+				Sets the body's collision priority.
+			</description>
+		</method>
 		<method name="body_set_constant_force">
 			<return type="void" />
 			<param index="0" name="body" type="RID" />
diff --git a/doc/classes/PhysicsServer3DExtension.xml b/doc/classes/PhysicsServer3DExtension.xml
index 4188b04e4a..200065de54 100644
--- a/doc/classes/PhysicsServer3DExtension.xml
+++ b/doc/classes/PhysicsServer3DExtension.xml
@@ -286,6 +286,12 @@
 			<description>
 			</description>
 		</method>
+		<method name="_body_get_collision_priority" qualifiers="virtual const">
+			<return type="float" />
+			<param index="0" name="body" type="RID" />
+			<description>
+			</description>
+		</method>
 		<method name="_body_get_constant_force" qualifiers="virtual const">
 			<return type="Vector3" />
 			<param index="0" name="body" type="RID" />
@@ -430,6 +436,13 @@
 			<description>
 			</description>
 		</method>
+		<method name="_body_set_collision_priority" qualifiers="virtual">
+			<return type="void" />
+			<param index="0" name="body" type="RID" />
+			<param index="1" name="priority" type="float" />
+			<description>
+			</description>
+		</method>
 		<method name="_body_set_constant_force" qualifiers="virtual">
 			<return type="void" />
 			<param index="0" name="body" type="RID" />
diff --git a/modules/csg/csg_shape.cpp b/modules/csg/csg_shape.cpp
index 56be4e65f0..fd6183c7e0 100644
--- a/modules/csg/csg_shape.cpp
+++ b/modules/csg/csg_shape.cpp
@@ -53,6 +53,7 @@ void CSGShape3D::set_use_collision(bool p_enable) {
 		PhysicsServer3D::get_singleton()->body_attach_object_instance_id(root_collision_instance, get_instance_id());
 		set_collision_layer(collision_layer);
 		set_collision_mask(collision_mask);
+		set_collision_priority(collision_priority);
 		_make_dirty(); //force update
 	} else {
 		PhysicsServer3D::get_singleton()->free(root_collision_instance);
@@ -124,6 +125,17 @@ bool CSGShape3D::get_collision_mask_value(int p_layer_number) const {
 	return get_collision_mask() & (1 << (p_layer_number - 1));
 }
 
+void CSGShape3D::set_collision_priority(real_t p_priority) {
+	collision_priority = p_priority;
+	if (root_collision_instance.is_valid()) {
+		PhysicsServer3D::get_singleton()->body_set_collision_priority(root_collision_instance, p_priority);
+	}
+}
+
+real_t CSGShape3D::get_collision_priority() const {
+	return collision_priority;
+}
+
 bool CSGShape3D::is_root_shape() const {
 	return !parent_shape;
 }
@@ -545,6 +557,7 @@ void CSGShape3D::_notification(int p_what) {
 				PhysicsServer3D::get_singleton()->body_attach_object_instance_id(root_collision_instance, get_instance_id());
 				set_collision_layer(collision_layer);
 				set_collision_mask(collision_mask);
+				set_collision_priority(collision_priority);
 				_update_collision_faces();
 			}
 		} break;
@@ -632,6 +645,9 @@ void CSGShape3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_collision_layer_value", "layer_number", "value"), &CSGShape3D::set_collision_layer_value);
 	ClassDB::bind_method(D_METHOD("get_collision_layer_value", "layer_number"), &CSGShape3D::get_collision_layer_value);
 
+	ClassDB::bind_method(D_METHOD("set_collision_priority", "priority"), &CSGShape3D::set_collision_priority);
+	ClassDB::bind_method(D_METHOD("get_collision_priority"), &CSGShape3D::get_collision_priority);
+
 	ClassDB::bind_method(D_METHOD("set_calculate_tangents", "enabled"), &CSGShape3D::set_calculate_tangents);
 	ClassDB::bind_method(D_METHOD("is_calculating_tangents"), &CSGShape3D::is_calculating_tangents);
 
@@ -645,6 +661,7 @@ void CSGShape3D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_collision"), "set_use_collision", "is_using_collision");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_layer", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_layer", "get_collision_layer");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_priority"), "set_collision_priority", "get_collision_priority");
 
 	BIND_ENUM_CONSTANT(OPERATION_UNION);
 	BIND_ENUM_CONSTANT(OPERATION_INTERSECTION);
diff --git a/modules/csg/csg_shape.h b/modules/csg/csg_shape.h
index 0b49dc4609..65658ce68b 100644
--- a/modules/csg/csg_shape.h
+++ b/modules/csg/csg_shape.h
@@ -65,6 +65,7 @@ private:
 	bool use_collision = false;
 	uint32_t collision_layer = 1;
 	uint32_t collision_mask = 1;
+	real_t collision_priority = 1.0;
 	Ref<ConcavePolygonShape3D> root_collision_shape;
 	RID root_collision_instance;
 
@@ -144,6 +145,9 @@ public:
 	void set_collision_mask_value(int p_layer_number, bool p_value);
 	bool get_collision_mask_value(int p_layer_number) const;
 
+	void set_collision_priority(real_t p_priority);
+	real_t get_collision_priority() const;
+
 	void set_snap(float p_snap);
 	float get_snap() const;
 
diff --git a/modules/csg/doc_classes/CSGShape3D.xml b/modules/csg/doc_classes/CSGShape3D.xml
index 4fd4a12a1d..7e92d667b3 100644
--- a/modules/csg/doc_classes/CSGShape3D.xml
+++ b/modules/csg/doc_classes/CSGShape3D.xml
@@ -66,6 +66,8 @@
 		<member name="collision_mask" type="int" setter="set_collision_mask" getter="get_collision_mask" default="1">
 			The physics layers this CSG shape scans for collisions. See [url=$DOCS_URL/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information.
 		</member>
+		<member name="collision_priority" type="float" setter="set_collision_priority" getter="get_collision_priority" default="1.0">
+		</member>
 		<member name="operation" type="int" setter="set_operation" getter="get_operation" enum="CSGShape3D.Operation" default="0">
 			The operation that is performed on this shape. This is ignored for the first CSG child node as the operation is between this node and the previous child of this nodes parent.
 		</member>
diff --git a/scene/2d/collision_object_2d.cpp b/scene/2d/collision_object_2d.cpp
index a8c12f4893..7f991df36c 100644
--- a/scene/2d/collision_object_2d.cpp
+++ b/scene/2d/collision_object_2d.cpp
@@ -186,6 +186,17 @@ bool CollisionObject2D::get_collision_mask_value(int p_layer_number) const {
 	return get_collision_mask() & (1 << (p_layer_number - 1));
 }
 
+void CollisionObject2D::set_collision_priority(real_t p_priority) {
+	collision_priority = p_priority;
+	if (!area) {
+		PhysicsServer2D::get_singleton()->body_set_collision_priority(get_rid(), p_priority);
+	}
+}
+
+real_t CollisionObject2D::get_collision_priority() const {
+	return collision_priority;
+}
+
 void CollisionObject2D::set_disable_mode(DisableMode p_mode) {
 	if (disable_mode == p_mode) {
 		return;
@@ -574,6 +585,8 @@ void CollisionObject2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_collision_layer_value", "layer_number"), &CollisionObject2D::get_collision_layer_value);
 	ClassDB::bind_method(D_METHOD("set_collision_mask_value", "layer_number", "value"), &CollisionObject2D::set_collision_mask_value);
 	ClassDB::bind_method(D_METHOD("get_collision_mask_value", "layer_number"), &CollisionObject2D::get_collision_mask_value);
+	ClassDB::bind_method(D_METHOD("set_collision_priority", "priority"), &CollisionObject2D::set_collision_priority);
+	ClassDB::bind_method(D_METHOD("get_collision_priority"), &CollisionObject2D::get_collision_priority);
 	ClassDB::bind_method(D_METHOD("set_disable_mode", "mode"), &CollisionObject2D::set_disable_mode);
 	ClassDB::bind_method(D_METHOD("get_disable_mode"), &CollisionObject2D::get_disable_mode);
 	ClassDB::bind_method(D_METHOD("set_pickable", "enabled"), &CollisionObject2D::set_pickable);
@@ -611,6 +624,7 @@ void CollisionObject2D::_bind_methods() {
 	ADD_GROUP("Collision", "collision_");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_layer", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_collision_layer", "get_collision_layer");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_collision_mask", "get_collision_mask");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_priority"), "set_collision_priority", "get_collision_priority");
 
 	ADD_GROUP("Input", "input_");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "input_pickable"), "set_pickable", "is_pickable");
diff --git a/scene/2d/collision_object_2d.h b/scene/2d/collision_object_2d.h
index 997afee6c4..f03c6fc72e 100644
--- a/scene/2d/collision_object_2d.h
+++ b/scene/2d/collision_object_2d.h
@@ -49,6 +49,7 @@ public:
 private:
 	uint32_t collision_layer = 1;
 	uint32_t collision_mask = 1;
+	real_t collision_priority = 1.0;
 
 	bool area = false;
 	RID rid;
@@ -115,6 +116,9 @@ public:
 	void set_collision_mask_value(int p_layer_number, bool p_value);
 	bool get_collision_mask_value(int p_layer_number) const;
 
+	void set_collision_priority(real_t p_priority);
+	real_t get_collision_priority() const;
+
 	void set_disable_mode(DisableMode p_mode);
 	DisableMode get_disable_mode() const;
 
diff --git a/scene/2d/physical_bone_2d.cpp b/scene/2d/physical_bone_2d.cpp
index 2999736d64..62f4d855ef 100644
--- a/scene/2d/physical_bone_2d.cpp
+++ b/scene/2d/physical_bone_2d.cpp
@@ -158,6 +158,7 @@ void PhysicalBone2D::_start_physics_simulation() {
 	// Apply the layers and masks.
 	PhysicsServer2D::get_singleton()->body_set_collision_layer(get_rid(), get_collision_layer());
 	PhysicsServer2D::get_singleton()->body_set_collision_mask(get_rid(), get_collision_mask());
+	PhysicsServer2D::get_singleton()->body_set_collision_priority(get_rid(), get_collision_priority());
 
 	// Apply the correct mode.
 	_apply_body_mode();
@@ -176,6 +177,7 @@ void PhysicalBone2D::_stop_physics_simulation() {
 		set_physics_process_internal(false);
 		PhysicsServer2D::get_singleton()->body_set_collision_layer(get_rid(), 0);
 		PhysicsServer2D::get_singleton()->body_set_collision_mask(get_rid(), 0);
+		PhysicsServer2D::get_singleton()->body_set_collision_priority(get_rid(), 1.0);
 		PhysicsServer2D::get_singleton()->body_set_mode(get_rid(), PhysicsServer2D::BodyMode::BODY_MODE_STATIC);
 	}
 }
diff --git a/scene/3d/collision_object_3d.cpp b/scene/3d/collision_object_3d.cpp
index 9a5d4f5480..22de9a47a2 100644
--- a/scene/3d/collision_object_3d.cpp
+++ b/scene/3d/collision_object_3d.cpp
@@ -183,6 +183,17 @@ bool CollisionObject3D::get_collision_mask_value(int p_layer_number) const {
 	return get_collision_mask() & (1 << (p_layer_number - 1));
 }
 
+void CollisionObject3D::set_collision_priority(real_t p_priority) {
+	collision_priority = p_priority;
+	if (!area) {
+		PhysicsServer3D::get_singleton()->body_set_collision_priority(get_rid(), p_priority);
+	}
+}
+
+real_t CollisionObject3D::get_collision_priority() const {
+	return collision_priority;
+}
+
 void CollisionObject3D::set_disable_mode(DisableMode p_mode) {
 	if (disable_mode == p_mode) {
 		return;
@@ -432,6 +443,8 @@ void CollisionObject3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_collision_layer_value", "layer_number"), &CollisionObject3D::get_collision_layer_value);
 	ClassDB::bind_method(D_METHOD("set_collision_mask_value", "layer_number", "value"), &CollisionObject3D::set_collision_mask_value);
 	ClassDB::bind_method(D_METHOD("get_collision_mask_value", "layer_number"), &CollisionObject3D::get_collision_mask_value);
+	ClassDB::bind_method(D_METHOD("set_collision_priority", "priority"), &CollisionObject3D::set_collision_priority);
+	ClassDB::bind_method(D_METHOD("get_collision_priority"), &CollisionObject3D::get_collision_priority);
 	ClassDB::bind_method(D_METHOD("set_disable_mode", "mode"), &CollisionObject3D::set_disable_mode);
 	ClassDB::bind_method(D_METHOD("get_disable_mode"), &CollisionObject3D::get_disable_mode);
 	ClassDB::bind_method(D_METHOD("set_ray_pickable", "ray_pickable"), &CollisionObject3D::set_ray_pickable);
@@ -466,6 +479,7 @@ void CollisionObject3D::_bind_methods() {
 	ADD_GROUP("Collision", "collision_");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_layer", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_layer", "get_collision_layer");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_priority"), "set_collision_priority", "get_collision_priority");
 
 	ADD_GROUP("Input", "input_");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "input_ray_pickable"), "set_ray_pickable", "is_ray_pickable");
diff --git a/scene/3d/collision_object_3d.h b/scene/3d/collision_object_3d.h
index 3ec3aa0fc1..3f0d070db4 100644
--- a/scene/3d/collision_object_3d.h
+++ b/scene/3d/collision_object_3d.h
@@ -47,6 +47,7 @@ public:
 private:
 	uint32_t collision_layer = 1;
 	uint32_t collision_mask = 1;
+	real_t collision_priority = 1.0;
 
 	bool area = false;
 
@@ -125,6 +126,9 @@ public:
 	void set_collision_mask_value(int p_layer_number, bool p_value);
 	bool get_collision_mask_value(int p_layer_number) const;
 
+	void set_collision_priority(real_t p_priority);
+	real_t get_collision_priority() const;
+
 	void set_disable_mode(DisableMode p_mode);
 	DisableMode get_disable_mode() const;
 
diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index cbdef02826..eba6bed54a 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -3417,6 +3417,7 @@ void PhysicalBone3D::_start_physics_simulation() {
 	set_body_mode(PhysicsServer3D::BODY_MODE_DYNAMIC);
 	PhysicsServer3D::get_singleton()->body_set_collision_layer(get_rid(), get_collision_layer());
 	PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), get_collision_mask());
+	PhysicsServer3D::get_singleton()->body_set_collision_priority(get_rid(), get_collision_priority());
 	PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback);
 	set_as_top_level(true);
 	_internal_simulate_physics = true;
@@ -3430,10 +3431,12 @@ void PhysicalBone3D::_stop_physics_simulation() {
 		set_body_mode(PhysicsServer3D::BODY_MODE_KINEMATIC);
 		PhysicsServer3D::get_singleton()->body_set_collision_layer(get_rid(), get_collision_layer());
 		PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), get_collision_mask());
+		PhysicsServer3D::get_singleton()->body_set_collision_priority(get_rid(), get_collision_priority());
 	} else {
 		set_body_mode(PhysicsServer3D::BODY_MODE_STATIC);
 		PhysicsServer3D::get_singleton()->body_set_collision_layer(get_rid(), 0);
 		PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), 0);
+		PhysicsServer3D::get_singleton()->body_set_collision_priority(get_rid(), 1.0);
 	}
 	if (_internal_simulate_physics) {
 		PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
diff --git a/servers/extensions/physics_server_3d_extension.cpp b/servers/extensions/physics_server_3d_extension.cpp
index 3694dcdb9a..7d797bf611 100644
--- a/servers/extensions/physics_server_3d_extension.cpp
+++ b/servers/extensions/physics_server_3d_extension.cpp
@@ -196,6 +196,9 @@ void PhysicsServer3DExtension::_bind_methods() {
 	GDVIRTUAL_BIND(_body_set_collision_mask, "body", "mask");
 	GDVIRTUAL_BIND(_body_get_collision_mask, "body");
 
+	GDVIRTUAL_BIND(_body_set_collision_priority, "body", "priority");
+	GDVIRTUAL_BIND(_body_get_collision_priority, "body");
+
 	GDVIRTUAL_BIND(_body_add_shape, "body", "shape", "transform", "disabled");
 	GDVIRTUAL_BIND(_body_set_shape, "body", "shape_idx", "shape");
 	GDVIRTUAL_BIND(_body_set_shape_transform, "body", "shape_idx", "transform");
diff --git a/servers/extensions/physics_server_3d_extension.h b/servers/extensions/physics_server_3d_extension.h
index c4b4a00eaf..3200438253 100644
--- a/servers/extensions/physics_server_3d_extension.h
+++ b/servers/extensions/physics_server_3d_extension.h
@@ -319,6 +319,9 @@ public:
 	EXBIND2(body_set_collision_mask, RID, uint32_t)
 	EXBIND1RC(uint32_t, body_get_collision_mask, RID)
 
+	EXBIND2(body_set_collision_priority, RID, real_t)
+	EXBIND1RC(real_t, body_get_collision_priority, RID)
+
 	EXBIND2(body_set_user_flags, RID, uint32_t)
 	EXBIND1RC(uint32_t, body_get_user_flags, RID)
 
diff --git a/servers/physics_2d/godot_collision_object_2d.h b/servers/physics_2d/godot_collision_object_2d.h
index 1a683a7b0f..7965e8a94d 100644
--- a/servers/physics_2d/godot_collision_object_2d.h
+++ b/servers/physics_2d/godot_collision_object_2d.h
@@ -70,6 +70,7 @@ private:
 	Transform2D inv_transform;
 	uint32_t collision_mask = 1;
 	uint32_t collision_layer = 1;
+	real_t collision_priority = 1.0;
 	bool _static = true;
 
 	SelfList<GodotCollisionObject2D> pending_shape_update_list;
@@ -166,6 +167,13 @@ public:
 	}
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
 
+	_FORCE_INLINE_ void set_collision_priority(real_t p_priority) {
+		ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0.");
+		collision_priority = p_priority;
+		_shape_changed();
+	}
+	_FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; }
+
 	void remove_shape(GodotShape2D *p_shape) override;
 	void remove_shape(int p_index);
 
diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp
index 99e68de07c..c728dccd4f 100644
--- a/servers/physics_2d/godot_physics_server_2d.cpp
+++ b/servers/physics_2d/godot_physics_server_2d.cpp
@@ -718,6 +718,20 @@ uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const {
 	return body->get_collision_mask();
 }
 
+void GodotPhysicsServer2D::body_set_collision_priority(RID p_body, real_t p_priority) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_priority(p_priority);
+}
+
+real_t GodotPhysicsServer2D::body_get_collision_priority(RID p_body) const {
+	const GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_priority();
+}
+
 void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
 	GodotBody2D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_COND(!body);
diff --git a/servers/physics_2d/godot_physics_server_2d.h b/servers/physics_2d/godot_physics_server_2d.h
index 2af6e5c97c..20e492d87a 100644
--- a/servers/physics_2d/godot_physics_server_2d.h
+++ b/servers/physics_2d/godot_physics_server_2d.h
@@ -199,6 +199,9 @@ public:
 	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
 	virtual uint32_t body_get_collision_mask(RID p_body) const override;
 
+	virtual void body_set_collision_priority(RID p_body, real_t p_priority) override;
+	virtual real_t body_get_collision_priority(RID p_body) const override;
+
 	virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
 	virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
 
diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp
index 166ec3049e..4166191be8 100644
--- a/servers/physics_2d/godot_space_2d.cpp
+++ b/servers/physics_2d/godot_space_2d.cpp
@@ -594,6 +594,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
 		const int max_results = 32;
 		int recover_attempts = 4;
 		Vector2 sr[max_results * 2];
+		real_t priorities[max_results];
 
 		do {
 			GodotPhysicsServer2D::CollCbkData cbk;
@@ -606,6 +607,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
 
 			GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
 			GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
+			int priority_amount = 0;
 
 			bool collided = false;
 
@@ -664,6 +666,10 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
 					if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) {
 						did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
 					}
+					while (cbk.amount > priority_amount) {
+						priorities[priority_amount] = col_obj->get_collision_priority();
+						priority_amount++;
+					}
 
 					if (!did_collide && cbk.invalid_by_dir > 0) {
 						//this shape must be excluded
@@ -686,6 +692,12 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
 				break;
 			}
 
+			real_t inv_total_weight = 0.0;
+			for (int i = 0; i < cbk.amount; i++) {
+				inv_total_weight += priorities[i];
+			}
+			inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight;
+
 			recovered = true;
 
 			Vector2 recover_motion;
@@ -701,7 +713,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
 				real_t depth = n.dot(a + recover_motion) - d;
 				if (depth > min_contact_depth + CMP_EPSILON) {
 					// Only recover if there is penetration.
-					recover_motion -= n * (depth - min_contact_depth) * 0.4;
+					recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight;
 				}
 			}
 
diff --git a/servers/physics_3d/godot_collision_object_3d.h b/servers/physics_3d/godot_collision_object_3d.h
index 0f09f21962..2d342f65f3 100644
--- a/servers/physics_3d/godot_collision_object_3d.h
+++ b/servers/physics_3d/godot_collision_object_3d.h
@@ -59,6 +59,7 @@ private:
 	ObjectID instance_id;
 	uint32_t collision_layer = 1;
 	uint32_t collision_mask = 1;
+	real_t collision_priority = 1.0;
 
 	struct Shape {
 		Transform3D xform;
@@ -165,6 +166,13 @@ public:
 	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
+	_FORCE_INLINE_ void set_collision_priority(real_t p_priority) {
+		ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0.");
+		collision_priority = p_priority;
+		_shape_changed();
+	}
+	_FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; }
+
 	_FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const {
 		return p_other->collision_layer & collision_mask;
 	}
diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
index b735283ebe..9c1535f561 100644
--- a/servers/physics_3d/godot_physics_server_3d.cpp
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -593,6 +593,20 @@ uint32_t GodotPhysicsServer3D::body_get_collision_mask(RID p_body) const {
 	return body->get_collision_mask();
 }
 
+void GodotPhysicsServer3D::body_set_collision_priority(RID p_body, real_t p_priority) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_priority(p_priority);
+}
+
+real_t GodotPhysicsServer3D::body_get_collision_priority(RID p_body) const {
+	const GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_priority();
+}
+
 void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	if (body) {
diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h
index 1d57451925..b429f23a0c 100644
--- a/servers/physics_3d/godot_physics_server_3d.h
+++ b/servers/physics_3d/godot_physics_server_3d.h
@@ -192,6 +192,9 @@ public:
 	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
 	virtual uint32_t body_get_collision_mask(RID p_body) const override;
 
+	virtual void body_set_collision_priority(RID p_body, real_t p_priority) override;
+	virtual real_t body_get_collision_priority(RID p_body) const override;
+
 	virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override;
 	virtual uint32_t body_get_user_flags(RID p_body) const override;
 
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp
index 13e9a89b2e..074232dd66 100644
--- a/servers/physics_3d/godot_space_3d.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -701,6 +701,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 		const int max_results = 32;
 		int recover_attempts = 4;
 		Vector3 sr[max_results * 2];
+		real_t priorities[max_results];
 
 		do {
 			GodotPhysicsServer3D::CollCbkData cbk;
@@ -710,6 +711,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 
 			GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk;
 			GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk;
+			int priority_amount = 0;
 
 			bool collided = false;
 
@@ -737,6 +739,10 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 					if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, margin)) {
 						collided = cbk.amount > 0;
 					}
+					while (cbk.amount > priority_amount) {
+						priorities[priority_amount] = col_obj->get_collision_priority();
+						priority_amount++;
+					}
 				}
 			}
 
@@ -744,6 +750,12 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 				break;
 			}
 
+			real_t inv_total_weight = 0.0;
+			for (int i = 0; i < cbk.amount; i++) {
+				inv_total_weight += priorities[i];
+			}
+			inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight;
+
 			recovered = true;
 
 			Vector3 recover_motion;
@@ -759,7 +771,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 				real_t depth = n.dot(a + recover_motion) - d;
 				if (depth > min_contact_depth + CMP_EPSILON) {
 					// Only recover if there is penetration.
-					recover_motion -= n * (depth - min_contact_depth) * 0.4;
+					recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight;
 				}
 			}
 
diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp
index 26768e300c..ee6764d8e1 100644
--- a/servers/physics_server_2d.cpp
+++ b/servers/physics_server_2d.cpp
@@ -706,6 +706,9 @@ void PhysicsServer2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("body_set_collision_mask", "body", "mask"), &PhysicsServer2D::body_set_collision_mask);
 	ClassDB::bind_method(D_METHOD("body_get_collision_mask", "body"), &PhysicsServer2D::body_get_collision_mask);
 
+	ClassDB::bind_method(D_METHOD("body_set_collision_priority", "body", "priority"), &PhysicsServer2D::body_set_collision_priority);
+	ClassDB::bind_method(D_METHOD("body_get_collision_priority", "body"), &PhysicsServer2D::body_get_collision_priority);
+
 	ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &PhysicsServer2D::body_set_param);
 	ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &PhysicsServer2D::body_get_param);
 
diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h
index 6d95c591c2..df8b641ffc 100644
--- a/servers/physics_server_2d.h
+++ b/servers/physics_server_2d.h
@@ -393,6 +393,9 @@ public:
 	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) = 0;
 	virtual uint32_t body_get_collision_mask(RID p_body) const = 0;
 
+	virtual void body_set_collision_priority(RID p_body, real_t p_priority) = 0;
+	virtual real_t body_get_collision_priority(RID p_body) const = 0;
+
 	// common body variables
 	enum BodyParameter {
 		BODY_PARAM_BOUNCE,
diff --git a/servers/physics_server_2d_wrap_mt.h b/servers/physics_server_2d_wrap_mt.h
index ddb071f603..d080aac438 100644
--- a/servers/physics_server_2d_wrap_mt.h
+++ b/servers/physics_server_2d_wrap_mt.h
@@ -205,6 +205,9 @@ public:
 	FUNC2(body_set_collision_mask, RID, uint32_t);
 	FUNC1RC(uint32_t, body_get_collision_mask, RID);
 
+	FUNC2(body_set_collision_priority, RID, real_t);
+	FUNC1RC(real_t, body_get_collision_priority, RID);
+
 	FUNC3(body_set_param, RID, BodyParameter, const Variant &);
 	FUNC2RC(Variant, body_get_param, RID, BodyParameter);
 
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index f25db22e66..c985df83b2 100644
--- a/servers/physics_server_3d.cpp
+++ b/servers/physics_server_3d.cpp
@@ -750,6 +750,9 @@ void PhysicsServer3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("body_set_collision_mask", "body", "mask"), &PhysicsServer3D::body_set_collision_mask);
 	ClassDB::bind_method(D_METHOD("body_get_collision_mask", "body"), &PhysicsServer3D::body_get_collision_mask);
 
+	ClassDB::bind_method(D_METHOD("body_set_collision_priority", "body", "priority"), &PhysicsServer3D::body_set_collision_priority);
+	ClassDB::bind_method(D_METHOD("body_get_collision_priority", "body"), &PhysicsServer3D::body_get_collision_priority);
+
 	ClassDB::bind_method(D_METHOD("body_add_shape", "body", "shape", "transform", "disabled"), &PhysicsServer3D::body_add_shape, DEFVAL(Transform3D()), DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("body_set_shape", "body", "shape_idx", "shape"), &PhysicsServer3D::body_set_shape);
 	ClassDB::bind_method(D_METHOD("body_set_shape_transform", "body", "shape_idx", "transform"), &PhysicsServer3D::body_set_shape_transform);
diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h
index 12497c0bdf..01324be0f5 100644
--- a/servers/physics_server_3d.h
+++ b/servers/physics_server_3d.h
@@ -421,6 +421,9 @@ public:
 	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) = 0;
 	virtual uint32_t body_get_collision_mask(RID p_body) const = 0;
 
+	virtual void body_set_collision_priority(RID p_body, real_t p_priority) = 0;
+	virtual real_t body_get_collision_priority(RID p_body) const = 0;
+
 	virtual void body_set_user_flags(RID p_body, uint32_t p_flags) = 0;
 	virtual uint32_t body_get_user_flags(RID p_body) const = 0;
 
diff --git a/servers/physics_server_3d_wrap_mt.h b/servers/physics_server_3d_wrap_mt.h
index d4a4ad3132..ed4546b240 100644
--- a/servers/physics_server_3d_wrap_mt.h
+++ b/servers/physics_server_3d_wrap_mt.h
@@ -202,6 +202,9 @@ public:
 	FUNC2(body_set_collision_mask, RID, uint32_t);
 	FUNC1RC(uint32_t, body_get_collision_mask, RID);
 
+	FUNC2(body_set_collision_priority, RID, real_t);
+	FUNC1RC(real_t, body_get_collision_priority, RID);
+
 	FUNC2(body_set_user_flags, RID, uint32_t);
 	FUNC1RC(uint32_t, body_get_user_flags, RID);