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)
