Python BulletWorld.rayTestClosest方法代码示例

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import rayTestClosest [as 别名]
def rayHit(pfrom, pto, geom):
    """
    NOTE: this function is quite slow
    find the nearest collision point between vec(pto-pfrom) and the mesh of nodepath

    :param pfrom: starting point of the ray, Point3
    :param pto: ending point of the ray, Point3
    :param geom: meshmodel, a panda3d datatype
    :return: None or Point3

    author: weiwei
    date: 20161201
    """

    bulletworld = BulletWorld()
    facetmesh = BulletTriangleMesh()
    facetmesh.addGeom(geom)
    facetmeshnode = BulletRigidBodyNode('facet')
    bullettmshape = BulletTriangleMeshShape(facetmesh, dynamic=True)
    bullettmshape.setMargin(0)
    facetmeshnode.addShape(bullettmshape)
    bulletworld.attachRigidBody(facetmeshnode)
    result = bulletworld.rayTestClosest(pfrom, pto)

    if result.hasHit():
        return result.getHitPos()
    else:
        return None

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import rayTestClosest [as 别名]
    def planContactpairs(self, torqueresist = 50, fgrtipdist = 82):
        """
        find the grasps using parallel pairs

        :param: torqueresist the maximum allowable distance to com
        :param: fgrtipdist the maximum dist between finger tips
        :return:

        author: weiwei
        date: 20161130, harada office @ osaka university
        """

        # note that pairnormals and pairfacets are duplicated for each contactpair
        # the duplication is performed on purpose for convenient access
        # also, each contactpair"s" corresponds to a facetpair
        # it is empty when no contactpair is available
        self.gripcontactpairs = []
        # gripcontactpairnormals and gripcontactpairfacets are not helpful
        # they are kept for convenience (they could be accessed using facetnormals and facetpairs)
        self.gripcontactpairnormals = []
        self.gripcontactpairfacets = []

        # for precollision detection
        # bulletworldprecc = BulletWorld()
        # # build the collision shape of objtrimesh
        # geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals,
        #                                   self.objtrimesh.faces)
        # objmesh = BulletTriangleMesh()
        # objmesh.addGeom(geomobj)
        # objmeshnode = BulletRigidBodyNode('objmesh')
        # objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True))
        # bulletworldprecc.attachRigidBody(objmeshnode)

        # for raytracing
        bulletworldray = BulletWorld()
        nfacets = self.facets.shape[0]
        self.facetpairs = list(itertools.combinations(range(nfacets), 2))
        for facetpair in self.facetpairs:
            # print "facetpair"
            # print facetpair, len(self.facetpairs)
            self.gripcontactpairs.append([])
            self.gripcontactpairnormals.append([])
            self.gripcontactpairfacets.append([])
            # if one of the facet doesnt have samples, jump to next
            if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \
                            self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0:
                # print "no sampled points"
                continue
            # check if the faces are opposite and parallel
            dotnorm = np.dot(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]])
            if dotnorm < -0.95:
                # check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1]
                facet0pnts = self.objsamplepnts_refcls[facetpair[0]]
                facet0normal = self.facetnormals[facetpair[0]]
                facet1normal = self.facetnormals[facetpair[1]]
                # generate collision mesh
                facetmesh = BulletTriangleMesh()
                faceidsonfacet = self.facets[facetpair[1]]
                geom = pandageom.packpandageom(self.objtrimesh.vertices,
                                               self.objtrimesh.face_normals[faceidsonfacet],
                                               self.objtrimesh.faces[faceidsonfacet])
                facetmesh.addGeom(geom)
                facetmeshbullnode = BulletRigidBodyNode('facet')
                facetmeshbullnode.addShape(BulletTriangleMeshShape(facetmesh, dynamic=True))
                bulletworldray.attachRigidBody(facetmeshbullnode)
                # check the projection of a ray
                for facet0pnt in facet0pnts:
                    pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2])
                    pTo = pFrom + Vec3(facet1normal[0], facet1normal[1], facet1normal[2])*9999
                    result = bulletworldray.rayTestClosest(pFrom, pTo)
                    if result.hasHit():
                        hitpos = result.getHitPos()
                        if np.linalg.norm(np.array(facet0pnt.tolist())-np.array([hitpos[0], hitpos[1], hitpos[2]])) < fgrtipdist:
                            fgrcenter = (np.array(facet0pnt.tolist())+np.array([hitpos[0], hitpos[1], hitpos[2]]))/2.0
                            # avoid large torque
                            if np.linalg.norm(self.objtrimesh.center_mass - fgrcenter) < torqueresist:
                                self.gripcontactpairs[-1].append([facet0pnt.tolist(), [hitpos[0], hitpos[1], hitpos[2]]])
                                self.gripcontactpairnormals[-1].append([[facet0normal[0], facet0normal[1], facet0normal[2]],
                                                                    [facet1normal[0], facet1normal[1], facet1normal[2]]])
                                self.gripcontactpairfacets[-1].append(facetpair)

                        # pre collision checking: it takes one finger as a cylinder and
                        # computes its collision with the object
                        ## first finger
                        # cylindernode0 = BulletRigidBodyNode('cylindernode0')
                        # cylinder0height = 50
                        # cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2])
                        # cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius,
                        #                                            height=cylinder0height,
                        #                                            up=cylinder0normal),
                        #                        TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6))
                        # bulletworldprecc.attachRigidBody(cylindernode0)
                        # ## second finger
                        # cylindernode1 = BulletRigidBodyNode('cylindernode1')
                        # cylinder1height = cylinder1height
                        # # use the inverse of facet0normal instead of facet1normal to follow hand orientation
                        # cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2])
                        # cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius,
                        #                                            height=cylinder1height,
                        #                                            up=cylinder1normal),
