Python core.TransformState类代码示例

 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 doCollisionSweepTestZ(self,col_mask = CollisionMasks.PLATFORM_RIGID_BODY,from_z = 0, to_z = 0):
    '''
    doCollisionSweepTestZ(double from_z = 0, double to_z = 0)
    Performs a collision sweep test along z in order to determine the height 
    at which the character's active rigid body comes into contact with an obstacle.  This is useful during 
    landing actions.
    
    @param col_mask: [optional] collision mask of the object(s) to check for collisions with.
    @param from_z:   [optional] z value for the start position
    @param to_z:     [optional] z value for the end position  
    '''
    
    pos = self.getPos()
    if abs(from_z - to_z) < 1e-5:
      height = self.getHeight()
      from_z = pos.getZ() + 0.5* height
      to_z = pos.getZ() - 0.5* height
    t0 = TransformState.makePos(Vec3(pos.getX(),pos.getY(),from_z))
    t1 = TransformState.makePos(Vec3(pos.getX(),pos.getY(),to_z))
    
    rigid_body = self.getRigidBody()
    if rigid_body.node().getNumShapes() <= 0:
      logging.warn("Rigid body contains no shapes, collision sweep test canceled")
      return
    
    aabb_shape = rigid_body.node().getShape(0)
    result = self.physics_world_.sweepTestClosest(aabb_shape,t0,t1,col_mask,0.0)

    if not result.hasHit():
      logging.warn("No collision from collision sweep closest test from %s to %s "%(t0.getPos(),t1.getPos()))
    else:
      logging.debug("Found collision at point %s between p0: %s to p1 %s"%(result.getHitPos(),t0.getPos(),t1.getPos()))
      
    return result

 def _clear_state_cache(self, task=None):
     """ Task which repeatedly clears the state cache to avoid storing
     unused states. """
     task.delayTime = 2.0
     TransformState.clear_cache()
     RenderState.clear_cache()
     return task.again

 def createConstraints(self,bodyA,bodyB):
   
   left_constraint = BulletGenericConstraint(bodyA,bodyB,TransformState.makeIdentity(),TransformState.makeHpr(Vec3(180,0,0)),False)
   right_constraint = BulletGenericConstraint(bodyA,bodyB,TransformState.makeIdentity(),TransformState.makeIdentity(),False)
   left_constraint.setEnabled(False)
   right_constraint.setEnabled(False)
   return (right_constraint,left_constraint)

 def complete_shape(self, id_mb1, node, transform):
     """ create the shape then call recursive function"""
     #print "complete shape {}".format(node)
     ## construct the node
     self.add_node_to_shape(node, id_mb1, transform)
     if node.gen_type == 'piece':
         self.shape_building_status[node] = True
     elif node.gen_type() == 'link':
         self.link_building_status[node][1] = (id_mb1, TransformState.makeMat(transform.getMat()))
         self.build_link(node)
     ## recursive loop over the edges
     for face, edge in enumerate(node.edges[1:]):
         if edge.type() != 'empty':
             transform = self.change_transform(transform, face + 1, type)
             if edge.gen_type() == 'shape':
                 if not self.shape_building_status[edge]:
                 #then we have to add this node (because it
                 #is not entirely finished
                     self.complete_shape(id_mb1, edge, transform)
             elif edge.gen_type() == 'link':
                 if self.link_building_status[edge][0][0] is None:
                     # first time we see this link
                     #print "hi new link"
                     self.add_node_to_shape(edge, id_mb1, transform)
                     self.link_building_status[edge][0] = (id_mb1, TransformState.makeMat(transform.getMat()))
                 else:
                     # link is complete:
                     #print " hi end link"
                     self.add_node_to_shape(edge, id_mb1)
                     self.link_building_status[edge][1] = (id_mb1, TransformState.makeMat(transform.getMat()))
                     self.build_link(edge)
             transform = self.change_back_transform(transform, face + 1, type)

 def __init__(self,name,parent_np , physics_world, tr = TransformState.makeIdentity()):
   """
   Sector(NodePath parent_np, TransformState tr = TransformState.makeIdentity()
   
     Creates a level Sector object which ensures that all objects in it only move in the x and z
     directions relative to the sector. The z and x vectors lie on the plane with +X pointing to the right
     and +Z pointing up.  +Y is perpedicular to the plane into the screen from the user's perspective
     
     @param parent_np: NodePath to the parent of the sector, usually is the Level object that contains the sector.
     @param physics_world: The physics world
     @param tf: Transform of the sector relative to the parent_np NodePath
   """
   
   NodePath.__init__(self,name)
   self.reparentTo(parent_np)    
   self.setTransform(self,tr)
   self.physics_world_ = physics_world
   
   # creating 2d motion plane
   self.motion_plane_np_ = self.attachNewNode(BulletRigidBodyNode(self.getName() + '-motion-plane'))
   self.motion_plane_np_.node().setMass(0)
   self.motion_plane_np_.node().setIntoCollideMask(CollisionMasks.NO_COLLISION)
   self.motion_plane_np_.node().addShape(BulletPlaneShape(Vec3(0,1,0),0))
   self.physics_world_.attach(self.motion_plane_np_.node())
   self.motion_plane_np_.reparentTo(parent_np)
   self.motion_plane_np_.setTransform(self,TransformState.makeIdentity())
   
   # game objects
   self.object_constraints_dict_ = {} # stores tuples of the form (String,BulletConstraint)
   
   # sector transitions
   self.sector_transitions_ = []
   self.destination_sector_dict_ = {}

 def _checkForStateClear(self):
     """ This method regulary clears the state cache """
     if not hasattr(self, "lastStateClear"):
         self.lastStateClear = 0
     if Globals.clock.getFrameTime() - self.lastStateClear > self.settings.stateCacheClearInterval:
         RenderState.clearCache()
         TransformState.clearCache()
         self.lastStateClear = Globals.clock.getFrameTime()

  def setup(self):
    self.worldNP = render.attachNewNode('World')

    # World
    self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
    self.debugNP.show()
    self.debugNP.node().showWireframe(True)
    self.debugNP.node().showConstraints(True)
    self.debugNP.node().showBoundingBoxes(False)
    self.debugNP.node().showNormals(False)

    self.world = BulletWorld()
    self.world.setGravity(Vec3(0, 0, -9.81))
    self.world.setDebugNode(self.debugNP.node())

    # Box A
    shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))

    bodyA = BulletRigidBodyNode('Box A')
    bodyNP = self.worldNP.attachNewNode(bodyA)
    bodyNP.node().addShape(shape)
    bodyNP.setCollideMask(BitMask32.allOn())
    bodyNP.setPos(-3, 0, 4)

    visNP = loader.loadModel('models/box.egg')
    visNP.clearModelNodes()
    visNP.reparentTo(bodyNP)

    self.world.attachRigidBody(bodyA)

    # Box B
    shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))

    bodyB = BulletRigidBodyNode('Box B')
    bodyNP = self.worldNP.attachNewNode(bodyB)
    bodyNP.node().addShape(shape)
    bodyNP.node().setMass(1.0)
    bodyNP.node().setDeactivationEnabled(False)
    bodyNP.setCollideMask(BitMask32.allOn())
    bodyNP.setPos(0, 0, 0)

    visNP = loader.loadModel('models/box.egg')
    visNP.clearModelNodes()
    visNP.reparentTo(bodyNP)

    self.world.attachRigidBody(bodyB)

    # Slider
    frameA = TransformState.makePosHpr(Point3(2, 0, 0), Vec3(0, 0, 45))
    frameB = TransformState.makePosHpr(Point3(0, -3, 0), Vec3(0, 0, 0))

    slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True)
    slider.setDebugDrawSize(2.0)
    slider.setLowerLinearLimit(0)
    slider.setUpperLinearLimit(6)
    slider.setLowerAngularLimit(-60)
    slider.setUpperAngularLimit(60)
    self.world.attachConstraint(slider)

 def doShoot(self, ccd):
   if(self.fire and self.attempts > 0 and self.gameState == 'PLAY'):
     self.cameraState = 'LOOK'
     self.fire = False
     self.candleThrow.play()
     self.attempts -= 1
     #get from/to points from mouse click
     ## pMouse = base.mouseWatcherNode.getMouse()
     ## pFrom = Point3()
     ## pTo = Point3()
     ## base.camLens.extrude(pMouse, pFrom, pTo)
     
     ## pFrom = render.getRelativePoint(base.cam, pFrom)
     ## pTo = render.getRelativePoint(base.cam, pTo)
     
     # calculate initial velocity
     v = self.pTo - self.pFrom
     ratio = v.length() / 40
     v.normalize()
     v *= 70.0 * ratio
     torqueOffset = random.random() * 10
     
     #create bullet
     shape = BulletSphereShape(0.5)
     shape01 = BulletSphereShape(0.5)
     shape02 = BulletSphereShape(0.5)
     shape03 = BulletSphereShape(0.5)
     body = BulletRigidBodyNode('Candle')
     bodyNP = self.worldNP.attachNewNode(body)
     bodyNP.node().addShape(shape, TransformState.makePos(Point3(0,0,0)))
     bodyNP.node().addShape(shape01, TransformState.makePos(Point3(0,0.5,-0.5)))
     bodyNP.node().addShape(shape02,TransformState.makePos(Point3(0,1,-1)))
     bodyNP.node().addShape(shape03,TransformState.makePos(Point3(0,1.5,-1.5)))
     bodyNP.node().setMass(100)
     bodyNP.node().setFriction(1.0)
     bodyNP.node().setLinearVelocity(v)
     bodyNP.node().applyTorque(v*torqueOffset)
     bodyNP.setPos(self.pFrom)
     bodyNP.setCollideMask(BitMask32.allOn())
     
     visNP = loader.loadModel('models/projectile.X')
     visNP.setScale(0.7)
     visNP.clearModelNodes()
     visNP.reparentTo(bodyNP)
     
     #self.bird = bodyNP.node()
     
     if ccd:
         bodyNP.node().setCcdMotionThreshold(1e-7)
         bodyNP.node().setCcdSweptSphereRadius(0.5)
         
     self.world.attachRigidBody(bodyNP.node())
     
     #remove the bullet again after 1 sec
     self.bullets = bodyNP
     taskMgr.doMethodLater(5, self.removeBullet, 'removeBullet', extraArgs=[bodyNP], 
                           appendTask = True)

