ignore linear impulse if bodies are separating

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()]
+    closest_vertex = hull.closest_vertex(b_transform.global_position)
-    circle_normal = min([(b_transform.global_position - v) for v in hull.vertices()], key=lambda v: v.length()).normalize()
+    circle_normal = (b_transform.global_position-closest_vertex).normalize()
-    normals.append(circle_normal)
+    normals = [*[face.normal for face in hull.faces()], circle_normal]
-    collision_normal: pg.Vector2 | None = None
     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)
+    try:
-    clip(left_normal, ref_face.begin)
+        clip(right_normal, ref_face.end)
-    clip(ref_face.normal, ref_face.begin)
+        clip(left_normal, ref_face.begin)
+        clip(ref_face.normal, ref_face.begin)
+    except:
+        return None
 
     return ColliderContact(
         points=contact_manifold,

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

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_collider = CircleCollider(10)
-
+    rect = RectCollider(pg.Vector2(20,20))
-    ball_transform = Transform(position=pg.Vector2(250,440), rotation=pi/8.0)
+    linecollider = RectCollider((495, 5))
-    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
-        )
-    )
 
     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
         

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)
+            a.apply_impulse( friction_impulse * tangent, point)
-            b.apply_impulse(-1 * collision.normal*collision_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)
 
     

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,
+                    points=contact.points,
-                    width=3
+                    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)