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

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
+
+