Expose body_test_motion in 3D physics server

Results are exposed through PhysicsTestMotionResult3D, the same way it's
done for 2D.

Also cleaned a few things in the 2D version.

7 files changed, 182 insertions, 4 deletions

diff --git a/doc/classes/PhysicsServer3D.xml b/doc/classes/PhysicsServer3D.xml
index aa1f7597ea..88ce222324 100644
--- a/doc/classes/PhysicsServer3D.xml
+++ b/doc/classes/PhysicsServer3D.xml
@@ -772,6 +772,25 @@
 				Sets a body state (see [enum BodyState] constants).
 			</description>
 		</method>
+		<method name="body_test_motion">
+			<return type="bool">
+			</return>
+			<argument index="0" name="body" type="RID">
+			</argument>
+			<argument index="1" name="from" type="Transform3D">
+			</argument>
+			<argument index="2" name="motion" type="Vector3">
+			</argument>
+			<argument index="3" name="infinite_inertia" type="bool">
+			</argument>
+			<argument index="4" name="margin" type="float" default="0.001">
+			</argument>
+			<argument index="5" name="result" type="PhysicsTestMotionResult3D" default="null">
+			</argument>
+			<description>
+				Returns [code]true[/code] if a collision would result from moving in the given direction from a given point in space. Margin increases the size of the shapes involved in the collision detection. [PhysicsTestMotionResult3D] can be passed to return additional information in.
+			</description>
+		</method>
 		<method name="box_shape_create">
 			<return type="RID">
 			</return>
diff --git a/doc/classes/PhysicsTestMotionResult3D.xml b/doc/classes/PhysicsTestMotionResult3D.xml
new file mode 100644
index 0000000000..08c72ba965
--- /dev/null
+++ b/doc/classes/PhysicsTestMotionResult3D.xml
@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="PhysicsTestMotionResult3D" inherits="RefCounted" version="4.0">
+	<brief_description>
+	</brief_description>
+	<description>
+	</description>
+	<tutorials>
+	</tutorials>
+	<methods>
+	</methods>
+	<members>
+		<member name="collider" type="Object" setter="" getter="get_collider">
+		</member>
+		<member name="collider_id" type="int" setter="" getter="get_collider_id" default="0">
+		</member>
+		<member name="collider_rid" type="RID" setter="" getter="get_collider_rid">
+		</member>
+		<member name="collider_shape" type="int" setter="" getter="get_collider_shape" default="0">
+		</member>
+		<member name="collider_velocity" type="Vector3" setter="" getter="get_collider_velocity" default="Vector3(0, 0, 0)">
+		</member>
+		<member name="collision_depth" type="float" setter="" getter="get_collision_depth" default="0.0">
+		</member>
+		<member name="collision_normal" type="Vector3" setter="" getter="get_collision_normal" default="Vector3(0, 0, 0)">
+		</member>
+		<member name="collision_point" type="Vector3" setter="" getter="get_collision_point" default="Vector3(0, 0, 0)">
+		</member>
+		<member name="collision_safe_fraction" type="float" setter="" getter="get_collision_safe_fraction" default="0.0">
+		</member>
+		<member name="collision_unsafe_fraction" type="float" setter="" getter="get_collision_unsafe_fraction" default="0.0">
+		</member>
+		<member name="motion" type="Vector3" setter="" getter="get_motion" default="Vector3(0, 0, 0)">
+		</member>
+		<member name="motion_remainder" type="Vector3" setter="" getter="get_motion_remainder" default="Vector3(0, 0, 0)">
+		</member>
+	</members>
+	<constants>
+	</constants>
+</class>
diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h
index c776641699..a8f7ff3393 100644
--- a/servers/physics_2d/physics_server_2d_wrap_mt.h
+++ b/servers/physics_2d/physics_server_2d_wrap_mt.h
@@ -253,12 +253,12 @@ public:
 
 	FUNC2(body_set_pickable, RID, bool);
 
