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

This commit is contained in:
=
2026-03-11 12:14:45 -04:00
parent ebfffe5c09
commit 55f20a950f
11 changed files with 386 additions and 152 deletions

0
.gitignore vendored Normal file
View File

Binary file not shown.

Binary file not shown.

Binary file not shown.

206
collider.py Normal file
View File

@@ -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)}")

216
main.py
View File

@@ -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
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
@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)
from math import pi
from rigidbody import *
from collider import LineCollider, CircleCollider
@abstractmethod
def collide_circle(self, other: "CircleCollider", this_position: pg.Vector2, other_position: pg.Vector2) -> ColliderContact | None:
pass
class Ball:
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,
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
)
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
def draw(self, screen: pg.Surface):
screen.blit(
self.sprite,
(
self.body.position.x - self.radius,
self.body.position.y - self.radius
)
surface = pg.transform.rotate(self.surface, self.transform.global_degrees)
screen.blit(surface, self.transform.global_position - pg.Vector2(self.radius, self.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)
)
class World:
def draw(self, screen: pg.Surface):
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)
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
)
self.entities.append(Circle(radius,body,sprite))
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

BIN
requirements.txt Normal file

Binary file not shown.

76
rigidbody.py Normal file
View File

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

0
sprite.py Normal file
View File

0
tools.py Normal file
View File

38
transform.py Normal file
View File

@@ -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)