def physicsStep(self, entity):
force = Vector()
if not isinstance(self.root, Sprite):
stack = [self.root]
while len(stack) > 0:
curr = stack.pop()
d = curr.size/Vector.Distance(curr.center_of_mass, entity.pos)
if d < THETA:
force += QuadTree.calc_force(curr.center_of_mass,
entity.pos,
curr.total_mass,
entity.mass)
else:
for c in curr.children.values():
if c is None or c is entity:
continue
elif isinstance(c, Sprite):
force += QuadTree.calc_force(c.pos, entity.pos,
c.mass, entity.mass)
else:
stack.append(c)
entity.pos = entity.pos + entity.vel * DT
entity.vel = entity.vel + (force / entity.mass) * DT
if Vector.Length(entity.vel) > SPEED_LIMIT:
entity.vel *= OVERSPEED_DAMP
else:
entity.vel *= NORMAL_DAMP
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