Example #1
0
    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.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)
Example #3
0
class EscapeFromJCMB(object, DirectObject):
    def __init__(self):
        print "Loading..."
        self.init_window()
        self.init_music()
        self.init_bullet()
        self.init_key()
        self.load_world()
        self.init_player()
        self.init_objects()
        render.prepareScene(base.win.getGsg())
        print "Done"
        self.start_physics()

    def init_window(self):
        # Hide the mouse cursor
        props = WindowProperties()
        props.setCursorHidden(True)
        base.win.requestProperties(props)

    def init_music(self):
        music = base.loader.loadSfx("../data/snd/Bent_and_Broken.mp3")
        music.play()
        self.playferscream = base.loader.loadSfx("../data/snd/deadscrm.wav")

    def init_bullet(self):
        self.world = BulletWorld()
        self.world.setGravity(Vec3(0, 0, -9.81))

        #    debugNode = BulletDebugNode('Debug')
        #    debugNode.showWireframe(True)
        #    debugNode.showConstraints(True)
        #    debugNode.showBoundingBoxes(False)
        #    debugNode.showNormals(False)
        #    debugNP = render.attachNewNode(debugNode)
        #    debugNP.show()
        #    self.world.setDebugNode(debugNP.node())

        alight = AmbientLight('alight')
        alight.setColor(VBase4(0.05, 0.05, 0.05, 1))
        alnp = render.attachNewNode(alight)
        render.setLight(alnp)

    def init_key(self):

        # Stores the state of the keys, 1 for pressed and 0 for unpressed
        self.key_state = {
            'up': False,
            'right': False,
            'down': False,
            'left': False
        }

        # Assign the key event handler
        self.accept('w', self.set_key_state, ['up', True])
        self.accept('w-up', self.set_key_state, ['up', False])
        self.accept('d', self.set_key_state, ['right', True])
        self.accept('d-up', self.set_key_state, ['right', False])
        self.accept('s', self.set_key_state, ['down', True])
        self.accept('s-up', self.set_key_state, ['down', False])
        self.accept('a', self.set_key_state, ['left', True])
        self.accept('a-up', self.set_key_state, ['left', False])

        # Stores the state of the mouse buttons, 1 for pressed and 0 for unpressed
        self.mouse_state = {'left_click': False, 'right_click': False}

        # Assign the mouse click event handler
        self.accept('mouse1', self.set_mouse_state, ['left_click', True])
        self.accept('mouse1-up', self.set_mouse_state, ['left_click', False])
        self.accept('mouse2', self.set_mouse_state, ['right_click', True])
        self.accept('mouse2-up', self.set_mouse_state, ['right_click', False])

        self.accept('z', self.print_pos)

        # Esc
        self.accept('escape', sys.exit)

    def set_key_state(self, key, state):
        self.key_state[key] = state

    def set_mouse_state(self, button, state):
        self.mouse_state[button] = state

    def print_pos(self):
        print self.playernp.getPos()

    def egg_to_BulletTriangleMeshShape(self, modelnp):
        mesh = BulletTriangleMesh()
        for collisionNP in modelnp.findAllMatches('**/+CollisionNode'):
            collisionNode = collisionNP.node()
            for collisionPolygon in collisionNode.getSolids():
                tri_points = collisionPolygon.getPoints()
                mesh.addTriangle(tri_points[0], tri_points[1], tri_points[2])
        shape = BulletTriangleMeshShape(mesh, dynamic=False)
        return shape

    def load_world(self):
        stairwell = loader.loadModel('../data/mod/jcmbstairs.egg')
        stairwell.reparentTo(render)
        stairwell_shape = self.egg_to_BulletTriangleMeshShape(stairwell)
        stairwellnode = BulletRigidBodyNode('stairwell')
        stairwellnode.addShape(stairwell_shape)
        self.world.attachRigidBody(stairwellnode)

    def init_player(self):
        # Stop the default mouse behaviour
        base.disableMouse()

        # Character has a capsule shape
        shape = BulletCapsuleShape(0.2, 1, ZUp)
        self.player = BulletRigidBodyNode('Player')
        self.player.setMass(80.0)
        self.player.addShape(shape)
        self.playernp = render.attachNewNode(self.player)
        self.playernp.setPos(0, 0, 1)
        self.world.attachRigidBody(self.player)

        self.player.setLinearSleepThreshold(0.0)
        self.player.setAngularFactor(0.0)

        self.player_speed = 3
        self.player_is_grabbing = False

        #    self.head = BulletRigidBodyNode('Player Head')
        #    self.head.addShape(BulletSphereShape(0.2))
        #    self.head.setMass(10)
        #    self.head.setInertia(Vec3(1,1,1))
        #    self.head.setAngularFactor(1.0)
        #    self.head.setLinearFactor(0.0)
        #    self.headnp = self.playernp.attachNewNode(self.head)
        #    self.headnp.setPos(0,0,0)
        #    self.headnp.setCollideMask(BitMask32.allOff())
        #    self.world.attachRigidBody(self.head)

        # Attach the camera to the player's head
        base.camera.reparentTo(self.playernp)
        #    base.camera.setPos(0,0,.5)
        base.camLens.setFov(80)
        base.camLens.setNear(0.01)

        # Make the torch and attach it to our character
        torch = Spotlight('torch')
        torch.setColor(VBase4(1, 1, 1, 1))
        lens = PerspectiveLens()
        lens.setFov(80)
        lens.setNearFar(20, 100)
        torch.setLens(lens)
        self.torchnp = base.camera.attachNewNode(torch)
        self.torchnp.setPos(0, 0, 0)

        # Allow the world to be illuminated by our torch
        render.setLight(self.torchnp)

        self.cs = None

        # Player's "hand" in the world
        hand_model = loader.loadModel('../data/mod/hand.egg')
        hand_model.setScale(1)
        hand_model.setBillboardPointEye()

        self.hand = BulletRigidBodyNode('Hand')
        self.hand.addShape(BulletSphereShape(0.1))
        self.hand.setLinearSleepThreshold(0.0)
        self.hand.setLinearDamping(0.9999999)
        self.hand.setAngularFactor(0.0)
        self.hand.setMass(1.0)
        self.world.attachRigidBody(self.hand)
        self.handnp = render.attachNewNode(self.hand)
        self.handnp.setCollideMask(BitMask32.allOff())
        #    hand_model.reparentTo(self.handnp)

        # Create a text node to display object identification information and attach it to the player's "hand"
        self.hand_text = TextNode('Hand Text')
        self.hand_text.setText("Ch-ch-chek yoself befo yo rek yoself, bro.")
        self.hand_text.setAlign(TextNode.ACenter)
        self.hand_text.setTextColor(1, 1, 1, 1)
        self.hand_text.setShadow(0.05, 0.05)
        self.hand_text.setShadowColor(0, 0, 0, 1)
        self.hand_text_np = render.attachNewNode(self.hand_text)
        self.hand_text_np.setScale(0.03)
        self.hand_text_np.setBillboardPointEye()
        self.hand_text_np.hide()

        # Disable the depth testing for the hand and the text - we always want these things on top, with no clipping
        self.handnp.setDepthTest(False)
        self.hand_text_np.setDepthTest(False)

        # Add the player update task
        taskMgr.add(self.update, 'update_player_task')

    def init_objects(self):

        # Make Playfer Box
        shape = BulletBoxShape(Vec3(0.25, 0.25, 0.25))
        node = BulletRigidBodyNode('Playfer Box')
        node.setMass(110.0)
        node.setFriction(1.0)
        node.addShape(shape)
        node.setAngularDamping(0.0)
        np = render.attachNewNode(node)
        np.setPos(-1.4, 1.7, -1.7)
        self.world.attachRigidBody(node)
        playferboxmodel = loader.loadModel('../data/mod/playferbox.egg')
        playferboxmodel.reparentTo(np)

        # Make Pendlepot
        shape = BulletBoxShape(Vec3(0.2, 0.15, 0.1))
        node = BulletRigidBodyNode('Pendlepot')
        node.setMass(5.0)
        node.setFriction(1.0)
        node.addShape(shape)
        node.setAngularDamping(0.0)
        np = render.attachNewNode(node)
        np.setPos(-1.4, 1.7, -1.5)
        self.world.attachRigidBody(node)
        pendlepotmodel = loader.loadModel('../data/mod/pendlepot.egg')
        pendlepotmodel.reparentTo(np)

    def update(self, task):
        # Update camera orientation
        md = base.win.getPointer(0)
        mouse_x = md.getX()
        mouse_y = md.getY()
        centre_x = base.win.getXSize() / 2
        centre_y = base.win.getYSize() / 2
        if base.win.movePointer(0, centre_x, centre_y):
            new_H = base.camera.getH() + (centre_x - mouse_x)
            new_P = base.camera.getP() + (centre_y - mouse_y)
            if new_P < -90:
                new_P = -90
            elif new_P > 90:
                new_P = 90
            base.camera.setH(new_H)
            base.camera.setP(new_P)

        # Update player position
        speed = 3
        self.player_is_moving = False
        if (self.key_state["up"] == True):
            self.player_is_moving = True
            dir = 0
        if (self.key_state["down"] == True):
            self.player_is_moving = True
            dir = 180
        if (self.key_state["left"] == True):
            self.player_is_moving = True
            dir = 90
        if (self.key_state["right"] == True):
            self.player_is_moving = True
            dir = 270

        self.player.clearForces()
        old_vel = self.player.getLinearVelocity()
        new_vel = Vec3(0, 0, 0)
        if self.player_is_moving == True:
            new_vel.setX(-speed * math.sin(
                (base.camera.getH() + dir) * 3.1415 / 180.0))
            new_vel.setY(speed * math.cos(
                (base.camera.getH() + dir) * 3.1415 / 180.0))

        timescale = 0.001
        linear_force = (new_vel - old_vel) / (timescale)
        linear_force.setZ(0.0)
        self.player.applyCentralForce(linear_force)

        if self.player_is_grabbing == False:
            new_hand_pos = LPoint3f(
                render.getRelativePoint(base.camera, Vec3(0, 0.2, 0)))
            self.handnp.setPos(new_hand_pos)
        else:
            new_hand_pos = LPoint3f(
                render.getRelativePoint(base.camera, Vec3(0, 0.5, 0)))
            diff = new_hand_pos - self.handnp.getPos()
            self.hand.applyCentralForce(diff * 1000 -
                                        self.hand.getLinearVelocity() * 100)
            if diff.length() > .5:
                self.player.setLinearVelocity(Vec3(0, 0, 0))

        # Identify what lies beneath the player's hand (unless player is holding something)
        if self.player_is_grabbing == False:
            ray_from = self.playernp.getPos()
            ray_to = LPoint3f(
                render.getRelativePoint(base.camera, Vec3(0, 1, 0)))
            result = self.world.rayTestClosest(ray_from, ray_to)
            if result.hasHit() == True:
                self.hand_text.setText(result.getNode().getName())
                self.hand_text_np.setPos(result.getHitPos())
                self.hand_text_np.show()

                # If player clicks, grab the object by the nearest point (as chosen by ray)
                if self.mouse_state["left_click"] == True:
                    if result.getNode().getNumChildren() == 1:
                        obj = NodePath(result.getNode().getChild(0))

                        if self.player_is_grabbing == False:
                            self.player_is_grabbing = True

                            # Find the position of contact in terms of the object's local coordinates.
                            # Parent the player's hand to the grabbed object at that position.
                            pos = obj.getRelativePoint(render,
                                                       result.getHitPos())

                            self.grabbed_node = result.getNode()
                            self.grabbed_node.setActive(True)
                            print self.grabbed_node

                            frameA = TransformState.makePosHpr(
                                Vec3(0, 0, 0), Vec3(0, 0, 0))
                            frameB = TransformState.makePosHpr(
                                Vec3(0, 0, 0), Vec3(0, 0, 0))

                            swing1 = 20  # degrees
                            swing2 = 20  # degrees
                            twist = 20  # degrees

                            self.cs = BulletConeTwistConstraint(
                                self.hand, result.getNode(), frameA, frameB)
                            self.cs.setLimit(swing1, swing2, twist)
                            self.world.attachConstraint(self.cs)

                            # Stop the held object swinging all over the place
                            result.getNode().setAngularDamping(0.7)
            else:
                self.hand_text_np.hide()
                self.player_is_grabbing = False

        if self.mouse_state["left_click"] == False:
            self.player_is_grabbing = False
            if self.cs != None:
                self.world.remove_constraint(self.cs)
                self.cs = None
                self.grabbed_node.setAngularDamping(0.0)

        if self.player_is_grabbing == True and self.mouse_state[
                "right_click"] == True:
            self.world.remove_constraint(self.cs)
            self.cs = None
            self.grabbed_node.setAngularDamping(0.0)
            self.grabbed_node.setActive(True)
            self.grabbed_node.applyCentralImpulse(1000, 0, 0)

        if self.player_is_grabbing == True:
            self.hand_text_np.hide()

        return task.cont

    def update_physics(self, task):
        dt = globalClock.getDt()
        self.world.doPhysics(dt)
        return task.cont

    def start_physics(self):
        taskMgr.add(self.update_physics, 'update_physics')
    def insert(self, world, render, i, pos):
        # Important numbers
        head_radius = 0.5
        head_elevation = 1.5
        torso_x = 0.3
        torso_y = 0.5
        torso_z = 0.75
        arm_radius = 0.15
        shoulder_space = 0.05

        shoulder_elevation = head_elevation - head_radius - 0.1 - arm_radius
        torso_elevation = head_elevation - head_radius - torso_z

        x, y = pos

        # measurements below are in degrees
        neck_yaw_limit = 90
        neck_pitch_limit = 45
        shoulder_twist_limit = 90  # limit for twisting arm along the bicep axis
        shoulder_in_limit = 175  # maximum declination from T-pose towards torso
        shoulder_out_limit = 90  # maximum elevation from T-pose away from torso
        shoulder_forward_limit = 175  # maximum angle from down by side to pointing forward
        shoulder_backward_limit = 90  # maximum angle from down by side to pointing backward

        # Create a head
        head_node = BulletRigidBodyNode('Head')
        head_node.addShape(BulletSphereShape(head_radius))
        head_node.setMass(1.0)
        head_pointer = render.attachNewNode(head_node)
        head_pointer.setPos(x, y, head_elevation)
        world.attachRigidBody(head_node)

        # Create a torso
        torso_node = BulletRigidBodyNode('Torso')
        torso_node.addShape(BulletBoxShape(Vec3(torso_x, torso_y, torso_z)))
        torso_node.setMass(0.0)  # remain in place
        torso_pointer = render.attachNewNode(torso_node)
        torso_pointer.setPos(x, y, torso_elevation)
        world.attachRigidBody(torso_node)

        # Attach the head to the torso
        head_frame = TransformState.makePosHpr(Point3(0, 0, -head_radius),
                                               Vec3(0, 0, -90))
        torso_frame = TransformState.makePosHpr(Point3(0, 0, torso_z),
                                                Vec3(0, 0, -90))
        neck = BulletConeTwistConstraint(head_node, torso_node, head_frame,
                                         torso_frame)
        neck.setDebugDrawSize(0.5)
        neck.setLimit(neck_pitch_limit, neck_pitch_limit, neck_yaw_limit)
        world.attachConstraint(neck)

        # Create arms
        shoulder_pos_l = Point3(
            x, y - i * (torso_y + shoulder_space + arm_radius),
            shoulder_elevation)
        shoulder_pos_r = Point3(
            x, y + i * (torso_y + shoulder_space + arm_radius),
            shoulder_elevation)
        limits = (shoulder_in_limit, shoulder_out_limit,
                  shoulder_forward_limit, shoulder_backward_limit,
                  shoulder_twist_limit)
        arm_l = Arm(world, render, shoulder_pos_l, -i, LEFT, torso_pointer,
                    limits)
        arm_r = Arm(world, render, shoulder_pos_r, i, RIGHT, torso_pointer,
                    limits)

        self.head = head_pointer
        self.torso = torso_pointer
        self.arm_l, self.arm_r = arm_l, arm_r