-	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 {
+	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 {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
 		return physics_2d_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 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 {
+	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 {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
 		return physics_2d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin);
 	}
diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h
index 90f50810f9..1059c197cc 100644
--- a/servers/physics_server_2d.h
+++ b/servers/physics_server_2d.h
@@ -503,7 +503,7 @@ public:
 		Variant collider_metadata;
 	};
 
-	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) = 0;
+	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) = 0;
 
 	struct SeparationResult {
 		real_t collision_depth;
@@ -517,7 +517,7 @@ public:
 		Variant collider_metadata;
 	};
 
-	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) = 0;
+	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) = 0;
 
 	/* JOINT API */
 
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index 1634169e8a..144b2e18cd 100644
--- a/servers/physics_server_3d.cpp
+++ b/servers/physics_server_3d.cpp
@@ -396,8 +396,94 @@ void PhysicsShapeQueryResult3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_result_object_shape", "idx"), &PhysicsShapeQueryResult3D::get_result_object_shape);
 }
 
+///////////////////////////////
+
+Vector3 PhysicsTestMotionResult3D::get_motion() const {
+	return result.motion;
+}
+
+Vector3 PhysicsTestMotionResult3D::get_motion_remainder() const {
+	return result.remainder;
+}
+
+Vector3 PhysicsTestMotionResult3D::get_collision_point() const {
+	return result.collision_point;
+}
+
+Vector3 PhysicsTestMotionResult3D::get_collision_normal() const {
+	return result.collision_normal;
+}
+
+Vector3 PhysicsTestMotionResult3D::get_collider_velocity() const {
+	return result.collider_velocity;
+}
+
+ObjectID PhysicsTestMotionResult3D::get_collider_id() const {
+	return result.collider_id;
+}
+
+RID PhysicsTestMotionResult3D::get_collider_rid() const {
+	return result.collider;
+}
+
+Object *PhysicsTestMotionResult3D::get_collider() const {
+	return ObjectDB::get_instance(result.collider_id);
+}
+
+int PhysicsTestMotionResult3D::get_collider_shape() const {
+	return result.collider_shape;
+}
+
+real_t PhysicsTestMotionResult3D::get_collision_depth() const {
+	return result.collision_depth;
+}
+
+real_t PhysicsTestMotionResult3D::get_collision_safe_fraction() const {
+	return result.collision_safe_fraction;
+}
+
+real_t PhysicsTestMotionResult3D::get_collision_unsafe_fraction() const {
+	return result.collision_unsafe_fraction;
+}
+
+void PhysicsTestMotionResult3D::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("get_motion"), &PhysicsTestMotionResult3D::get_motion);
+	ClassDB::bind_method(D_METHOD("get_motion_remainder"), &PhysicsTestMotionResult3D::get_motion_remainder);
+	ClassDB::bind_method(D_METHOD("get_collision_point"), &PhysicsTestMotionResult3D::get_collision_point);
+	ClassDB::bind_method(D_METHOD("get_collision_normal"), &PhysicsTestMotionResult3D::get_collision_normal);
+	ClassDB::bind_method(D_METHOD("get_collider_velocity"), &PhysicsTestMotionResult3D::get_collider_velocity);
+	ClassDB::bind_method(D_METHOD("get_collider_id"), &PhysicsTestMotionResult3D::get_collider_id);
+	ClassDB::bind_method(D_METHOD("get_collider_rid"), &PhysicsTestMotionResult3D::get_collider_rid);
+	ClassDB::bind_method(D_METHOD("get_collider"), &PhysicsTestMotionResult3D::get_collider);
+	ClassDB::bind_method(D_METHOD("get_collider_shape"), &PhysicsTestMotionResult3D::get_collider_shape);
+	ClassDB::bind_method(D_METHOD("get_collision_depth"), &PhysicsTestMotionResult3D::get_collision_depth);
+	ClassDB::bind_method(D_METHOD("get_collision_safe_fraction"), &PhysicsTestMotionResult3D::get_collision_safe_fraction);
+	ClassDB::bind_method(D_METHOD("get_collision_unsafe_fraction"), &PhysicsTestMotionResult3D::get_collision_unsafe_fraction);
+
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "motion"), "", "get_motion");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "motion_remainder"), "", "get_motion_remainder");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collision_point"), "", "get_collision_point");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collision_normal"), "", "get_collision_normal");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collider_velocity"), "", "get_collider_velocity");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_id", PROPERTY_HINT_OBJECT_ID), "", "get_collider_id");
+	ADD_PROPERTY(PropertyInfo(Variant::RID, "collider_rid"), "", "get_collider_rid");
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider"), "", "get_collider");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_shape"), "", "get_collider_shape");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_depth"), "", "get_collision_depth");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_safe_fraction"), "", "get_collision_safe_fraction");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_unsafe_fraction"), "", "get_collision_unsafe_fraction");
+}
+
 ///////////////////////////////////////
 
