overhaul to the collision system... should have maybe made more commits.

collider.py

@@ -0,0 +1,206 @@
+from dataclasses import dataclass
+from abc import ABC, abstractmethod
+from typing import Generator
+
+import pygame as pg
+from math import pi
+
+from transform import Transform
+
+@dataclass
+class Face:
+    begin: pg.Vector2
+    end: pg.Vector2
+
+    @property
+    def normal(self) -> pg.Vector2:
+        return (self.end-self.begin).rotate(-90).normalize()
+    
+@dataclass
+class ColliderContact:
+    __slots__ = ["points", "normal", "penetration"]
+    points: list[pg.Vector2]
+    normal: pg.Vector2
+    penetration: float
+
+@dataclass(frozen=True)
+class PolygonalHull:
+    _vertices: list[pg.Vector2]
+
+    def get_vertex_at_index(self, key: int) -> pg.Vector2:
+        return self._vertices[key]
+
+    def get_face_at_index(self, key: int) -> Face:
+        return Face(self._vertices[key], self._vertices[(key + 1) % len(self._vertices)])
+    
+    def vertices(self) -> Generator[pg.Vector2, None, None]:
+        for v in self._vertices:
+            yield v
+    
+    def faces(self) -> Generator[Face, None, None]:
+        for i in range(len(self._vertices)):
+            yield self.get_face_at_index(i)
+
+    def project(self, axis: pg.Vector2) -> tuple[float, float]:
+        projections = [v.dot(axis) for v in self._vertices]
+        return (min(projections), max(projections))
+
+
+class BaseCollider(ABC):
+
+    @abstractmethod
+    def moment_of_inertia(self, mass: float) -> float:
+        pass
+
+class ConvexCollider(BaseCollider):
+
+    @abstractmethod
+    def hull(self, transform: Transform) -> PolygonalHull:
+        pass
+    
+@dataclass
+class CircleCollider(BaseCollider):
+
+    radius: float
+
+    def moment_of_inertia(self, mass: float) -> float:
+        return 0.5 * self.radius ** 2 * mass
+
+@dataclass
+class LineCollider(ConvexCollider):
+
+    length: float
+
+    def hull(self, transform: Transform) -> PolygonalHull:
+        return PolygonalHull([
+            transform.global_position - pg.Vector2(self.length / 2.0, 0).rotate(transform.global_degrees) * transform.global_scale,
+            transform.global_position + pg.Vector2(self.length / 2.0, 0).rotate(transform.global_degrees) * transform.global_scale
+        ])
+    
+    def moment_of_inertia(self, mass):
+        return 1.0 / 12.0 * mass * self.length**2
+
+
+@dataclass
+class RectCollider(ConvexCollider):
+
+    dimensions: tuple[float, float]
+
+    @property
+    def width(self):
+        return self.dimensions[0]
+    
+    @property
+    def height(self):
+        return self.dimensions[1]
+
+    def hull(self, transform: Transform) -> PolygonalHull:
+        return PolygonalHull([
+            transform.global_position - pg.Vector2(self.width / 2.0, self.height / 2.0).rotate(transform.global_degrees) * transform.global_scale,
+            transform.global_position - pg.Vector2(-self.width / 2.0, self.height / 2.0).rotate(transform.global_degrees) * transform.global_scale,
+            transform.global_position - pg.Vector2(-self.width / 2.0, -self.height / 2.0).rotate(transform.global_degrees) * transform.global_scale,
+            transform.global_position - pg.Vector2(self.width / 2.0, -self.height / 2.0).rotate(transform.global_degrees) * transform.global_scale
+        ])
+
+    def moment_of_inertia(self, mass: float) -> float:
+        return (1.0 / 12.0) * mass * (self.width ** 2 + self.height ** 2) 
+
+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)}")

main.py

@@ -1,176 +1,90 @@
 import pygame as pg
-from abc import ABC, abstractmethod
-from dataclasses import dataclass
-from itertools import combinations
-from collections import namedtuple
-from random import random
+from math import pi
+from rigidbody import *
+from collider import LineCollider, CircleCollider
 
