def makeGeom(self, points, colors, sizes):
#format = GeomVertexFormat.getV3c4()
array = GeomVertexArrayFormat()
array.addColumn(InternalName.get_vertex(), 3, Geom.NTFloat32, Geom.CPoint)
array.addColumn(InternalName.get_color(), 4, Geom.NTFloat32, Geom.CColor)
array.addColumn(InternalName.get_size(), 1, Geom.NTFloat32, Geom.COther)
format = GeomVertexFormat()
format.addArray(array)
format = GeomVertexFormat.registerFormat(format)
vdata = GeomVertexData('vdata', format, Geom.UH_static)
vdata.unclean_set_num_rows(len(points))
self.vwriter = GeomVertexWriter(vdata, InternalName.get_vertex())
self.colorwriter = GeomVertexWriter(vdata, InternalName.get_color())
self.sizewriter = GeomVertexWriter(vdata, InternalName.get_size())
geompoints = GeomPoints(Geom.UH_static)
geompoints.reserve_num_vertices(len(points))
index = 0
for (point, color, size) in zip(points, colors, sizes):
self.vwriter.addData3f(*point)
self.colorwriter.addData4f(*color)
self.sizewriter.addData1f(size)
geompoints.addVertex(index)
#geompoints.closePrimitive()
index += 1
geom = Geom(vdata)
geom.addPrimitive(geompoints)
return geom
def create_GeomNode_Single_Point(color_vec4=Vec4(1., 1., 1., 1.)):
# ---- step 1: create point at (0,0,0) and close the primitive
format = GeomVertexFormat.getV3c4()
vdata = GeomVertexData("colored_point", format, Geom.UHStatic)
vdata.setNumRows(4)
# add color to each vertex
colorWriter = GeomVertexWriter(vdata, "color")
# add a vertex position to each vertex
vertexPosWriter = GeomVertexWriter(vdata, "vertex")
# just one origin point vertex, it gets transformed later
# to it's intended position
vertexPosWriter.addData3f(0., 0., 0.)
colorWriter.addData4f(color_vec4)
# build the primitive
pointsprimitive = GeomPoints(Geom.UHStatic)
pointsprimitive.addVertex(0)
pointsprimitive.closePrimitive() # this resets all the data contained in the vertexPosWriter and colorWriter
# ----- step 3: make a GeomNode out of the Geom (to which the Primitives have been added)
# make a Geom object to hold the primitives
geom = Geom(vdata)
geom.addPrimitive(pointsprimitive)
geom_node = GeomNode("colored_point_node")
geom_node.addGeom(geom)
return geom_node
def makeSprite(name, texture, scale, add = False):
from panda3d.core import (GeomVertexFormat, GeomVertexData, GeomEnums,
InternalName, GeomVertexWriter, GeomPoints,
Geom, GeomNode, NodePath, TextureStage,
TexGenAttrib, BoundingSphere)
format = GeomVertexFormat.getV3()
data = GeomVertexData(name + "_data", format, GeomEnums.UHStatic)
writer = GeomVertexWriter(data, InternalName.getVertex())
writer.addData3f((0, 0, 0))
primitive = GeomPoints(GeomEnums.UHStatic)
primitive.addVertex(0)
primitive.closePrimitive()
geom = Geom(data)
geom.addPrimitive(primitive)
geomNode = GeomNode(name)
geomNode.addGeom(geom)
np = NodePath(geomNode)
np.setLightOff(1)
np.setMaterialOff(1)
np.setRenderModePerspective(True)
ts = TextureStage('sprite')
if add:
ts.setMode(TextureStage.MAdd)
np.setTexture(ts, texture)
np.setTexGen(ts, TexGenAttrib.MPointSprite)
np.setDepthWrite(False)
np.setDepthOffset(1)
np.setTransparency(True)
np.node().setBounds(BoundingSphere((0, 0, 0), 1))
np.node().setFinal(True)
np.flattenStrong()
np.setScale(scale)
return np
def makePoint(point=(0,0,0)):
clr4 = [1,1,1,1]
fmt = GeomVertexFormat.getV3c4() #3 component vertex, w/ 4 comp color
vertexData = GeomVertexData('points', fmt, Geom.UHStatic)
verts = GeomVertexWriter(vertexData, 'vertex')
verts.addData3f(*point)
color = GeomVertexWriter(vertexData, 'color')
color.addData4f(*clr4)
pointCloud = GeomPoints(Geom.UHStatic)
pointCloud.addVertex(0)
pointCloud.closePrimitive()
cloud = Geom(vertexData)
cloud.addPrimitive(pointCloud)
cloudNode = GeomNode('point')
cloudNode.addGeom(cloud)
return cloudNode
def makePoint(point=(0, 0, 0)):
clr4 = [1, 1, 1, 1]
fmt = GeomVertexFormat.getV3c4() #3 component vertex, w/ 4 comp color
vertexData = GeomVertexData('points', fmt, Geom.UHStatic)
verts = GeomVertexWriter(vertexData, 'vertex')
verts.addData3f(*point)
color = GeomVertexWriter(vertexData, 'color')
color.addData4f(*clr4)
pointCloud = GeomPoints(Geom.UHStatic)
pointCloud.addVertex(0)
pointCloud.closePrimitive()
cloud = Geom(vertexData)
cloud.addPrimitive(pointCloud)
cloudNode = GeomNode('point')
cloudNode.addGeom(cloud)
return cloudNode
def draw_vert(self,v, v_color):
format = GeomVertexFormat.getV3cp()
vdata=GeomVertexData('vert', format, Geom.UHDynamic)
vertex=GeomVertexWriter(vdata, 'vertex')
color=GeomVertexWriter(vdata, 'color')
vertex.addData3f(0.0,0.0,0.0)
color.addData4f(v_color)
mesh = Geom(vdata)
point = GeomPoints(Geom.UHDynamic)
point.addVertex(0)
point.closePrimitive()
mesh.addPrimitive(point)
vert_node = GeomNode(self.mesh.name+'_vert_'+str(v.ID))
vert_node.addGeom(mesh)
vert_node.setTag('ID',str(v.ID))
rendered_vert = self.render_root.attachNewNode(vert_node)
rendered_vert.setPos(v.pos.x,v.pos.y,v.pos.z)
rendered_vert.setRenderModeThickness(5.0)
self.render_nodes['vert_'+str(v.ID)] = rendered_vert
def draw_vert(self, v, v_color):
format = GeomVertexFormat.getV3cp()
vdata = GeomVertexData('vert', format, Geom.UHDynamic)
vertex = GeomVertexWriter(vdata, 'vertex')
color = GeomVertexWriter(vdata, 'color')
vertex.addData3f(0.0, 0.0, 0.0)
color.addData4f(v_color)
mesh = Geom(vdata)
point = GeomPoints(Geom.UHDynamic)
point.addVertex(0)
point.closePrimitive()
mesh.addPrimitive(point)
vert_node = GeomNode(self.mesh.name + '_vert_' + str(v.ID))
vert_node.addGeom(mesh)
vert_node.setTag('ID', str(v.ID))
rendered_vert = self.render_root.attachNewNode(vert_node)
rendered_vert.setPos(v.pos.x, v.pos.y, v.pos.z)
rendered_vert.setRenderModeThickness(5.0)
self.render_nodes['vert_' + str(v.ID)] = rendered_vert
def load_pcd_content(content,
w=2,
color_mode="intensity",
intensity_filter=50):
pc = pypcd.PointCloud.from_buffer(content)
fmt = GeomVertexFormat() #3 component vertex, w/ 4 comp color
fmt_arr = GeomVertexArrayFormat()
fmt_arr.addColumn(InternalName.make('vertex'), 3, Geom.NTFloat32,
Geom.CPoint)
fmt_color_arr = GeomVertexArrayFormat()
fmt_color_arr.addColumn(InternalName.make('color'), 4, Geom.NTUint8,
Geom.CColor)
fmt.addArray(fmt_arr)
fmt.addArray(fmt_color_arr)
fmt = GeomVertexFormat.registerFormat(fmt)
vertexData = GeomVertexData('points', fmt, Geom.UHStatic)
pointCloud = GeomPoints(Geom.UHStatic)
pc.pc_data.dtype = numpy.dtype("<f4")
v, c = arrayfilter.vertices_filter(pc.pc_data.reshape((pc.points, 4)))
for i in xrange(len(v)):
pointCloud.addVertex(i)
pointCloud.closePrimitive()
arr = GeomVertexArrayData(fmt.getArray(0), GeomEnums.UHStream)
datahandle = arr.modifyHandle()
datahandle.copyDataFrom(v)
vertexData.setArray(0, arr)
arr = GeomVertexArrayData(fmt.getArray(1), GeomEnums.UHStream)
datahandle = arr.modifyHandle()
datahandle.copyDataFrom(c)
vertexData.setArray(1, arr)
cloud = Geom(vertexData)
cloud.addPrimitive(pointCloud)
cloudNode = GeomNode('points')
cloudNode.addGeom(cloud)
cloudNodePath = NodePath(cloudNode)
cloudNodePath.setRenderModeThickness(w)
cloudNodePath.setRenderModePerspective(True)
return cloudNode
class UI(ShowBase):
def __init__(self):
ShowBase.__init__(self)
vertexFormat = GeomVertexFormat.getV3c4()
self.vertexData = GeomVertexData("data", vertexFormat, Geom.UHDynamic)
geometry = Geom(self.vertexData)
self.primitive = GeomPoints(Geom.UHDynamic)
geometry.addPrimitive(self.primitive)
node = GeomNode("node")
node.addGeom(geometry)
self.nodePath = NodePath(node)
self.nodePath.setPos(0, 0, 0)
self.nodePath.setRenderModeThickness(10.0)
self.camera.setPos(0.0, -10.0, 0.0)
self.disableMouse()
self.nodePath.reparentTo(self.render)
def add_task(self, task):
def task_wrapper(taskObject):
task()
return Task.cont
self.taskMgr.add(task_wrapper)
def set_points(self, position, color):
assert len(position) == len(color)
self.vertexData.setNumRows(len(position))
self.primitive.clearVertices()
for i in range(len(position)):
self.primitive.addVertex(i)
self.primitive.closePrimitive()
positionWriter = GeomVertexWriter(self.vertexData, "vertex")
colorWriter = GeomVertexWriter(self.vertexData, "color")
for p, c in zip(position, color):
positionWriter.addData3f(p[0], p[1], p[2])
colorWriter.addData4f(c[0], c[1], c[2], c[3])
def draw_shape(angles, width, radius):
res = 10
if radius == 0:
point = GeomNode('gnode')
vdata = GeomVertexData('occ', GeomVertexFormat.getV3(), Geom.UHStatic)
vdata.setNumRows(1)
vertex = GeomVertexWriter(vdata, 'vertex')
vertex.addData3f(0, 0, 0)
geom = Geom(vdata)
g = GeomPoints(Geom.UHStatic)
g.addVertex(0)
geom.addPrimitive(g)
point.addGeom(geom)
return point, point
#first, sort angles in ascending order
#if negative is given, translate to range of 2pi
for i in range(len(angles)):
if angles[i] < 0: angles[i] = 360 + angles[i]
angles.sort()
angles = [math.radians(a) for a in angles]
angles.append(angles[0]) #read first angle to account for wrapping around
#function = 'w/sin(theta)'
vdata = GeomVertexData('occ', GeomVertexFormat.getV3(), Geom.UHStatic)
vdata.setNumRows(1)
vertex = GeomVertexWriter(vdata, 'vertex')
numverts = [3] * (len(angles) - 1
) #center, and two points on function lines
for i in range(len(angles) - 1):
#find center point
#L = [None,None]
#sign = [-1,1]
#for p in range(2):
# theta1 = angles[i+p] - sign[p]*math.pi/2
# theta2 = angles[i+p] + sign[p]*math.pi/4
# x1 = width*math.cos(theta1)
# y1 = width*math.sin(theta1)
# r = math.sqrt(2*width**2)
# x2 = r*math.cos(theta2)
# y2 = r*math.sin(theta2)
# L[p] = line([x1,y1], [x2,y2])
#R = intersection(L[0],L[1])
#if R is False:
# R = [x1,y1] #if parallel, shift both lines down by y_width
#vertex.addData3f(R[0],R[1],1)
difang = (angles[i + 1] - angles[i]) / 2
l = width / math.sin(difang)
avgang = (angles[i] + angles[i + 1]) / 2
x0 = l * math.cos(avgang)
y0 = l * math.sin(avgang)
vertex.addData3f(x0, y0, 1)
newang1 = angles[i] + (math.pi / 2 - math.acos(width / radius))
x = radius * math.cos(newang1)
y = radius * math.sin(newang1)
vertex.addData3f(x, y, 1)
newang2 = angles[i + 1] - (math.pi / 2 - math.acos(width / radius))
for angle in arc(newang1, newang2, res):
vertex.addData3f(radius * math.cos(math.radians(angle)),
radius * math.sin(math.radians(angle)), 1)
numverts[i] = numverts[i] + 1
x = radius * math.cos(newang2)
y = radius * math.sin(newang2)
vertex.addData3f(x, y, 1)
#copy all data to the bottom, moving it down to z = -1
vertread = GeomVertexReader(vdata, 'vertex')
while not vertread.isAtEnd():
v = vertread.getData3f()
vertex.addData3f(v[0], v[1], -1)
#draw
geom = Geom(vdata)
#draw top
for i in range(len(angles) - 1):
for j in range(numverts[i] - 2):
ind = sum(numverts[0:i]) + j
g = GeomTriangles(Geom.UHStatic)
g.add_vertices(ind - j, ind + 1, ind + 2)
geom.addPrimitive(g)
#draw bottom
for i in range(len(angles) - 1):
for j in range(numverts[i] - 2):
ind = sum(numverts) + sum(numverts[0:i]) + j
g = GeomTriangles(Geom.UHStatic)
g.add_vertices(ind - j, ind + 2, ind + 1)
geom.addPrimitive(g)
#draw edges
for i in range(len(angles) - 1):
for j in range(numverts[i] - 1):
ind = sum(numverts[0:i]) + j
g = GeomTriangles(Geom.UHStatic)
g.add_vertices(ind, ind + sum(numverts), ind + 1)
g.add_vertices(ind + sum(numverts), ind + 1 + sum(numverts),
ind + 1)
geom.addPrimitive(g)
g = GeomTriangles(Geom.UHStatic)
ind = sum(numverts[0:i])
indx = sum(numverts[0:i + 1])
g.add_vertices(indx - 1, indx + sum(numverts) - 1, ind)
g.add_vertices(indx + sum(numverts) - 1, sum(numverts) + ind, ind)
geom.addPrimitive(g)
#outline object
lines = Geom(vdata)
l = GeomLines(Geom.UHStatic)
for i in range(len(angles) - 1):
for j in range(numverts[i] - 1):
ind = sum(numverts[0:i]) + j
l.add_vertices(ind, ind + 1)
l.add_vertices(sum(numverts) + ind, sum(numverts) + ind + 1)
ind = sum(numverts[0:i])
indx = sum(numverts[0:i + 1])
l.add_vertices(ind, indx - 1)
l.add_vertices(sum(numverts) + ind, sum(numverts) + indx - 1)
l.add_vertices(ind + 1, sum(numverts) + ind + 1)
l.add_vertices(indx - 1, sum(numverts) + indx - 1)
lines.addPrimitive(l)
node = GeomNode('gnode')
node.addGeom(geom)
nodeL = GeomNode('gnode')
nodeL.addGeom(lines)
return node, nodeL
