Safe margin cleanup

Safe margin property on CharacterBody only, used as argument in
move_and_collide.

Removed kinematic_safe_margin in 3D physics server, not really useful
and now harmonized with 2D.

17 files changed, 90 insertions, 134 deletions

diff --git a/doc/classes/CharacterBody2D.xml b/doc/classes/CharacterBody2D.xml
index 7858b0a68e..412d05080b 100644
--- a/doc/classes/CharacterBody2D.xml
+++ b/doc/classes/CharacterBody2D.xml
@@ -95,6 +95,12 @@
 		</method>
 	</methods>
 	<members>
+		<member name="collision/safe_margin" type="float" setter="set_safe_margin" getter="get_safe_margin" default="0.08">
+			Extra margin used for collision recovery when calling [method move_and_slide].
+			If the body is at least this close to another body, it will consider them to be colliding and will be pushed away before performing the actual motion.
+			A higher value means it's more flexible for detecting collision, which helps with consistently detecting walls and floors.
+			A lower value forces the collision algorithm to use more exact detection, so it can be used in cases that specifically require precision, e.g at very low scale to avoid visible jittering, or for stability with a stack of character bodies.
+		</member>
 		<member name="floor_max_angle" type="float" setter="set_floor_max_angle" getter="get_floor_max_angle" default="0.785398">
 			Maximum angle (in radians) where a slope is still considered a floor (or a ceiling), rather than a wall, when calling [method move_and_slide]. The default value equals 45 degrees.
 		</member>
diff --git a/doc/classes/CharacterBody3D.xml b/doc/classes/CharacterBody3D.xml
index b2689623aa..7632c1aa7b 100644
--- a/doc/classes/CharacterBody3D.xml
+++ b/doc/classes/CharacterBody3D.xml
@@ -81,6 +81,12 @@
 		</method>
 	</methods>
 	<members>
+		<member name="collision/safe_margin" type="float" setter="set_safe_margin" getter="get_safe_margin" default="0.001">
+			Extra margin used for collision recovery when calling [method move_and_slide].
+			If the body is at least this close to another body, it will consider them to be colliding and will be pushed away before performing the actual motion.
+			A higher value means it's more flexible for detecting collision, which helps with consistently detecting walls and floors.
+			A lower value forces the collision algorithm to use more exact detection, so it can be used in cases that specifically require precision, e.g at very low scale to avoid visible jittering, or for stability with a stack of character bodies.
+		</member>
 		<member name="floor_max_angle" type="float" setter="set_floor_max_angle" getter="get_floor_max_angle" default="0.785398">
 			Maximum angle (in radians) where a slope is still considered a floor (or a ceiling), rather than a wall, when calling [method move_and_slide]. The default value equals 45 degrees.
 		</member>
diff --git a/doc/classes/PhysicsBody2D.xml b/doc/classes/PhysicsBody2D.xml
index 644ec3f9e3..654b0fb668 100644
--- a/doc/classes/PhysicsBody2D.xml
+++ b/doc/classes/PhysicsBody2D.xml
@@ -37,9 +37,12 @@
 			</argument>
 			<argument index="3" name="test_only" type="bool" default="false">
 			</argument>
+			<argument index="4" name="safe_margin" type="float" default="0.08">
+			</argument>
 			<description>
 				Moves the body along the vector [code]rel_vec[/code]. The body will stop if it collides. Returns a [KinematicCollision2D], which contains information about the collision.
 				If [code]test_only[/code] is [code]true[/code], the body does not move but the would-be collision information is given.
+				[code]safe_margin[/code] is the extra margin used for collision recovery (see [member CharacterBody2D.collision/safe_margin] for more details).
 			</description>
 		</method>
 		<method name="remove_collision_exception_with">
@@ -64,19 +67,16 @@
 			</argument>
 			<argument index="4" name="collision" type="KinematicCollision2D" default="null">
 			</argument>