-GLOBAL_GRAVITY=250
-DIMENSIONS = namedtuple("Dimensions", ["WIDTH", "HEIGHT"])(500,500)
 
-@dataclass
-class ColliderContact:
-    __slots__ = ["point", "normal", "penetration"]
-    point: pg.Vector2
-    normal: pg.Vector2
-    penetration: float
+class Ball:
 
-@dataclass
-class RigidBody:
-    position: pg.Vector2
-    collider: "BaseCollider"
-    velocity: pg.Vector2
-    mass: float = 1.0
-    restitution: float = 0.5
-
-    @property
-    def inv_mass(self):
-        return 0.0 if self.mass == 0 else 1.0 / self.mass
-
-class BaseCollider(ABC):
-     
-    def collide(self, other: "BaseCollider", this_position: pg.Vector2, other_position: pg.Vector2) -> ColliderContact | None:
-        if isinstance(other, CircleCollider):
-            return self.collide_circle(other, this_position, other_position)
-        
-
-    @abstractmethod
-    def collide_circle(self, other: "CircleCollider", this_position: pg.Vector2, other_position: pg.Vector2) -> ColliderContact | None:
-        pass
-
-class CircleCollider(BaseCollider):
-
-    def __init__(self, radius: float):
+    def __init__(self, transform: Transform, radius: float):
+        self.transform = transform
         self.radius = radius
-
-    def collide_circle(self, other: "CircleCollider", this_position: pg.Vector2, other_position: pg.Vector2) -> ColliderContact | None:
-        delta = this_position - other_position
-        dist = delta.length()
-        radii = self.radius + other.radius
-        if dist >= radii or dist == 0:
-            return None
-        normal = delta.normalize()
-        return ColliderContact(
-            point=other_position + normal * other.radius,
-            normal=normal,
-            penetration=radii - dist,
-        )  
-
-class PhysicsSystem:
-    
-    def __init__(self):
-        self.bodies: list[RigidBody] = []
-        self.gravity = pg.Vector2(0,GLOBAL_GRAVITY)
-    
-    def add_body(self, body: RigidBody) -> None:
-        self.bodies.append(body)
-
-    def update(self, dt: float):
-        g = self.gravity * dt
-        for body in self.bodies:
-            body.velocity += g
-            body.position += dt * body.velocity
-            self.resolve_bounds(body)
-
-        for a, b in combinations(self.bodies, 2):
-            if collision := a.collider.collide(b.collider, a.position, b.position):
-                self.resolve_collision(a, b, collision)
-        
-    def resolve_collision(self, a: RigidBody, b: RigidBody, collision: ColliderContact) -> None:
-        SLACK=0.4
-        correction = collision.penetration / (a.inv_mass + b.inv_mass) * SLACK * collision.normal
-        a.position += correction
-        b.position -= correction
-
-        v_rel = a.velocity - b.velocity
-        restitution = a.restitution * b.restitution
-        impulse = (-(1 + restitution) * (collision.normal.dot(v_rel))) / (a.inv_mass + b.inv_mass)
-        a.velocity += collision.normal*impulse*a.inv_mass
-        b.velocity -= collision.normal*impulse*b.inv_mass
-
-    def resolve_bounds(self, body: RigidBody) -> None:
-        r = body.collider.radius
-
-        if body.position.x - r < 0:
-            body.position.x = r
-            body.velocity.x = abs(body.velocity.x) * body.restitution
-        elif body.position.x + r > DIMENSIONS.WIDTH:
-            body.position.x = DIMENSIONS.WIDTH - r
-            body.velocity.x = -abs(body.velocity.x) * body.restitution
-
-        if body.position.y - r < 0:
-            body.position.y = r
-            body.velocity.y = abs(body.velocity.y) * body.restitution
-        elif body.position.y + r > DIMENSIONS.HEIGHT:
-            body.position.y = DIMENSIONS.HEIGHT - r
-            body.velocity.y = -abs(body.velocity.y) * body.restitution
-
-class Circle:
-
-    def __init__(self, radius: float, body: RigidBody, sprite: pg.Surface):
-        self.radius = radius
-        self.body = body
-        self.sprite = sprite
+        self.surface = pg.Surface((2*self.radius, 2*self.radius), pg.SRCALPHA)
+        self.surface.fill((255,255,255,0))
+        pg.draw.circle(
+            self.surface,
+            color=(0,255,0,255),
+            center=(self.radius,self.radius),
+            radius=self.radius
+        )
 
     def draw(self, screen: pg.Surface):
