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)
