def removeFgrpccShow(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
:return:
author: weiwei
date: 20161201, osaka
"""
# 6 is used because I am supposing 4+2 where 4 is the default
# margin of bullet in panda3d. (NOTE: This is a guess)
plotoffsetfp = 6
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
#
# np0collection = base.render.findAllMatches("**/rtq85fgrpcc0")
# for np0 in np0collection:
# np0.removeNode()
# np1collection = base.render.findAllMatches("**/rtq85fgrpcc1")
# for np1 in np1collection:
# np1.removeNode()
#
# npscollection = base.render.findAllMatches("**/sphere")
# for nps in npscollection:
# nps.removeNode()
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
facetcolorarray = self.facetcolorarray
star0.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(brchild)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
star1.setColor(
Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
star1.setTwoSided(True)
star1.reparentTo(brchild)
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
# the following two choices decide the way to detect contacts
cctnormal00 = np.array(
self.gripcontactpairnormals[self.counter][j][0])
cctnormal01 = -np.array(
self.gripcontactpairnormals[self.counter][j][1])
cctnormal0raw = (cctnormal00 + cctnormal01)
cctnormal0 = (cctnormal0raw /
np.linalg.norm(cctnormal0raw)).tolist()
# the following two choices decide the way to detect contacts
cctnormal10 = -cctnormal00
cctnormal11 = -cctnormal01
cctnormal1raw = (cctnormal10 + cctnormal11)
cctnormal1 = (cctnormal1raw /
np.linalg.norm(cctnormal1raw)).tolist()
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0],
cctpnt0[1] + cctnormal0[1],
cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("handfgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc1)
handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2])
handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0],
cctpnt1[1] + cctnormal1[1],
cctpnt1[2] + cctnormal1[2])
handfgrpcc = NodePath("handfgrpcc")
handfgrpcc0.reparentTo(handfgrpcc)
handfgrpcc1.reparentTo(handfgrpcc)
# prepare the model for collision detection
facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild)
result = self.bulletworld.contactTest(facetmeshbullnode)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
pandageom.plotSphere(brchild,
pos=cp.getLocalPointA(),
radius=3,
rgba=Vec4(1, 0, 0, 1))
pandageom.plotSphere(brchild,
pos=cp.getLocalPointB(),
radius=3,
rgba=Vec4(0, 0, 1, 1))
if result.getNumContacts():
handfgrpcc0.setColor(1, 0, 0, .3)
handfgrpcc1.setColor(1, 0, 0, .3)
else:
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
pandageom.plotArrow(star0,
spos=cctpnt0,
epos=cctpnt0 +
plotoffsetfp * self.facetnormals[facetidx0] +
cctnormal0,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
pandageom.plotArrow(star1,
spos=cctpnt1,
epos=cctpnt1 +
plotoffsetfp * self.facetnormals[facetidx1] +
cctnormal1,
rgba=[
facetcolorarray[facetidx1][0],
facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2],
facetcolorarray[facetidx1][3]
],
length=10)
pg.plotLinesegs(base.render, onedcurve, rgba=[1.0, 0.0, 1.0, 1], thickness=50)
# seg 1 1 seg 1 2
for i, onedcurve in enumerate(onedcurvesSeg11):
pg.plotLinesegs(base.render, onedcurve, rgba=[0.0, 1.0, 1.0, 1], thickness=10)
for i, onedcurve in enumerate(newonedcurvesSeg11):
pg.plotLinesegs(base.render, onedcurve, rgba=[0.0, 1.0, 1.0, 1], thickness=50)
for i, onedcurve in enumerate(onedcurvesSeg12):
pg.plotLinesegs(base.render, onedcurve, rgba=[.0, 0.0, 1.0, 1], thickness=10)
for i, onedcurve in enumerate(newonedcurvesSeg12):
pg.plotLinesegs(base.render, onedcurve, rgba=[.0, 0.0, 1.0, 1], thickness=50)
# 1 2
for i, onedcurve in enumerate(onedcurves12):
onedcavities = stb.cavities(onedcurve)
for cavity in onedcavities:
# pg.plotLinesegs(base.render, onedcurve[cavity[0]:cavity[2]+1], rgba=[0.0, 1.0, 0.0, 1], thickness=50)
pg.plotSphere(base.render, onedcurve[cavity[1]], radius = 100, rgba = [0,0,1,1])
pg.plotSphere(base.render, onedcurve[cavity[0]], radius = 100, rgba = [1,0,0,1])
pg.plotSphere(base.render, onedcurve[cavity[2]], radius = 100, rgba = [1,0,0,1])
pg.plotLinesegs(base.render, onedcurve, rgba=[0.0, 0.5, 0.0, 1], thickness=10)
for i, onedcurve in enumerate(newonedcurves12):
pg.plotLinesegs(base.render, onedcurve, rgba=[0.0, .5, 0.0, 1], thickness=50)
# plot maxima
# seg 01 02
for i, onedcurve in enumerate(newonedcurvesSeg01):
onedcavities = stb.cavities(onedcurve)
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, onedcurve in enumerate(newonedcurvesSeg02):
onedcavities = stb.cavities(onedcurve)
# rtq85hnd.rtq85np.find("**/rtq85lfgr").showTightBounds()
# rtq85hnd.rtq85np.showTightBounds()
base.taskMgr.add(updateworld,
"updateworld",
extraArgs=[base.world],
appendTask=True)
result = base.world.contactTestPair(ilkbullnode, lftbullnode)
print result
print result.getContacts()
import pandaplotutils.pandageom as pandageom
for contact in result.getContacts():
cp = contact.getManifoldPoint()
print cp.getLocalPointA()
pandageom.plotSphere(base,
pos=cp.getLocalPointA(),
radius=10,
rgba=Vec4(1, 0, 0, 1))
pandageom.plotAxisSelf(base.render, spos=Vec3(0, 0, 0))
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
debugNP = bullcldrnp.attachNewNode(debugNode)
# debugNP.show()
base.world.setDebugNode(debugNP.node())
base.run()
def segShow2(self, base, togglesamples=False, togglenormals=False,
togglesamples_ref=False, togglenormals_ref=False,
togglesamples_refcls=False, togglenormals_refcls=False, specificface = True):
"""
:param base:
:param togglesamples:
:param togglenormals:
:param togglesamples_ref: toggles the sampled points that fulfills the dist requirements
:param togglenormals_ref:
:return:
"""
nfacets = self.facets.shape[0]
facetcolorarray = self.facetcolorarray
rgbapnts0 = [1, 1, 1, 1]
rgbapnts1 = [0.2, 0.7, 1, 1]
rgbapnts2 = [1, 0.7, 0.2, 1]
# offsetf = facet
plotoffsetf = .0
faceplotted = False
# plot the segments
for i, facet in enumerate(self.facets):
if not specificface:
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(.77, .17, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j],
rgba=rgbapnts0, length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.9, rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1, length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2, length=10)
if specificface:
plotoffsetf = .1
if faceplotted:
continue
else:
if len(self.objsamplepnts[i])>25:
faceplotted = True
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(facetcolorarray[i][0], facetcolorarray[i][1], facetcolorarray[i][2], 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j],
rgba=rgbapnts0, length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1, length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3.5, rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2, length=10)
onedcurves_s0g1_cdfree = mgc.cvtcurveDict2List(onedcurvedict_s0g1_rs1g0)
polygonxoncurves_s0g1_cdfree = mgc.cvtpolygonDict2List(polygonxoncurvedict_s0g1_rs1g0)
## remove collided curves between curve_s1g1 and obt g0, obt s0
onedcurvedict_s1g1_rg0, polygonxoncurvedict_s1g1_rg0 = \
mgc.removeCollisiononOnedCurves(onedcurvedict_s1g1, polygonxoncurvedict_s1g1, cobtsdict_g0)
onedcurvedict_s1g1_rg0s0, polygonxoncurvedict_s1g1_rg0s0 = \
mgc.removeCollisiononOnedCurves(onedcurvedict_s1g1_rg0, polygonxoncurvedict_s1g1_rg0, cobtsdict_s0)
onedcurves_s1g1_cdfree = mgc.cvtcurveDict2List(onedcurvedict_s1g1_rg0s0)
polygonxoncurves_s1g1_cdfree = mgc.cvtpolygonDict2List(polygonxoncurvedict_s1g1_rg0s0)
# plot maxima
# g0s0
for i, onedcurve in enumerate(onedcurves_g0s0_cdfree):
onedcavities = mgc.stb.cavities(onedcurve)
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])
# g0s1
for i, onedcurve in enumerate(onedcurves_g0s1_cdfree):
onedcavities = mgc.stb.cavities(onedcurve)
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])
# g0g1
for i, onedcurve in enumerate(onedcurves_g0g1_cdfree):
onedcavities = mgc.stb.cavities(onedcurve)
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])
# s0s1
for i, onedcurve in enumerate(onedcurves_s0s1_cdfree):
cobtsdict_s0 = mgc.stb.genCobt(polygon, sppoint0)
cobtsdict_s1 = mgc.stb.genCobt(polygon, sppoint1)
## remove collided curves (dummy operation)
onedcurves_s0s1_cdfree = mgc.cvtcurveDict2List(onedcurvedict_s0s1)
polygonxoncurves_s0s1_cdfree = mgc.cvtpolygonDict2List(polygonxoncurvedict_s0s1)
#
# # plot maxima
# # s0s1
for i, onedcurve in enumerate(onedcurves_s0s1_cdfree):
onedcavities = mgc.stb.cavities(onedcurve)
for j, cavity in enumerate(onedcavities):
if j == 2:
# pg.plotSphere(base.render, onedcurve[cavity[0]], radius = 200, rgba = [0,0,1,1])
pg.plotSphere(base.render, onedcurve[cavity[1]], radius = 200, rgba = [1,0,0,1])
# pg.plotSphere(base.render, onedcurve[cavity[2]], radius = 200, rgba = [0,0,1,1])
# s0s1
for i, onedcurve in enumerate(onedcurves_s0s1_cdfree):
pg.plotLinesegs(base.render, onedcurve, rgba=[0.0, 0.0, 0.0, 1], thickness=100)
# for i, onedcurve in enumerate(onedcurves_s0s1_cdfree):
# onedcavities = mgc.stb.cavities(onedcurve)
# for j, cavity in enumerate(onedcavities):
# # if j == 2:
# pg.plotLinesegs(base.render, onedcurve[cavity[0]:cavity[2]], rgba=[0.0, 0.0, 0.0, 1], thickness=100)
# startingconf = []
# breakingconf = []
lowestconfatbreaking = []
currentkey = 0
currentpolygon0=[]
for i, polygonxoncurve in enumerate(polygonxoncurves_s0s1):
if a[1] > mgc.minusinf + 1:
othercurvepoints.append([a[0], a[1], currentkey])
if b[1] > mgc.minusinf + 1:
othercurvepoints.append([b[0], b[1], currentkey])
if c[1] > mgc.minusinf + 1:
othercurvepoints.append([c[0], c[1], currentkey])
if d[1] > mgc.minusinf + 1:
othercurvepoints.append([d[0], d[1], currentkey])
if e[1] > mgc.minusinf + 1:
othercurvepoints.append([e[0], e[1], currentkey])
if f[1] > mgc.minusinf + 1:
othercurvepoints.append([f[0], f[1], currentkey])
breakingmaxima, breakingconfiguration, valleyminima, bottomconfiguration = \
mgc.lstb.maxmin_oneslice(curvepoint, polylist_onecobtslice, othercurvepoints)
# pg.plotSphere(base.render, breakingconfiguration, radius = 200, rgba=[.3,0,.3,1])
pg.plotSphere(base.render, bottomconfiguration, radius = 300, rgba=[1,0,0,1])
#
lowestonedcurve = {}
# for key in range(0, mgc.heightrange, mgc.steplength):
# vert = [mgc.minusinf, mgc.minusinf, key]
# a = onedcurvedict_g0s0_rg1s1[key]
# b = onedcurvedict_g0s1_rg1s0[key]
# c = onedcurvedict_g0g1_rs0s1[key]
# d = onedcurvedict_s0s1_rg0g1[key]
# e = onedcurvedict_s0g1_rs1g0[key]
# f = onedcurvedict_s1g1_rg0s0[key]
# # print a,b,c,d,e,f
# # time.sleep(1.0)
# vertlist = []
# if a[1] > mgc.minusinf + 1:
# vertlist.append([a[0], a[1], key])
def removeFgrpccShow(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
:return:
author: weiwei
date: 20161201, osaka
"""
# 6 is used because I am supposing 4+2 where 4 is the default
# margin of bullet in panda3d. (NOTE: This is a guess)
plotoffsetfp = 6
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
#
# np0collection = base.render.findAllMatches("**/rtq85fgrpcc0")
# for np0 in np0collection:
# np0.removeNode()
# np1collection = base.render.findAllMatches("**/rtq85fgrpcc1")
# for np1 in np1collection:
# np1.removeNode()
#
# npscollection = base.render.findAllMatches("**/sphere")
# for nps in npscollection:
# nps.removeNode()
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
facetcolorarray = self.facetcolorarray
star0.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(brchild)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
star1.setTwoSided(True)
star1.reparentTo(brchild)
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
# the following two choices decide the way to detect contacts
cctnormal00 = np.array(self.gripcontactpairnormals[self.counter][j][0])
cctnormal01 = -np.array(self.gripcontactpairnormals[self.counter][j][1])
cctnormal0raw = (cctnormal00 + cctnormal01)
cctnormal0 = (cctnormal0raw/np.linalg.norm(cctnormal0raw)).tolist()
# the following two choices decide the way to detect contacts
cctnormal10 = -cctnormal00
cctnormal11 = -cctnormal01
cctnormal1raw = (cctnormal10 + cctnormal11)
cctnormal1 = (cctnormal1raw/np.linalg.norm(cctnormal1raw)).tolist()
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("handfgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc1)
handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2])
handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0], cctpnt1[1] + cctnormal1[1], cctpnt1[2] + cctnormal1[2])
handfgrpcc = NodePath("handfgrpcc")
handfgrpcc0.reparentTo(handfgrpcc)
handfgrpcc1.reparentTo(handfgrpcc)
# prepare the model for collision detection
facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild)
result = self.bulletworld.contactTest(facetmeshbullnode)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
if result.getNumContacts():
handfgrpcc0.setColor(1, 0, 0, .3)
handfgrpcc1.setColor(1, 0, 0, .3)
else:
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
pandageom.plotArrow(star0, spos=cctpnt0,
epos=cctpnt0 + plotoffsetfp*self.facetnormals[facetidx0] + cctnormal0,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
pandageom.plotArrow(star1, spos=cctpnt1,
epos=cctpnt1 + plotoffsetfp*self.facetnormals[facetidx1] + cctnormal1,
rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
othercurvepoints.append([a[0], a[1], currentkey])
if b[1] > mgc.minusinf + 1:
othercurvepoints.append([b[0], b[1], currentkey])
if c[1] > mgc.minusinf + 1:
othercurvepoints.append([c[0], c[1], currentkey])
if d[1] > mgc.minusinf + 1:
othercurvepoints.append([d[0], d[1], currentkey])
if e[1] > mgc.minusinf + 1:
othercurvepoints.append([e[0], e[1], currentkey])
if f[1] > mgc.minusinf + 1:
othercurvepoints.append([f[0], f[1], currentkey])
breakingmaxima, breakingconfiguration, valleyminima, bottomconfiguration = \
mgc.lstb.maxmin_oneslice(curvepoint, polylist_onecobtslice, othercurvepoints)
# pg.plotSphere(base.render, breakingconfiguration, radius = 200, rgba=[.3,0,.3,1])
pg.plotSphere(base.render,
bottomconfiguration,
radius=300,
rgba=[1, 0, 0, 1])
#
lowestonedcurve = {}
# for key in range(0, mgc.heightrange, mgc.steplength):
# vert = [mgc.minusinf, mgc.minusinf, key]
# a = onedcurvedict_g0s0_rg1s1[key]
# b = onedcurvedict_g0s1_rg1s0[key]
# c = onedcurvedict_g0g1_rs0s1[key]
# d = onedcurvedict_s0s1_rg0g1[key]
# e = onedcurvedict_s0g1_rs1g0[key]
# f = onedcurvedict_s1g1_rg0s0[key]
# # print a,b,c,d,e,f
# # time.sleep(1.0)
# vertlist = []
# if a[1] > mgc.minusinf + 1:
print sdmbs.getHpr()
# sdmbs.reparentTo(base.render)
handbullnp = cd.genCollisionMeshNp(sdmbs.handnp)
# second hand
sdmbs1 = Sdmbs()
sdmbs1.reparentTo(base.render)
hand1bullnp = cd.genCollisionMeshNp(sdmbs1.handnp)
pg.plotAxisSelf(base.render, Vec3(0,0,0))
bullcldrnp = base.render.attachNewNode("bulletcollider")
base.world = BulletWorld()
base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True)
result = base.world.contactTestPair(handbullnp, hand1bullnp)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
pg.plotSphere(base.render, pos=cp.getLocalPointA(), radius=1, rgba=Vec4(1,0,0,1))
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
debugNP = bullcldrnp.attachNewNode(debugNode)
# debugNP.show()
base.world.setDebugNode(debugNP.node())
base.run()
print sdmbs.getHpr()
# sdmbs.reparentTo(base.render)
handbullnp = cd.genCollisionMeshNp(sdmbs.handnp)
# second hand
sdmbs1 = SdmbsSd()
sdmbs1.reparentTo(base.render)
hand1bullnp = cd.genCollisionMeshNp(sdmbs1.handnp)
pg.plotAxisSelf(base.render, Vec3(0,0,0))
bullcldrnp = base.render.attachNewNode("bulletcollider")
base.world = BulletWorld()
base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True)
result = base.world.contactTestPair(handbullnp, hand1bullnp)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
pg.plotSphere(base.render, pos=cp.getLocalPointA(), radius=1, rgba=Vec4(1,0,0,1))
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
debugNP = bullcldrnp.attachNewNode(debugNode)
# debugNP.show()
base.world.setDebugNode(debugNP.node())
base.run()
cobtsdict_s1 = mgc.stb.genCobt(polygon, sppoint1)
## remove collided curves (dummy operation)
onedcurves_s0s1_cdfree = mgc.cvtcurveDict2List(onedcurvedict_s0s1)
polygonxoncurves_s0s1_cdfree = mgc.cvtpolygonDict2List(
polygonxoncurvedict_s0s1)
#
# # plot maxima
# # s0s1
for i, onedcurve in enumerate(onedcurves_s0s1_cdfree):
onedcavities = mgc.stb.cavities(onedcurve)
for j, cavity in enumerate(onedcavities):
if j == 2:
# pg.plotSphere(base.render, onedcurve[cavity[0]], radius = 200, rgba = [0,0,1,1])
pg.plotSphere(base.render,
onedcurve[cavity[1]],
radius=200,
rgba=[1, 0, 0, 1])
# pg.plotSphere(base.render, onedcurve[cavity[2]], radius = 200, rgba = [0,0,1,1])
# s0s1
for i, onedcurve in enumerate(onedcurves_s0s1_cdfree):
pg.plotLinesegs(base.render,
onedcurve,
rgba=[0.0, 0.0, 0.0, 1],
thickness=100)
# for i, onedcurve in enumerate(onedcurves_s0s1_cdfree):
# onedcavities = mgc.stb.cavities(onedcurve)
# for j, cavity in enumerate(onedcavities):
# # if j == 2:
# pg.plotLinesegs(base.render, onedcurve[cavity[0]:cavity[2]], rgba=[0.0, 0.0, 0.0, 1], thickness=100)
# startingconf = []
# breakingconf = []
for i, onedcurve in enumerate(onedcurves02):
pg.plotLinesegs(base.render,
onedcurve,
rgba=[1.0, 0.0, 1.0, 1],
thickness=30)
for i, onedcurve in enumerate(newonedcurves02):
pg.plotLinesegs(base.render,
onedcurve,
rgba=[0.0, .5, 1.0, 1],
thickness=50)
for i, onedcurve in enumerate(onedcurves12):
onedcavities = stb.cavities(onedcurve)
for cavity in onedcavities:
# pg.plotLinesegs(base.render, onedcurve[cavity[0]:cavity[2]+1], rgba=[0.0, 1.0, 0.0, 1], thickness=50)
pg.plotSphere(base.render,
onedcurve[cavity[1]],
radius=100,
rgba=[0, 0, 1, 1])
pg.plotSphere(base.render,
onedcurve[cavity[0]],
radius=100,
rgba=[1, 0, 0, 1])
pg.plotSphere(base.render,
onedcurve[cavity[2]],
radius=100,
rgba=[1, 0, 0, 1])
pg.plotLinesegs(base.render,
onedcurve,
rgba=[0.0, 0.5, 0.0, 1],
thickness=30)
for i, onedcurve in enumerate(newonedcurves12):
pg.plotLinesegs(base.render,
lfgrbullnode = BulletRigidBodyNode('glfgr')
lfgrbullnode.addShape(BulletTriangleMeshShape(glfgrmesh, dynamic=True), glfgrts)
lfgrcollidernp=bullcldrnp.attachNewNode(lfgrbullnode)
base.world.attachRigidBody(lfgrbullnode)
lfgrcollidernp.setCollideMask(BitMask32.allOn())
# rtq85hnd.rtq85np.find("**/rtq85lfgr").showTightBounds()
# rtq85hnd.rtq85np.showTightBounds()
base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True)
result = base.world.contactTestPair(ilkbullnode, lftbullnode)
print result
print result.getContacts()
import pandaplotutils.pandageom as pandageom
for contact in result.getContacts():
cp = contact.getManifoldPoint()
print cp.getLocalPointA()
pandageom.plotSphere(base, pos=cp.getLocalPointA(), radius=10, rgba=Vec4(1,0,0,1))
pandageom.plotAxisSelf(base.render, spos = Vec3(0,0,0))
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
debugNP = bullcldrnp.attachNewNode(debugNode)
# debugNP.show()
base.world.setDebugNode(debugNP.node())
base.run()
def segShow2(self,
base,
togglesamples=False,
togglenormals=False,
togglesamples_ref=False,
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False,
specificface=True):
"""
:param base:
:param togglesamples:
:param togglenormals:
:param togglesamples_ref: toggles the sampled points that fulfills the dist requirements
:param togglenormals_ref:
:return:
"""
nfacets = self.facets.shape[0]
facetcolorarray = self.facetcolorarray
rgbapnts0 = [1, 1, 1, 1]
rgbapnts1 = [0, 0, 1, 1]
rgbapnts2 = [1, 0, 0, 1]
# offsetf = facet
plotoffsetf = .0
faceplotted = False
# plot the segments
for i, facet in enumerate(self.facets):
if not specificface:
geom = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetf * i * self.facetnormals[i],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[facet],
self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(.77, .17, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=2.8,
rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls[i][j],
rgba=rgbapnts0,
length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=2.9,
rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1,
length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=3,
rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2,
length=10)
if specificface:
plotoffsetf = .3
if faceplotted:
continue
else:
if len(self.objsamplepnts[i]) > 85:
faceplotted = True
geom = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetf * i * self.facetnormals[i],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[facet],
self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(
Vec4(facetcolorarray[i][0], facetcolorarray[i][1],
facetcolorarray[i][2], 1))
star.setColor(Vec4(.7, .3, .3, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=2.8,
rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls[i][j],
rgba=rgbapnts0,
length=10)
if togglesamples_ref:
for j, apnt in enumerate(
self.objsamplepnts_ref[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=2.9,
rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(
self.objsamplepnts_ref[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1,
length=10)
if togglesamples_refcls:
for j, apnt in enumerate(
self.objsamplepnts_refcls[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=3,
rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(
self.objsamplepnts_refcls[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2,
length=10)
def pairShow(self, base, togglecontacts = False, togglecontactnormals = False):
# the following sentence requires segshow to be executed first
facetcolorarray = self.facetcolorarray
# offsetfp = facetpair
plotoffsetfp = np.random.random()*50
# plot the pairs and their contacts
# for i in range(self.counter+1, len(self.facetpairs)):
# if self.gripcontactpairs[i]:
# self.counter = i
# break
# if i is len(self.facetpairs):
# return
# delete the facetpair after show
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
# self.counter = 0
return
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
star0.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(base.render)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
# star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
# set to the same color
star1.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star1.setTwoSided(True)
star1.reparentTo(base.render)
if togglecontacts:
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotSphere(star0, pos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0], radius=4,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]])
# pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]])
# use the same color
pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]])
if togglecontactnormals:
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotArrow(star0, spos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0],
epos=cttpnt0 + plotoffsetfp*self.facetnormals[facetidx0] +
self.gripcontactpairnormals[self.counter][j][0],
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
# pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
# epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
# self.gripcontactpairnormals[self.counter][j][1],
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
# use the same color
pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
self.gripcontactpairnormals[self.counter][j][1],
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
def pairShow(self, base, togglecontacts=False, togglecontactnormals=False):
# the following sentence requires segshow to be executed first
facetcolorarray = self.facetcolorarray
# offsetfp = facetpair
plotoffsetfp = np.random.random() * 50
# plot the pairs and their contacts
# for i in range(self.counter+1, len(self.facetpairs)):
# if self.gripcontactpairs[i]:
# self.counter = i
# break
# if i is len(self.facetpairs):
# return
# delete the facetpair after show
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
# self.counter = 0
return
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
star0.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(base.render)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
# star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
# set to the same color
star1.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star1.setTwoSided(True)
star1.reparentTo(base.render)
if togglecontacts:
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotSphere(
star0,
pos=cttpnt0 + plotoffsetfp * self.facetnormals[facetidx0],
radius=4,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
])
# pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]])
# use the same color
pandageom.plotSphere(
star1,
pos=cttpnt1 + plotoffsetfp * self.facetnormals[facetidx1],
radius=4,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
])
if togglecontactnormals:
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotArrow(
star0,
spos=cttpnt0 + plotoffsetfp * self.facetnormals[facetidx0],
epos=cttpnt0 +
plotoffsetfp * self.facetnormals[facetidx0] +
self.gripcontactpairnormals[self.counter][j][0],
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
# pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
# epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
# self.gripcontactpairnormals[self.counter][j][1],
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
# use the same color
pandageom.plotArrow(
star1,
spos=cttpnt1 + plotoffsetfp * self.facetnormals[facetidx1],
epos=cttpnt1 +
plotoffsetfp * self.facetnormals[facetidx1] +
self.gripcontactpairnormals[self.counter][j][1],
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
## remove collided curves between curve_s1g1 and obt g0, obt s0
onedcurvedict_s1g1_rg0, polygonxoncurvedict_s1g1_rg0 = \
mgc.removeCollisiononOnedCurves(onedcurvedict_s1g1, polygonxoncurvedict_s1g1, cobtsdict_g0)
onedcurvedict_s1g1_rg0s0, polygonxoncurvedict_s1g1_rg0s0 = \
mgc.removeCollisiononOnedCurves(onedcurvedict_s1g1_rg0, polygonxoncurvedict_s1g1_rg0, cobtsdict_s0)
onedcurves_s1g1_cdfree = mgc.cvtcurveDict2List(onedcurvedict_s1g1_rg0s0)
polygonxoncurves_s1g1_cdfree = mgc.cvtpolygonDict2List(
polygonxoncurvedict_s1g1_rg0s0)
# plot maxima
# g0s0
for i, onedcurve in enumerate(onedcurves_g0s0_cdfree):
onedcavities = mgc.stb.cavities(onedcurve)
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])
# g0s1
for i, onedcurve in enumerate(onedcurves_g0s1_cdfree):
onedcavities = mgc.stb.cavities(onedcurve)
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]],
