def _initAgents(self):
# Load agents
for agent in self.scene.scene.findAllMatches('**/agents/agent*'):
transform = TransformState.makeIdentity()
if self.agentMode == 'capsule':
shape = BulletCapsuleShape(
self.agentRadius, self.agentHeight - 2 * self.agentRadius)
elif self.agentMode == 'sphere':
shape = BulletCapsuleShape(self.agentRadius,
2 * self.agentRadius)
# XXX: use BulletCharacterControllerNode class, which already handles local transform?
node = BulletRigidBodyNode('physics')
node.setMass(self.agentMass)
node.setStatic(False)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.addShape(shape)
self.bulletWorld.attach(node)
# Constrain the agent to have fixed position on the Z-axis
node.setLinearFactor(Vec3(1.0, 1.0, 0.0))
# Constrain the agent not to be affected by rotations
node.setAngularFactor(Vec3(0.0, 0.0, 0.0))
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
# Enable continuous collision detection (CCD)
node.setCcdMotionThreshold(1e-7)
node.setCcdSweptSphereRadius(0.50)
if node.isStatic():
agent.setTag('physics-mode', 'static')
else:
agent.setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = NodePath(node)
physicsNp.setTransform(transform)
# Reparent all child nodes below the new physic node
for child in agent.getChildren():
child.reparentTo(physicsNp)
physicsNp.reparentTo(agent)
# NOTE: we need this to update the transform state of the internal bullet node
physicsNp.node().setTransformDirty()
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(agent.getNetTransform().getMat()),
atol=1e-6)
def _initLayoutModels(self):
# Load layout objects as meshes
for model in self.scene.scene.findAllMatches('**/layouts/object*/model*'):
# NOTE: ignore models that have no geometry defined
if model.getTightBounds() is None:
logger.warning('Object %s has no geometry defined and will be ignored for physics!' % (str(model)))
continue
shape, transform = getCollisionShapeFromModel(
model, mode='mesh', defaultCentered=False)
node = BulletRigidBodyNode('physics')
node.setMass(0.0)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setStatic(True)
node.addShape(shape)
node.setDeactivationEnabled(True)
node.setIntoCollideMask(BitMask32.allOn())
self.bulletWorld.attach(node)
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Reparent render and acoustics nodes (if any) below the new physic node
# XXX: should be less error prone to just reparent all children
# (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
semanticsNp = model.getParent().find('**/semantics')
if not semanticsNp.isEmpty():
semanticsNp.reparentTo(physicsNp)
# NOTE: we need this to update the transform state of the internal
# bullet node
physicsNp.node().setTransformDirty()
# Validation
assert np.allclose(mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(model.getNetTransform().getMat()), atol=1e-6)
def createPlane(self, pos):
rb = BulletRigidBodyNode("plane")
rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1))
rb.setFriction(1.0)
rb.setAnisotropicFriction(1.0)
rb.setRestitution(1.0)
np = self.render.attachNewNode(rb)
np.setPos(pos)
np.setCollideMask(BitMask32.bit(0))
self.world.attachRigidBody(rb)
return np
def makeBall(self, num, pos = (0, 0, 0)):
shape = BulletSphereShape(0.5)
node = BulletRigidBodyNode('ball_' + str(num))
node.setMass(1.0)
node.setRestitution(.9)
node.setDeactivationEnabled(False)
node.addShape(shape)
physics = render.attachNewNode(node)
physics.setPos(*pos)
self.world.attachRigidBody(node)
model = loader.loadModel('models/ball')
color = self.makeColor()
model.setColor(color)
self.ballColors['ball_' + str(num)] = color
model.reparentTo(physics)
def createPlane(self, pos):
rb = BulletRigidBodyNode("GroundPlane")
rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1))
rb.setFriction(1.0)
rb.setAnisotropicFriction(1.0)
rb.setRestitution(1.0)
np = self.render.attachNewNode(rb)
np.setPos(pos)
np.setCollideMask(BitMask32.bit(0))
plane = self.loader.loadModel("cube")
plane.setScale(Vec3(100, 100, 1))
plane.setPos(Vec3(0, 0, -0.5))
plane.setColor(VBase4(0.8, 0.8, 0.8, 1.0))
plane.reparentTo(np)
self.world.attachRigidBody(rb)
return np
def createBox(self, mass, pos, scale, color):
rb = BulletRigidBodyNode('box')
rb.addShape(BulletBoxShape(scale))
rb.setMass(mass)
rb.setLinearDamping(0.2)
rb.setAngularDamping(0.9)
rb.setFriction(1.0)
rb.setAnisotropicFriction(1.0)
rb.setRestitution(0.0)
self.world.attachRigidBody(rb)
np = self.render.attachNewNode(rb)
np.setPos(pos)
np.setCollideMask(BitMask32.bit(1))
cube = self.loader.loadModel('cube')
cube.setScale(scale)
cube.setColor(color)
cube.reparentTo(np)
return np
def _initObjects(self):
# Load objects
for model in self.scene.scene.findAllMatches(
'**/objects/object*/model*'):
modelId = model.getParent().getTag('model-id')
# XXX: we could create BulletGhostNode instance for non-collidable objects, but we would need to filter out the collisions later on
if not modelId in self.openedDoorModelIds:
shape, transform = getCollisionShapeFromModel(
model, self.objectMode, defaultCentered=True)
node = BulletRigidBodyNode('physics')
node.addShape(shape)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
if self.suncgDatasetRoot is not None:
# Check if it is a movable object
category = self.modelCatMapping.getCoarseGrainedCategoryForModelId(
modelId)
if category in self.movableObjectCategories:
# Estimate mass of object based on volumetric data and default material density
objVoxFilename = os.path.join(self.suncgDatasetRoot,
'object_vox',
'object_vox_data',
modelId,
modelId + '.binvox')
voxelData = ObjectVoxelData.fromFile(objVoxFilename)
mass = Panda3dBulletPhysics.defaultDensity * voxelData.getFilledVolume(
)
node.setMass(mass)
else:
node.setMass(0.0)
node.setStatic(True)
else:
node.setMass(0.0)
node.setStatic(True)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
# Reparent render and acoustics nodes (if any) below the new physic node
#XXX: should be less error prone to just reparent all children (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
# NOTE: we need this to update the transform state of the internal bullet node
node.setTransformDirty()
# NOTE: we need to add the node to the bullet engine only after setting all attributes
self.bulletWorld.attach(node)
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(
model.getParent().getNetTransform().getMat()),
atol=1e-6)
else:
logger.debug('Object %s ignored from physics' % (modelId))
def setupObstacleThree(self, pos, scale, turn):
# Box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1))
bodyA = BulletRigidBodyNode('Box A')
bodyA.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 0.1, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
#Box B
shape = BulletBoxShape(Vec3(0.1,0.1,0.1))
bodyB = BulletRigidBodyNode('Box B')
bodyB.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1,0.1,0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Slider
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(0)
slider.setUpperLinearLimit(12)
slider.setLowerAngularLimit(-90)
slider.setUpperAngularLimit(-85)
self.world.attachConstraint(slider)
# Box C
shape = BulletBoxShape(Vec3(1, 3, 0.1))
bodyC = BulletRigidBodyNode('Box C')
bodyC.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(0.1)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(Vec3(pos.getX() + 3, pos.getY() - 4, pos.getZ()))
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(1, 3, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyC)
bodyNP.node().setLinearVelocity(-100)
# Slider
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyC, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(2)
slider.setUpperLinearLimit(6)
slider.setLowerAngularLimit(-90)
slider.setUpperAngularLimit(-85)
self.world.attachConstraint(slider)
