diff --git a/src/box2d-wrapper/box2d_wrapper.cpp b/src/box2d-wrapper/box2d_wrapper.cpp
index 57f8641..9d5674d 100644
--- a/src/box2d-wrapper/box2d_wrapper.cpp
+++ b/src/box2d-wrapper/box2d_wrapper.cpp
@@ -987,9 +987,12 @@ ShapeCollideResult box2d::shape_collide(const b2Vec2 motion1,
 				continue;
 			}
 			ShapeCollideResult result;
-			result.collided = is_toi_intersected(toi_output);
-			result.witness1 = transformA.p + toi_output.t * motion1;
-			result.witness2 = transformB.p + toi_output.t * motion2;
+			result.collided = true;
+			transformA.p = transformA.p + toi_output.t * motion1;
+			transformB.p = transformB.p + toi_output.t * motion2;
+			b2DistanceOutput distance_output = _call_b2_distance(transformA, shape_info1.handle.handles[i], 0, transformB, shape_info2.handle.handles[j], 0);
+			result.witness1 = distance_output.pointA;
+			result.witness2 = distance_output.pointB;
 			return result;
 		}
 	}
diff --git a/src/servers/box2d_physics_server_2d.cpp b/src/servers/box2d_physics_server_2d.cpp
index fb56ca7..8120d28 100644
--- a/src/servers/box2d_physics_server_2d.cpp
+++ b/src/servers/box2d_physics_server_2d.cpp
@@ -141,16 +141,19 @@ bool Box2DPhysicsServer2D::_shape_collide(const RID &p_shape_A, const Transform2
 	box2d::QueryExcludedInfo query_excluded_info = box2d::default_query_excluded_info();
 
 	int array_idx = 0;
-	box2d::ShapeCollideResult result = box2d::shape_collide(box2d_A_motion, shape_A_info, box2d_A_motion, shape_A_info);
+	box2d::ShapeCollideResult result = box2d::shape_collide(box2d_A_motion, shape_A_info, box2d_B_motion, shape_B_info);
 	if (!result.collided) {
 		return false;
 	}
 	(*p_result_count)++;
+	if (p_result_max > 0) {
+		results_out[array_idx++] = Vector2(result.witness1.x, result.witness1.y);
+	}
+	if (p_result_max > 1) {
+		results_out[array_idx++] = Vector2(result.witness2.x, result.witness2.y);
+	}
 
-	results_out[array_idx++] = Vector2(result.witness1.x, result.witness1.y);
-	results_out[array_idx++] = Vector2(result.witness2.x, result.witness2.y);
-
-	return array_idx > 0;
+	return true;
 }
 
 RID Box2DPhysicsServer2D::_space_create() {