feat: added CollisionDetectionInformation.ContactPoint

Engine.Physics2D/CollisionDetectionInformation.cs

@@ -7,14 +7,16 @@ public readonly struct CollisionDetectionInformation
 (
     ICollider2D Detector,
     ICollider2D Detected,
+    Vector2D ContactPoint,
     Vector2D Normal,
     float Penetration
 )
 {
     public ICollider2D Detector { get; init; } = Detector;
     public ICollider2D Detected { get; init; } = Detected;
+    public Vector2D ContactPoint { get; init; } = ContactPoint;
     public Vector2D Normal { get; init; } = Normal;
     public float Penetration { get; init; } = Penetration;
 
-    public CollisionDetectionInformation Reverse() => new(Detected, Detector, -Normal, Penetration);
+    public CollisionDetectionInformation Reverse() => new(Detected, Detector, ContactPoint, -Normal, Penetration);
 }

Engine.Physics2D/CollisionDetector2D.cs

@@ -1,3 +1,5 @@
+using System.Collections.Generic;
+
 using Engine.Core;
 using Engine.Physics2D;
 
@@ -41,12 +43,13 @@ public class CollisionDetector2D : ICollisionDetector2D
 
     private static bool DetectShapeShapeOneWay(IShapeCollider2D left, IShapeCollider2D right, ref CollisionDetectionInformation collisionInformation)
     {
-        System.Collections.Generic.IReadOnlyList<Vector2D> vertices = left.ShapeWorld.Vertices;
+        IReadOnlyList<Vector2D> vertices = left.ShapeWorld.Vertices;
         int count = vertices.Count;
 
         for (int indexProjection = 0; indexProjection < count; indexProjection++)
         {
-            Vector2D projectionVector = vertices[indexProjection].FromTo(vertices[(indexProjection + 1) % count]).Perpendicular().Normalized;
+            Vector2D leftEdge = vertices[indexProjection].FromTo(vertices[(indexProjection + 1) % count]);
+            Vector2D projectionVector = leftEdge.Perpendicular().Normalized;
 
             Projection1D leftProjection = left.ShapeWorld.ToProjection(projectionVector);
             Projection1D rightProjection = right.ShapeWorld.ToProjection(projectionVector);
@@ -55,17 +58,61 @@ public class CollisionDetector2D : ICollisionDetector2D
                 return false;
 
             if (collisionInformation.Detector is null || Math.Abs(collisionInformation.Penetration) > Math.Abs(depth))
-                collisionInformation = new(left, right, projectionVector, depth);
+            {
+                Vector2D contactPoint = FindShapeToShapeContactPoint(left, right, projectionVector);
+                collisionInformation = new(left, right, contactPoint, projectionVector, depth);
+            }
         }
 
         return true;
     }
 
+    private static Vector2D FindShapeToShapeContactPoint(IShapeCollider2D left, IShapeCollider2D right, Vector2D contactProjectionVector)
+    {
+        IReadOnlyList<Vector2D> leftVertices = left.ShapeWorld.Vertices;
+        IReadOnlyList<Vector2D> rightVertices = right.ShapeWorld.Vertices;
+
+        Line2D leftSupportLine = GetSupportLine(leftVertices, contactProjectionVector);
+        Line2D rightSupportLine = GetSupportLine(rightVertices, -contactProjectionVector);
+
+        if (leftSupportLine.Direction.Dot(rightSupportLine.Direction).Abs() > .99f)
+            return (leftSupportLine.From + leftSupportLine.To + rightSupportLine.From + rightSupportLine.To) / 4f;
+
+        return leftSupportLine.IntersectionPoint(rightSupportLine);
+    }
+
+    private static Line2D GetSupportLine(IReadOnlyList<Vector2D> vertices, Vector2D contactProjectionVector)
+    {
+        System.Span<Vector2D> points = stackalloc Vector2D[2];
+        System.Span<float> distances = stackalloc float[2] { float.MaxValue, float.MaxValue };
+        for (int i = 0; i < vertices.Count; i++)
+        {
+            Vector2D point = vertices[i];
+            float distance = contactProjectionVector.Dot(point);
+
+            if (distance < distances[0])
+            {
+                points[1] = points[0];
+                distances[1] = distances[0];
+
+                points[0] = point;
+                distances[0] = distance;
+            }
+            else if (distance < distances[1])
+            {
+                points[1] = point;
+                distances[1] = distance;
+            }
+        }
+
+        return new(points[0], points[1]);
+    }
+
     private static bool DetectShapeCircle(IShapeCollider2D shapeCollider, ICircleCollider2D circleCollider, out CollisionDetectionInformation collisionInformation)
     {
         collisionInformation = default;
 
-        System.Collections.Generic.IReadOnlyList<Vector2D> vertices = shapeCollider.ShapeWorld.Vertices;
+        IReadOnlyList<Vector2D> vertices = shapeCollider.ShapeWorld.Vertices;
         int count = vertices.Count;
 
         for (int indexProjection = 0; indexProjection < count; indexProjection++)
@@ -78,8 +125,10 @@ public class CollisionDetector2D : ICollisionDetector2D
             if (!shapeProjection.Overlaps(circleProjection, out float depth))
                 return false;
 
+            Vector2D contactPoint = circleCollider.CircleWorld.Center + projectionVector * circleCollider.CircleWorld.Radius;
+
             if (collisionInformation.Detector is null || Math.Abs(collisionInformation.Penetration) > Math.Abs(depth))
-                collisionInformation = new(shapeCollider, circleCollider, projectionVector, depth);
+                collisionInformation = new(shapeCollider, circleCollider, contactPoint, projectionVector, depth);
         }
 
         {
@@ -91,8 +140,10 @@ public class CollisionDetector2D : ICollisionDetector2D
             if (!shapeProjection.Overlaps(circleProjection, out float depth))
                 return false;
 
+            Vector2D contactPoint = circleCollider.CircleWorld.Center + shapeToCircleProjectionVector * circleCollider.CircleWorld.Radius;
+
             if (collisionInformation.Detector is null || Math.Abs(collisionInformation.Penetration) > Math.Abs(depth))
-                collisionInformation = new(shapeCollider, circleCollider, shapeToCircleProjectionVector, depth);
+                collisionInformation = new(shapeCollider, circleCollider, contactPoint, shapeToCircleProjectionVector, depth);
         }
 
         return true;
@@ -110,7 +161,10 @@ public class CollisionDetector2D : ICollisionDetector2D
         bool collision = leftProjection.Overlaps(rightProjection, out float depth);
 
         if (collision)
-            collisionInformation = new(left, right, leftToRightCenterProjectionVector, depth);
+        {
+            Vector2D contactPoint = left.CircleWorld.Center + leftToRightCenterProjectionVector * left.CircleWorld.Radius;
+            collisionInformation = new(left, right, contactPoint, leftToRightCenterProjectionVector, depth);
+        }
 
         return collision;
     }