#.........这里部分代码省略.........

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import rayTestClosest [as 别名]

#.........这里部分代码省略.........
        ts = node.getTransform() 
        m = ts.getMat().getUpper3()
        p = ts.getMat().getRow3(3)
        points=[]
        geom = geomNode.getGeoms()[0]
        vdata = geom.getVertexData()
        reader = GeomVertexReader(vdata, 'vertex')
        while not reader.isAtEnd():
            v = reader.getData3f()
            v = m.xform(v) + p
            points.append(Point3(v))
        return numpy.array(points,dtype=numpy.float32)
        
    def pandaMatrice(self,mat):
        mat = mat.transpose().reshape((16,))
#        print mat,len(mat),mat.shape
        pMat = Mat4(mat[0],mat[1],mat[2],mat[3],
                   mat[4],mat[5],mat[6],mat[7],
                   mat[8],mat[9],mat[10],mat[11],
                   mat[12],mat[13],mat[14],mat[15],)
        return pMat
        
#    
#    def addRB(self,rtype,static,**kw):
#        # Sphere
#        if panda3d is None :
#            return None
#        if rotMatis not None and trans is not None:
#            mat = rotMat.copy()
#            mat = mat.transpose().reshape((16,))
#
#            pMat = TransformState.makeMat(Mat4(mat[0],mat[1],mat[2],mat[3],
#                                           mat[4],mat[5],mat[6],mat[7],
#                                           mat[8],mat[9],mat[10],mat[11],
#                                           trans[0],trans[1],trans[2],mat[15],))
#        shape = None
#        inodenp = None
##        print (pMat) 
#        inodenp = self.rb_func_dic[rtype](ingr,pMat,trans,rotMat)

#        inodenp.setCollideMask(BitMask32.allOn())
    
#        self.world.attachRigidBody(inodenp.node())
#        if static :
#            self.static.append(inodenp.node())
#        else :
#            self.moving = inodenp.node()
#        self.rb_panda.append(inodenp.node())
#        return inodenp.node()
    
    def moveRBnode(self,node, trans, rotMat):
        mat = rotMat.copy()
#        mat[:3, 3] = trans
#        mat = mat.transpose()
        mat = mat.transpose().reshape((16,))
#        print mat,len(mat),mat.shape
        pMat = Mat4(mat[0],mat[1],mat[2],mat[3],
                   mat[4],mat[5],mat[6],mat[7],
                   mat[8],mat[9],mat[10],mat[11],
                   trans[0],trans[1],trans[2],mat[15],)
        pTrans = TransformState.makeMat(pMat)
        nodenp = NodePath(node)
        nodenp.setMat(pMat)
#        nodenp.setPos(trans[0],trans[1],trans[2])
#        print nodenp.getPos()

    def applyForce(self,node,F):
        F*=self.scale
        node.node().applyCentralForce(Vec3(F[0],F[1],F[2]))