+			<argument index="5" name="safe_margin" type="float" default="0.08">
+			</argument>
 			<description>
 				Checks for collisions without moving the body. Virtually sets the node's position, scale and rotation to that of the given [Transform2D], then tries to move the body along the vector [code]rel_vec[/code]. Returns [code]true[/code] if a collision would occur.
 				[code]collision[/code] is an optional object of type [KinematicCollision2D], which contains additional information about the collision (should there be one).
+				[code]safe_margin[/code] is the extra margin used for collision recovery (see [member CharacterBody2D.collision/safe_margin] for more details).
 			</description>
 		</method>
 	</methods>
 	<members>
-		<member name="collision/safe_margin" type="float" setter="set_safe_margin" getter="get_safe_margin" default="0.08">
-			Extra margin used for collision recovery in motion functions (see [method move_and_collide] and [method CharacterBody2D.move_and_slide]).
-			If the body is at least this close to another body, it will consider them to be colliding and will be pushed away before performing the actual motion.
-			A higher value means it's more flexible for detecting collision, which helps with consistently detecting walls and floors.
-			A lower value forces the collision algorithm to use more exact detection, so it can be used in cases that specifically require precision, e.g at very low scale to avoid visible jittering, or for stability with a stack of character bodies.
-		</member>
 		<member name="input_pickable" type="bool" setter="set_pickable" getter="is_pickable" override="true" default="false" />
 	</members>
 	<constants>
diff --git a/doc/classes/PhysicsBody3D.xml b/doc/classes/PhysicsBody3D.xml
index ee8139507f..1ec38000be 100644
--- a/doc/classes/PhysicsBody3D.xml
+++ b/doc/classes/PhysicsBody3D.xml
@@ -46,9 +46,12 @@
 			</argument>
 			<argument index="3" name="test_only" type="bool" default="false">
 			</argument>
+			<argument index="4" name="safe_margin" type="float" default="0.001">
+			</argument>
 			<description>
 				Moves the body along the vector [code]rel_vec[/code]. The body will stop if it collides. Returns a [KinematicCollision3D], which contains information about the collision.
 				If [code]test_only[/code] is [code]true[/code], the body does not move but the would-be collision information is given.
+				[code]safe_margin[/code] is the extra margin used for collision recovery (see [member CharacterBody3D.collision/safe_margin] for more details).
 			</description>
 		</method>
 		<method name="remove_collision_exception_with">
@@ -84,9 +87,12 @@
 			</argument>
 			<argument index="4" name="collision" type="KinematicCollision3D" default="null">
 			</argument>
+			<argument index="5" name="safe_margin" type="float" default="0.001">
+			</argument>
 			<description>
 				Checks for collisions without moving the body. Virtually sets the node's position, scale and rotation to that of the given [Transform3D], then tries to move the body along the vector [code]rel_vec[/code]. Returns [code]true[/code] if a collision would occur.
 				[code]collision[/code] is an optional object of type [KinematicCollision3D], which contains additional information about the collision (should there be one).
+				[code]safe_margin[/code] is the extra margin used for collision recovery (see [member CharacterBody3D.collision/safe_margin] for more details).
 			</description>
 		</method>
 	</methods>
@@ -109,12 +115,6 @@
 		<member name="axis_lock_linear_z" type="bool" setter="set_axis_lock" getter="get_axis_lock" default="false">
 			Lock the body's linear movement in the Z axis.
 		</member>
-		<member name="collision/safe_margin" type="float" setter="set_safe_margin" getter="get_safe_margin" default="0.001">
-			Extra margin used for collision recovery in motion functions (see [method move_and_collide] and [method CharacterBody3D.move_and_slide]).
-			If the body is at least this close to another body, it will consider them to be colliding and will be pushed away before performing the actual motion.
-			A higher value means it's more flexible for detecting collision, which helps with consistently detecting walls and floors.
-			A lower value forces the collision algorithm to use more exact detection, so it can be used in cases that specifically require precision, e.g at very low scale to avoid visible jittering, or for stability with a stack of character bodies.
-		</member>
 	</members>
 	<constants>
 	</constants>
