206 lines
7.7 KiB
Python
206 lines
7.7 KiB
Python
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)}") |