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 testMat4ToNumpyArray(self):
xr = np.random.random((4, 4))
mat = LMatrix4(*xr.ravel())
x = mat4ToNumpyArray(mat)
self.assertTrue(np.allclose(x, xr, atol=1e-6))
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 loadHouseFromJson(houseId, datasetRoot):
filename = SunCgSceneLoader.getHouseJsonPath(datasetRoot, houseId)
with open(filename) as f:
data = json.load(f)
assert houseId == data['id']
houseId = str(data['id'])
# Create new node for house instance
houseNp = NodePath('house-' + str(houseId))
objectIds = {}
for levelId, level in enumerate(data['levels']):
logger.debug('Loading Level %s to scene' % (str(levelId)))
# Create new node for level instance
levelNp = houseNp.attachNewNode('level-' + str(levelId))
roomNpByNodeIndex = {}
for nodeIndex, node in enumerate(level['nodes']):
if not node['valid'] == 1: continue
modelId = str(node['modelId'])
if node['type'] == 'Room':
logger.debug('Loading Room %s to scene' % (modelId))
# Create new nodes for room instance
roomNp = levelNp.attachNewNode('room-' + str(modelId))
roomLayoutsNp = roomNp.attachNewNode('layouts')
roomObjectsNp = roomNp.attachNewNode('objects')
# Load models defined for this room
for roomObjFilename in reglob(
os.path.join(datasetRoot, 'room', houseId),
modelId + '[a-z].obj'):
# Convert extension from OBJ + MTL to EGG format
f, _ = os.path.splitext(roomObjFilename)
modelFilename = f + ".egg"
if not os.path.exists(modelFilename):
raise Exception(
'The SUNCG dataset object models need to be convert to Panda3D EGG format!'
)
# Create new node for object instance
objectNp = NodePath('object-' + str(modelId) + '-0')
objectNp.reparentTo(roomLayoutsNp)
model = loadModel(modelFilename)
model.setName('model-' + os.path.basename(f))
model.reparentTo(objectNp)
model.hide()
if 'nodeIndices' in node:
for childNodeIndex in node['nodeIndices']:
roomNpByNodeIndex[childNodeIndex] = roomObjectsNp
elif node['type'] == 'Object':
logger.debug('Loading Object %s to scene' % (modelId))
# Instance identification
if modelId in objectIds:
objectIds[modelId] = objectIds[modelId] + 1
else:
objectIds[modelId] = 0
# Create new node for object instance
objectNp = NodePath('object-' + str(modelId) + '-' +
str(objectIds[modelId]))
#TODO: loading the BAM format would be much more efficient
# Convert extension from OBJ + MTL to EGG format
objFilename = os.path.join(datasetRoot, 'object',
node['modelId'],
node['modelId'] + '.obj')
assert os.path.exists(objFilename)
f, _ = os.path.splitext(objFilename)
modelFilename = f + ".egg"
if not os.path.exists(modelFilename):
raise Exception(
'The SUNCG dataset object models need to be convert to Panda3D EGG format!'
)
model = loadModel(modelFilename)
model.setName('model-' + os.path.basename(f))
model.reparentTo(objectNp)
model.hide()
# 4x4 column-major transformation matrix from object coordinates to scene coordinates
transform = np.array(node['transform']).reshape((4, 4))
# Transform from Y-UP to Z-UP coordinate systems
#TODO: use Mat4.convertMat(CS_zup_right, CS_yup_right)
yupTransform = np.array([[1, 0, 0, 0], [0, 0, -1, 0],
[0, 1, 0, 0], [0, 0, 0, 1]])
zupTransform = np.array([[1, 0, 0, 0], [0, 0, 1, 0],
[0, -1, 0, 0], [0, 0, 0, 1]])
transform = np.dot(np.dot(yupTransform, transform),
zupTransform)
transform = TransformState.makeMat(
LMatrix4f(*transform.ravel()))
# Calculate the center of this object
minBounds, maxBounds = model.getTightBounds()
centerPos = minBounds + (maxBounds - minBounds) / 2.0
# Add offset transform to make position relative to the center
objectNp.setTransform(
transform.compose(TransformState.makePos(centerPos)))
model.setTransform(TransformState.makePos(-centerPos))
# Get the parent nodepath for the object (room or level)
if nodeIndex in roomNpByNodeIndex:
objectNp.reparentTo(roomNpByNodeIndex[nodeIndex])
else:
objectNp.reparentTo(levelNp)
# Validation
assert np.allclose(mat4ToNumpyArray(
model.getNetTransform().getMat()),
mat4ToNumpyArray(transform.getMat()),
atol=1e-6)
objectNp.setTag('model-id', str(modelId))
objectNp.setTag('level-id', str(levelId))
objectNp.setTag('house-id', str(houseId))
elif node['type'] == 'Ground':
logger.debug('Loading Ground %s to scene' % (modelId))
# Create new nodes for ground instance
groundNp = levelNp.attachNewNode('ground-' + str(modelId))
groundLayoutsNp = groundNp.attachNewNode('layouts')
# Load model defined for this ground
for groundObjFilename in reglob(
os.path.join(datasetRoot, 'room', houseId),
modelId + '[a-z].obj'):
# Convert extension from OBJ + MTL to EGG format
f, _ = os.path.splitext(groundObjFilename)
modelFilename = f + ".egg"
if not os.path.exists(modelFilename):
raise Exception(
'The SUNCG dataset object models need to be convert to Panda3D EGG format!'
)
objectNp = NodePath('object-' + str(modelId) + '-0')
objectNp.reparentTo(groundLayoutsNp)
model = loadModel(modelFilename)
model.setName('model-' + os.path.basename(f))
model.reparentTo(objectNp)
model.hide()
else:
raise Exception('Unsupported node type: %s' %
(node['type']))
scene = Scene()
houseNp.reparentTo(scene.scene)
# Recenter objects in rooms
for room in scene.scene.findAllMatches('**/room*'):
# Calculate the center of this room
minBounds, maxBounds = room.getTightBounds()
centerPos = minBounds + (maxBounds - minBounds) / 2.0
# Add offset transform to room node
room.setTransform(TransformState.makePos(centerPos))
# Add recentering transform to all children nodes
for childNp in room.getChildren():
childNp.setTransform(TransformState.makePos(-centerPos))
# Recenter objects in grounds
for ground in scene.scene.findAllMatches('**/ground*'):
# Calculate the center of this ground
minBounds, maxBounds = ground.getTightBounds()
centerPos = minBounds + (maxBounds - minBounds) / 2.0
# Add offset transform to ground node
ground.setTransform(TransformState.makePos(centerPos))
# Add recentering transform to all children nodes
for childNp in ground.getChildren():
childNp.setTransform(TransformState.makePos(-centerPos))
return scene