Example #5
0
  def setupObstacleFour(self, pos, scale, turn):
    #Start Here
    # Box A
    shape = BulletBoxShape(Vec3(0.01, 0.01, 0.01) * scale)
    bodyA = BulletRigidBodyNode('Box A')
    bodyNP = self.worldNP.attachNewNode(bodyA)
    bodyNP.node().addShape(shape)
    bodyNP.setCollideMask(BitMask32.allOn())
    bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 4) #(0, 0, 4)

    visNP = loader.loadModel('media/models/box.egg')
    visNP.setScale(Vec3(0.01, 0.01, 0.01)*2*scale)
    visNP.clearModelNodes()
    visNP.reparentTo(bodyNP)

    self.world.attachRigidBody(bodyA)

    # Box B
    shape = BulletSphereShape(0.5*scale)

    bodyB = BulletRigidBodyNode('Sphere B')
    bodyNP = self.worldNP.attachNewNode(bodyB)
    bodyNP.node().addShape(shape)
    bodyNP.node().setMass(10.0)
    bodyNP.node().setDeactivationEnabled(False)
    bodyNP.setCollideMask(BitMask32.allOn())
    bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 5) #(0, 0, 0.001)

    visNP = loader.loadModel('media/models/ball.egg')
    visNP.clearModelNodes()
    visNP.setScale(1.25*scale)
    visNP.reparentTo(bodyNP)
    
    bodyNP.node().setLinearVelocity(100)

    self.world.attachRigidBody(bodyB)

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

    cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB)
    cone.setDebugDrawSize(2.0)
    cone.setLimit(30, 90, 270, softness=1.0, bias=0.3, relaxation=10.0)
    self.world.attachConstraint(cone)
    
    # Box C
    shape = BulletBoxShape(Vec3(0.1, 0.1, 1)*scale)

    bodyC = BulletRigidBodyNode('Box C')
    bodyNP = self.worldNP.attachNewNode(bodyC)
    bodyNP.node().addShape(shape)
    bodyNP.node().setDeactivationEnabled(False)
    bodyNP.setCollideMask(BitMask32.allOn())
    bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3)
    
    self.world.attachRigidBody(bodyC)

    visNP = loader.loadModel('media/models/box.egg')
    visNP.setScale(Vec3(0.1, 0.1, 1)*2*scale)
    visNP.clearModelNodes()
    visNP.reparentTo(bodyNP)