#        node.node().applyForce(Vec3(F[0],F[1],F[2]),Point3(0, 0, 0))
#        print F
        
    def rayCast(self, startp,endp,closest=False):
        start=Point3(startp[0],startp[1],startp[2])
        end=Point3(endp[0],endp[1],endp[2])
        if closest :
            res = self.world.rayTestClosest(start, end)
        else :
            res = self.world.rayTestAll(start, end)  
        return res

    def sweepRay(self, startp,endp):
        tsFrom = TransformState.makePos(Point3(0, 0, 0))
        tsTo = TransformState.makePos(Point3(10, 0, 0))
        shape = BulletSphereShape(0.5)
        penetration = 0.0
        result = self.world.sweepTestClosest(shape, tsFrom, tsTo, penetration)
        return result 

    def update(self,task,cb=None):
#        print "update"
        dt = globalClock.getDt()
#        print dt
        self.world.doPhysics(dt, 10, 0.008)#,100,1.0/500.0)#world.doPhysics(dt, 10, 1.0/180.0)
        #this may be different for relaxing ?
#        print task.time
        if cb is not None :
            cb()
        if task is not None:
            return task.cont

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import rayTestClosest [as 别名]

#.........这里部分代码省略.........
		self.worldBullet.attachRigidBody(planeNode)
		return planeNP
	
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	# Sets up and returns the collision handler.
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	
	def setupCollisionHandler(self):
		colHand = SMCollisionHandler(self.worldBullet)
		return colHand
	
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	# Toggles showing bounding boxes.
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	
	def toggleDebug(self):
		if self.debugNode.isHidden():
			self.debugNode.show()
		else:
			self.debugNode.hide()
	
	
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	# Returns the terrain height of the nearest vertical descending raycast from the passed Point3.
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	
	def getTerrainHeight(self, pos):
		result = 0
		x = pos.getX()
		y = pos.getY()
		z = pos.getZ()
		rayTerrA = Point3(x, y, z)
		rayTerrB = Point3(x, y, z - 256)
		rayTest = self.worldBullet.rayTestClosest(rayTerrA, rayTerrB)
		
		rayNode = rayTest.getNode()
		if (rayTest.hasHit()):
			rayPos = rayTest.getHitPos()
			result = rayPos.getZ()
		else:
			self.playerObj.respawn()
		return result
		# return self.hmTerrain.get_elevation(x + self.hmOffset, y + self.hmOffset) * self.hmHeight
	
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	# Handles player movement
	#------------------------------------------------------------------------------------------------------------------------------------------------------------
	
	def playerMove(self):
	
		# Go through the collision and flag tests, and update them
		self.doPlayerTests()
		
		# Rotation and camera movement
		if self.kh.poll(self.keymap['Left']):
			self.playerObj.turn(True)
		elif self.kh.poll(self.keymap['Right']):
			self.playerObj.turn(False)
		elif(self.cameraControl):
			newMousePos = self.kh.getMouse()
			mx = newMousePos.getX()
			self.camObj.rotateCamera(mx)
		
		self.camObj.calculatePosition()
		
		# Movement

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import rayTestClosest [as 别名]

