def _add_side_walk2bullet(self, lane_start, lane_end, middle, radius=0.0, direction=0): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) body_node = BulletRigidBodyNode(BodyName.Side_walk) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) side_np = self.side_walk_node_path.attachNewNode(body_node) shape = BulletBoxShape(Vec3(1 / 2, 1 / 2, 1 / 2)) body_node.addShape(shape) body_node.setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_node) if radius == 0: factor = 1 else: if direction == 1: factor = (1 - self.SIDE_WALK_LINE_DIST / radius) else: factor = (1 + self.SIDE_WALK_WIDTH / radius) * (1 + self.SIDE_WALK_LINE_DIST / radius) direction_v = lane_end - lane_start vertical_v = (-direction_v[1], direction_v[0]) / numpy.linalg.norm(direction_v) middle += vertical_v * (self.SIDE_WALK_WIDTH / 2 + self.SIDE_WALK_LINE_DIST) side_np.setPos(panda_position(middle, 0)) theta = -numpy.arctan2(direction_v[1], direction_v[0]) side_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) side_np.setScale( length * factor, self.SIDE_WALK_WIDTH, self.SIDE_WALK_THICKNESS * (1 + 0.1 * numpy.random.rand()) ) if self.render: side_np.setTexture(self.ts_color, self.side_texture) self.side_walk.instanceTo(side_np)
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 _add_lane_line2bullet( self, lane_start, lane_end, middle, parent_np: NodePath, color: Vec4, line_type: LineType, straight_stripe=False ): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if length <= 0: return if LineType.prohibit(line_type): node_name = BodyName.Continuous_line else: node_name = BodyName.Stripped_line # add bullet body for it if straight_stripe: body_np = parent_np.attachNewNode(node_name) else: scale = 2 if line_type == LineType.STRIPED else 1 body_node = BulletRigidBodyNode(node_name) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape(Vec3(scale / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_THICKNESS)) body_np.node().addShape(shape) body_np.node().setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_np.node()) # position and heading body_np.setPos(panda_position(middle, 0)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) if self.render: # For visualization lane_line = self.loader.loadModel(AssetLoader.file_path("models", "box.bam")) lane_line.getChildren().reparentTo(body_np) body_np.setScale(length, Block.LANE_LINE_WIDTH, Block.LANE_LINE_THICKNESS) body_np.set_color(color)
def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.Continuous_line else: node_name = BodyName.Stripped_line body_node = BulletRigidBodyNode(node_name) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape(Vec3(length / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_THICKNESS)) body_np.node().addShape(shape) body_np.node().setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, 0)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2)))
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))