def test_add_get_remove_iterate(self):
"""
Test the various functions
"""
# 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))
rb_b = getRB(pos=pos_b, cshape=BoxShape(Vec3(1, 2, 3)))
rb_c = getRB(pos=pos_c, cshape=SphereShape(1))
rb_d = getRB(pos=pos_d, cshape=BoxShape(Vec3(1, 2, 3)))
frameInA = Transform(Quaternion(0, 0, 0, 1), pos_b)
frameInB = Transform(Quaternion(0, 0, 0, 1), pos_a)
# Connect the two rigid bodies at their left/right boundary.
pivot_a, pivot_b, pivot_c, pivot_d = pos_a, pos_b, pos_c, pos_d
p2p_ab = Point2PointConstraint(rb_a, rb_b, pivot_a, pivot_b)
dof_bc = Generic6DofSpring2Constraint(rb_b, rb_c, frameInA, frameInB)
p2p_cd = Point2PointConstraint(rb_c, rb_d, pivot_c, pivot_d)
# Add both rigid bodies into a simulation.
bb = BulletBase()
bb.addRigidBody(rb_a)
bb.addRigidBody(rb_b)
# So far we have not added any constraints.
assert bb.getNumConstraints() == 0
assert bb.getConstraint(0) == None
assert bb.getConstraint(10) == None
# Add the first constraint.
bb.addConstraint(p2p_ab)
assert bb.getNumConstraints() == 1
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == None
assert list(bb.iterateConstraints()) == [p2p_ab]
# Add the first constraint a second time. The function call must suceed
# but the constraint must not have been added again.
bb.addConstraint(p2p_ab)
assert bb.getNumConstraints() == 1
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == None
assert list(bb.iterateConstraints()) == [p2p_ab]
# Add the second and third constraint.
bb.addConstraint(dof_bc)
assert bb.getNumConstraints() == 2
assert list(bb.iterateConstraints()) == [p2p_ab, dof_bc]
bb.addConstraint(p2p_cd)
assert bb.getNumConstraints() == 3
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == dof_bc
assert bb.getConstraint(2) == p2p_cd
assert list(bb.iterateConstraints()) == [p2p_ab, dof_bc, p2p_cd]
# Remove the middle constraint twice.
p2p_none = Point2PointConstraint(rb_a, rb_d, pivot_b, pivot_c)
for ii in range(2):
bb.removeConstraint(dof_bc)
assert bb.getNumConstraints() == 2
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == p2p_cd
assert bb.getConstraint(2) == None
# Remove non-existing constraint.
bb.removeConstraint(p2p_none)