diff --git a/doc/classes/PhysicsServer3D.xml b/doc/classes/PhysicsServer3D.xml
index 68c7306173..2972d5155c 100644
--- a/doc/classes/PhysicsServer3D.xml
+++ b/doc/classes/PhysicsServer3D.xml
@@ -443,14 +443,6 @@
 				Returns the [PhysicsDirectBodyState3D] of the body.
 			</description>
 		</method>
-		<method name="body_get_kinematic_safe_margin" qualifiers="const">
-			<return type="float">
-			</return>
-			<argument index="0" name="body" type="RID">
-			</argument>
-			<description>
-			</description>
-		</method>
 		<method name="body_get_max_contacts_reported" qualifiers="const">
 			<return type="int">
 			</return>
@@ -661,16 +653,6 @@
 				Sets the function used to calculate physics for an object, if that object allows it (see [method body_set_omit_force_integration]).
 			</description>
 		</method>
-		<method name="body_set_kinematic_safe_margin">
-			<return type="void">
-			</return>
-			<argument index="0" name="body" type="RID">
-			</argument>
-			<argument index="1" name="margin" type="float">
-			</argument>
-			<description>
-			</description>
-		</method>
 		<method name="body_set_max_contacts_reported">
 			<return type="void">
 			</return>
diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index 75bcf2eef2..0e1bd0e558 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -39,17 +39,12 @@
 #include "scene/scene_string_names.h"
 
 void PhysicsBody2D::_bind_methods() {
-	ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &PhysicsBody2D::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false));
-	ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "collision"), &PhysicsBody2D::test_move, DEFVAL(true), DEFVAL(true), DEFVAL(Variant()));
-
-	ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &PhysicsBody2D::set_safe_margin);
-	ClassDB::bind_method(D_METHOD("get_safe_margin"), &PhysicsBody2D::get_safe_margin);
+	ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only", "safe_margin"), &PhysicsBody2D::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false), DEFVAL(0.08));
+	ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "collision", "safe_margin"), &PhysicsBody2D::test_move, DEFVAL(true), DEFVAL(true), DEFVAL(Variant()), DEFVAL(0.08));
 
 	ClassDB::bind_method(D_METHOD("get_collision_exceptions"), &PhysicsBody2D::get_collision_exceptions);
 	ClassDB::bind_method(D_METHOD("add_collision_exception_with", "body"), &PhysicsBody2D::add_collision_exception_with);
 	ClassDB::bind_method(D_METHOD("remove_collision_exception_with", "body"), &PhysicsBody2D::remove_collision_exception_with);
-
-	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
 }
 
 PhysicsBody2D::PhysicsBody2D(PhysicsServer2D::BodyMode p_mode) :
@@ -64,10 +59,10 @@ PhysicsBody2D::~PhysicsBody2D() {
 	}
 }
 
