def collide_body_static_body(b, sb):
delta = b.pos - sb.pos
d = Vector.Length(delta)
mtd = delta * ((b.rad + sb.rad)-d)/d
b.pos = b.pos + mtd
vn = Vector.Dot(b.vel, Vector.Normalize(mtd))
if vn > 0:
return
i = -(1 + RESTITUTION) * vn
b.vel += Vector.Normalize(mtd) * i
def collide_body_body(b1, b2):
delta = b1.pos - b2.pos
d = Vector.Length(delta)
mtd = delta * ((b1.rad + b2.rad)-d)/d
im1 = 1 / b1.mass
im2 = 1 / b2.mass
b1.pos = b1.pos + mtd * (im1 / (im1 + im2))
b2.pos = b2.pos - mtd * (im2 / (im1 + im2))
v = b1.vel - b2.vel
vn = Vector.Dot(v, Vector.Normalize(mtd))
if vn > 0:
return
i = (-(1 + RESTITUTION) * vn) / (im1 + im2)
impulse = Vector.Normalize(mtd) * i
b1.vel = b1.vel + impulse*im1
b2.vel = b2.vel - impulse*im2