reorganized collider code

collider/system.py

@@ -0,0 +1,104 @@
+import pygame as pg
+
+from collider.types import *
+from transform import Transform
+
+def _interval_overlap(a: tuple[float, float], b: tuple[float, float]) -> float | None:
+    if a[0] <= b[1] and b[0] <= a[1]:
+        return min(a[1], b[1]) - max(a[0], b[0]) 
+    return None
+
+def _collide_circle_circle(a: CircleCollider, b: CircleCollider, a_transform: Transform, b_transform: Transform) -> ColliderContact | None:
+    delta = a_transform.global_position - b_transform.global_position
+    dist = delta.length()
+    radii = b.radius + b.radius
+    if dist >= radii or dist == 0:
+        return None
+    normal = delta.normalize()
+    return ColliderContact(
+        point=a_transform.global_position + normal * b.radius,
+        normal=normal,
+        penetration=radii - dist,
+        )
+
+def _collide_convex_circle(a: ConvexCollider, b: CircleCollider, a_transform: Transform, b_transform: Transform) -> ColliderContact | None:
+    hull = a.hull(a_transform)
+    normals = [face.normal for face in hull.faces()]
+    circle_normal = min([(b_transform.global_position - v) for v in hull.vertices()], key=lambda v: v.length()).normalize()
+    normals.append(circle_normal)
+    collision_normal: pg.Vector2 | None = None
+    lowest_pen = float('inf')
+    for normal in normals:
+        center_proj = normal.dot(b_transform.global_position)
+        circle_interval = (center_proj - b.radius, center_proj + b.radius)
+        convex_interval = hull.project(normal)
+        penetration = _interval_overlap(circle_interval, convex_interval)
+        if penetration is None:
+            return None
+        if penetration < lowest_pen:
+            lowest_pen = penetration
+            collision_normal = normal
+    return ColliderContact(
+        points=[b_transform.global_position - b.radius*normal],
+        normal=collision_normal,
+        penetration=lowest_pen
+    )
+
+def _collide_convex_convex(a: ConvexCollider, b: ConvexCollider, a_transform: Transform, b_transform: Transform) -> ColliderContact | None:  
+    #SAT
+    hull_a = a.hull(a_transform)
+    hull_b = b.hull(b_transform)
+    normals = [*[face.normal for face in hull_a.faces()],*[face.normal for face in hull_b.faces()]]
+    collision_normal: pg.Vector2 | None = None
+    lowest_pen = float('inf')
+    for normal in normals:
+        a_interval = hull_a.project(normal)
+        b_interval = hull_b.project(normal)
+        penetration = _interval_overlap(a_interval, b_interval)
+        if penetration is None:
+            return None
+        if penetration < lowest_pen and (a_transform.position - b_transform.position).dot(normal) > 0:
+            lowest_pen = penetration
+            collision_normal = normal
+
+
+    #sutherland hodgman clipping
+    ref_face = max(hull_b.faces(), key=lambda f: f.normal.dot(collision_normal))
+    incident_face = min(hull_a.faces(), key=lambda f: f.normal.dot(collision_normal))
+    left_normal = (ref_face.begin - ref_face.end).normalize()
+    right_normal = (ref_face.end - ref_face.begin).normalize()
+
+    contact_manifold = [incident_face.begin, incident_face.end]
+
+    def clip(normal: pg.Vector2, ref_point: pg.Vector2) -> None:
+        d1 = (contact_manifold[0] - ref_point).dot(normal)
+        d2 = (contact_manifold[1] - ref_point).dot(normal)
+        if d1 > 0 and d2 > 0:
+            raise Exception("CLIPPING ERROR")
+        if d1 > 0:
+            contact_manifold[0] = contact_manifold[0] + (d1 / (d1 - d2)) * (contact_manifold[1] - contact_manifold[0])
+        if d2 > 0:
+            contact_manifold[1] = contact_manifold[1] + (d2 / (d2 - d1)) * (contact_manifold[0] - contact_manifold[1])
+
+    clip(right_normal, ref_face.end)
+    clip(left_normal, ref_face.begin)
+    clip(ref_face.normal, ref_face.begin)
+
+    return ColliderContact(
+        points=contact_manifold,
+        normal=collision_normal,
+        penetration=lowest_pen
+    )
+
+def intersect(a: BaseCollider, b: BaseCollider, a_transform: Transform, b_transform: Transform) -> ColliderContact | None:
+    if isinstance(a, ConvexCollider) and isinstance(b, ConvexCollider):
+        return _collide_convex_convex(a,b,a_transform,b_transform)
+    if isinstance(a, ConvexCollider) and isinstance(b, CircleCollider):
+        return _collide_convex_circle(a, b, a_transform, b_transform)
+    if isinstance(a, CircleCollider) and isinstance(b, ConvexCollider):
+        collision = _collide_convex_circle(b,a,b_transform,a_transform)
+        if collision: collision.normal *= -1
+        return collision
+    if isinstance(a, CircleCollider) and isinstance(b, CircleCollider):
+        return _collide_circle_circle(a,b,a_transform,b_transform)
+    raise NotImplementedError(f"No collision defined between collider types {type(a)} and {type(b)}")