#.........这里部分代码省略.........
        :return:
        """
        tpsmat4s = self.gdb.loadFreeTabletopPlacement(self.dbobjname)
        if tpsmat4s is not None:
            self.tpsmat4s = tpsmat4s
            return True
        else:
            self.tpsmat4s = []
            return False

    def removebadfacets(self, base, doverh=.1):
        """
        remove the facets that cannot support stable placements

        :param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
                when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
                when f>0.1mg, the object is judged to be unstable
        :return:

        author: weiwei
        date: 20161213
        """
        self.tpsmat4s = []

        for i in range(len(self.ocfacets)):
            geom = pg.packpandageom(self.objtrimeshconv.vertices,
                                           self.objtrimeshconv.face_normals[self.ocfacets[i]],
                                           self.objtrimeshconv.faces[self.ocfacets[i]])
            geombullnode = cd.genCollisionMeshGeom(geom)
            self.bulletworldray.attachRigidBody(geombullnode)
            pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
            pTo = self.objcom+self.ocfacetnormals[i]*99999
            pTo = Point3(pTo[0], pTo[1], pTo[2])
            result = self.bulletworldray.rayTestClosest(pFrom, pTo)
            self.bulletworldray.removeRigidBody(geombullnode)
            if result.hasHit():
                hitpos = result.getHitPos()

                facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
                facetnormal = np.array(self.ocfacetnormals[i])
                bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
                                                                     facetinterpnt, facetnormal)
                facetp = Polygon(bdverts2d)
                facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
                apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
                dist2p = apntpnt.distance(facetp.exterior)
                dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
                if dist2p/dist2c >= doverh:
                    # hit and stable
                    self.tpsmat4s.append(pg.cvtMat4np4(facetmat4))

    def gentpsgrip(self, base):
        """
        Originally the code of this function is embedded in the removebadfacet function
        It is separated on 20170608 to enable common usage of placements for different hands

        :return:

        author: weiwei
        date: 20170608
        """

        self.tpsgripcontacts = []
        self.tpsgripnormals = []
        self.tpsgriprotmats = []
        self.tpsgripjawwidth = []

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import rayTestClosest [as 别名]
class GameBase(ShowBase):
	def __init__(self, KEY_LIST):
		ShowBase.__init__(self)
		
		#---------------------------------------------------------------
		# clock
		self.globalClock = ClockObject()
		
		#---------------------------------------------------------------
		# KeyHandler
		self.kh = KeyHandler(KEY_LIST)
		
		#---------------------------------------------------------------
		# Bullet
		self.world = BulletWorld()
		self.world.setGravity(Vec3(0, 0, -12.81))
		self.gravityUp = False
		
		self.debugNode = BulletDebugNode('Debug')
		self.debugNode.showWireframe(True)
		self.debugNode.showConstraints(True)
		self.debugNode.showBoundingBoxes(True)
		self.debugNode.showNormals(True)
		self.debugNP = self.render.attachNewNode(self.debugNode)
		#self.debugNP.show()
		self.world.setDebugNode(self.debugNode)
		self._debug = False
		
		#---------------------------------------------------------------
		# Player
		#---------------------------------------------------------------
		# CharacterController
		self.player = CharacterController(self)
		self.player.setActor('models/characters/female', {
				'walk' : 'models/characters/female-walk.egg'
			},
			flip = True,
			pos = (0,0,-1),
			scale = 1.0)
		self.player.setPos(0, -5, 3)
		self.player.playerModel.loop("walk")
		self.playerNp = self.player.np
		
		#---------------------------------------------------------------
		# Camera
		self.camHandler = CamHandler(self)
		
		#---------------------------------------------------------------
		# task
		#self.taskMgr.add(self.update, "update")
	
	#---------------------------------------------------------------
	# FPS
	def toggleFPS(self):
		if self.frameRateMeter:
			self.setFrameRateMeter(False)
		else:
			self.setFrameRateMeter(True)
	
	#---------------------------------------------------------------
	# Mouse cursor
	def hideCursor(self):
		props = WindowProperties()
		props.setCursorHidden(True) 
		self.win.requestProperties(props)
	def showCursor(self):
		props = WindowProperties()
		props.setCursorHidden(False)
		self.win.requestProperties(props)
	
	def getObjectHoverName(self):
		if not self.mouseWatcherNode.hasMouse():
			return None
		pMouse = self.mouseWatcherNode.getMouse()
		pFrom = Point3()
		pTo = Point3()
		self.camLens.extrude(pMouse, pFrom, pTo)
		# Transform to global coordinates
		pFrom = self.render.getRelativePoint(self.cam, pFrom)
		pTo = self.render.getRelativePoint(self.cam, pTo)
		result = self.world.rayTestClosest(pFrom, pTo)
		if result.hasHit():
			pos = result.getHitPos()
			name = result.getNode().getName()
			return name
		else:
			return None
		
	def getObjectCenterScreen(self):
		pFrom = Point3()
		pTo = Point3()
		self.camLens.extrude((0,0), pFrom, pTo)
		# Transform to global coordinates
		pFrom = self.render.getRelativePoint(self.cam, pFrom)
		pTo = self.render.getRelativePoint(self.cam, pTo)
		result = self.world.rayTestClosest(pFrom, pTo)
		if result.hasHit():
			pos = result.getHitPos()
			name = result.getNode().getName()
			return name
#.........这里部分代码省略.........