def test_broadphase_collection(self):
def moveBody(rb, pos):
trans = Transform()
trans.setOrigin(pos)
ms = DefaultMotionState()
ms.setWorldTransform(trans)
rb.setMotionState(ms)
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-3, 0, 0)
pos_b = Vec3(-1, 0, 0)
pos_c = Vec3(1, 0, 0)
pos_d = Vec3(3, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1), bodyID=1)
rb_b = getRB(pos=pos_b, cshape=SphereShape(1), bodyID=2)
rb_c = getRB(pos=pos_c, cshape=SphereShape(1), bodyID=3)
rb_d = getRB(pos=pos_d, cshape=SphereShape(1), bodyID=4)
# Create a simulation, install the Broadphase Pair Cache Builder, and
# add bodies A, B, D. The first two are touching, the last one is
# considerably away from the others. The bodies are arranged like this:
# "AB D"
sim = BulletBase()
sim.installBroadphaseCallback()
sim.addRigidBody(rb_a)
sim.addRigidBody(rb_b)
sim.addRigidBody(rb_d)
# Step the simulation and fetch the collision pairs.
sim.azResetPairCache()
assert sim.azReturnPairCache() == set([])
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([(1, 2)])
# Move the middle body towards the right. Step the simulation again
# (this will re-populate the broadphase cache) and verify that the
# broadphase now returns the two objects on the right: "A BD"
moveBody(rb_b, pos_c)
sim.azResetPairCache()
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([(2, 4)])
# Move the middle body towards the far right so that none of the bodies
# overlap: "A D B"
moveBody(rb_b, Vec3(30, 0, 0))
sim.azResetPairCache()
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([])
# Move the middle body back to its original position and insert the
# fourth body. Now all bodies touch their immediate neighbours:
# "ABCD"
moveBody(rb_b, pos_b)
sim.addRigidBody(rb_c)
sim.azResetPairCache()
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([(1, 2), (2, 3), (3, 4)])