-Ref<KinematicCollision2D> PhysicsBody2D::_move(const Vector2 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, bool p_test_only) {
+Ref<KinematicCollision2D> PhysicsBody2D::_move(const Vector2 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, bool p_test_only, real_t p_margin) {
 	PhysicsServer2D::MotionResult result;
 
-	if (move_and_collide(p_motion, p_infinite_inertia, result, p_exclude_raycast_shapes, p_test_only)) {
+	if (move_and_collide(p_motion, p_infinite_inertia, result, p_margin, p_exclude_raycast_shapes, p_test_only)) {
 		if (motion_cache.is_null()) {
 			motion_cache.instance();
 			motion_cache->owner = this;
@@ -81,12 +76,12 @@ Ref<KinematicCollision2D> PhysicsBody2D::_move(const Vector2 &p_motion, bool p_i
 	return Ref<KinematicCollision2D>();
 }
 
-bool PhysicsBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, bool p_exclude_raycast_shapes, bool p_test_only) {
+bool PhysicsBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only) {
 	if (is_only_update_transform_changes_enabled()) {
 		ERR_PRINT("Move functions do not work together with 'sync to physics' option. Please read the documentation.");
 	}
 	Transform2D gt = get_global_transform();
-	bool colliding = PhysicsServer2D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, margin, &r_result, p_exclude_raycast_shapes);
+	bool colliding = PhysicsServer2D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_margin, &r_result, p_exclude_raycast_shapes);
 
 	if (!p_test_only) {
 		gt.elements[2] += r_result.motion;
@@ -96,7 +91,7 @@ bool PhysicsBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_in
 	return colliding;
 }
 
-bool PhysicsBody2D::test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, const Ref<KinematicCollision2D> &r_collision) {
+bool PhysicsBody2D::test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, const Ref<KinematicCollision2D> &r_collision, real_t p_margin) {
 	ERR_FAIL_COND_V(!is_inside_tree(), false);
 
 	PhysicsServer2D::MotionResult *r = nullptr;
@@ -105,15 +100,7 @@ bool PhysicsBody2D::test_move(const Transform2D &p_from, const Vector2 &p_motion
 		r = const_cast<PhysicsServer2D::MotionResult *>(&r_collision->result);
 	}
 
-	return PhysicsServer2D::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia, margin, r, p_exclude_raycast_shapes);
-}
-
-void PhysicsBody2D::set_safe_margin(real_t p_margin) {
-	margin = p_margin;
-}
-
-real_t PhysicsBody2D::get_safe_margin() const {
-	return margin;
+	return PhysicsServer2D::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia, p_margin, r, p_exclude_raycast_shapes);
 }
 
 TypedArray<PhysicsBody2D> PhysicsBody2D::get_collision_exceptions() {
@@ -964,7 +951,7 @@ Vector2 CharacterBody2D::move_and_slide(const Vector2 &p_linear_velocity) {
 		for (int i = 0; i < 2; ++i) {
 			bool collided;
 			if (i == 0) { //collide
-				collided = move_and_collide(motion, infinite_inertia, result);
+				collided = move_and_collide(motion, infinite_inertia, result, margin);
 				if (!collided) {
 					motion = Vector2(); //clear because no collision happened and motion completed
 				}
@@ -1027,7 +1014,7 @@ Vector2 CharacterBody2D::move_and_slide(const Vector2 &p_linear_velocity) {
 	// Apply snap.
 	Transform2D gt = get_global_transform();
 	PhysicsServer2D::MotionResult result;
-	if (move_and_collide(snap, infinite_inertia, result, false, true)) {
+	if (move_and_collide(snap, infinite_inertia, result, margin, false, true)) {
 		bool apply = true;
 		if (up_direction != Vector2()) {
 			if (Math::acos(result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
@@ -1174,6 +1161,14 @@ void CharacterBody2D::_direct_state_changed(Object *p_state) {
 	set_notify_local_transform(true);
 }
 
+void CharacterBody2D::set_safe_margin(real_t p_margin) {
+	margin = p_margin;
+}
+
+real_t CharacterBody2D::get_safe_margin() const {
+	return margin;
+}
+
 bool CharacterBody2D::is_stop_on_slope_enabled() const {
 	return stop_on_slope;
 }
@@ -1249,6 +1244,8 @@ void CharacterBody2D::_notification(int p_what) {
 void CharacterBody2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity"), &CharacterBody2D::move_and_slide);
 
+	ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &CharacterBody2D::set_safe_margin);
+	ClassDB::bind_method(D_METHOD("get_safe_margin"), &CharacterBody2D::get_safe_margin);
 	ClassDB::bind_method(D_METHOD("is_stop_on_slope_enabled"), &CharacterBody2D::is_stop_on_slope_enabled);
 	ClassDB::bind_method(D_METHOD("set_stop_on_slope_enabled", "enabled"), &CharacterBody2D::set_stop_on_slope_enabled);
 	ClassDB::bind_method(D_METHOD("is_infinite_inertia_enabled"), &CharacterBody2D::is_infinite_inertia_enabled);
@@ -1281,6 +1278,7 @@ void CharacterBody2D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "up_direction"), "set_up_direction", "get_up_direction");
 
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "motion/sync_to_physics"), "set_sync_to_physics", "is_sync_to_physics_enabled");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
 }
 
 CharacterBody2D::CharacterBody2D() :
diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h
index afe2eef55f..d14b72dd8e 100644
--- a/scene/2d/physics_body_2d.h
+++ b/scene/2d/physics_body_2d.h
@@ -45,17 +45,13 @@ protected:
 	static void _bind_methods();
 	PhysicsBody2D(PhysicsServer2D::BodyMode p_mode);
 
-	real_t margin = 0.08;
 	Ref<KinematicCollision2D> motion_cache;
 
-	Ref<KinematicCollision2D> _move(const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
+	Ref<KinematicCollision2D> _move(const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false, real_t p_margin = 0.08);
 
 public:
-	bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
-	bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, const Ref<KinematicCollision2D> &r_collision = Ref<KinematicCollision2D>());
-
-	void set_safe_margin(real_t p_margin);
-	real_t get_safe_margin() const;
+	bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
+	bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, const Ref<KinematicCollision2D> &r_collision = Ref<KinematicCollision2D>(), real_t p_margin = 0.08);
 
 	TypedArray<PhysicsBody2D> get_collision_exceptions();
 	void add_collision_exception_with(Node *p_node); //must be physicsbody
@@ -263,6 +259,8 @@ class CharacterBody2D : public PhysicsBody2D {
 	GDCLASS(CharacterBody2D, PhysicsBody2D);
 
 private:
+	real_t margin = 0.08;
+
 	bool stop_on_slope = false;
 	bool infinite_inertia = true;
 	int max_slides = 4;
@@ -288,6 +286,9 @@ private:
 	Transform2D last_valid_transform;
 	void _direct_state_changed(Object *p_state);
 
+	void set_safe_margin(real_t p_margin);
+	real_t get_safe_margin() const;
+
 	bool is_stop_on_slope_enabled() const;
 	void set_stop_on_slope_enabled(bool p_enabled);
 
diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index aa000d4d3a..9555c3f7a1 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -44,11 +44,8 @@
 #endif
 
 void PhysicsBody3D::_bind_methods() {
-	ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &PhysicsBody3D::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false));
-	ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "collision"), &PhysicsBody3D::test_move, DEFVAL(true), DEFVAL(true), DEFVAL(Variant()));
-
-	ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &PhysicsBody3D::set_safe_margin);
-	ClassDB::bind_method(D_METHOD("get_safe_margin"), &PhysicsBody3D::get_safe_margin);
+	ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only", "safe_margin"), &PhysicsBody3D::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false), DEFVAL(0.001));
+	ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "collision", "safe_margin"), &PhysicsBody3D::test_move, DEFVAL(true), DEFVAL(true), DEFVAL(Variant()), DEFVAL(0.001));
 
 	ClassDB::bind_method(D_METHOD("set_axis_lock", "axis", "lock"), &PhysicsBody3D::set_axis_lock);
 	ClassDB::bind_method(D_METHOD("get_axis_lock", "axis"), &PhysicsBody3D::get_axis_lock);
