Merge pull request #53280 from nekomatata/test-body-motion-parameters

2 files changed, 46 insertions, 96 deletions

diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index fc0df3650f..b7c808398c 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -91,12 +91,15 @@ 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_test_only, real_t p_margin, int p_max_collisions) {
-	PhysicsServer3D::MotionResult result;
+Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_linear_velocity, bool p_test_only, real_t p_margin, int p_max_collisions) {
 	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
 	double delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time();
 
-	if (move_and_collide(p_motion * delta, result, p_margin, p_test_only, p_max_collisions)) {
+	PhysicsServer3D::MotionParameters parameters(get_global_transform(), p_linear_velocity * delta, p_margin);
+	parameters.max_collisions = p_max_collisions;
+
+	PhysicsServer3D::MotionResult result;
+	if (move_and_collide(parameters, result, p_test_only)) {
 		// Create a new instance when the cached reference is invalid or still in use in script.
 		if (motion_cache.is_null() || motion_cache->reference_get_count() > 1) {
 			motion_cache.instantiate();
@@ -111,23 +114,22 @@ Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_t
 	return Ref<KinematicCollision3D>();
 }
 
-bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_test_only, int p_max_collisions, bool p_cancel_sliding, bool p_collide_separation_ray, const Set<RID> &p_exclude) {
-	Transform3D gt = get_global_transform();
-	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_margin, &r_result, p_max_collisions, p_collide_separation_ray, p_exclude);
+bool PhysicsBody3D::move_and_collide(const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult &r_result, bool p_test_only, bool p_cancel_sliding) {
+	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), p_parameters, &r_result);
 
 	// Restore direction of motion to be along original motion,
 	// in order to avoid sliding due to recovery,
 	// but only if collision depth is low enough to avoid tunneling.
 	if (p_cancel_sliding) {
-		real_t motion_length = p_motion.length();
+		real_t motion_length = p_parameters.motion.length();
 		real_t precision = 0.001;
 
 		if (colliding) {
 			// Can't just use margin as a threshold because collision depth is calculated on unsafe motion,
 			// so even in normal resting cases the depth can be a bit more than the margin.
-			precision += motion_length * (r_result.unsafe_fraction - r_result.safe_fraction);
+			precision += motion_length * (r_result.collision_unsafe_fraction - r_result.collision_safe_fraction);
 
-			if (r_result.collisions[0].depth > (real_t)p_margin + precision) {
+			if (r_result.collisions[0].depth > p_parameters.margin + precision) {
 				p_cancel_sliding = false;
 			}
 		}
@@ -136,7 +138,7 @@ bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::M
 			// When motion is null, recovery is the resulting motion.
 			Vector3 motion_normal;
 			if (motion_length > CMP_EPSILON) {
-				motion_normal = p_motion / motion_length;
+				motion_normal = p_parameters.motion / motion_length;
 			}
 
 			// Check depth of recovery.
@@ -145,10 +147,10 @@ bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::M
 			real_t recovery_length = recovery.length();
 			// Fixes cases where canceling slide causes the motion to go too deep into the ground,
 			// because we're only taking rest information into account and not general recovery.
-			if (recovery_length < (real_t)p_margin + precision) {
+			if (recovery_length < p_parameters.margin + precision) {
 				// Apply adjustment to motion.
 				r_result.travel = motion_normal * projected_length;
-				r_result.remainder = p_motion - r_result.travel;
+				r_result.remainder = p_parameters.motion - r_result.travel;
 			}
 		}
 	}
@@ -160,6 +162,7 @@ bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::M
 	}
 
 	if (!p_test_only) {
+		Transform3D gt = p_parameters.from;
 		gt.origin += r_result.travel;
 		set_global_transform(gt);
 	}
@@ -167,7 +170,7 @@ bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::M
 	return colliding;
 }
 
