From 0ff308e1102d906871e4733ae608a854dad0a3a0 Mon Sep 17 00:00:00 2001 From: = <=> Date: Fri, 13 Mar 2026 00:55:01 -0400 Subject: [PATCH] ignore linear impulse if bodies are separating --- collider/system.py | 25 +++++----- collider/types.py | 3 ++ main.py | 119 +++++++++++++-------------------------------- rigidbody.py | 22 +++++---- tools.py | 27 ++++++---- 5 files changed, 81 insertions(+), 115 deletions(-) diff --git a/collider/system.py b/collider/system.py index e688045..4fe72c4 100644 --- a/collider/system.py +++ b/collider/system.py @@ -16,18 +16,18 @@ def _collide_circle_circle(a: CircleCollider, b: CircleCollider, a_transform: Tr return None normal = delta.normalize() return ColliderContact( - point=a_transform.global_position + normal * b.radius, + points=[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 + closest_vertex = hull.closest_vertex(b_transform.global_position) + circle_normal = (b_transform.global_position-closest_vertex).normalize() + normals = [*[face.normal for face in hull.faces()], circle_normal] lowest_pen = float('inf') + collision_normal = None for normal in normals: center_proj = normal.dot(b_transform.global_position) circle_interval = (center_proj - b.radius, center_proj + b.radius) @@ -35,11 +35,11 @@ def _collide_convex_circle(a: ConvexCollider, b: CircleCollider, a_transform: Tr penetration = _interval_overlap(circle_interval, convex_interval) if penetration is None: return None - if penetration < lowest_pen: + if penetration < lowest_pen and (a_transform.position - b_transform.position).dot(normal) < 0: lowest_pen = penetration - collision_normal = normal + collision_normal = -1 * normal #struggling to keep the convention correct but whatever return ColliderContact( - points=[b_transform.global_position - b.radius*normal], + points=[b_transform.global_position + b.radius*collision_normal], normal=collision_normal, penetration=lowest_pen ) @@ -80,9 +80,12 @@ def _collide_convex_convex(a: ConvexCollider, b: ConvexCollider, a_transform: Tr 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) + try: + clip(right_normal, ref_face.end) + clip(left_normal, ref_face.begin) + clip(ref_face.normal, ref_face.begin) + except: + return None return ColliderContact( points=contact_manifold, diff --git a/collider/types.py b/collider/types.py index 5e7692c..f5e04dc 100644 --- a/collider/types.py +++ b/collider/types.py @@ -42,6 +42,9 @@ class PolygonalHull: def project(self, axis: pg.Vector2) -> tuple[float, float]: projections = [v.dot(axis) for v in self._vertices] return (min(projections), max(projections)) + + def closest_vertex(self, v: pg.Vector2) -> pg.Vector2: + return min(self._vertices, key=lambda p: (v-p).length()) class BaseCollider(ABC): diff --git a/main.py b/main.py index ab03300..f921544 100644 --- a/main.py +++ b/main.py @@ -1,48 +1,10 @@ import pygame as pg from math import pi from rigidbody import * -from collider.types import RectCollider +from collider.types import RectCollider, CircleCollider from tools import debug - -class Ball: - - def __init__(self, transform: Transform, radius: float): - self.transform = transform - self.radius = radius - 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): - - 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) - ) - - 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() @@ -52,52 +14,17 @@ def main(): debug._screen = screen physics = PhysicsSystem() - linecollider = RectCollider((495, 1)) - - ball_transform = Transform(position=pg.Vector2(250,440), rotation=pi/8.0) - square_transform = Transform(position=pg.Vector2(250, 460)) - #ball2_transform = Transform(position=pg.Vector2(250, 50)) - - ball = Square(ball_transform, 20) - - ball2 = Square(square_transform, 20, color=(0,255,0,255)) - - #ball3 = Ball(ball2_transform, 10) - - physics.add_body( - RigidBody( - ball_transform, - RectCollider((20,20)), - velocity=pg.Vector2(0,-400), - restitution=0.2, - coef_friction=0.4 - ) - ) - - """physics.add_body( - RigidBody( - ball2_transform, - CircleCollider(20), - velocity=pg.Vector2(0,0) - ) - )""" - - physics.add_body( - RigidBody( - square_transform, - RectCollider((20,20)), - pg.Vector2(0,0), - restitution=0.2, - coef_friction=0.2 - ) - ) + ball_collider = CircleCollider(10) + rect = RectCollider(pg.Vector2(20,20)) + linecollider = RectCollider((495, 5)) physics.add_body( RigidBody( Transform(pg.Vector2(250,0)), linecollider, pg.Vector2(0,0), - mass=0.0 + mass=0.0, + restitution=1 ) ) @@ -106,7 +33,8 @@ def main(): Transform(pg.Vector2(250,500)), linecollider, pg.Vector2(0,0), - mass=0.0 + mass=0.0, + restitution=1 ) ) @@ -115,7 +43,8 @@ def main(): Transform(pg.Vector2(0,250),rotation=pi/2.0), linecollider, pg.Vector2(0,0), - mass=0.0 + mass=0.0, + restitution=1 ) ) @@ -124,23 +53,41 @@ def main(): Transform(pg.Vector2(500,250),rotation=pi/2), linecollider, pg.Vector2(0,0), - mass=0.0 + mass=0.0, + restitution=1 ) ) while running: dt = clock.tick(144) / 1000 - screen.fill((0,0,0,0)) + screen.fill((0,0,0)) physics.update(dt) - ball.draw(screen) - ball2.draw(screen) + debug.debug() #ball3.draw(screen) pg.display.flip() for event in pg.event.get(): - + + if event.type == pg.MOUSEBUTTONDOWN: + if event.button == 1: + physics.add_body( + RigidBody( + transform=Transform(pg.Vector2(pg.mouse.get_pos())), + collider=ball_collider, + velocity=pg.Vector2(0,0), + ) + ) + if event.button == 3: + physics.add_body( + RigidBody( + Transform(pg.Vector2(pg.mouse.get_pos())), + collider=rect, + velocity=pg.Vector2(0,0), + ) + ) + if event.type == pg.QUIT: running = False diff --git a/rigidbody.py b/rigidbody.py index d70c988..3e31c3b 100644 --- a/rigidbody.py +++ b/rigidbody.py @@ -5,7 +5,7 @@ import pygame as pg from tools import debug from collider.system import intersect -from collider.types import * +from collider.types import BaseCollider, ColliderContact from transform import Transform @dataclass @@ -55,21 +55,22 @@ class PhysicsSystem: 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) + self.resolve_collision(a, b, collision, dt) - def resolve_collision(self, a: RigidBody, b: RigidBody, collision: ColliderContact) -> None: + def resolve_collision(self, a: RigidBody, b: RigidBody, collision: ColliderContact, dt: float) -> None: + v_rel_linear = pg.Vector2(a.velocity - b.velocity) + if a.mass == 0.0 and b.mass == 0.0: return SLACK=0.2 - correction = collision.penetration / (a.inv_mass + b.inv_mass) * SLACK * collision.normal + correction = collision.penetration * SLACK * collision.normal if a.mass != 0.0: a.transform.position += correction if b.mass != 0.0: b.transform.position -= correction - + debug.draw_contact(collision) restitution = a.restitution * b.restitution friction = a.coef_friction * b.coef_friction tangent = collision.normal.rotate(90) - v_rel_linear = pg.Vector2(a.velocity - b.velocity) a_w = a.angular_velocity b_w = b.angular_velocity @@ -81,7 +82,7 @@ class PhysicsSystem: w_cross_r_b = pg.Vector2(-r_b.y * b_w, r_b.x * b_w) v_rel = v_rel_linear + w_cross_r_a - w_cross_r_b v_rel_tangent = v_rel.dot(tangent) - + collision_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) @@ -89,7 +90,10 @@ class PhysicsSystem: (r_a.cross(tangent))**2 * a.inv_moment_of_inertia +\ (r_b.cross(tangent)**2 * b.inv_moment_of_inertia))) / len(collision.points) friction_impulse = pg.math.clamp(friction_impulse, -abs(collision_impulse)*friction, abs(collision_impulse)*friction) - a.apply_impulse(collision.normal*collision_impulse + friction_impulse * tangent, point) - b.apply_impulse(-1 * collision.normal*collision_impulse - friction_impulse * tangent, point) + a.apply_impulse( friction_impulse * tangent, point) + b.apply_impulse( -1 * friction_impulse * tangent, point) + if v_rel_linear.dot(collision.normal) < 0.0: + a.apply_impulse(collision_impulse * collision.normal, point) + b.apply_impulse(-1 * collision_impulse * collision.normal, point) diff --git a/tools.py b/tools.py index 622836b..cdd2149 100644 --- a/tools.py +++ b/tools.py @@ -1,7 +1,9 @@ import pygame as pg from queue import Queue +from functools import reduce -from collider.types import CircleCollider, ConvexCollider, BaseCollider + +from collider.types import CircleCollider, ConvexCollider, BaseCollider, ColliderContact from transform import Transform def _debug(fn): @@ -23,30 +25,37 @@ class Debug: self._debug_queue.get()() @_debug - def draw_lines(self, points) -> None: + def draw_contact(self, contact: ColliderContact) -> None: def _draw_lines(): - if len(points) > 1: + if len(contact.points) > 1: pg.draw.lines( self._screen, (255,0,255), False, - points=points, - width=3 + points=contact.points, + width=1 ) - for point in points: + for point in contact.points: pg.draw.circle( self._screen, (255,0,255), point, - 2 + 1 ) - if len(points) > 0: + if len(contact.points) > 0: pg.draw.circle( self._screen, (255,0,255), - points[0], + contact.points[0], 2 ) + base = reduce(lambda a,b: a+b,contact.points) / len(contact.points) + pg.draw.line( + self._screen, + (255,255,0), + base, + base+(contact.normal*15) + ) self._debug_queue.put(_draw_lines)