@@ -57,8 +54,6 @@ void PhysicsBody3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("add_collision_exception_with", "body"), &PhysicsBody3D::add_collision_exception_with);
 	ClassDB::bind_method(D_METHOD("remove_collision_exception_with", "body"), &PhysicsBody3D::remove_collision_exception_with);
 
-	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
-
 	ADD_GROUP("Axis Lock", "axis_lock_");
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_linear_x"), "set_axis_lock", "get_axis_lock", PhysicsServer3D::BODY_AXIS_LINEAR_X);
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_linear_y"), "set_axis_lock", "get_axis_lock", PhysicsServer3D::BODY_AXIS_LINEAR_Y);
@@ -107,9 +102,9 @@ void PhysicsBody3D::remove_collision_exception_with(Node *p_node) {
 	PhysicsServer3D::get_singleton()->body_remove_collision_exception(get_rid(), collision_object->get_rid());
 }
 
-Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, bool p_test_only) {
+Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, bool p_test_only, real_t p_margin) {
 	PhysicsServer3D::MotionResult result;
-	if (move_and_collide(p_motion, p_infinite_inertia, result, p_exclude_raycast_shapes, p_test_only)) {
+	if (move_and_collide(p_motion, p_infinite_inertia, result, p_margin, p_exclude_raycast_shapes, p_test_only)) {
 		if (motion_cache.is_null()) {
 			motion_cache.instance();
 			motion_cache->owner = this;
@@ -123,9 +118,9 @@ Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_i
 	return Ref<KinematicCollision3D>();
 }
 
-bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, bool p_exclude_raycast_shapes, bool p_test_only) {
+bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only) {
 	Transform3D gt = get_global_transform();
-	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, &r_result, p_exclude_raycast_shapes);
+	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_margin, &r_result, p_exclude_raycast_shapes);
 
 	for (int i = 0; i < 3; i++) {
 		if (locked_axis & (1 << i)) {
@@ -141,7 +136,7 @@ bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
 	return colliding;
 }
 
-bool PhysicsBody3D::test_move(const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, const Ref<KinematicCollision3D> &r_collision) {
+bool PhysicsBody3D::test_move(const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, const Ref<KinematicCollision3D> &r_collision, real_t p_margin) {
 	ERR_FAIL_COND_V(!is_inside_tree(), false);
 
 	PhysicsServer3D::MotionResult *r = nullptr;
@@ -150,16 +145,7 @@ bool PhysicsBody3D::test_move(const Transform3D &p_from, const Vector3 &p_motion
 		r = const_cast<PhysicsServer3D::MotionResult *>(&r_collision->result);
 	}
 
-	return PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia, r, p_exclude_raycast_shapes);
-}
-
-void PhysicsBody3D::set_safe_margin(real_t p_margin) {
-	margin = p_margin;
-	PhysicsServer3D::get_singleton()->body_set_kinematic_safe_margin(get_rid(), margin);
-}
-
-real_t PhysicsBody3D::get_safe_margin() const {
-	return margin;
+	return PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia, p_margin, r, p_exclude_raycast_shapes);
 }
 
 void PhysicsBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock) {
@@ -985,7 +971,7 @@ Vector3 CharacterBody3D::move_and_slide(const Vector3 &p_linear_velocity) {
 		for (int i = 0; i < 2; ++i) {
 			bool collided;
 			if (i == 0) { //collide
-				collided = move_and_collide(motion, infinite_inertia, result);
+				collided = move_and_collide(motion, infinite_inertia, result, margin);
 				if (!collided) {
 					motion = Vector3(); //clear because no collision happened and motion completed
 				}
@@ -1054,7 +1040,7 @@ Vector3 CharacterBody3D::move_and_slide(const Vector3 &p_linear_velocity) {
 	// Apply snap.
 	Transform3D gt = get_global_transform();
 	PhysicsServer3D::MotionResult result;
-	if (move_and_collide(snap, infinite_inertia, result, false, true)) {
+	if (move_and_collide(snap, infinite_inertia, result, margin, false, true)) {
 		bool apply = true;
 		if (up_direction != Vector3()) {
 			if (Math::acos(result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
@@ -1116,6 +1102,14 @@ bool CharacterBody3D::separate_raycast_shapes(PhysicsServer3D::MotionResult &r_r
 	}
 }
 
+void CharacterBody3D::set_safe_margin(real_t p_margin) {
+	margin = p_margin;
+}
+
+real_t CharacterBody3D::get_safe_margin() const {
+	return margin;
+}
+
 bool CharacterBody3D::is_on_floor() const {
 	return on_floor;
 }
@@ -1223,6 +1217,8 @@ void CharacterBody3D::_notification(int p_what) {
 void CharacterBody3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity"), &CharacterBody3D::move_and_slide);
 
+	ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &CharacterBody3D::set_safe_margin);
+	ClassDB::bind_method(D_METHOD("get_safe_margin"), &CharacterBody3D::get_safe_margin);
 	ClassDB::bind_method(D_METHOD("is_stop_on_slope_enabled"), &CharacterBody3D::is_stop_on_slope_enabled);
 	ClassDB::bind_method(D_METHOD("set_stop_on_slope_enabled", "enabled"), &CharacterBody3D::set_stop_on_slope_enabled);
 	ClassDB::bind_method(D_METHOD("is_infinite_inertia_enabled"), &CharacterBody3D::is_infinite_inertia_enabled);
@@ -1251,6 +1247,7 @@ void CharacterBody3D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_max_angle"), "set_floor_max_angle", "get_floor_max_angle");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "snap"), "set_snap", "get_snap");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_direction"), "set_up_direction", "get_up_direction");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
 }
 
 void CharacterBody3D::_direct_state_changed(Object *p_state) {
diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index b513602ba8..0e744dab0b 100644
--- a/scene/3d/physics_body_3d.h
+++ b/scene/3d/physics_body_3d.h
@@ -46,19 +46,15 @@ protected:
 	static void _bind_methods();
 	PhysicsBody3D(PhysicsServer3D::BodyMode p_mode);
 
-	real_t margin = 0.001;
 	Ref<KinematicCollision3D> motion_cache;
 
 	uint16_t locked_axis = 0;
 
-	Ref<KinematicCollision3D> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
+	Ref<KinematicCollision3D> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false, real_t p_margin = 0.001);
 
 public:
-	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
-	bool test_move(const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, const Ref<KinematicCollision3D> &r_collision = Ref<KinematicCollision3D>());
-
-	void set_safe_margin(real_t p_margin);
-	real_t get_safe_margin() const;
+	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
+	bool test_move(const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, const Ref<KinematicCollision3D> &r_collision = Ref<KinematicCollision3D>(), real_t p_margin = 0.001);
 
 	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);
 	bool get_axis_lock(PhysicsServer3D::BodyAxis p_axis) const;
@@ -264,6 +260,8 @@ class CharacterBody3D : public PhysicsBody3D {
 	GDCLASS(CharacterBody3D, PhysicsBody3D);
 
 private:
+	real_t margin = 0.001;
+
 	bool stop_on_slope = false;
 	bool infinite_inertia = true;
 	int max_slides = 4;
@@ -287,6 +285,9 @@ private:
 
 	bool separate_raycast_shapes(PhysicsServer3D::MotionResult &r_result);
 
+	void set_safe_margin(real_t p_margin);
+	real_t get_safe_margin() const;
+
 	bool is_stop_on_slope_enabled() const;
 	void set_stop_on_slope_enabled(bool p_enabled);
 
diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h
index f1eb78a776..5002bf5fc8 100644
--- a/servers/physics_2d/physics_server_2d_sw.h
+++ b/servers/physics_2d/physics_server_2d_sw.h
@@ -247,8 +247,8 @@ public:
 
 	virtual void body_set_pickable(RID p_body, bool p_pickable) override;
 
-	virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override;
-	virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override;
+	virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override;
+	virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.08) override;
 
 	// this function only works on physics process, errors and returns null otherwise
 	virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override;
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 64bc10b8eb..ea6064cb4c 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -712,10 +712,6 @@ void Body3DSW::set_force_integration_callback(const Callable &p_callable, const
 	}
 }
 
-void Body3DSW::set_kinematic_margin(real_t p_margin) {
-	kinematic_safe_margin = p_margin;
-}
-
 Body3DSW::Body3DSW() :
 		CollisionObject3DSW(TYPE_BODY),
 
@@ -726,8 +722,6 @@ Body3DSW::Body3DSW() :
 	active = true;
 
 	mass = 1;
-	kinematic_safe_margin = 0.001;
-	//_inv_inertia=Transform3D();
 	_inv_mass = 1;
 	bounce = 0;
 	friction = 1;
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index 9ef0cd8467..0fa31c5037 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -55,7 +55,6 @@ class Body3DSW : public CollisionObject3DSW {
 
 	uint16_t locked_axis = 0;
 
-	real_t kinematic_safe_margin;
 	real_t _inv_mass;
 	Vector3 _inv_inertia; // Relative to the principal axes of inertia
 
@@ -144,9 +143,6 @@ class Body3DSW : public CollisionObject3DSW {
 public:
 	void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
 
-	void set_kinematic_margin(real_t p_margin);
-	_FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; }
-
 	_FORCE_INLINE_ void add_area(Area3DSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index 48d8a016ce..7a95a8abc8 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -656,19 +656,6 @@ real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) cons
 	return body->get_param(p_param);
 };
 
-void PhysicsServer3DSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_kinematic_margin(p_margin);
-}
-
-real_t PhysicsServer3DSW::body_get_kinematic_safe_margin(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_kinematic_margin();
-}
-
 void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
@@ -867,7 +854,7 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) {
 	body->set_ray_pickable(p_enable);
 }
 
-bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) {
+bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes) {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND_V(!body, false);
 	ERR_FAIL_COND_V(!body->get_space(), false);
@@ -875,7 +862,7 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from,
 
 	_update_shapes();
 
-	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes);
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
 }
 
 int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) {
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index a5776202c9..57b6385758 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/physics_server_3d_sw.h
@@ -204,9 +204,6 @@ public:
 	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override;
 	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override;
 
-	virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin) override;
-	virtual real_t body_get_kinematic_safe_margin(RID p_body) const override;
-
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
 
