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):