diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e69de29 diff --git a/__pycache__/collider.cpython-311.pyc b/__pycache__/collider.cpython-311.pyc new file mode 100644 index 0000000..d5fb6b0 Binary files /dev/null and b/__pycache__/collider.cpython-311.pyc differ diff --git a/__pycache__/rigidbody.cpython-311.pyc b/__pycache__/rigidbody.cpython-311.pyc new file mode 100644 index 0000000..903b10a Binary files /dev/null and b/__pycache__/rigidbody.cpython-311.pyc differ diff --git a/__pycache__/transform.cpython-311.pyc b/__pycache__/transform.cpython-311.pyc new file mode 100644 index 0000000..659331a Binary files /dev/null and b/__pycache__/transform.cpython-311.pyc differ diff --git a/collider.py b/collider.py new file mode 100644 index 0000000..8c902d9 --- /dev/null +++ b/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)}") \ No newline at end of file diff --git a/main.py b/main.py index 49238fd..a98bcc3 100644 --- a/main.py +++ 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 diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..39051cd Binary files /dev/null and b/requirements.txt differ diff --git a/rigidbody.py b/rigidbody.py new file mode 100644 index 0000000..46ce3f7 --- /dev/null +++ b/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 + + diff --git a/sprite.py b/sprite.py new file mode 100644 index 0000000..e69de29 diff --git a/tools.py b/tools.py new file mode 100644 index 0000000..e69de29 diff --git a/transform.py b/transform.py new file mode 100644 index 0000000..b7de6b2 --- /dev/null +++ b/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) \ No newline at end of file