@@ -245,7 +242,7 @@ public:
 
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable) override;
 
-	virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override;
+	virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override;
 	virtual int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override;
 
 	// this function only works on physics process, errors and returns null otherwise
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
index 8be38b723f..bda2e30dd1 100644
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ b/servers/physics_3d/physics_server_3d_wrap_mt.h
@@ -213,9 +213,6 @@ public:
 	FUNC3(body_set_param, RID, BodyParameter, real_t);
 	FUNC2RC(real_t, body_get_param, RID, BodyParameter);
 
-	FUNC2(body_set_kinematic_safe_margin, RID, real_t);
-	FUNC1RC(real_t, body_get_kinematic_safe_margin, RID);
-
 	FUNC3(body_set_state, RID, BodyState, const Variant &);
 	FUNC2RC(Variant, body_get_state, RID, BodyState);
 
@@ -253,9 +250,9 @@ public:
 
 	FUNC2(body_set_ray_pickable, RID, bool);
 
-	bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override {
+	bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
-		return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, r_result, p_exclude_raycast_shapes);
+		return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
 	}
 
 	int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override {
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index ed07a3e6df..1634169e8a 100644
--- a/servers/physics_server_3d.cpp
+++ b/servers/physics_server_3d.cpp
@@ -523,9 +523,6 @@ void PhysicsServer3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &PhysicsServer3D::body_set_param);
 	ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &PhysicsServer3D::body_get_param);
 
