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