def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type, line_color): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.White_continuous_line if line_color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line body_node = BulletGhostNode(node_name) body_node.set_active(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_GHOST_HEIGHT)) body_np.node().addShape(shape) mask = Block.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else Block.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(BitMask32.bit(mask)) self.static_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, Block.LANE_LINE_GHOST_HEIGHT / 2)) 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 _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.White_continuous_line if color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line # add bullet body for it if straight_stripe: body_np = parent_np.attachNewNode(node_name) else: body_node = BulletGhostNode(node_name) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) # its scale will change by setScale body_height = DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT shape = BulletBoxShape( Vec3(length / 2 if line_type != LineType.BROKEN else length, DrivableAreaProperty.LANE_LINE_WIDTH / 2, body_height)) body_np.node().addShape(shape) mask = DrivableAreaProperty.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else DrivableAreaProperty.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(mask) self.static_nodes.append(body_np.node()) # position and heading body_np.setPos( panda_position(middle, DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)) direction_v = lane_end - lane_start # theta = -numpy.arctan2(direction_v[1], direction_v[0]) theta = -math.atan2(direction_v[1], direction_v[0]) body_np.setQuat( LQuaternionf(math.cos(theta / 2), 0, 0, math.sin(theta / 2))) if self.render: # For visualization lane_line = self.loader.loadModel( AssetLoader.file_path("models", "box.bam")) lane_line.setScale(length, DrivableAreaProperty.LANE_LINE_WIDTH, DrivableAreaProperty.LANE_LINE_THICKNESS) lane_line.setPos( Vec3(0, 0 - DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)) lane_line.reparentTo(body_np) body_np.set_color(color)