def test_ConstructionInfo_to_RigidBody(self):
"""
Verify that the initial motion state is transferred correctly to the
RigidBody.
"""
mass = 10
pos = Vec3(1, 2, 3)
rot = Quaternion(0, 1, 0, 0)
t = Transform(rot, pos)
ms = DefaultMotionState(t)
cs = EmptyShape()
inert = Vec3(1, 2, 4)
# Compile the Rigid Body parameters.
ci = RigidBodyConstructionInfo(mass, ms, cs, Vec3(2, 4, 6))
assert ci.localInertia == Vec3(2, 4, 6)
ci.localInertia = inert
assert ci.localInertia == inert
# Construct the rigid body and delete the construction info.
body = RigidBody(ci)
del ci
# Verify that the object is at the correct position and has the correct
# mass and inertia.
t = body.getMotionState().getWorldTransform()
assert t.getOrigin() == pos
assert t.getRotation() == rot
assert body.getInvMass() == 1 / mass
assert body.getInvInertiaDiagLocal() == Vec3(1, 0.5, 0.25)
def runSimulation(sim):
# Create the collision shapes for the ball and ground.
cs_ball = SphereShape(1)
cs_ground = StaticPlaneShape(Vec3(0, 1, 0), 1)
# Create a Rigid Body for the static (ie mass=0) Ground.
q0 = Quaternion(0, 0, 0, 1)
ms = DefaultMotionState(Transform(q0, Vec3(0, -1, 0)))
ci = RigidBodyConstructionInfo(0, ms, cs_ground)
rb_ground = RigidBody(ci, bodyID=1)
del ms, ci
# Create a Rigid body for the dynamic (ie mass > 0) Ball.
ms = DefaultMotionState(Transform(q0, Vec3(0, 5, 0)))
inertia = cs_ball.calculateLocalInertia(1)
ci = RigidBodyConstructionInfo(1, ms, cs_ball, inertia)
rb_ball = RigidBody(ci, bodyID=2)
del ms, inertia, ci
# Ensure that Bullet never deactivates the objects.
rb_ground.forceActivationState(4)
rb_ball.forceActivationState(4)
# Add both bodies to the simulation.
sim.addRigidBody(rb_ground)
sim.addRigidBody(rb_ball)
# Sanity check: the ball must be at position y=5
pos = rb_ball.getMotionState().getWorldTransform().getOrigin()
pos = pos.topy()
assert pos[1] == 5
# Step the simulation long enough for the ball to fall down and
# come to rest on the plane.
for ii in range(10):
sim.stepSimulation(1, 100)
# Verify that the y-position of the ball is such that the ball
# rests on the plane.
pos = rb_ball.getMotionState().getWorldTransform().getOrigin()
return pos.topy()
Transform(Quaternion(0, 0, 0, 1), Vec3(0, -1, 0)))
ci = RigidBodyConstructionInfo(mass, groundRigidBodyState, groundShape)
groundRigidBody = RigidBody(ci)
groundRigidBody.setRestitution(1.0)
sim.addRigidBody(groundRigidBody)
del mass, ci
# Create a Rigid body for the Ball based on its collision shape. Let Bullet do
# the heavy lifting and compute the mass and inertia for us.
fallRigidBodyState = DefaultMotionState(
Transform(Quaternion(0, 0, 0, 1), Vec3(0, 20, 0)))
mass = 1
fallInertia = ballShape.calculateLocalInertia(mass)
ci = RigidBodyConstructionInfo(
mass, fallRigidBodyState, ballShape, fallInertia)
fallRigidBody = RigidBody(ci)
fallRigidBody.setRestitution(0.5)
sim.addRigidBody(fallRigidBody)
del mass, fallInertia, ci
# Step the simulation and print the position of the ball.
for ii in range(10):
sim.stepSimulation(0.5, 30)
ms = fallRigidBody.getMotionState()
wt = ms.getWorldTransform()
print(wt.getOrigin())
# Remove the rigid bodies from the simulation.
sim.removeRigidBody(fallRigidBody)
sim.removeRigidBody(groundRigidBody)
ci = azBullet.RigidBodyConstructionInfo(
sum(masses),
azBullet.DefaultMotionState(principal),
cs,
inertia,
)
body = RigidBody(ci)
# Add the body with its compound shape to the simulation.
sim.addRigidBody(body)
del masses, ci
# Apply an initial force and torque (only valid for the first simulation step
# because Bullet will clear all the forces afterwards).
body.clearForces()
body.applyCentralForce(Vec3(*(0, 1, 0)))
# body.applyTorque(Vec3(*(0, 1, 0)))
# Step the simulation and print the position of the Ball.
for ii in range(10):
wt = body.getMotionState().getWorldTransform()
pos = wt.getOrigin().topy()
rot = wt.getRotation().topy()
print('Pos: {:.2f} {:.2f} {:.2f}'.format(*pos))
print('Rot: {:.2f} {:.2f} {:.2f} {:.2f}'.format(*rot))
print('---')
sim.stepSimulation(0.5, 30)
# Remove the rigid body from the simulation.
sim.removeRigidBody(body)
ci = RigidBodyConstructionInfo(mass, groundRigidBodyState, groundShape)
groundRigidBody = RigidBody(ci)
groundRigidBody.setRestitution(1.0)
sim.addRigidBody(groundRigidBody)
del mass, ci
# Create the rigid body for the Ball. Since it is a dynamic body we need to
# specify mass and inertia. The former we need to do ourselves but Bullet can
# do the heavy lifting for the Inertia and compute it for us.
fallRigidBodyState = DefaultMotionState(
Transform(Quaternion(0, 0, 0, 1), Vec3(0, 20, 0)))
mass = 1
fallInertia = ballShape.calculateLocalInertia(mass)
ci = RigidBodyConstructionInfo(mass, fallRigidBodyState, ballShape,
fallInertia)
fallRigidBody = RigidBody(ci)
fallRigidBody.setRestitution(0.5)
sim.addRigidBody(fallRigidBody)
del mass, fallInertia, ci
# Step the simulation and print the position of the Ball.
for ii in range(10):
sim.stepSimulation(0.5, 30)
ms = fallRigidBody.getMotionState()
wt = ms.getWorldTransform()
print(wt.getOrigin())
# Remove the rigid bodies from the simulation.
sim.removeRigidBody(fallRigidBody)
sim.removeRigidBody(groundRigidBody)