def create_colliders(root, pose_rig, joints_config):
    for node, parent in pose_rig.exposed_joints:
        if node.getName() not in joints_config:
            continue

        joint_config = joints_config[node.getName()]
        if "joints" not in joint_config:
            continue

        joints = joint_config["joints"]
        if len(joints) == 0:
            continue

        mass = joint_config["mass"] if "mass" in joint_config else 1

        box_rb = BulletRigidBodyNode(node.getName())
        box_rb.setMass(1.0)
        # box_rb.setLinearDamping(0.2)
        # box_rb.setAngularDamping(0.9)
        # box_rb.setFriction(1.0)
        # box_rb.setAnisotropicFriction(1.0)
        # box_rb.setRestitution(0.0)

        for joint in joints:
            child_node, child_parent = next(
                (child_node, child_parent)
                for child_node, child_parent in pose_rig.exposed_joints
                if child_node.getName() == joint
            )

            pos = child_node.getPos(child_parent)
            width = pos.length() / 2.0
            scale = Vec3(3, width, 3)

            shape = BulletBoxShape(scale)

            quat = Quat()
            lookAt(quat, child_node.getPos(child_parent), Vec3.up())
            if len(joints) > 1:
                transform = TransformState.makePosHpr(child_node.getPos(child_parent) / 2.0, quat.getHpr())
            else:
                transform = TransformState.makeHpr(quat.getHpr())

            box_rb.addShape(shape, transform)

        box_np = root.attachNewNode(box_rb)

        if len(joints) > 1:
            pos = node.getPos(pose_rig.actor)
            hpr = node.getHpr(pose_rig.actor)
            box_np.setPosHpr(root, pos, hpr)
        else:
            box_np.setPos(child_parent, child_node.getPos(child_parent) / 2.0)
            box_np.lookAt(child_parent, child_node.getPos(child_parent))

        yield box_np

    def build_link(self, node):
        
        (id_bda, ta), (id_bdb, tb) = self.link_building_status[node] 

        bda = self.factory.multiboxes[id_bda]
        bdb = self.factory.multiboxes[id_bdb]
        
        pos =  bda.body.getPosition()
        quat = LQuaternionf(bda.body.getQuaternion())
        mat = TransformState.makePosQuatScale(pos, quat, Vec3(1, 1, 1)).getMat()
        mat = ta.getMat() * mat
        print "ta", ta

        print "absol", TransformState.makeMat(mat)
        mat = LMatrix4f.translateMat(Vec3(0.5, 0, 0)) * mat
        anchor = TransformState.makeMat(mat).getPos() 
        print "absol", TransformState.makeMat(mat)
        
        axis = self.quat_dict[1][1]
        if node.orientation == 1:
            t = LMatrix4f.rotateMat(*self.quat_dict[4]) * mat
        else:
            t = LMatrix4f.rotateMat(*self.quat_dict[2]) * mat
        row = t.getRow(0)
        print "rotation", t.getRow(0), type(t.getRow(0))
        #axis = t.getQuat().getAxis()
        axis = Vec3(row.getX(), row.getY(), row.getZ())
        print "axis",axis
        print "anchor", anchor

        mat = LMatrix4f.translateMat(Vec3(0.5, 0, 0)) * mat
        mat = tb.getInverse().getMat() * mat
        t = TransformState.makeMat(mat)
        posb = t.getPos()
        quatb = t.getQuat()

        bdb.body.setPosition(posb)
        bdb.body.setQuaternion(quatb)

        cs = OdeHingeJoint(self.physics.world, self.physics.servogroup)
        cs.attach(bda.body, bdb.body)
        cs.setAxis(axis)
        cs.setAnchor(anchor)
        
        #add the motor
        cs.setParamFMax(self.satu_cmd)
        cs.setParamFudgeFactor(0.5)
        cs.setParamCFM(11.1111)
        cs.setParamStopCFM(11.1111)
        cs.setParamStopERP(0.444444)
        cs.setParamLoStop(- math.pi * 0.5)
        cs.setParamHiStop(math.pi * 0.5)
        pid = PID()

        self.dof_motors[node] = (cs, pid)
        print "add constraint"

 def _clear_state_cache(self, task=None):
     """ Task which repeatedly clears the state cache to avoid storing
     unused states. While running once a while, this task prevents over-polluting
     the state-cache with unused states. This complements Panda3D's internal
     state garbarge collector, which does a great job, but still cannot clear
     up all states. """
     task.delayTime = 2.0
     TransformState.clear_cache()
     RenderState.clear_cache()
     return task.again

  def setup(self):
    self.worldNP = render.attachNewNode('World')

    # World
    self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
    self.debugNP.show()
    self.debugNP.node().showWireframe(True)
    self.debugNP.node().showConstraints(True)
    self.debugNP.node().showBoundingBoxes(False)
    self.debugNP.node().showNormals(False)

    self.world = BulletWorld()
    self.world.setGravity(Vec3(0, 0, -9.81))
    self.world.setDebugNode(self.debugNP.node())

    # Box A
    shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))

    bodyA = BulletRigidBodyNode('Box A')
    bodyNP = self.worldNP.attachNewNode(bodyA)
    bodyNP.node().addShape(shape)
    bodyNP.setCollideMask(BitMask32.allOn())
    bodyNP.setPos(-2, 0, 4)

    visNP = loader.loadModel('models/box.egg')
    visNP.clearModelNodes()
    visNP.reparentTo(bodyNP)

    self.world.attachRigidBody(bodyA)

    # Box B
    shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))

    bodyB = BulletRigidBodyNode('Box B')
    bodyNP = self.worldNP.attachNewNode(bodyB)
    bodyNP.node().addShape(shape)
    bodyNP.node().setMass(1.0)
    bodyNP.node().setDeactivationEnabled(False)
    bodyNP.setCollideMask(BitMask32.allOn())
    bodyNP.setPos(0, 0, 0)

    visNP = loader.loadModel('models/box.egg')
    visNP.clearModelNodes()
    visNP.reparentTo(bodyNP)

    self.world.attachRigidBody(bodyB)

    # Cone
    frameA = TransformState.makePosHpr(Point3(0, 0, -2), Vec3(0, 0, 90))
    frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0))

    cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB)
    cone.setDebugDrawSize(2.0)
    cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0)
    self.world.attachConstraint(cone)

  def setup(self):
    self.worldNP = render.attachNewNode('World')

    # World
    self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
    self.debugNP.show()

    self.world = BulletWorld()
    self.world.setGravity(Vec3(0, 0, -9.81))
    self.world.setDebugNode(self.debugNP.node())

    # Plane
    shape = BulletPlaneShape(Vec3(0, 0, 1), 0)

    np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
    np.node().addShape(shape)
    np.setPos(0, 0, -1)
    np.setCollideMask(BitMask32.allOn())

    self.world.attachRigidBody(np.node())

    # Box
    shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))

    np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box'))
    np.node().setMass(1.0)
    np.node().addShape(shape)
    np.node().addShape(shape, TransformState.makePos(Point3(0, 1, 0)))
    np.node().setDeactivationEnabled(False)
    np.setPos(2, 0, 4)
    np.setCollideMask(BitMask32.allOn())

    self.world.attachRigidBody(np.node())

    visualNP = loader.loadModel('models/box.egg')
    visualNP.reparentTo(np)

    self.box = np.node()

    # Sphere
    shape = BulletSphereShape(0.6)

    np = self.worldNP.attachNewNode(BulletRigidBodyNode('Sphere'))
    np.node().setMass(1.0)
    np.node().addShape(shape)
    np.node().addShape(shape, TransformState.makePos(Point3(0, 1, 0)))
    np.setPos(-2, 0, 4)
    np.setCollideMask(BitMask32.allOn())

    self.world.attachRigidBody(np.node())

    self.sphere = np.node()

  def addBox(self,name,size,pos,visual):

    # Box (dynamic)
    box = NodePath(BulletRigidBodyNode(name))
    box.node().setMass(1.0)
    box.node().addShape(BulletBoxShape(size))
    box.setCollideMask(GAME_OBJECT_BITMASK)
    #box.node().setLinearFactor((1,0,1))
    #box.node().setAngularFactor((0,1,0))
    visual.instanceTo(box)
    box.reparentTo(self.getParent())    
    box.setPos(self,pos)
    box.setHpr(self,Vec3.zero())

    self.physics_world_.attachRigidBody(box.node())
    self.object_nodes_.append(box)
    
    # adding constraint
    motion2d_constraint = BulletGenericConstraint(self.constraint_plane_.node(),box.node(),
                                        TransformState.makeIdentity(),
                                        TransformState.makeIdentity(),
                                        False)
    
    rot_constraint = BulletGenericConstraint(self.constraint_plane_.node(),box.node(),
                                        TransformState.makeIdentity(),
                                        TransformState.makeIdentity(),
                                        False)
    

 
    motion2d_constraint.setLinearLimit(0,-sys.float_info.max,sys.float_info.max)
    motion2d_constraint.setLinearLimit(1,0,0)
    motion2d_constraint.setLinearLimit(2,-sys.float_info.max,sys.float_info.max)
    
    # All angular constraints must be either locked or released for the simulation to be stable
    #motion2d_constraint.setAngularLimit(0,-0.1,0.1) 
    #motion2d_constraint.setAngularLimit(1,-100,100)
    #motion2d_constraint.setAngularLimit(2,-0.1,0.1)
    
    #motion2d_constraint.setBreakingThreshold(1000)
    motion2d_constraint.setDebugDrawSize(0)
    
#     for axis in range(0,6):
#       motion2d_constraint.setParam(BulletGenericConstraint.CP_cfm,0,axis)
#       motion2d_constraint.setParam(BulletGenericConstraint.CP_erp,0.4,axis)
    
    self.physics_world_.attach(motion2d_constraint)
    self.box_contraints_.append(motion2d_constraint)
    
    return box