class Simulation(DirectObject):
    scale = 10

    R = RE
    throttle = 0.0

    def __init__(self):
        base.setBackgroundColor(0.1, 0.1, 0.8, 1)
        base.setFrameRateMeter(True)

        base.cam.setPos(-8 * self.scale, -8 * self.scale, 4 * self.scale)
        base.cam.lookAt(0, 0, 1 * self.scale)

        # Light
        alight = AmbientLight('ambientLight')
        alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
        alightNP = render.attachNewNode(alight)

        dlight = DirectionalLight('directionalLight')
        dlight.setDirection(Vec3(1, 1, -1))
        dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
        dlightNP = render.attachNewNode(dlight)

        render.clearLight()
        render.setLight(alightNP)
        render.setLight(dlightNP)

        # Input
        self.accept('escape', self.doExit)
        self.accept('r', self.doReset)
        self.accept('f1', self.toggleWireframe)
        self.accept('f2', self.toggleTexture)
        self.accept('f3', self.toggleDebug)
        self.accept('f5', self.doScreenshot)

        inputState.watchWithModifiers('forward', 'w')
        inputState.watchWithModifiers('left', 'a')
        inputState.watchWithModifiers('reverse', 's')
        inputState.watchWithModifiers('right', 'd')
        inputState.watchWithModifiers('turnLeft', 'q')
        inputState.watchWithModifiers('turnRight', 'e')

        # Task
        taskMgr.add(self.update, 'updateWorld')

        self.ostData = OnscreenText(text='ready',
                                    pos=(-1.3, 0.9),
                                    scale=0.07,
                                    fg=Vec4(1, 1, 1, 1),
                                    align=TextNode.ALeft)

        # Physics
        self.setup()

    # _____HANDLER_____

    def doExit(self):
        self.cleanup()
        sys.exit(1)

    def doReset(self):
        self.cleanup()
        self.setup()

    def toggleWireframe(self):
        base.toggleWireframe()

    def toggleTexture(self):
        base.toggleTexture()

    def toggleDebug(self):
        if self.debugNP.isHidden():
            self.debugNP.show()
        else:
            self.debugNP.hide()

    def doScreenshot(self):
        base.screenshot('Bullet')

    # ____TASK___

    def processInput(self, dt):
        force = Vec3(0, 0, 0)
        throttleChange = 0.0
        #torque = Vec3(0, 0, 0)

        if inputState.isSet('forward'): force.setX(1)
        if inputState.isSet('reverse'): force.setX(-1)
        if inputState.isSet('left'): force.setY(-1)
        if inputState.isSet('right'): force.setY(1)
        if inputState.isSet('turnLeft'): throttleChange = -1.0
        if inputState.isSet('turnRight'): throttleChange = 1.0

        force *= -20.0 / 180.0 * math.pi
        self.throttle += throttleChange / 100.0
        self.throttle = min(max(self.throttle, 0), 1)
        thrust = Vec3(0, 0, self.throttle * 50)
        quatGimbal = self.rocketNozzle.getTransform(self.worldNP).getQuat()

        self.rocketNozzle.node().applyForce(quatGimbal.xform(thrust),
                                            LPoint3f(0, 0, 0))

        #torque *= 10.0

        #force = render.getRelativeVector(self.rocketNP, force)
        #torque = render.getRelativeVector(self.rocketNP, torque)

        self.rocketNozzle.node().setActive(True)
        self.rocketNP.node().setActive(True)
        #self.rocketNozzle.node().applyCentralForce(force)
        #self.rocketNozzle.node().applyTorque(torque)
        force = rot.from_euler('zyx', force).as_quat()

        self.cone.setMotorTarget(
            LQuaternionf(force[0], force[1], force[2], force[3]))

    def update(self, task):
        dt = globalClock.getDt()

        self.processInput(dt)
        #self.world.doPhysics(dt)
        self.world.doPhysics(dt, 5, 1.0 / 180.0)

        yawpitchroll = rot.from_quat(
            self.rocketNozzle.getTransform(self.rocketNP).getQuat())
        yawpitchroll = yawpitchroll.as_euler('zyx', degrees=True)

        #self.ostData.destroy()
        telemetry = []
        telemetry.append(
            'Nozzle Position:\n Yaw: {}\n Pitch: {}\n Roll: {}'.format(
                int(yawpitchroll[0]), int(yawpitchroll[1]),
                int(yawpitchroll[2])))
        telemetry.append('\nThrottle: {}'.format(self.throttle))

        self.ostData.setText('\n'.join(telemetry))

        return task.cont

    def cleanup(self):
        self.world.removeRigidBody(self.groundNP.node())
        self.world.removeRigidBody(self.rocketNP.node())
        self.world = None

        self.debugNP = None
        self.groundNP = None
        self.rocketNP = None

        self.worldNP.removeNode()

    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(True)

        #self.debugNP.showTightBounds()
        #self.debugNP.showBounds()

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

        # Ground (static)
        shape = BulletPlaneShape(Vec3(0, 0, 1), 1)

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

        self.world.attachRigidBody(self.groundNP.node())

        #Rocket
        shape = BulletCylinderShape(0.2 * self.scale, 2 * self.scale, ZUp)

        self.rocketNP = self.worldNP.attachNewNode(
            BulletRigidBodyNode('Cylinder'))
        self.rocketNP.node().setMass(3.0)
        self.rocketNP.node().addShape(shape)
        self.rocketNP.setPos(0, 0, 2 * self.scale)
        self.rocketNP.setCollideMask(BitMask32.allOn())

        self.world.attachRigidBody(self.rocketNP.node())

        for i in range(4):
            leg = BulletCylinderShape(0.02 * self.scale, 1 * self.scale, XUp)
            self.rocketNP.node().addShape(
                leg,
                TransformState.makePosHpr(
                    Vec3(0.6 * self.scale * math.cos(i * math.pi / 2),
                         0.6 * self.scale * math.sin(i * math.pi / 2),
                         -1.2 * self.scale), Vec3(i * 90, 0, 30)))

        shape = BulletConeShape(0.15 * self.scale, 0.3 * self.scale, ZUp)

        self.rocketNozzle = self.worldNP.attachNewNode(
            BulletRigidBodyNode('Cone'))
        self.rocketNozzle.node().setMass(1)
        self.rocketNozzle.node().addShape(shape)
        self.rocketNozzle.setPos(0, 0, 0.8 * self.scale)
        self.rocketNozzle.setCollideMask(BitMask32.allOn())
        self.rocketNozzle.node().setCollisionResponse(0)

        self.world.attachRigidBody(self.rocketNozzle.node())

        frameA = TransformState.makePosHpr(Point3(0, 0, -1 * self.scale),
                                           Vec3(0, 0, 90))
        frameB = TransformState.makePosHpr(Point3(0, 0, 0.2 * self.scale),
                                           Vec3(0, 0, 90))

        self.cone = BulletConeTwistConstraint(self.rocketNP.node(),
                                              self.rocketNozzle.node(), frameA,
                                              frameB)
        self.cone.enableMotor(1)
        #self.cone.setMaxMotorImpulse(2)
        #self.cone.setDamping(1000)
        self.cone.setDebugDrawSize(2.0)
        self.cone.setLimit(20, 20, 0, softness=1.0, bias=1.0, relaxation=10.0)
        self.world.attachConstraint(self.cone)

        self.npThrustForce = LineNodePath(self.render,
                                          'Thrust',
                                          thickness=4,
                                          colorVec=VBase4(1, 0.5, 0, 1))