-bool PhysicsBody3D::test_move(const Transform3D &p_from, const Vector3 &p_motion, const Ref<KinematicCollision3D> &r_collision, real_t p_margin, int p_max_collisions) {
+bool PhysicsBody3D::test_move(const Transform3D &p_from, const Vector3 &p_linear_velocity, const Ref<KinematicCollision3D> &r_collision, real_t p_margin, int p_max_collisions) {
 	ERR_FAIL_COND_V(!is_inside_tree(), false);
 
 	PhysicsServer3D::MotionResult *r = nullptr;
@@ -179,7 +182,9 @@ bool PhysicsBody3D::test_move(const Transform3D &p_from, const Vector3 &p_motion
 	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
 	double delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time();
 
-	return PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), p_from, p_motion * delta, p_margin, r, p_max_collisions);
+	PhysicsServer3D::MotionParameters parameters(p_from, p_linear_velocity * delta, p_margin);
+
+	return PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), parameters, r);
 }
 
 void PhysicsBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock) {
@@ -1124,10 +1129,11 @@ bool CharacterBody3D::move_and_slide() {
 	last_motion = Vector3();
 
 	if (!current_platform_velocity.is_equal_approx(Vector3())) {
+		PhysicsServer3D::MotionParameters parameters(get_global_transform(), current_platform_velocity * delta, margin);
+		parameters.exclude_bodies.insert(platform_rid);
+
 		PhysicsServer3D::MotionResult floor_result;
-		Set<RID> exclude;
-		exclude.insert(platform_rid);
-		if (move_and_collide(current_platform_velocity * delta, floor_result, margin, false, 1, false, false, exclude)) {
+		if (move_and_collide(parameters, floor_result, false, false)) {
 			motion_results.push_back(floor_result);
 
 			CollisionState result_state;
@@ -1181,8 +1187,12 @@ void CharacterBody3D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 	Vector3 total_travel;
 
 	for (int iteration = 0; iteration < max_slides; ++iteration) {
+		PhysicsServer3D::MotionParameters parameters(get_global_transform(), motion, margin);
+		parameters.max_collisions = 4;
+
 		PhysicsServer3D::MotionResult result;
-		bool collided = move_and_collide(motion, result, margin, false, 4, !sliding_enabled);
+		bool collided = move_and_collide(parameters, result, false, !sliding_enabled);
+
 		last_motion = result.travel;
 
 		if (collided) {
@@ -1411,9 +1421,11 @@ void CharacterBody3D::_move_and_slide_free(double p_delta) {
 
 	bool first_slide = true;
 	for (int iteration = 0; iteration < max_slides; ++iteration) {
+		PhysicsServer3D::MotionParameters parameters(get_global_transform(), motion, margin);
+
 		PhysicsServer3D::MotionResult result;
+		bool collided = move_and_collide(parameters, result, false, false);
 
-		bool collided = move_and_collide(motion, result, margin, false, 1, false);
 		last_motion = result.travel;
 
 		if (collided) {
@@ -1462,9 +1474,12 @@ void CharacterBody3D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
 	// Snap by at least collision margin to keep floor state consistent.
 	real_t length = MAX(floor_snap_length, margin);
 
-	Transform3D gt = get_global_transform();
+	PhysicsServer3D::MotionParameters parameters(get_global_transform(), -up_direction * length, margin);
+	parameters.max_collisions = 4;
+	parameters.collide_separation_ray = true;
+
 	PhysicsServer3D::MotionResult result;
-	if (move_and_collide(-up_direction * length, result, margin, true, 4, false, true)) {
+	if (move_and_collide(parameters, result, true, false)) {
 		CollisionState result_state;
 		// Apply direction for floor only.
 		_set_collision_direction(result, result_state, CollisionState(true, false, false));
@@ -1480,8 +1495,8 @@ void CharacterBody3D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
 				}
 			}
 
-			gt.origin += result.travel;
-			set_global_transform(gt);
+			parameters.from.origin += result.travel;
+			set_global_transform(parameters.from);
 		}
 	}
 }
@@ -1494,8 +1509,12 @@ bool CharacterBody3D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facin
 	// Snap by at least collision margin to keep floor state consistent.
 	real_t length = MAX(floor_snap_length, margin);
 
+	PhysicsServer3D::MotionParameters parameters(get_global_transform(), -up_direction * length, margin);
+	parameters.max_collisions = 4;
+	parameters.collide_separation_ray = true;
+
 	PhysicsServer3D::MotionResult result;
-	if (move_and_collide(-up_direction * length, result, margin, true, 4, false, true)) {
+	if (move_and_collide(parameters, result, true, false)) {
 		CollisionState result_state;
 		// Don't apply direction for any type.
 		_set_collision_direction(result, result_state, CollisionState());
@@ -2002,42 +2021,6 @@ Vector3 KinematicCollision3D::get_collider_velocity(int p_collision_index) const
 	return result.collisions[p_collision_index].collider_velocity;
 }
 
-Vector3 KinematicCollision3D::get_best_position() const {
-	return result.collision_count ? get_position() : Vector3();
-}
-
-Vector3 KinematicCollision3D::get_best_normal() const {
-	return result.collision_count ? get_normal() : Vector3();
-}
-
-Object *KinematicCollision3D::get_best_local_shape() const {
-	return result.collision_count ? get_local_shape() : nullptr;
-}
-
-Object *KinematicCollision3D::get_best_collider() const {
-	return result.collision_count ? get_collider() : nullptr;
-}
-
-ObjectID KinematicCollision3D::get_best_collider_id() const {
-	return result.collision_count ? get_collider_id() : ObjectID();
-}
-
-RID KinematicCollision3D::get_best_collider_rid() const {
-	return result.collision_count ? get_collider_rid() : RID();
-}
-
-Object *KinematicCollision3D::get_best_collider_shape() const {
-	return result.collision_count ? get_collider_shape() : nullptr;
-}
-
-int KinematicCollision3D::get_best_collider_shape_index() const {
-	return result.collision_count ? get_collider_shape_index() : 0;
-}
-
-Vector3 KinematicCollision3D::get_best_collider_velocity() const {
-	return result.collision_count ? get_collider_velocity() : Vector3();
-}
-
 void KinematicCollision3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_travel"), &KinematicCollision3D::get_travel);
 	ClassDB::bind_method(D_METHOD("get_remainder"), &KinematicCollision3D::get_remainder);
@@ -2052,29 +2035,6 @@ void KinematicCollision3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_collider_shape", "collision_index"), &KinematicCollision3D::get_collider_shape, DEFVAL(0));
 	ClassDB::bind_method(D_METHOD("get_collider_shape_index", "collision_index"), &KinematicCollision3D::get_collider_shape_index, DEFVAL(0));
 	ClassDB::bind_method(D_METHOD("get_collider_velocity", "collision_index"), &KinematicCollision3D::get_collider_velocity, DEFVAL(0));
-
-	ClassDB::bind_method(D_METHOD("get_best_position"), &KinematicCollision3D::get_best_position);
-	ClassDB::bind_method(D_METHOD("get_best_normal"), &KinematicCollision3D::get_best_normal);
-	ClassDB::bind_method(D_METHOD("get_best_local_shape"), &KinematicCollision3D::get_best_local_shape);
-	ClassDB::bind_method(D_METHOD("get_best_collider"), &KinematicCollision3D::get_best_collider);
-	ClassDB::bind_method(D_METHOD("get_best_collider_id"), &KinematicCollision3D::get_best_collider_id);
-	ClassDB::bind_method(D_METHOD("get_best_collider_rid"), &KinematicCollision3D::get_best_collider_rid);
-	ClassDB::bind_method(D_METHOD("get_best_collider_shape"), &KinematicCollision3D::get_best_collider_shape);
-	ClassDB::bind_method(D_METHOD("get_best_collider_shape_index"), &KinematicCollision3D::get_best_collider_shape_index);
-	ClassDB::bind_method(D_METHOD("get_best_collider_velocity"), &KinematicCollision3D::get_best_collider_velocity);
-
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "travel"), "", "get_travel");
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "remainder"), "", "get_remainder");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_count"), "", "get_collision_count");
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "position"), "", "get_best_position");
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "normal"), "", "get_best_normal");
-	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "local_shape"), "", "get_best_local_shape");
-	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider"), "", "get_best_collider");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_id"), "", "get_best_collider_id");
-	ADD_PROPERTY(PropertyInfo(Variant::RID, "collider_rid"), "", "get_best_collider_rid");
-	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider_shape"), "", "get_best_collider_shape");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_shape_index"), "", "get_best_collider_shape_index");
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collider_velocity"), "", "get_best_collider_velocity");
 }
 
 ///////////////////////////////////////
diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index fdda681350..d2754e7726 100644
--- a/scene/3d/physics_body_3d.h
+++ b/scene/3d/physics_body_3d.h
@@ -50,11 +50,11 @@ protected:
 
 	uint16_t locked_axis = 0;
 
-	Ref<KinematicCollision3D> _move(const Vector3 &p_motion, bool p_test_only = false, real_t p_margin = 0.001, int p_max_collisions = 1);
+	Ref<KinematicCollision3D> _move(const Vector3 &p_linear_velocity, bool p_test_only = false, real_t p_margin = 0.001, int p_max_collisions = 1);
 
 public:
-	bool move_and_collide(const Vector3 &p_motion, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_test_only = false, int p_max_collisions = 1, bool p_cancel_sliding = true, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>());
-	bool test_move(const Transform3D &p_from, const Vector3 &p_motion, const Ref<KinematicCollision3D> &r_collision = Ref<KinematicCollision3D>(), real_t p_margin = 0.001, int p_max_collisions = 1);
+	bool move_and_collide(const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult &r_result, bool p_test_only = false, bool p_cancel_sliding = true);
+	bool test_move(const Transform3D &p_from, const Vector3 &p_linear_velocity, const Ref<KinematicCollision3D> &r_collision = Ref<KinematicCollision3D>(), real_t p_margin = 0.001, int p_max_collisions = 1);
 
 	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);
 	bool get_axis_lock(PhysicsServer3D::BodyAxis p_axis) const;
@@ -487,16 +487,6 @@ public:
 	Object *get_collider_shape(int p_collision_index = 0) const;
 	int get_collider_shape_index(int p_collision_index = 0) const;
 	Vector3 get_collider_velocity(int p_collision_index = 0) const;
-
-	Vector3 get_best_position() const;
-	Vector3 get_best_normal() const;
-	Object *get_best_local_shape() const;
-	Object *get_best_collider() const;
-	ObjectID get_best_collider_id() const;
-	RID get_best_collider_rid() const;
-	Object *get_best_collider_shape() const;
-	int get_best_collider_shape_index() const;
-	Vector3 get_best_collider_velocity() const;
 };
 
 class PhysicalBone3D : public PhysicsBody3D {