class PointCloudVertexBuffer(object): def __init__(self, point_cloud_size): format = GeomVertexFormat.getV3c4t2() vdata = GeomVertexData('point', format, Geom.UHDynamic) self._pos_writer = GeomVertexWriter(vdata, 'vertex') self._color_writer = GeomVertexWriter(vdata, 'color') self._tex_writer = GeomVertexWriter(vdata, 'texcoord') self._point_cloud_size = point_cloud_size self._prev_point_cloud_size = 0 assert point_cloud_size > 0 vdata.setNumRows(point_cloud_size * 6) self._tex_writer.set_row(0) set_texture(self._tex_writer, point_cloud_size) pnts = GeomTriangles(Geom.UHStatic) pnts.addConsecutiveVertices(0, 3 * 2 * point_cloud_size) pnts.closePrimitive() points_geom = Geom(vdata) points_geom.addPrimitive(pnts) snode = GeomNode('points') snode.addGeom(points_geom) dir_name = osp.dirname(__file__) # print(osp.join(dir_name, 'pnts_vs.glsl')) vs_shader = osp.join(dir_name, 'pnts_vs.glsl') fs_shader = osp.join(dir_name, 'pnts_fs.glsl') myShader = Shader.load( Shader.SL_GLSL, vertex=Filename.fromOsSpecific(vs_shader).getFullpath(), fragment=Filename.fromOsSpecific(fs_shader).getFullpath()) assert myShader is not None self.points_node = base.render.attachNewNode(snode) self.points_node.setPos(0., 0., 0.) self.points_node.set_shader(myShader) self.points_node.set_shader_input( "view_size", (base.win.getXSize(), base.win.getYSize())) self.points_node.node().setBounds( BoundingBox((-1000., -1000., -1000.), (1000., 1000., 1000.))) self.points_node.setTransparency(TransparencyAttrib.MAlpha) def assign_points(self, point_cloud, color): assert color.shape[1] == 4 assert color.shape[0] == point_cloud.shape[0] load_point_cloud(self._pos_writer, self._color_writer, point_cloud.astype(np.float64), color.astype(np.float64), self._point_cloud_size, self._prev_point_cloud_size) self._prev_point_cloud_size = point_cloud.shape[0] def clear_pc(self): self._color_writer.set_row(0) for i in range(self._point_cloud_size * 6): self._color_writer.setData4f(1., 1., 1., 0.)
def add_vertices(self, whl_radius, car_h): base_pos = self.last_pos + (0, 0, -whl_radius + .05) rot_mat = Mat4() rot_mat.set_rotate_mat(car_h, (0, 0, 1)) vdata = self.node.modify_geom(0).modify_vertex_data() vwriter = GeomVertexWriter(vdata, 'vertex') vwriter.set_row(vdata.get_num_rows()) vwriter.add_data3f(base_pos + rot_mat.xform_vec((-.12, 0, 0))) vwriter.add_data3f(base_pos + rot_mat.xform_vec((.12, 0, 0))) cnt = self.vtx_cnt prim = self.node.modify_geom(0).modify_primitive(0) if cnt >= 3: prim.add_vertices(cnt - 3, cnt - 2, cnt - 1) prim.add_vertices(cnt - 2, cnt, cnt - 1) self.vtx_cnt += 2
class Cuboid(object): def __init__(self, size, translation, rotation, color, text): self._visible = False wy, wx, wz = size[0], size[1], size[2] format = GeomVertexFormat.getV3c4() vdata = GeomVertexData('cu_points', format, Geom.UHStatic) vdata.setNumRows(8) self._pos_writer = GeomVertexWriter(vdata, 'vertex') self._color_writer = GeomVertexWriter(vdata, 'color') self._pos_writer.set_row(0) self._color_writer.set_row(0) self._pos_writer.addData3f(-0.5 * wx, -0.5 * wy, 0.) self._pos_writer.addData3f(-0.5 * wx, -0.5 * wy, wz) self._pos_writer.addData3f(0.5 * wx, -0.5 * wy, wz) self._pos_writer.addData3f(0.5 * wx, -0.5 * wy, 0.) self._pos_writer.addData3f(-0.5 * wx, 0.5 * wy, 0.) self._pos_writer.addData3f(-0.5 * wx, 0.5 * wy, wz) self._pos_writer.addData3f(0.5 * wx, 0.5 * wy, wz) self._pos_writer.addData3f(0.5 * wx, 0.5 * wy, 0.) for i in range(8): self._color_writer.addData4f(color[0], color[1], color[2], color[3]) lines = GeomLines(Geom.UHStatic) lines.addVertices(0, 1) lines.addVertices(1, 2) lines.addVertices(2, 3) lines.addVertices(3, 0) lines.addVertices(4, 5) lines.addVertices(5, 6) lines.addVertices(6, 7) lines.addVertices(7, 4) lines.addVertices(0, 4) lines.addVertices(1, 5) lines.addVertices(2, 6) lines.addVertices(3, 7) cuboid = Geom(vdata) cuboid.addPrimitive(lines) node = GeomNode('cuboid') node.addGeom(cuboid) self._node_path = NodePath(node) # self.title = OnscreenText(text=text, style=1, fg=(1, 1, 1, 1), pos=(-0.1, 0.1), scale=.05, # parent=self._node_path, align=TextNode.ARight) self._txt_node = TextNode('id') self._txt_node.setText(text) self._txt_node.setTextScale(0.2) self._txt_node.setCardColor(0, 0, 1, 1) self._txt_node.setCardAsMargin(0, 0, 0, 0) self._txt_node.setCardDecal(True) self._txt_node.set_align(2) text_geom = GeomNode('text') text_geom.addChild(self._txt_node) self._txt_np = NodePath(text_geom) self._txt_np.reparentTo(self._node_path) self.show() self.update_values(size, translation, rotation, color, text) def update_values(self, size, translation, rotation, color, text): if self._visible: self.update_bb(self._node_path, self._pos_writer, self._color_writer, tuple(size), tuple(translation), tuple(rotation), tuple(color), text, self._txt_node, self._txt_np) def hide(self): if self._visible: self._node_path.detachNode() self._visible = False def show(self): if not self._visible: self._node_path.reparentTo(base.render) self._node_path.setTwoSided(True) self._node_path.setTransparency(TransparencyAttrib.MAlpha) self._visible = True def update_bb(self, cuboid, pos_writer, color_writer, size, translation, rotation, color, text, text_node, text_np): wx, wy, wz = size pos_writer.setRow(0) color_writer.setRow(0) pos_writer.setData3f(-0.5 * wx, -0.5 * wy, 0.) pos_writer.setData3f(-0.5 * wx, -0.5 * wy, wz) pos_writer.setData3f(0.5 * wx, -0.5 * wy, wz) pos_writer.setData3f(0.5 * wx, -0.5 * wy, 0.) pos_writer.setData3f(-0.5 * wx, 0.5 * wy, 0.) pos_writer.setData3f(-0.5 * wx, 0.5 * wy, wz) pos_writer.setData3f(0.5 * wx, 0.5 * wy, wz) pos_writer.setData3f(0.5 * wx, 0.5 * wy, 0.) r, g, b, a = color for i in range(8): color_writer.setData4f(r, g, b, a) tx, ty, tz = translation ty = -ty rx, ry, rz = rotation rz = -rz cuboid.setPos(ty, tx, tz) cuboid.setHpr(-rz - 90., ry, rx) text_node.setText(text) text_np.setPos((0., 0., wz + 0.2)) text_np.setHpr(rz + 90., 0, 0)