-        screen.blit(
-            self.sprite,
-            (
-                self.body.position.x - self.radius,
-                self.body.position.y - self.radius
-            )
-        )
-
-class World:
-
-    def __init__(self, screen: pg.Surface):
-        self.physics = PhysicsSystem()
-        self.screen = screen
-        self.entities = []
-
-    def update(self, dt) -> None:
-        self.physics.update(dt)
         
-    def draw(self) -> None:
-        for entity in self.entities:
-            entity.draw(self.screen)
+        surface = pg.transform.rotate(self.surface, self.transform.global_degrees)
+        screen.blit(surface, self.transform.global_position - pg.Vector2(self.radius, self.radius))
 
-    def spawn_circle(self, radius: float, position: pg.Vector2, velocity: pg.Vector2 | None = None, restitution:float = 0.5, mass: float = 1.0) -> None:
-        if velocity is None:
-            velocity = pg.Vector2(0,0)
-        collider = CircleCollider(radius)
-        body = RigidBody(position=position, collider=collider, velocity=velocity, restitution=restitution,mass=mass)
-        self.physics.add_body(body)
-        sprite = pg.Surface((2*radius, 2*radius),pg.SRCALPHA)
-        sprite.fill((255,255,255,0))
-        pg.draw.circle(
-            sprite,
-            color=(0,255,0,255),
-            center=(radius, radius),
-            radius=radius
+class Square:
+    
+    def __init__(self, transform: Transform, side: float, color=(255,0,0,255)):
+        self.transform = transform
+        self.side = side
+        self.surface = pg.Surface((side, side), pg.SRCALPHA)
+        self.surface.fill((255,255,255,0))
+        pg.draw.rect(
+            self.surface,
+            color=color,
+            rect=pg.Rect(0, 0, self.side, self.side)
         )
-        self.entities.append(Circle(radius,body,sprite))
+
+    def draw(self, screen: pg.Surface):
+        
+        surface = pg.transform.rotate(self.surface, self.transform.global_degrees)
+        screen.blit(surface, self.transform.global_position - pg.Vector2(self.side / 2.0, self.side / 2.0))
 
 def main():
     running=True
     pg.init()
     clock=pg.time.Clock()
-    screen = pg.display.set_mode(size=DIMENSIONS)
-    world=World(screen)
+    screen = pg.display.set_mode((500,500))
 
+    physics = PhysicsSystem()
+
+    ball_transform = Transform(position=pg.Vector2(250,250), rotation=pi/8.0)
+    square_transform = Transform(position=pg.Vector2(250, 100))
+
+    ball = Square(ball_transform, 20)
+
+    ball2 = Square(square_transform, 20, color=(0,255,0,255))
+
+    physics.add_body(
+        RigidBody(
+            ball_transform,
+            RectCollider((20,20)),
+            velocity=pg.Vector2(0,-400),
+            restitution=1.0,
+        )
+    )
+
+    physics.add_body(
+        RigidBody(
+            square_transform,
+            RectCollider((20,20)),
+            pg.Vector2(0,0),
+            restitution=1.0
+        )
+    )
+    
     while running:
+        
         dt = clock.tick(144) / 1000
         screen.fill((0,0,0,0))
-        world.update(dt)
-        world.draw()
+        
+        physics.update(dt)
+        ball.draw(screen)
+        ball2.draw(screen)
         pg.display.flip()
         for event in pg.event.get():
-            if event.type == pg.MOUSEBUTTONDOWN:
-                world.spawn_circle(
-                    10,
-                    pg.Vector2(pg.mouse.get_pos()),
-                    restitution=random()
-                )
+            
             if event.type == pg.QUIT:
                 running = False
         

rigidbody.py

@@ -0,0 +1,76 @@
+from dataclasses import dataclass
+from itertools import combinations
+
+import pygame as pg
+
+from collider import * 
+from transform import Transform
+
+@dataclass
+class RigidBody:
+    transform: Transform
+    collider: BaseCollider
+    velocity: pg.Vector2
+    angular_velocity: float = 0
+    mass: float = 1.0
+    restitution: float = 0.5
+
+    @property
+    def inv_mass(self) -> float:
+        return 0.0 if self.mass == 0.0 else 1/self.mass
+    
+    @property
+    def moment_of_inertia(self) -> float:
+        return 0.0 if self.mass == 0.0 else self.collider.moment_of_inertia(self.mass)
+    
+    @property
+    def inv_moment_of_inertia(self) -> float:
+        return 0.0 if self.mass == 0.0 else 1/self.moment_of_inertia
+
+class PhysicsSystem:
+
+    def __init__(self, gravity: int = 250):
+        self.bodies: list[RigidBody] = []
+        self.gravity = pg.Vector2(0,gravity)
+    
+    def add_body(self, body: RigidBody) -> None:
+        self.bodies.append(body)
+
+    def update(self, dt: float):
+        g = self.gravity * dt
+        for body in self.bodies:
+            if body.mass != 0.0:
+                body.velocity += g
+            body.transform.position += dt * body.velocity
+            body.transform.rotation += dt * body.angular_velocity
+
+        for a, b in combinations(self.bodies, 2):
+            if collision := intersect(a.collider, b.collider, a.transform, b.transform):
+                self.resolve_collision(a, b, collision)
+        
+    def resolve_collision(self, a: RigidBody, b: RigidBody, collision: ColliderContact) -> None:
+        SLACK=9000
+        correction = collision.penetration / (a.inv_mass + b.inv_mass) * SLACK * collision.normal
+        
+        if a.mass != 0.0:
+            a.transform.position += correction
+        if b.mass != 0.0:
+            b.transform.position -= correction
+
+        for point in collision.points:
+            r_a = point - a.transform.position
+            r_b = point - b.transform.position
+
+            w_cross_r_a = pg.Vector2(-r_a.y * a.angular_velocity, r_a.x * a.angular_velocity) #omega is not encoded as a vector, so we pretend it is
+            w_cross_r_b = pg.Vector2(-r_b.y * b.angular_velocity, r_b.x * b.angular_velocity)
+            v_rel = a.velocity + w_cross_r_a - b.velocity - w_cross_r_b
+            restitution = a.restitution * b.restitution
+            impulse = -(1+restitution)*(v_rel.dot(collision.normal))\
+                / (a.inv_mass + b.inv_mass + (r_a.cross(collision.normal)**2 * a.inv_moment_of_inertia)\
+                + (r_b.cross(collision.normal)**2 * b.inv_moment_of_inertia)) / len(collision.points)
+            a.velocity += collision.normal*impulse*a.inv_mass
+            b.velocity -= collision.normal*impulse*b.inv_mass
+            a.angular_velocity += (r_a.cross(impulse*collision.normal)) * a.inv_moment_of_inertia
+            b.angular_velocity -= (r_b.cross(impulse*collision.normal)) * b.inv_moment_of_inertia
+
+    

transform.py

@@ -0,0 +1,38 @@
+from dataclasses import dataclass
+from math import pi
+
+import pygame as pg
+
+@dataclass
+class Transform:
+    
+    position: pg.Vector2
+    rotation: pg.Vector2 = 0.0
+    scale: float = 1.0
+    parent: 'Transform' = None
+
+    @property
+    def global_position(self):
+        if self.parent is None:
+            return self.position
+        return self.position + self.parent.global_position
+    
+    @property
+    def global_rotation(self):
+        if self.parent is None:
+            return self.rotation
+        return self.position + self.parent.global_rotation
+    
+    @property
+    def global_scale(self):
+        if self.parent is None:
+            return self.scale
+        return self.scale * self.parent.global_scale
+
+    @property
+    def global_degrees(self):
+        return self.global_rotation * (180.0 / pi)
+
+    @property
+    def degrees(self):
+        return self.rotation * (180.0 / pi)