-	ClassDB::bind_method(D_METHOD("body_set_kinematic_safe_margin", "body", "margin"), &PhysicsServer3D::body_set_kinematic_safe_margin);
-	ClassDB::bind_method(D_METHOD("body_get_kinematic_safe_margin", "body"), &PhysicsServer3D::body_get_kinematic_safe_margin);
-
 	ClassDB::bind_method(D_METHOD("body_set_state", "body", "state", "value"), &PhysicsServer3D::body_set_state);
 	ClassDB::bind_method(D_METHOD("body_get_state", "body", "state"), &PhysicsServer3D::body_get_state);
 
diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h
index fdaec981c3..4e76f7ce7e 100644
--- a/servers/physics_server_3d.h
+++ b/servers/physics_server_3d.h
@@ -428,9 +428,6 @@ public:
 	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) = 0;
 	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const = 0;
 
-	virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin) = 0;
-	virtual real_t body_get_kinematic_safe_margin(RID p_body) const = 0;
-
 	//state
 	enum BodyState {
 		BODY_STATE_TRANSFORM,
@@ -507,7 +504,7 @@ public:
 		Variant collider_metadata;
 	};
 
-	virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) = 0;
+	virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) = 0;
 
 	struct SeparationResult {
 		real_t collision_depth;