1 files changed, 80 insertions, 121 deletions

diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 100e3563a3..12c91271d4 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -30,22 +30,16 @@
 
 #include "physics_body_3d.h"
 
-#include "core/config/engine.h"
 #include "core/core_string_names.h"
-#include "core/object/class_db.h"
-#include "core/templates/list.h"
-#include "core/templates/rid.h"
-#include "scene/3d/collision_shape_3d.h"
 #include "scene/scene_string_names.h"
-#include "servers/navigation_server_3d.h"
 
 #ifdef TOOLS_ENABLED
 #include "editor/plugins/node_3d_editor_plugin.h"
 #endif
 
 void PhysicsBody3D::_bind_methods() {
-	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("move_and_collide", "rel_vec", "test_only", "safe_margin"), &PhysicsBody3D::_move, DEFVAL(false), DEFVAL(0.001));
+	ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "collision", "safe_margin"), &PhysicsBody3D::test_move, 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);
@@ -101,9 +95,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, real_t p_margin) {
+Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_test_only, real_t p_margin) {
 	PhysicsServer3D::MotionResult result;
-	if (move_and_collide(p_motion, p_infinite_inertia, result, p_margin, p_exclude_raycast_shapes, p_test_only)) {
+	if (move_and_collide(p_motion, result, p_margin, p_test_only)) {
 		if (motion_cache.is_null()) {
 			motion_cache.instantiate();
 			motion_cache->owner = this;
@@ -117,9 +111,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, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only, bool p_cancel_sliding) {
+bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_test_only, bool p_cancel_sliding, const Set<RID> &p_exclude) {
 	Transform3D gt = get_global_transform();
-	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_margin, &r_result, p_exclude_raycast_shapes);
+	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_margin, &r_result, p_exclude);
 
 	// Restore direction of motion to be along original motion,
 	// in order to avoid sliding due to recovery,
@@ -173,7 +167,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, real_t p_margin) {
+bool PhysicsBody3D::test_move(const Transform3D &p_from, const Vector3 &p_motion, const Ref<KinematicCollision3D> &r_collision, real_t p_margin) {
 	ERR_FAIL_COND_V(!is_inside_tree(), false);
 
 	PhysicsServer3D::MotionResult *r = nullptr;
@@ -182,7 +176,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, p_margin, r, p_exclude_raycast_shapes);
+	return PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_margin, r);
 }
 
 void PhysicsBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock) {
@@ -1090,6 +1084,9 @@ void CharacterBody3D::move_and_slide() {
 
 	bool was_on_floor = on_floor;
 
+	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
+	float delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time();
+
 	for (int i = 0; i < 3; i++) {
 		if (locked_axis & (1 << i)) {
 			linear_velocity[i] = 0.0;
@@ -1097,105 +1094,93 @@ void CharacterBody3D::move_and_slide() {
 	}
 
 	Vector3 current_floor_velocity = floor_velocity;
-	if (on_floor && on_floor_body.is_valid()) {
+	if ((on_floor || on_wall) && on_floor_body.is_valid()) {
 		//this approach makes sure there is less delay between the actual body velocity and the one we saved
 		PhysicsDirectBodyState3D *bs = PhysicsServer3D::get_singleton()->body_get_direct_state(on_floor_body);
 		if (bs) {
-			current_floor_velocity = bs->get_linear_velocity();
+			Transform3D gt = get_global_transform();
+			Vector3 local_position = gt.origin - bs->get_transform().origin;
+			current_floor_velocity = bs->get_velocity_at_local_position(local_position);
 		}
 	}
 
-	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
-	Vector3 motion = (floor_velocity + linear_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
-
+	motion_results.clear();
 	on_floor = false;
-	on_floor_body = RID();
 	on_ceiling = false;
 	on_wall = false;
-	motion_results.clear();
 	floor_normal = Vector3();
 	floor_velocity = Vector3();
 
+	if (current_floor_velocity != Vector3() && on_floor_body.is_valid()) {
+		PhysicsServer3D::MotionResult floor_result;
+		Set<RID> exclude;
+		exclude.insert(on_floor_body);
+		if (move_and_collide(current_floor_velocity * delta, floor_result, margin, false, false, exclude)) {
+			motion_results.push_back(floor_result);
+			_set_collision_direction(floor_result);
+		}
+	}
+
+	on_floor_body = RID();
+	Vector3 motion = linear_velocity * delta;
+
 	// No sliding on first attempt to keep floor motion stable when possible,
 	// when stop on slope is enabled.
 	bool sliding_enabled = !stop_on_slope;
+
 	for (int iteration = 0; iteration < max_slides; ++iteration) {
 		PhysicsServer3D::MotionResult result;
 		bool found_collision = false;
 
-		for (int i = 0; i < 2; ++i) {
-			bool collided;
-			if (i == 0) { //collide
-				collided = move_and_collide(motion, infinite_inertia, result, margin, true, false, !sliding_enabled);
-				if (!collided) {
-					motion = Vector3(); //clear because no collision happened and motion completed
-				}
-			} else { //separate raycasts (if any)
-				collided = separate_raycast_shapes(result);
-				if (collided) {
-					result.remainder = motion; //keep
-					result.motion = Vector3();
-				}
-			}
+		bool collided = move_and_collide(motion, result, margin, false, !sliding_enabled);
+		if (!collided) {
+			motion = Vector3(); //clear because no collision happened and motion completed
+		} else {
+			found_collision = true;
+
+			motion_results.push_back(result);
+			_set_collision_direction(result);
 
-			if (collided) {
-				found_collision = true;
-
-				motion_results.push_back(result);
-
-				if (up_direction == Vector3()) {
-					//all is a wall
-					on_wall = true;
-				} else {
-					if (Math::acos(result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
-
-						on_floor = true;
-						floor_normal = result.collision_normal;
-						on_floor_body = result.collider;
-						floor_velocity = result.collider_velocity;
-
-						if (stop_on_slope) {
-							if ((body_velocity_normal + up_direction).length() < 0.01) {
-								Transform3D gt = get_global_transform();
-								if (result.motion.length() > margin) {
-									gt.origin -= result.motion.slide(up_direction);
-								} else {
-									gt.origin -= result.motion;
-								}
-								set_global_transform(gt);
-								linear_velocity = Vector3();
-								return;
-							}
-						}
-					} else if (Math::acos(result.collision_normal.dot(-up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
-						on_ceiling = true;
+			if (on_floor && stop_on_slope) {
+				if ((body_velocity_normal + up_direction).length() < 0.01) {
+					Transform3D gt = get_global_transform();
+					if (result.motion.length() > margin) {
+						gt.origin -= result.motion.slide(up_direction);
 					} else {
-						on_wall = true;
+						gt.origin -= result.motion;
 					}
+					set_global_transform(gt);
+					linear_velocity = Vector3();
+					return;
 				}
+			}
 
-				if (sliding_enabled || !on_floor) {
-					motion = result.remainder.slide(result.collision_normal);
-					linear_velocity = linear_velocity.slide(result.collision_normal);
+			if (sliding_enabled || !on_floor) {
+				motion = result.remainder.slide(result.collision_normal);
+				linear_velocity = linear_velocity.slide(result.collision_normal);
 
-					for (int j = 0; j < 3; j++) {
-						if (locked_axis & (1 << j)) {
-							linear_velocity[j] = 0.0;
-						}
+				for (int j = 0; j < 3; j++) {
+					if (locked_axis & (1 << j)) {
+						linear_velocity[j] = 0.0;
 					}
-				} else {
-					motion = result.remainder;
 				}
+			} else {
+				motion = result.remainder;
 			}
-
-			sliding_enabled = true;
 		}
 
+		sliding_enabled = true;
+
 		if (!found_collision || motion == Vector3()) {
 			break;
 		}
 	}
 
+	if (!on_floor && !on_wall) {
+		// Add last platform velocity when just left a moving platform.
+		linear_velocity += current_floor_velocity;
+	}
+
 	if (!was_on_floor || snap == Vector3()) {
 		return;
 	}
@@ -1203,7 +1188,7 @@ void CharacterBody3D::move_and_slide() {
 	// Apply snap.
 	Transform3D gt = get_global_transform();
 	PhysicsServer3D::MotionResult result;
-	if (move_and_collide(snap, infinite_inertia, result, margin, false, true, false)) {
+	if (move_and_collide(snap, result, margin, true, false)) {
 		bool apply = true;
 		if (up_direction != Vector3()) {
 			if (Math::acos(result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
@@ -1231,39 +1216,23 @@ void CharacterBody3D::move_and_slide() {
 	}
 }
 
-bool CharacterBody3D::separate_raycast_shapes(PhysicsServer3D::MotionResult &r_result) {
-	PhysicsServer3D::SeparationResult sep_res[8]; //max 8 rays
-
-	Transform3D gt = get_global_transform();
-
-	Vector3 recover;
-	int hits = PhysicsServer3D::get_singleton()->body_test_ray_separation(get_rid(), gt, infinite_inertia, recover, sep_res, 8, margin);
-	int deepest = -1;
-	real_t deepest_depth;
-	for (int i = 0; i < hits; i++) {
-		if (deepest == -1 || sep_res[i].collision_depth > deepest_depth) {
-			deepest = i;
-			deepest_depth = sep_res[i].collision_depth;
-		}
-	}
-
-	gt.origin += recover;
-	set_global_transform(gt);
-
-	if (deepest != -1) {
-		r_result.collider_id = sep_res[deepest].collider_id;
-		r_result.collider_metadata = sep_res[deepest].collider_metadata;
-		r_result.collider_shape = sep_res[deepest].collider_shape;
-		r_result.collider_velocity = sep_res[deepest].collider_velocity;
-		r_result.collision_point = sep_res[deepest].collision_point;
-		r_result.collision_normal = sep_res[deepest].collision_normal;
-		r_result.collision_local_shape = sep_res[deepest].collision_local_shape;
-		r_result.motion = recover;
-		r_result.remainder = Vector3();
-
-		return true;
+void CharacterBody3D::_set_collision_direction(const PhysicsServer3D::MotionResult &p_result) {
+	if (up_direction == Vector3()) {
+		//all is a wall
+		on_wall = true;
 	} else {
-		return false;
+		if (Math::acos(p_result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
+			on_floor = true;
+			floor_normal = p_result.collision_normal;
+			on_floor_body = p_result.collider;
+			floor_velocity = p_result.collider_velocity;
+		} else if (Math::acos(p_result.collision_normal.dot(-up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
+			on_ceiling = true;
+		} else {
+			on_wall = true;
+			on_floor_body = p_result.collider;
+			floor_velocity = p_result.collider_velocity;
+		}
 	}
 }
 
@@ -1335,13 +1304,6 @@ void CharacterBody3D::set_stop_on_slope_enabled(bool p_enabled) {
 	stop_on_slope = p_enabled;
 }
 
-bool CharacterBody3D::is_infinite_inertia_enabled() const {
-	return infinite_inertia;
-}
-void CharacterBody3D::set_infinite_inertia_enabled(bool p_enabled) {
-	infinite_inertia = p_enabled;
-}
-
 int CharacterBody3D::get_max_slides() const {
 	return max_slides;
 }
@@ -1399,8 +1361,6 @@ void CharacterBody3D::_bind_methods() {
 	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);
-	ClassDB::bind_method(D_METHOD("set_infinite_inertia_enabled", "enabled"), &CharacterBody3D::set_infinite_inertia_enabled);
 	ClassDB::bind_method(D_METHOD("get_max_slides"), &CharacterBody3D::get_max_slides);
 	ClassDB::bind_method(D_METHOD("set_max_slides", "max_slides"), &CharacterBody3D::set_max_slides);
 	ClassDB::bind_method(D_METHOD("get_floor_max_angle"), &CharacterBody3D::get_floor_max_angle);
@@ -1421,7 +1381,6 @@ void CharacterBody3D::_bind_methods() {
 
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "linear_velocity"), "set_linear_velocity", "get_linear_velocity");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "stop_on_slope"), "set_stop_on_slope_enabled", "is_stop_on_slope_enabled");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "infinite_inertia"), "set_infinite_inertia_enabled", "is_infinite_inertia_enabled");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "max_slides", PROPERTY_HINT_RANGE, "1,8,1,or_greater"), "set_max_slides", "get_max_slides");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_max_angle", PROPERTY_HINT_RANGE, "0,180,0.1,radians"), "set_floor_max_angle", "get_floor_max_angle");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "snap"), "set_snap", "get_snap");