def showObjObj(self, obj0, obj1):
"""
show the collision meshes of the given objects
:param obj0, obj1
author: weiwei
date: 20180621
:return:
"""
def updateworld(world, task):
world.doPhysics(globalClock.getDt())
return task.cont
base.taskMgr.add(updateworld,
"updateworld",
extraArgs=[self.__base.world],
appendTask=True)
obj0bullnode = cd.genCollisionMeshMultiNp(obj0)
obj1bullnode = cd.genCollisionMeshMultiNp(obj1)
cdnode0 = self.__base.world.attachRigidBody(obj0bullnode)
cdnode1 = self.__base.world.attachRigidBody(obj1bullnode)
self.worldrigidbodylist.append(cdnode0)
self.worldrigidbodylist.append(cdnode1)
self.__debugNP.show()
def isObjObjCollided(self, obj0, obj1):
"""
check if two objects obj0 as obj1 are in collision with each other
the two objects are in the form of pandanodes
:param obj0: the first object
:param obj1: the second object
:return: boolean value showing if the two objects are in collision
author: weiwei
date: 20180621
"""
obj0bullnode = cd.genCollisionMeshMultiNp(obj0)
obj1bullnode = cd.genCollisionMeshMultiNp(obj1)
result = self.__base.world.contactTestPair(obj0bullnode, obj1bullnode)
if not result.getNumContacts():
return False
else:
return True
def removeFgrpccShowLeft(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
Plot the available grips
:return:
author: weiwei
date: 20161212, tsukuba
"""
plotoffsetfp = 6
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
return
else:
print(str(self.counter) + "/" + str(self.facetpairs.shape[0]-1))
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(base.render)
handfgrpcc1.reparentTo(base.render)
bullcldrnp = base.render.attachNewNode("bulletcollider")
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.pggen.plotAxis(base.render)
base.pggen.plotAxis(base.render, spos=rtq85hnd.handnp.getPos(), pandamat3=rtq85hnd.handnp.getMat().getUpper3())
obj1bullnode = cd.genCollisionMeshMultiNp(rtq85hnd.handnp)
base.world.attachRigidBody(obj1bullnode)
base.run()
# # hand base
# # rtq85hnd.rtq85np.find("**/rtq85base").showTightBounds()
# gbnp = rtq85hnd.rtq85np.find("**/rtq85base").find("**/+GeomNode")
# gb = gbnp.node().getGeom(0)
# gbts = gbnp.getTransform(base.render)
# gbmesh = BulletTriangleMesh()
# gbmesh.addGeom(gb)
# bbullnode = BulletRigidBodyNode('gb')
# bbullnode.addShape(BulletTriangleMeshShape(gbmesh, dynamic=True), gbts)
# bcollidernp=bullcldrnp.attachNewNode(bbullnode)
# base.world.attachRigidBody(bbullnode)
# bcollidernp.setCollideMask(BitMask32.allOn())
# sck918hnd.gripAt(0,0,0, 1,0,0, 0)
# sck918hnd.reparentTo(base.render)
#
# # axis = loader.loadModel('zup-axis.egg')
# # axis.reparentTo(base.render)
# # axis.setPos(hndpos)
# # axis.setScale(50)
# # axis.lookAt(hndpos+ydirect)
# import pandaplotutils.pandageom as pandageom
# pgg = pandageom.PandaGeomGen()
# pgg.plotAxis(base.render)
#
bullcldrnp = base.render.attachNewNode("bulletcollider")
base.world = BulletWorld()
import pandaplotutils.collisiondetection as cd
obj1bullnode = cd.genCollisionMeshMultiNp(sck918hnd.handnp)
base.world.attachRigidBody(obj1bullnode)
#
# # hand base
# # rtq85hnd.rtq85np.find("**/rtq85base").showTightBounds()
# gbnp = sck918hnd.rtq85np.find("**/sck918base").find("**/+GeomNode")
# gb = gbnp.node().getGeom(0)
# gbts = gbnp.getTransform(base.render)
# gbmesh = BulletTriangleMesh()
# gbmesh.addGeom(gb)
# bbullnode = BulletRigidBodyNode('gb')
# bbullnode.addShape(BulletTriangleMeshShape(gbmesh, dynamic=True), gbts)
# bcollidernp=bullcldrnp.attachNewNode(bbullnode)
# base.world.attachRigidBody(bbullnode)
# bcollidernp.setCollideMask(BitMask32.allOn())
#
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 1
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
# cc0 first
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
# tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = self.hand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
if fgrdist > self.hand.jawwidthopen:
continue
# tmphand.setJawwidth(fgrdist)
# tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
# rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
# rotmat = rm.rodrigues(rotax, rotangle)
# tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
# axx = tmphand.getMat().getRow3(0)
# # 130 is the distance from hndbase to fingertip
# cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
# tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
fc = (cctpnt0 + cctpnt1)/2.0
self.hand.gripAt(fc[0], fc[1], fc[2], cctnormal0[0], cctnormal0[1], cctnormal0[2], rotangle, jawwidth = fgrdist)
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(self.hand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(self.hand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# reset initial hand pose
self.hand.setMat(initmat)
# cc1 first
for j, contactpair in enumerate(self.flipgripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt1 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt0 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal1 = self.flipgripcontactpairnormals_precc[self.counter][j][0]
cctnormal0 = [-cctnormal1[0], -cctnormal1[1], -cctnormal1[2]]
# tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = self.hand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
if fgrdist > self.hand.jawwidthopen:
continue
# tmphand.setJawwidth(fgrdist)
# tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
# rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
# rotmat = rm.rodrigues(rotax, rotangle)
# tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
# axx = tmphand.getMat().getRow3(0)
# # 130 is the distance from hndbase to fingertip
# cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
# tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
fc = (cctpnt0 + cctpnt1)/2.0
self.hand.gripAt(fc[0], fc[1], fc[2], cctnormal0[0], cctnormal0[1], cctnormal0[2], rotangle, jawwidth = fgrdist)
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(self.hand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(self.hand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.flipgripcontactpairnormals_precc[self.counter][j])
# reset initial hand pose
self.hand.setMat(initmat)
self.counter+=1
self.counter = 0
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print(str(self.counter) + "/" + str(self.facetpairs.shape[0]-1))
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
if j == 0:
print(j, contactpair)
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
if fgrdist > self.hand.jawwidthopen:
continue
# tmprtq85.setJawwidth(fgrdist)
# since fgrpcc already detects inner collisions
rotangle = 360.0 / discretesize * angleid
fc = (cctpnt0 + cctpnt1) / 2.0
tmprtq85.gripAt(fc[0], fc[1], fc[2], cctnormal0[0], cctnormal0[1], cctnormal0[2], rotangle, fgrdist)
# tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
# rotax = [0, 1, 0]
# rotangle = 360.0 / discretesize * angleid
# rotmat = rm.rodrigues(rotax, rotangle)
# tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
# axx = tmprtq85.getMat().getRow3(0)
# # 130 is the distance from hndbase to fingertip
# cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
# tmprtq85.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([.3, .3, .3, 1])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
# 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))
tmprtq85.setColor([.5, 0, 0, 1])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
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 = 3
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
print(self.facetpairs.shape)
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom_fn(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_fn(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(handfgrpcc0)
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(handfgrpcc1)
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()
base.pggen.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
base.pggen.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
if result.getNumContacts():
handfgrpcc0.setColor(.5, 0, 0, 1)
handfgrpcc1.setColor(.5, 0, 0, 1)
else:
handfgrpcc0.setColor(1, 1, 1, 1)
handfgrpcc1.setColor(1, 1, 1, 1)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
base.pggen.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)
base.pggen.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)
def removeFgrpcc(self, base):
"""
Fgrpcc means finger pre collision detection
:return:
author: weiwei
date: 20161212, tsukuba
"""
# cct0 first
self.gripcontactpairs_precc = []
self.gripcontactpairnormals_precc = []
self.gripcontactpairfacets_precc = []
# cct1 first
self.flipgripcontactpairs_precc = []
self.flipgripcontactpairnormals_precc = []
self.flipgripcontactpairfacets_precc = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs
# cct0 first
self.gripcontactpairs_precc.append([])
self.gripcontactpairnormals_precc.append([])
self.gripcontactpairfacets_precc.append([])
# cct1 first
self.flipgripcontactpairs_precc.append([])
self.flipgripcontactpairnormals_precc.append([])
self.flipgripcontactpairfacets_precc.append([])
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
# cctpnt0 first
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
# self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.gripcontactpairs_precc[-1].append(contactpair)
self.gripcontactpairnormals_precc[-1].append(self.gripcontactpairnormals[self.counter][j])
self.gripcontactpairfacets_precc[-1].append(self.gripcontactpairfacets[self.counter])
# cctpnt1 first
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt1 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt0 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal1 = self.facetnormals[facetidx0]
cctnormal0 = [-cctnormal1[0], -cctnormal1[1], -cctnormal1[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
# self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.flipgripcontactpairs_precc[-1].append(contactpair)
self.flipgripcontactpairnormals_precc[-1].append(self.gripcontactpairnormals[self.counter][j])
self.flipgripcontactpairfacets_precc[-1].append(self.gripcontactpairfacets[self.counter])
self.counter += 1
self.counter=0