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

added baumgarte stabilization

Browse Source
This commit is contained in:
=
2026-03-13 14:20:14 -04:00
parent 0ff308e110
commit 42bf990193
1 changed files with 38 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
rigidbody.py
View File

@@ -44,25 +44,32 @@ class PhysicsSystem:
def add_body(self, body: RigidBody) -> None: def add_body(self, body: RigidBody) -> None:
self.bodies.append(body) self.bodies.append(body)
def update(self, dt: float): def update(self, dtr: float):
g = self.gravity * dt substeps = 2
for body in self.bodies: for _ in range(substeps):
if body.mass != 0.0: dt = dtr / substeps
body.velocity += g g = self.gravity * dt
body.transform.position += dt * body.velocity for body in self.bodies:
body.transform.rotation += dt * body.angular_velocity if body.mass != 0.0:
debug.draw_collider(body.collider, body.transform) body.velocity += g
body.transform.position += dt * body.velocity
body.transform.rotation += dt * body.angular_velocity
debug.draw_collider(body.collider, body.transform)
for a, b in combinations(self.bodies, 2): for a, b in combinations(self.bodies, 2):
if collision := intersect(a.collider, b.collider, a.transform, b.transform): if collision := intersect(a.collider, b.collider, a.transform, b.transform):
self.resolve_collision(a, b, collision, dt) self.resolve_collision(a, b, collision, dt)
def resolve_collision(self, a: RigidBody, b: RigidBody, collision: ColliderContact, dt: float) -> None: #TODO: refactor this and clean up
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 if a.mass == 0.0 and b.mass == 0.0: return
SLACK=0.2 SLACK=0.3
SLOP = 0.001
BAUMGARTE_CONSTANT = 0.3
correction = collision.penetration * SLACK * collision.normal correction = collision.penetration * SLACK * collision.normal
baumgarte_bias = BAUMGARTE_CONSTANT * (collision.penetration - SLOP / dt)
if a.mass != 0.0: if a.mass != 0.0:
a.transform.position += correction a.transform.position += correction
if b.mass != 0.0: if b.mass != 0.0:
@@ -73,27 +80,33 @@ class PhysicsSystem:
tangent = collision.normal.rotate(90) tangent = collision.normal.rotate(90)
a_w = a.angular_velocity a_w = a.angular_velocity
b_w = b.angular_velocity b_w = b.angular_velocity
v_rel_linear = pg.Vector2(a.velocity - b.velocity)
for point in collision.points: for point in collision.points:
r_a = point - a.transform.position r_a = point - a.transform.position
r_b = point - b.transform.position r_b = point - b.transform.position
w_cross_r_a = pg.Vector2(-r_a.y * a_w, r_a.x * a_w) #omega is not encoded as a vector, so we pretend it is w_cross_r_a = pg.Vector2(-r_a.y * a_w, r_a.x * a_w) #omega is not encoded as a vector, so we pretend it is
w_cross_r_b = pg.Vector2(-r_b.y * b_w, r_b.x * b_w) w_cross_r_b = pg.Vector2(-r_b.y * b_w, r_b.x * b_w)
normal_mass = a.inv_mass + b.inv_mass + (a.inv_moment_of_inertia * r_a.cross(collision.normal)**2) +(b.inv_moment_of_inertia * r_b.cross(collision.normal) ** 2)
v_rel = v_rel_linear + w_cross_r_a - w_cross_r_b v_rel = v_rel_linear + w_cross_r_a - w_cross_r_b
v_rel_tangent = v_rel.dot(tangent) 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)
friction_impulse = (-v_rel_tangent / (a.inv_mass + b.inv_mass +\
(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( friction_impulse * tangent, point)
b.apply_impulse( -1 * friction_impulse * tangent, point)
if v_rel_linear.dot(collision.normal) < 0.0: if v_rel_linear.dot(collision.normal) < 0.0:
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)
a.apply_impulse(collision_impulse * collision.normal, point) a.apply_impulse(collision_impulse * collision.normal, point)
b.apply_impulse(-1 * collision_impulse * collision.normal, point) b.apply_impulse(-1 * collision_impulse * collision.normal, point)
friction_impulse = (-v_rel_tangent / (a.inv_mass + b.inv_mass +\
(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( friction_impulse * tangent, point)
b.apply_impulse( -1 * friction_impulse * tangent, point)
baumgarte_impulse = -normal_mass * (v_rel_linear.length() + baumgarte_bias)
baumgarte_impulse = max(baumgarte_impulse, 0)
a.apply_impulse(baumgarte_impulse * collision.normal, point)
b.apply_impulse(-1 * baumgarte_impulse * collision.normal, point)