lizzy/rigid-body
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ignore linear impulse if bodies are separating

Browse Source
This commit is contained in:
=
2026-03-13 00:55:01 -04:00
parent c7fd7f0d25
commit 0ff308e110
5 changed files with 81 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
collider/system.py
View File

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

3
collider/types.py
View File

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

119
main.py
View File

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

22
rigidbody.py
View File

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

27
tools.py
View File

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