2 files changed, 195 insertions, 179 deletions

diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index 2a31cf1c22..f507cacdc3 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -35,7 +35,7 @@
 
 #define fallback_collision_solver gjk_epa_calculate_penetration
 
-// Cylinder SAT analytic methods for Cylinder-trimesh and Cylinder-box are based on ODE colliders.
+// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders.
 
 /*
  *	Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac
@@ -119,28 +119,9 @@ static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_poi
 	ERR_FAIL_COND(p_point_count_B != 3);
 #endif
 
-	const Vector3 &point_A = p_points_A[0];
-
-	const Vector3 &circle_B_pos = p_points_B[0];
-	Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
-	Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
-
-	real_t circle_B_radius = circle_B_line_1.length();
-	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
-
-	// Project point onto Circle B plane.
-	Plane circle_plane(circle_B_pos, circle_B_normal);
-	Vector3 proj_point_A = circle_plane.project(point_A);
-
-	// Clip point.
-	Vector3 delta_point_1 = proj_point_A - circle_B_pos;
-	real_t dist_point_1 = delta_point_1.length_squared();
-	if (!Math::is_zero_approx(dist_point_1)) {
-		dist_point_1 = Math::sqrt(dist_point_1);
-		proj_point_A = circle_B_pos + delta_point_1 * MIN(dist_point_1, circle_B_radius) / dist_point_1;
-	}
+	Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A);
 
-	p_callback->call(point_A, proj_point_A);
+	p_callback->call(*p_points_A, closest_B);
 }
 
 static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@@ -189,6 +170,104 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_
 	p_callback->call(closest_A, closest_B);
 }
 
+static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A != 2);
+	ERR_FAIL_COND(p_point_count_B != 3);
+#endif
+
+	const Vector3 &circle_B_pos = p_points_B[0];
+	Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
+	Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
+
+	real_t circle_B_radius = circle_B_line_1.length();
+	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
+
+	Plane circle_plane(circle_B_pos, circle_B_normal);
+
+	static const int max_clip = 2;
+	Vector3 contact_points[max_clip];
+	int num_points = 0;
+
+	// Project edge point in circle plane.
+	const Vector3 &edge_A_1 = p_points_A[0];
+	Vector3 proj_point_1 = circle_plane.project(edge_A_1);
+
+	Vector3 dist_vec = proj_point_1 - circle_B_pos;
+	real_t dist_sq = dist_vec.length_squared();
+
+	// Point 1 is inside disk, add as contact point.
+	if (dist_sq <= circle_B_radius * circle_B_radius) {
+		contact_points[num_points] = edge_A_1;
+		++num_points;
+	}
+
+	const Vector3 &edge_A_2 = p_points_A[1];
+	Vector3 proj_point_2 = circle_plane.project(edge_A_2);
+
+	Vector3 dist_vec_2 = proj_point_2 - circle_B_pos;
+	real_t dist_sq_2 = dist_vec_2.length_squared();
+
+	// Point 2 is inside disk, add as contact point.
+	if (dist_sq_2 <= circle_B_radius * circle_B_radius) {
+		contact_points[num_points] = edge_A_2;
+		++num_points;
+	}
+
+	if (num_points < 2) {
+		Vector3 line_vec = proj_point_2 - proj_point_1;
+		real_t line_length_sq = line_vec.length_squared();
+
+		// Create a quadratic formula of the form ax^2 + bx + c = 0
+		real_t a, b, c;
+
+		a = line_length_sq;
+		b = 2.0 * dist_vec.dot(line_vec);
+		c = dist_sq - circle_B_radius * circle_B_radius;
+
+		// Solve for t.
+		real_t sqrtterm = b * b - 4.0 * a * c;
+
+		// If the term we intend to square root is less than 0 then the answer won't be real,
+		// so the line doesn't intersect.
+		if (sqrtterm >= 0) {
+			sqrtterm = Math::sqrt(sqrtterm);
+
+			Vector3 edge_dir = edge_A_2 - edge_A_1;
+
+			real_t fraction_1 = (-b - sqrtterm) / (2.0 * a);
+			if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) {
+				Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir;
+				ERR_FAIL_COND(num_points >= max_clip);
+				contact_points[num_points] = face_point_1;
+				++num_points;
+			}
+
+			real_t fraction_2 = (-b + sqrtterm) / (2.0 * a);
+			if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) {
+				Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir;
+				ERR_FAIL_COND(num_points >= max_clip);
+				contact_points[num_points] = face_point_2;
+				++num_points;
+			}
+		}
+	}
+
+	// Generate contact points.
+	for (int i = 0; i < num_points; i++) {
+		const Vector3 &contact_point_A = contact_points[i];
+
+		real_t d = circle_plane.distance_to(contact_point_A);
+		Vector3 closest_B = contact_point_A - circle_plane.normal * d;
+
+		if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) {
+			continue;
+		}
+
+		p_callback->call(contact_point_A, closest_B);
+	}
+}
+
 static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 #ifdef DEBUG_ENABLED
 	ERR_FAIL_COND(p_point_count_A < 2);
@@ -280,7 +359,7 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
 
 static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND(p_point_count_A < 2);
+	ERR_FAIL_COND(p_point_count_A < 3);
 	ERR_FAIL_COND(p_point_count_B != 3);
 #endif
 
@@ -288,150 +367,60 @@ static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_poin
 	Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
 	Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
 
-	real_t circle_B_radius = circle_B_line_1.length();
+	// Clip face with circle segments.
+	static const int circle_segments = 8;
+	Vector3 circle_points[circle_segments];
+
+	real_t angle_delta = 2.0 * Math_PI / circle_segments;
+
+	for (int i = 0; i < circle_segments; ++i) {
+		Vector3 point_pos = circle_B_pos;
+		point_pos += circle_B_line_1 * Math::cos(i * angle_delta);
+		point_pos += circle_B_line_2 * Math::sin(i * angle_delta);
+		circle_points[i] = point_pos;
+	}
+
+	_generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback);
+
+	// Clip face with circle plane.
 	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
 
 	Plane circle_plane(circle_B_pos, circle_B_normal);
 
-	bool edge = (p_point_count_A == 2);
-
 	static const int max_clip = 32;
 	Vector3 contact_points[max_clip];
 	int num_points = 0;
 
-	// Clip edges with circle.
 	for (int i = 0; i < p_point_count_A; i++) {
 		int i_n = (i + 1) % p_point_count_A;
 
-		// Project edge point in circle plane.
-		const Vector3 &edge_A_1 = p_points_A[i];
-		Vector3 proj_point_1 = circle_plane.project(edge_A_1);
+		const Vector3 &edge0_A = p_points_A[i];
+		const Vector3 &edge1_A = p_points_A[i_n];
 
-		Vector3 dist_vec = proj_point_1 - circle_B_pos;
-		real_t dist_sq = dist_vec.length_squared();
+		real_t dist0 = circle_plane.distance_to(edge0_A);
+		real_t dist1 = circle_plane.distance_to(edge1_A);
 
-		// Point 1 is inside disk, add as contact point.
-		if (dist_sq <= circle_B_radius * circle_B_radius) {
-			//p_callback->call(edge_A_1, proj_point_1);
+		// First point in front of plane, generate contact point.
+		if (dist0 * circle_plane.d >= 0) {
 			ERR_FAIL_COND(num_points >= max_clip);
-			contact_points[num_points] = edge_A_1;
+			contact_points[num_points] = edge0_A;
 			++num_points;
 		}
-		// No need to test point 2 now, as it will be part of the next edge.
-
-		if (edge && i > 0) {
-			// Done with testing the only two points.
-			break;
-		}
 
-		// Project edge point in circle plane.
-		const Vector3 &edge_A_2 = p_points_A[i_n];
-		Vector3 proj_point_2 = circle_plane.project(edge_A_2);
+		// Points on different sides, generate contact point.
+		if (dist0 * dist1 < 0) {
+			// calculate intersection
+			Vector3 rel = edge1_A - edge0_A;
+			real_t den = circle_plane.normal.dot(rel);
+			real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den;
+			Vector3 inters = edge0_A + rel * dist;
 
-		Vector3 line_vec = proj_point_2 - proj_point_1;
-		real_t line_length_sq = line_vec.length_squared();
-
-		// Create a quadratic formula of the form ax^2 + bx + c = 0
-		real_t a, b, c;
-
-		a = line_length_sq;
-		b = 2.0 * dist_vec.dot(line_vec);
-		c = dist_sq - circle_B_radius * circle_B_radius;
-
-		// Solve for t.
-		real_t sqrtterm = b * b - 4.0 * a * c;
-
-		// If the term we intend to square root is less than 0 then the answer won't be real,
-		// so the line doesn't intersect.
-		if (sqrtterm < 0) {
-			continue;
-		}
-
-		sqrtterm = Math::sqrt(sqrtterm);
-
-		Vector3 edge_dir = edge_A_2 - edge_A_1;
-
-		real_t fraction_1 = (-b - sqrtterm) / (2.0 * a);
-		if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) {
-			//Vector3 intersection_1 = proj_point_1 + fraction_1 * line_vec;
-			Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir;
-			//p_callback->call(face_point_1, intersection_1);
-			ERR_FAIL_COND(num_points >= max_clip);
-			contact_points[num_points] = face_point_1;
-			++num_points;
-		}
-
-		real_t fraction_2 = (-b + sqrtterm) / (2.0 * a);
-		if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) {
-			//Vector3 intersection_2 = proj_point_1 + fraction_2 * line_vec;
-			Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir;
-			//p_callback->call(face_point_2, intersection_2);
 			ERR_FAIL_COND(num_points >= max_clip);
-			contact_points[num_points] = face_point_2;
+			contact_points[num_points] = inters;
 			++num_points;
 		}
 	}
 
-	// In case of a face, add extra contact points for proper support.
-	if (!edge) {
-		Plane plane_A(p_points_A[0], p_points_A[1], p_points_A[2]);
-
-		if (num_points < 3) {
-			if (num_points == 0) {
-				// Use 3 arbitrary equidistant points from the circle.
-				for (int i = 0; i < 3; ++i) {
-					Vector3 circle_point = circle_B_pos;
-					circle_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0);
-					circle_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0);
-
-					Vector3 face_point = plane_A.project(circle_point);
-
-					contact_points[num_points] = face_point;
-					++num_points;
-				}
-			} else if (num_points == 1) {
-				Vector3 line_center = circle_B_pos - contact_points[0];
-				Vector3 line_tangent = line_center.cross(plane_A.normal);
-
-				Vector3 dir = line_tangent.cross(plane_A.normal).normalized();
-				if (line_center.dot(dir) > 0.0) {
-					// Use 2 equidistant points on the circle inside the face.
-					line_center.normalize();
-					line_tangent.normalize();
-					for (int i = 0; i < 2; ++i) {
-						Vector3 circle_point = circle_B_pos;
-						circle_point -= line_center * circle_B_radius * Math::cos(2.0 * Math_PI * (i + 1) / 3.0);
-						circle_point += line_tangent * circle_B_radius * Math::sin(2.0 * Math_PI * (i + 1) / 3.0);
-
-						Vector3 face_point = plane_A.project(circle_point);
-
-						contact_points[num_points] = face_point;
-						++num_points;
-					}
-				}
-				// Otherwise the circle touches an edge from the outside, no extra contact point.
-			} else { // if (num_points == 2)
-				// Use equidistant 3rd point on the circle inside the face.
-				Vector3 contacts_line = contact_points[1] - contact_points[0];
-				Vector3 dir = contacts_line.cross(plane_A.normal).normalized();
-
-				Vector3 circle_point = contact_points[0] + 0.5 * contacts_line;
-				Vector3 line_center = (circle_B_pos - circle_point);
-
-				if (line_center.dot(dir) > 0.0) {
-					circle_point += dir * (line_center.length() + circle_B_radius);
-				} else {
-					circle_point += dir * (circle_B_radius - line_center.length());
-				}
-
-				Vector3 face_point = plane_A.project(circle_point);
-
-				contact_points[num_points] = face_point;
-				++num_points;
-			}
-		}
-	}
-
 	// Generate contact points.
 	for (int i = 0; i < num_points; i++) {
 		const Vector3 &contact_point_A = contact_points[i];
@@ -567,7 +556,7 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po
 				nullptr,
 				_generate_contacts_edge_edge,
 				_generate_contacts_face_face,
-				_generate_contacts_face_circle,
+				_generate_contacts_edge_circle,
 		},
 		{
 				nullptr,
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index 40537c7001..dd5754b9ac 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -384,6 +384,8 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_
 struct _RestCallbackData {
 	const CollisionObject3DSW *object;
 	const CollisionObject3DSW *best_object;
+	int local_shape;
+	int best_local_shape;
 	int shape;
 	int best_shape;
 	Vector3 best_contact;
@@ -409,6 +411,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B,
 	rd->best_normal = contact_rel / len;
 	rd->best_object = rd->object;
 	rd->best_shape = rd->shape;
+	rd->best_local_shape = rd->local_shape;
 }
 
 bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
@@ -739,8 +742,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
 	body_aabb = body_aabb.grow(p_margin);
 
+	real_t motion_length = p_motion.length();
+	Vector3 motion_normal = p_motion / motion_length;
+
 	Transform body_transform = p_from;
 
+	bool recovered = false;
+
 	{
 		//STEP 1, FREE BODY IF STUCK
 
@@ -791,7 +799,17 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 			for (int i = 0; i < cbk.amount; i++) {
 				Vector3 a = sr[i * 2 + 0];
 				Vector3 b = sr[i * 2 + 1];
-				recover_motion += (b - a) / cbk.amount;
+
+				// Compute plane on b towards a.
+				Vector3 n = (a - b).normalized();
+				real_t d = n.dot(b);
+
+				// Compute depth on recovered motion.
+				real_t depth = n.dot(a + recover_motion) - d;
+				if (depth > 0.0) {
+					// Only recover if there is penetration.
+					recover_motion -= n * depth * 0.4;
+				}
 			}
 
 			if (recover_motion == Vector3()) {
@@ -799,6 +817,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 				break;
 			}
 
+			recovered = true;
+
 			body_transform.origin += recover_motion;
 			body_aabb.position += recover_motion;
 
@@ -848,14 +868,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 
 				//test initial overlap, does it collide if going all the way?
 				Vector3 point_A, point_B;
-				Vector3 sep_axis = p_motion.normalized();
+				Vector3 sep_axis = motion_normal;
 
 				Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 				//test initial overlap, does it collide if going all the way?
 				if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
 					continue;
 				}
-				sep_axis = p_motion.normalized();
+				sep_axis = motion_normal;
 
 				if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
 					stuck = true;
@@ -865,13 +885,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 				//just do kinematic solving
 				real_t low = 0;
 				real_t hi = 1;
-				Vector3 mnormal = p_motion.normalized();
 
 				for (int k = 0; k < 8; k++) { //steps should be customizable..
 
 					real_t ofs = (low + hi) * 0.5;
 
-					Vector3 sep = mnormal; //important optimization for this to work fast enough
+					Vector3 sep = motion_normal; //important optimization for this to work fast enough
 
 					mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
 
@@ -912,16 +931,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 	}
 
 	bool collided = false;
-	if (safe >= 1) {
-		//not collided
-		collided = false;
-		if (r_result) {
-			r_result->motion = p_motion;
-			r_result->remainder = Vector3();
-			r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+	if (recovered || (safe < 1)) {
+		if (safe >= 1) {
+			best_shape = -1; //no best shape with cast, reset to -1
 		}
 
-	} else {
 		//it collided, let's get the rest info in unsafe advance
 		Transform ugt = body_transform;
 		ugt.origin += p_motion * unsafe;
@@ -930,25 +944,40 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 		rcd.best_len = 0;
 		rcd.best_object = nullptr;
 		rcd.best_shape = 0;
-		rcd.min_allowed_depth = test_motion_min_contact_depth;
 
-		Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
-		Shape3DSW *body_shape = p_body->get_shape(best_shape);
+		// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
+		rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
 
-		body_aabb.position += p_motion * unsafe;
+		int from_shape = best_shape != -1 ? best_shape : 0;
+		int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
 
-		int amount = _cull_aabb_for_body(p_body, body_aabb);
+		for (int j = from_shape; j < to_shape; j++) {
+			if (p_body->is_shape_set_as_disabled(j)) {
+				continue;
+			}
 
-		for (int i = 0; i < amount; i++) {
-			const CollisionObject3DSW *col_obj = intersection_query_results[i];
-			int shape_idx = intersection_query_subindex_results[i];
+			Transform body_shape_xform = ugt * p_body->get_shape_transform(j);
+			Shape3DSW *body_shape = p_body->get_shape(j);
 
-			rcd.object = col_obj;
-			rcd.shape = shape_idx;
-			bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
-			if (!sc) {
+			if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
 				continue;
 			}
+
+			body_aabb.position += p_motion * unsafe;
+
+			int amount = _cull_aabb_for_body(p_body, body_aabb);
+
+			for (int i = 0; i < amount; i++) {
+				const CollisionObject3DSW *col_obj = intersection_query_results[i];
+				int shape_idx = intersection_query_subindex_results[i];
+
+				rcd.object = col_obj;
+				rcd.shape = shape_idx;
+				bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
+				if (!sc) {
+					continue;
+				}
+			}
 		}
 
 		if (rcd.best_len != 0) {
@@ -956,7 +985,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 				r_result->collider = rcd.best_object->get_self();
 				r_result->collider_id = rcd.best_object->get_instance_id();
 				r_result->collider_shape = rcd.best_shape;
-				r_result->collision_local_shape = best_shape;
+				r_result->collision_local_shape = rcd.best_local_shape;
 				r_result->collision_normal = rcd.best_normal;
 				r_result->collision_point = rcd.best_contact;
 				//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
@@ -972,17 +1001,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 			}
 
 			collided = true;
-		} else {
-			if (r_result) {
-				r_result->motion = p_motion;
-				r_result->remainder = Vector3();
-				r_result->motion += (body_transform.get_origin() - p_from.get_origin());
-			}
-
-			collided = false;
 		}
 	}
 
+	if (!collided && r_result) {
+		r_result->motion = p_motion;
+		r_result->remainder = Vector3();
+		r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+	}
+
 	return collided;
 }