+bool PhysicsServer3D::_body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, const Ref<PhysicsTestMotionResult3D> &p_result) {
+	MotionResult *r = nullptr;
+	if (p_result.is_valid()) {
+		r = p_result->get_result_ptr();
+	}
+	return body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r);
+}
+
 RID PhysicsServer3D::shape_create(ShapeType p_shape) {
 	switch (p_shape) {
 		case SHAPE_PLANE:
@@ -551,6 +637,8 @@ void PhysicsServer3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("body_set_ray_pickable", "body", "enable"), &PhysicsServer3D::body_set_ray_pickable);
 
+	ClassDB::bind_method(D_METHOD("body_test_motion", "body", "from", "motion", "infinite_inertia", "margin", "result"), &PhysicsServer3D::_body_test_motion, DEFVAL(0.001), DEFVAL(Variant()));
+
 	ClassDB::bind_method(D_METHOD("body_get_direct_state", "body"), &PhysicsServer3D::body_get_direct_state);
 
 	/* SOFT BODY API */
diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h
index fcdd207843..1fabedc6ad 100644
--- a/servers/physics_server_3d.h
+++ b/servers/physics_server_3d.h
@@ -225,11 +225,15 @@ public:
 	virtual ~RenderingServerHandler() {}
 };
 
+class PhysicsTestMotionResult3D;
+
 class PhysicsServer3D : public Object {
 	GDCLASS(PhysicsServer3D, Object);
 
 	static PhysicsServer3D *singleton;
 
+	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, const Ref<PhysicsTestMotionResult3D> &p_result = Ref<PhysicsTestMotionResult3D>());
+
 protected:
 	static void _bind_methods();
 
@@ -770,6 +774,33 @@ public:
 	~PhysicsServer3D();
 };
 
+class PhysicsTestMotionResult3D : public RefCounted {
+	GDCLASS(PhysicsTestMotionResult3D, RefCounted);
+
+	PhysicsServer3D::MotionResult result;
+	friend class PhysicsServer3D;
+
+protected:
+	static void _bind_methods();
+
+public:
+	PhysicsServer3D::MotionResult *get_result_ptr() const { return const_cast<PhysicsServer3D::MotionResult *>(&result); }
+
+	Vector3 get_motion() const;
+	Vector3 get_motion_remainder() const;
+
+	Vector3 get_collision_point() const;
+	Vector3 get_collision_normal() const;
+	Vector3 get_collider_velocity() const;
+	ObjectID get_collider_id() const;
+	RID get_collider_rid() const;
+	Object *get_collider() const;
+	int get_collider_shape() const;
+	real_t get_collision_depth() const;
+	real_t get_collision_safe_fraction() const;
+	real_t get_collision_unsafe_fraction() const;
+};
+
 typedef PhysicsServer3D *(*CreatePhysicsServer3DCallback)();
 
 class PhysicsServer3DManager {
diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp
index 59c7dcc7d2..4e309927bb 100644
--- a/servers/register_server_types.cpp
+++ b/servers/register_server_types.cpp
@@ -220,6 +220,7 @@ void register_server_types() {
 	ClassDB::register_virtual_class<PhysicsDirectBodyState3D>();
 	ClassDB::register_virtual_class<PhysicsDirectSpaceState3D>();
 	ClassDB::register_virtual_class<PhysicsShapeQueryResult3D>();
+	ClassDB::register_class<PhysicsTestMotionResult3D>();
 
 	// Physics 2D
 	GLOBAL_DEF(PhysicsServer2DManager::setting_property_name, "DEFAULT");