def loadObjModel(self, ompath):
self.objtrimesh = trimesh.load_mesh(ompath)
# oversegmentation
self.facets, self.facetnormals = self.objtrimesh.facets_over(
faceangle=.95)
self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0])
self.sampleObjModel()
# prepare the model for collision detection
self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworld.attachRigidBody(self.objmeshbullnode)
# object center
self.objcenter = [0, 0, 0]
for pnt in self.objtrimesh.vertices:
self.objcenter[0] += pnt[0]
self.objcenter[1] += pnt[1]
self.objcenter[2] += pnt[2]
self.objcenter[
0] = self.objcenter[0] / self.objtrimesh.vertices.shape[0]
self.objcenter[
1] = self.objcenter[1] / self.objtrimesh.vertices.shape[0]
self.objcenter[
2] = self.objcenter[2] / self.objtrimesh.vertices.shape[0]
def loadObjModel(self, ompath):
self.objtrimesh=trimesh.load_mesh(ompath)
# oversegmentation
self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=.95, segangle = .95)
# self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=.95)
# conventional approach
# self.facets = self.objtrimesh.facets()
# self.facetnormals = []
# for i, faces in enumerate(self.objtrimesh.facets()):
# facetnormal = np.sum(self.objtrimesh.face_normals[faces], axis=0)
# facetnormal = facetnormal/np.linalg.norm(facetnormal)
# self.facetnormals.append(facetnormal)
self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0])
self.sampleObjModel()
def loadObjModel(self, ompath, objrotmat, objposmat):
self.objtrimesh = trimesh.load_mesh(ompath)
#add pos and rot to origin trimensh
self.objtrimesh.vertices = np.transpose(
np.dot(objrotmat, np.transpose(self.objtrimesh.vertices)))
self.objtrimesh.vertices = self.objtrimesh.vertices + objposmat
# oversegmentation
self.facets, self.facetnormals = self.objtrimesh.facets_over(
faceangle=.95, segangle=.95)
self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0])
self.sampleObjModel()
def loadObjModel(self, ompath):
self.objtrimesh=trimesh.load_mesh(ompath)
# oversegmentation
self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=.95)
self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0])
self.sampleObjModel()
# prepare the model for collision detection
self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworld.attachRigidBody(self.objmeshbullnode)
# object center
self.objcenter = [0,0,0]
for pnt in self.objtrimesh.vertices:
self.objcenter[0]+=pnt[0]
self.objcenter[1]+=pnt[1]
self.objcenter[2]+=pnt[2]
self.objcenter[0] = self.objcenter[0]/self.objtrimesh.vertices.shape[0]
self.objcenter[1] = self.objcenter[1]/self.objtrimesh.vertices.shape[0]
self.objcenter[2] = self.objcenter[2]/self.objtrimesh.vertices.shape[0]
def loadObjModel(self, ompath, dotnormplan=.95, dotnoarmovlp=.95):
self.objtrimesh=trimesh.load_mesh(ompath)
# oversegmentation
tic = time.time()
self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=dotnormplan, segangle = dotnoarmovlp)
toc = time.time()
print("facet cost", toc -tic)
# show original triangles
# self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=1.95, segangle = 1.95)
# self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=.95)
# conventional approach
# self.facets = self.objtrimesh.facets()
# self.facetnormals = []
# for i, faces in enumerate(self.objtrimesh.facets()):
# facetnormal = np.sum(self.objtrimesh.face_normals[faces], axis=0)
# facetnormal = facetnormal/np.linalg.norm(facetnormal)
# self.facetnormals.append(facetnormal)
self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0])
tic = time.time()
self.sampleObjModel()
toc = time.time()
print("sampling cost", toc-tic)
cobtsp1 = genMinkovPnt(polygon, sppoint1)
cobtsp2 = genMinkovPnt(polygon, sppoint2)
polygonsnp = genPolygonsnp(polygon, 0.0, color = [0,1,1,1], thickness=3)
for polygonnp in polygonsnp:
polygonnp.reparentTo(base.render)
# obstacles
stratcurveverts3d = []
lastxy = []
sppoint13d = []
sppoint23d = []
heightrange = 3600
steplength = 31
# colors
objcolorarray = pg.randomColorArray(int(heightrange/float(steplength))+1)
for height in range(0,heightrange,steplength):
if height/steplength > 3:
pg.plotLinesegs(base.render, stratcurveverts3d, rgba=[1.0, 1.0, 0.0, 1], thickness=30)
stratcurveverts3d = []
lastxy = []
break
rotedcobsp0 = rotate(cobtsp0, height/10.0, sppoint0)
rotedcobsp1 = rotate(cobtsp1, height/10.0, sppoint1)
rotedcobsp2 = rotate(cobtsp2, height/10.0, sppoint2)
# rotedcobtspunion = rotedcobsp0.union(rotedcobsp1).union(rotedcobsp2)
rotedcobtspunion = rotedcobsp1.union(rotedcobsp2)
rotedcobspolygonsnp = genPolygonsnp(rotedcobtspunion, height, color = [1.0,0,0,.3], thickness=1)
# for rotedcobspolygonnp in rotedcobspolygonsnp:
# rotedcobspolygonnp.reparentTo(base.render)
# stratum curve
for i, onedcurve in enumerate(newonedcurves12):
onedcavities = stb.cavities(onedcurve)
print onedcavities
for cavity in onedcavities:
pg.plotSphere(base.render, onedcurve[cavity[0]], radius = 100, rgba = [1,1,0,.5])
pg.plotSphere(base.render, onedcurve[cavity[2]], radius = 100, rgba = [1,1,0,.5])
# for i, segfgr3d in enumerate(segfgr0s3dsp1):
# for onefgrinsegfgr3d in segfgr3d:
# pg.plotLinesegs(base.render, onefgrinsegfgr3d, rgba=[1.0, 0.0, 0.0, 1], thickness=30)
for i, fingerp13d in enumerate(fingerp1s3d):
pg.plotLinesegs(base.render, fingerp13d, rgba=[0.0, 0.5, 0.0, 1], thickness=30)
for i, fingerp23d in enumerate(fingerp2s3d):
pg.plotLinesegs(base.render, fingerp23d, rgba=[0.0, 0.5, 0.0, 1], thickness=30)
objcolorarray = pg.randomColorArray(int(360*scale/float(steplength))+1, alpha = .2)
# for i, polygonxoncurve in enumerate(polygonxoncurves01):
# for j, polygonx in enumerate(polygonxoncurve):
# height = onedcurves01[i][j][2]
# polygonsnp = stb.genPolygonsnp(polygonx, height, color=[1.0, 1.0, 0.0, .2], thickness=20)
# for polygonnp in polygonsnp:
# polygonnp.reparentTo(base.render)
# for i, polygonxoncurve in enumerate(polygonxoncurves02):
# for j, polygonx in enumerate(polygonxoncurve):
# height = onedcurves02[i][j][2]
# polygonsnp = stb.genPolygonsnp(polygonx, height, color=[1.0, 0.0, 1.0, .2], thickness=20)
# for polygonnp in polygonsnp:
# polygonnp.reparentTo(base.render)
currentpolygon = None
for i, polygonxoncurve in enumerate(newpolygonxoncurves12):
for j, polygonx in enumerate(polygonxoncurve):
cobtsp1 = genMinkovPnt(polygon, sppoint1)
cobtsp2 = genMinkovPnt(polygon, sppoint2)
polygonsnp = genPolygonsnp(polygon, 0.0, color = [0,1,1,1], thickness=3)
for polygonnp in polygonsnp:
polygonnp.reparentTo(base.render)
# obstacles
stratcurveverts3d = []
lastxy = []
sppoint13d = []
sppoint23d = []
heightrange = 3600
steplength = 31
# colors
objcolorarray = pg.randomColorArray(int(heightrange/float(steplength))+1)
for height in range(0,heightrange,steplength):
if height/steplength > 3:
pg.plotLinesegs(base.render, stratcurveverts3d, rgba=[1.0, 1.0, 0.0, 1], thickness=30)
stratcurveverts3d = []
lastxy = []
break
rotedcobsp0 = rotate(cobtsp0, height/10.0, sppoint0)
rotedcobsp1 = rotate(cobtsp1, height/10.0, sppoint1)
rotedcobsp2 = rotate(cobtsp2, height/10.0, sppoint2)
# rotedcobtspunion = rotedcobsp0.union(rotedcobsp1).union(rotedcobsp2)
rotedcobtspunion = rotedcobsp1.union(rotedcobsp2)
rotedcobspolygonsnp = genPolygonsnp(rotedcobtspunion, height, color = [1.0,0,0,.3], thickness=1)
# for rotedcobspolygonnp in rotedcobspolygonsnp:
# rotedcobspolygonnp.reparentTo(base.render)
# stratum curve
fingerp03d,
rgba=[1.0, 0.0, 0.0, 1],
thickness=30)
for i, fingerp13d in enumerate(fingerp1s3d):
pg.plotLinesegs(base.render,
fingerp13d,
rgba=[0.0, 0.5, 0.0, 1],
thickness=30)
for i, fingerp23d in enumerate(fingerp2s3d):
pg.plotLinesegs(base.render,
fingerp23d,
rgba=[0.0, 0.5, 0.0, 1],
thickness=30)
objcolorarray = pg.randomColorArray(int(360 * scale / float(steplength)) +
1,
alpha=.2)
# for i, polygonxoncurve in enumerate(polygonxoncurves01):
# for j, polygonx in enumerate(polygonxoncurve):
# height = onedcurves01[i][j][2]
# polygonsnp = stb.genPolygonsnp(polygonx, height, color=[1.0, 1.0, 0.0, .2], thickness=20)
# for polygonnp in polygonsnp:
# polygonnp.reparentTo(base.render)
# for i, polygonxoncurve in enumerate(polygonxoncurves02):
# for j, polygonx in enumerate(polygonxoncurve):
# height = onedcurves02[i][j][2]
# polygonsnp = stb.genPolygonsnp(polygonx, height, color=[1.0, 0.0, 1.0, .2], thickness=20)
# for polygonnp in polygonsnp:
# polygonnp.reparentTo(base.render)
currentpolygon = None
for i, polygonxoncurve in enumerate(polygonxoncurves12):