Ejemplo n.º 1
0
    def rollTask(self, task):
        dt = globalClock.getDt()

        # if dt is large, then there has been a # hiccup that could cause the ball to leave the field if this functions runs, so ignore the frame
        if dt > .2:
            return Task.cont

        # collision handler
        for i in range(self.cHandler.getNumEntries()):
            entry = self.cHandler.getEntry(i)
            name = entry.getIntoNode().getName()
            if name == "wall_collide":
                self.wallCollideHandler(entry)
            elif name == "ground_collide":
                self.groundCollideHandler(entry)

        # move the ball
        # update the velocity based on acceleration
            self.ballV += self.accelV * dt * ACCEL
        # prevent velocity from going above max velocity
        if self.ballV.lengthSquared() > MAX_SPEED_SQ:
            self.ballV.normalize()
            self.ballV *= MAX_SPEED
        # update the position based on the velocity
        self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))

        # rotate the ball
        prevRot = LRotationf(self.ball.getQuat())
        axis = LVector3.up().cross(self.ballV)
        newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
        self.ball.setQuat(prevRot * newRot)

        return Task.cont
Ejemplo n.º 2
0
    def start(self):
        #startPos = self.maze.find("**/start").getPos()
        #self.ballRoot.setPos(0.5, 0, 0)
        #self.ballV = LVector3(0, 0.5, 0)         # Initial velocity is 0
        #self.accelV = LVector3(0, 0, 0)        # Initial acceleration is 0

        self.ballVs = []
        self.accelVs = []
        for ballIndex in xrange(len(self.balls)):
            self.ballVs.append(LVector3(0, 0, 0))
            self.accelVs.append(LVector3(0, 0, 0))
            continue
        self.ballRoots[0].setPos(0.2, 1.05, -0.1)
        #self.ballVs[0] = LVector3(0, 0.0, 0)
        self.ballRoots[1].setPos(0.32, 1.2, -0.1)
        #self.ballRoots[2].setHpr(0, 0, 90)
        self.ballRoots[2].setPos(-0.4, 1.1, 0.4)
        axis = LVector3.up()
        prevRot = LRotationf(self.balls[2].getQuat())
        newRot = LRotationf(axis, 90)
        self.balls[2].setQuat(prevRot * newRot)

        # Create the movement task, but first make sure it is not already
        # running
        taskMgr.remove("rollTask")
        #taskMgr.remove("mouseTask")
        self.mainLoop = taskMgr.add(self.rollTask, "rollTask")
        #self.mainLoop = taskMgr.add(self.mouseTask, "mouseTask")

        return
Ejemplo n.º 3
0
    def rollTask(self, task):
        # Standard technique for finding the amount of time since the last frame
        dt = task.time - task.last
        task.last = task.time

        # If dt is large, then there has been a # hiccup that could cause the ball
        # to leave the field if this functions runs, so ignore the frame
        if dt > .2: return Task.cont

        # The collision handler collects the collisions. We dispatch which function
        # to handle the collision based on the name of what was collided into
        for i in range(self.cHandler.getNumEntries()):
            entry = self.cHandler.getEntry(i)
            name = entry.getIntoNode().getName()
            if name == "wall_collide": self.wallCollideHandler(entry)
            elif name == "ground_collide": self.groundCollideHandler(entry)
            elif name == "loseTrigger": self.loseGame(entry)

        # Read the mouse position and tilt the maze accordingly
        #if base.mouseWatcherNode.hasMouse():
        #  mpos = base.mouseWatcherNode.getMouse() # get the mouse position
        linha = arduino.readline()
        if linha != '':
            ax, ay, nx, ny = arduino.readline().split(',')  # 70 - 182
            print ax, ay
            ax = float(ax)
            ay = float(ay)

            rato.y = translate(ay, 0, 255, -1, 1)
            rato.x = translate(ax, 0, 255, -1, 1)

        self.maze.setR(rato.x * 10)
        self.maze.setP(rato.y * -10)

        #if base.mouseWatcherNode.hasMouse():
        #    mpos = base.mouseWatcherNode.getMouse() # get the mouse position
        #    print mpos.getY(), mpos.getX()
        #    self.maze.setP(mpos.getY() * -10)
        #    self.maze.setR(mpos.getX() * 10)

        # Finally, we move the ball
        # Update the velocity based on acceleration
        self.ballV += self.accelV * dt * ACCEL
        # Clamp the velocity to the maximum speed
        if self.ballV.lengthSquared() > MAX_SPEED_SQ:
            self.ballV.normalize()
            self.ballV *= MAX_SPEED
        # Update the position based on the velocity
        self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))

        # This block of code rotates the ball. It uses something called a quaternion
        # to rotate the ball around an arbitrary axis. That axis perpendicular to
        # the balls rotation, and the amount has to do with the size of the ball
        # This is multiplied on the previous rotation to incrimentally turn it.
        prevRot = LRotationf(self.ball.getQuat())
        axis = UP.cross(self.ballV)
        newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
        self.ball.setQuat(prevRot * newRot)

        return Task.cont  # Continue the task indefinitely
Ejemplo n.º 4
0
  def rollTask(self, task):
    
    # Get the most recent frame
    frame = self.leap.frame()
    hands = frame.hands
    numHands = len(hands)

    if numHands >= 1:
        # Get the normal vector of the first hand
        hand = hands[0]
        handNormal = hand.palm_normal

        if handNormal is not None:
            # Standard technique for finding the amount of time since the last frame
            dt = task.time - task.last
            task.last = task.time
        
            # If dt is large, then there has been a # hiccup that could cause the ball
            # to leave the field if this functions runs, so ignore the frame
            if dt > .2: return Task.cont   
        
            # The collision handler collects the collisions. We dispatch which function
            # to handle the collision based on the name of what was collided into
            for i in range(self.cHandler.getNumEntries()):
              entry = self.cHandler.getEntry(i)
              name = entry.getIntoNode().getName()
              if   name == "wall_collide":   self.wallCollideHandler(entry)
              elif name == "ground_collide": self.groundCollideHandler(entry)
              elif name == "loseTrigger":    self.loseGame(entry)
        
            # Read the hands normal vector and tilt the maze accordingly
            self.maze.setP(handNormal.z * -30)
            self.maze.setR(handNormal.x * -30)
        
            # Finally, we move the ball
            # Update the velocity based on acceleration
            self.ballV += self.accelV * dt * ACCEL
            # Clamp the velocity to the maximum speed
            if self.ballV.lengthSquared() > MAX_SPEED_SQ:
              self.ballV.normalize()
              self.ballV *= MAX_SPEED
            # Update the position based on the velocity
            self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))
        
            # This block of code rotates the ball. It uses something called a quaternion
            # to rotate the ball around an arbitrary axis. That axis perpendicular to
            # the balls rotation, and the amount has to do with the size of the ball
            # This is multiplied on the previous rotation to incrimentally turn it.
            prevRot = LRotationf(self.ball.getQuat())
            axis = UP.cross(self.ballV)
            newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
            self.ball.setQuat(prevRot * newRot)
    
    return Task.cont       # Continue the task indefinitely
Ejemplo n.º 5
0
    def add_ramp(self, color, base, top, width, thickness, rot=None):
        midpoint = Point3((top + base) / 2.0)
        rot = LRotationf(0, 0, 0) if rot is None else rot

        # Temporarily move `base` and `top` to positions relative to a midpoint
        # at (0, 0, 0).
        if midpoint != Point3(0, 0, 0):
            base = Point3(base - (midpoint - Point3(0, 0, 0)))
            top = Point3(top - (midpoint - Point3(0, 0, 0)))
        p3 = Point3(top.get_x(), top.get_y(), top.get_z() - thickness)
        p4 = Point3(base.get_x(), base.get_y(), base.get_z() - thickness)

        # Use three points to calculate an offset vector we can apply to `base`
        # and `top` in order to find the required vertices.
        offset = (Point3(top + Vec3(0, 0, -1000)) - base).cross(top - base)
        offset.normalize()
        offset *= (width / 2.0)

        vertices = (
            Point3(top - offset),
            Point3(base - offset),
            Point3(base + offset),
            Point3(top + offset),
            Point3(p3 + offset),
            Point3(p3 - offset),
            Point3(p4 - offset),
            Point3(p4 + offset),
        )
        vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]

        faces = (
            # Top and bottom.
            [vertices[0], vertices[1], vertices[2], vertices[3]],
            [vertices[7], vertices[6], vertices[5], vertices[4]],
            # Back and front.
            [vertices[0], vertices[3], vertices[4], vertices[5]],
            [vertices[6], vertices[7], vertices[2], vertices[1]],
            # Left and right.
            [vertices[0], vertices[5], vertices[6], vertices[1]],
            [vertices[7], vertices[4], vertices[3], vertices[2]],
        )

        if width and (p3 - base).length():
            self._commit_polygon(Polygon(faces[0]), color)
            self._commit_polygon(Polygon(faces[1]), color)
        if width and thickness:
            self._commit_polygon(Polygon(faces[2]), color)
            self._commit_polygon(Polygon(faces[3]), color)
        if thickness and (p3 - base).length():
            self._commit_polygon(Polygon(faces[4]), color)
            self._commit_polygon(Polygon(faces[5]), color)

        return self
Ejemplo n.º 6
0
    def add_ramp(self, color, base, top, width, thickness, rot=None):
        midpoint = Point3((top + base) / 2.0)
        rot = LRotationf(0, 0, 0) if rot is None else rot

        # Temporarily move `base` and `top` to positions relative to a midpoint
        # at (0, 0, 0).
        if midpoint != Point3(0, 0, 0):
            base = Point3(base - (midpoint - Point3(0, 0, 0)))
            top = Point3(top - (midpoint - Point3(0, 0, 0)))
        p3 = Point3(top.get_x(), top.get_y() - thickness, top.get_z())
        p4 = Point3(base.get_x(), base.get_y() - thickness, base.get_z())

        # Use three points to calculate an offset vector we can apply to `base`
        # and `top` in order to find the required vertices.
        offset = (Point3(top + Vec3(0, -1000, 0)) - base).cross(top - base)
        offset.normalize()
        offset *= (width / 2.0)

        vertices = (
            Point3(top - offset),
            Point3(base - offset),
            Point3(base + offset),
            Point3(top + offset),
            Point3(p3 + offset),
            Point3(p3 - offset),
            Point3(p4 - offset),
            Point3(p4 + offset),
        )
        vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]

        faces = (
            # Top and bottom.
            [vertices[0], vertices[1], vertices[2], vertices[3]],
            [vertices[7], vertices[6], vertices[5], vertices[4]],
            # Back and front.
            [vertices[0], vertices[3], vertices[4], vertices[5]],
            [vertices[6], vertices[7], vertices[2], vertices[1]],
            # Left and right.
            [vertices[0], vertices[5], vertices[6], vertices[1]],
            [vertices[7], vertices[4], vertices[3], vertices[2]],
        )

        if width and (p3 - base).length():
            self._commit_polygon(Polygon(faces[0]), color)
            self._commit_polygon(Polygon(faces[1]), color)
        if width and thickness:
            self._commit_polygon(Polygon(faces[2]), color)
            self._commit_polygon(Polygon(faces[3]), color)
        if thickness and (p3 - base).length():
            self._commit_polygon(Polygon(faces[4]), color)
            self._commit_polygon(Polygon(faces[5]), color)

        return self
Ejemplo n.º 7
0
    def rollTask(self, task):
        self.gesture_controler.track()
        dt = globalClock.getDt()

        if dt > 0.2:
            return Task.cont

        if self.gesture_controler.getPause():
            return Task.cont

        # if self.gesture_controler.getQuit():
        # print("Gesture: Quit. You will quit from the game.")
        # sys.exit()

        # if self.gesture_controler.getReset():
        # self.resetGame()
        # return Task.cont

        for i in range(self.cHandler.getNumEntries()):
            entry = self.cHandler.getEntry(i)
            name = entry.getIntoNode().getName()

            if name == "wall_collide":
                self.wallCollideHandler(entry)
            elif name == "ground_collide":
                self.groundCollideHander(entry)
            elif name == "loseTriggers":
                self.loseGame(entry)

        if self.gesture_controler.isMoving():
            cur_pos = self.gesture_controler.getPos()
            mpos = LPoint2f(cur_pos[0], cur_pos[1])
            # print(mpos)
            self.maze.setP(mpos.getY() * -10)
            self.maze.setR(mpos.getX() * 10)

        self.ballV += self.accelV * dt * ACCEL

        if self.ballV.lengthSquared() > MAX_SPEED_SQ:
            self.ballV.normalize()
            self.ballV *= MAX_SPEED

        self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))

        prevRot = LRotationf(self.ball.getQuat())
        axis = LVector3.up().cross(self.ballV)
        newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
        self.ball.setQuat(prevRot * newRot)

        return Task.cont
Ejemplo n.º 8
0
    def add_dome(self, color, center, radius, samples, planes, rot=None):
        two_pi = pi * 2
        half_pi = pi / 2
        azimuths = [(two_pi * i) / samples for i in range(samples + 1)]
        elevations = [(half_pi * i) / (planes - 1) for i in range(planes)]
        rot = LRotationf(0, 0, 0) if rot is None else rot

        # Generate polygons for all but the top tier. (Quads)
        for i in range(0, len(elevations) - 2):
            for j in range(0, len(azimuths) - 1):
                x1, y1, z1 = to_cartesian(azimuths[j], elevations[i], radius)
                x2, y2, z2 = to_cartesian(azimuths[j], elevations[i + 1],
                                          radius)
                x3, y3, z3 = to_cartesian(azimuths[j + 1], elevations[i + 1],
                                          radius)
                x4, y4, z4 = to_cartesian(azimuths[j + 1], elevations[i],
                                          radius)

                vertices = (
                    Point3(x1, y1, z1),
                    Point3(x2, y2, z2),
                    Point3(x3, y3, z3),
                    Point3(x4, y4, z4),
                )
                vertices = [
                    rot.xform(v) + LVector3f(*center) for v in vertices
                ]

                self._commit_polygon(Polygon(vertices), color)

        # Generate polygons for the top tier. (Tris)
        for k in range(0, len(azimuths) - 1):
            x1, y1, z1 = to_cartesian(azimuths[k],
                                      elevations[len(elevations) - 2], radius)
            x2, y2, z2 = Vec3(0, radius, 0)
            x3, y3, z3 = to_cartesian(azimuths[k + 1],
                                      elevations[len(elevations) - 2], radius)

            vertices = (
                Point3(x1, y1, z1),
                Point3(x2, y2, z2),
                Point3(x3, y3, z3),
            )
            vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]

            self._commit_polygon(Polygon(vertices), color)

        return self
Ejemplo n.º 9
0
 def add_solid(self, node):
     mesh = BulletConvexHullShape()
     mesh.add_geom(GeomBuilder().add_ramp(self.color, self.base, self.top,
                                          self.width, self.thickness,
                                          LRotationf(*self.hpr)).get_geom())
     node.add_shape(mesh)
     return node
Ejemplo n.º 10
0
 def add_solid(self, node):
     mesh = BulletConvexHullShape()
     mesh.add_geom(GeomBuilder().add_dome(self.color, self.center,
                                          self.radius,
                                          self.samples, self.planes,
                                          LRotationf(*self.hpr)).get_geom())
     node.add_shape(mesh)
     return node
Ejemplo n.º 11
0
    def add_block(self, color, center, size, rot=None):
        x_shift = size[0] / 2.0
        y_shift = size[1] / 2.0
        z_shift = size[2] / 2.0
        rot = LRotationf(0, 0, 0) if rot is None else rot

        vertices = (
            Point3(-x_shift, +y_shift, +z_shift),
            Point3(-x_shift, -y_shift, +z_shift),
            Point3(+x_shift, -y_shift, +z_shift),
            Point3(+x_shift, +y_shift, +z_shift),
            Point3(+x_shift, +y_shift, -z_shift),
            Point3(+x_shift, -y_shift, -z_shift),
            Point3(-x_shift, -y_shift, -z_shift),
            Point3(-x_shift, +y_shift, -z_shift),
        )
        vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]

        faces = (
            # XY
            [vertices[0], vertices[1], vertices[2], vertices[3]],
            [vertices[4], vertices[5], vertices[6], vertices[7]],
            # XZ
            [vertices[0], vertices[3], vertices[4], vertices[7]],
            [vertices[6], vertices[5], vertices[2], vertices[1]],
            # YZ
            [vertices[5], vertices[4], vertices[3], vertices[2]],
            [vertices[7], vertices[6], vertices[1], vertices[0]],
        )

        if size[0] and size[1]:
            self._commit_polygon(Polygon(faces[0]), color)
            self._commit_polygon(Polygon(faces[1]), color)
        if size[0] and size[2]:
            self._commit_polygon(Polygon(faces[2]), color)
            self._commit_polygon(Polygon(faces[3]), color)
        if size[1] and size[2]:
            self._commit_polygon(Polygon(faces[4]), color)
            self._commit_polygon(Polygon(faces[5]), color)

        return self
Ejemplo n.º 12
0
    def add_block(self, color, center, size, rot=None):
        x_shift = size[0] / 2.0
        y_shift = size[1] / 2.0
        z_shift = size[2] / 2.0
        rot = LRotationf(0, 0, 0) if rot is None else rot

        vertices = (
            Point3(-x_shift, +y_shift, +z_shift),
            Point3(-x_shift, -y_shift, +z_shift),
            Point3(+x_shift, -y_shift, +z_shift),
            Point3(+x_shift, +y_shift, +z_shift),
            Point3(+x_shift, +y_shift, -z_shift),
            Point3(+x_shift, -y_shift, -z_shift),
            Point3(-x_shift, -y_shift, -z_shift),
            Point3(-x_shift, +y_shift, -z_shift),
        )
        vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]

        faces = (
            # XY
            [vertices[0], vertices[1], vertices[2], vertices[3]],
            [vertices[4], vertices[5], vertices[6], vertices[7]],
            # XZ
            [vertices[0], vertices[3], vertices[4], vertices[7]],
            [vertices[6], vertices[5], vertices[2], vertices[1]],
            # YZ
            [vertices[5], vertices[4], vertices[3], vertices[2]],
            [vertices[7], vertices[6], vertices[1], vertices[0]],
        )

        if size[0] and size[1]:
            self._commit_polygon(Polygon(faces[0]), color)
            self._commit_polygon(Polygon(faces[1]), color)
        if size[0] and size[2]:
            self._commit_polygon(Polygon(faces[2]), color)
            self._commit_polygon(Polygon(faces[3]), color)
        if size[1] and size[2]:
            self._commit_polygon(Polygon(faces[4]), color)
            self._commit_polygon(Polygon(faces[5]), color)

        return self
Ejemplo n.º 13
0
    def add_dome(self, color, center, radius, samples, planes, rot=None):
        two_pi = pi * 2
        half_pi = pi / 2
        azimuths = [(two_pi * i) / samples for i in range(samples + 1)]
        elevations = [(half_pi * i) / (planes - 1) for i in range(planes)]
        rot = LRotationf(0, 0, 0) if rot is None else rot

        # Generate polygons for all but the top tier. (Quads)
        for i in range(0, len(elevations) - 2):
            for j in range(0, len(azimuths) - 1):
                x1, y1, z1 = to_cartesian(azimuths[j], elevations[i], radius)
                x2, y2, z2 = to_cartesian(azimuths[j], elevations[i + 1], radius)
                x3, y3, z3 = to_cartesian(azimuths[j + 1], elevations[i + 1], radius)
                x4, y4, z4 = to_cartesian(azimuths[j + 1], elevations[i], radius)

                vertices = (
                    Point3(x1, y1, z1),
                    Point3(x2, y2, z2),
                    Point3(x3, y3, z3),
                    Point3(x4, y4, z4),
                )
                vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]

                self._commit_polygon(Polygon(vertices), color)

        # Generate polygons for the top tier. (Tris)
        for k in range(0, len(azimuths) - 1):
            x1, y1, z1 = to_cartesian(azimuths[k], elevations[len(elevations) - 2], radius)
            x2, y2, z2 = Vec3(0, radius, 0)
            x3, y3, z3 = to_cartesian(azimuths[k + 1], elevations[len(elevations) - 2], radius)

            vertices = (
                Point3(x1, y1, z1),
                Point3(x2, y2, z2),
                Point3(x3, y3, z3),
            )
            vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]

            self._commit_polygon(Polygon(vertices), color)

        return self
Ejemplo n.º 14
0
 def add_to(self, geom_builder):
     geom_builder.add_wedge(self.color, self.base, self.top, self.width,
                            LRotationf(*self.hpr))
Ejemplo n.º 15
0
    def updatePhysics(self, task):
        '''
        Use the motor PWM values calculated by the controller to apply forces
        to the simulated vehicle.

        This runs at every frame, so it needs to complete quickly.

        '''
        outputs = shm.kalman.get()
        self.tm.update(outputs)
        passive_wrench = vehicle.passive_forces(outputs, self.tm)
        passive_forces, passive_torques = passive_wrench[:3], \
                                          passive_wrench[3:]

        # Get motor thrusts
        thrusts = np.array(self.tm.get_thrusts())

        # Add passive forces and torques to that produced by thrusters,
        # converting them to sub space first.
        force = self.tm.total_thrust(thrusts) + \
                self.tm.orientation.conjugate() * passive_forces
        torque = self.tm.total_torque(thrusts) + \
                self.tm.orientation.conjugate() * passive_torques

        # Finally apply forces and torques to the model
        # we also need to account for panda3d's strange coordinate system
        # (x and y are flipped and z points up (instead of down))
        self.linearForce.setVector(force_subspace[1], \
                                   force_subspace[0], \
                                  -force_subspace[2])

        # We're supposed to use axis angle here, but I'm being sneaky
        # and using the torque vector directly, i.e. non normalized axis angle
        # with the hopes that this LRotationf constructor will figure it out
        self.angularForce.setQuat(\
            LRotationf(LVector3f(torque_subspace[1], \
                                 torque_subspace[0], \
                                -torque_subspace[2]), 1))

        # Update shared variables for controller
        outputs.heading = self.getHeading()
        outputs.pitch = self.myPath.getP()
        outputs.roll = self.myPath.getR()

        # This velocity is in world space
        # We need to put it into THRUST CONVENTION SPACE
        # which we assume kalman outputs in...
        velocity = self.getPhysicsObject().getVelocity()

        # Bring the velocity into THRUST CONVENTION SPACE
        # Don't forget to account for panda's coordinate system
        velocity = self.tm.heading_quat.conjugate() * \
                  np.array((velocity.getY(), velocity.getX(), -velocity.getZ()))

        outputs.velx = velocity[0]
        outputs.vely = velocity[1]
        outputs.depth_rate = velocity[2]

        outputs.depth = self.getDepth()
        outputs.north = self.myPath.getY()
        outputs.east = self.myPath.getX()

        dX = self.myPath.getX() - self.previousXY[0]
        dY = self.myPath.getY() - self.previousXY[1]

        # Forward and sway are in THRUST CONVENTION SPACE
        # don't forget to account for panda's coordinate system
        dF, dS, dD = self.tm.heading_quat.conjugate() * np.array((dY, dX, 0.0))

        outputs.forward += dF
        outputs.sway += dS

        # Output some quaternions, accounting for Panda's coordinate system
        outputs.q0, outputs.q2, outputs.q1, outputs.q3 = self.myPath.getQuat()
        outputs.q3 *= -1.0

        shm.kalman.set(outputs)

        svHeadingInt.set(self.getHeading())
        svDepth.set(self.getDepth())
        #XXX: Approximate altitude assuming that the pool is 12 feet deep
        svAltitude.set(3.6 - self.getDepth())
        svDvlDmgNorth.set(self.myPath.getY())
        svDvlDmgEast.set(self.myPath.getX())

        self.previousXY = (self.myPath.getX(), self.myPath.getY())  #update

        self.output_hydro_data()

        return Task.cont
Ejemplo n.º 16
0
    def add_wedge(self, color, base, top, width, rot=None):
        delta_y = top.get_y() - base.get_y()
        midpoint = Point3((top + base) / 2.0)
        rot = LRotationf(0, 0, 0) if rot is None else rot

        # Temporarily move `base` and `top` to positions relative to a midpoint
        # at (0, 0, 0).
        if midpoint != Point3(0, 0, 0):
            base = Point3(base - (midpoint - Point3(0, 0, 0)))
            top = Point3(top - (midpoint - Point3(0, 0, 0)))
        p3 = Point3(top.get_x(), base.get_y(), top.get_z())

        # Use three points to calculate an offset vector we can apply to `base`
        # and `top` in order to find the required vertices. Ideally we'd use
        # `p3` as the third point, but `p3` can potentially be the same as `top`
        # if delta_y is 0, so we'll just calculate a new point relative to top
        # that differs in elevation by 1000, because that sure seems unlikely.
        # The "direction" of that point relative to `top` does depend on whether
        # `base` or `top` is higher. Honestly, I don't know why that's important
        # for wedges but not for ramps.
        if base.get_y() > top.get_y():
            direction = Vec3(0, 1000, 0)
        else:
            direction = Vec3(0, -1000, 0)
        offset = (Point3(top + direction) - base).cross(top - base)
        offset.normalize()
        offset *= (width / 2.0)

        vertices = (
            Point3(top - offset),
            Point3(base - offset),
            Point3(base + offset),
            Point3(top + offset),
            Point3(p3 + offset),
            Point3(p3 - offset),
        )
        vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]

        faces = (
            # The slope.
            [vertices[0], vertices[1], vertices[2], vertices[3]],
            # The bottom.
            [vertices[5], vertices[4], vertices[2], vertices[1]],
            # The back.
            [vertices[0], vertices[3], vertices[4], vertices[5]],
            # The sides.
            [vertices[5], vertices[1], vertices[0]],
            [vertices[4], vertices[3], vertices[2]],
        )

        if width or delta_y:
            self._commit_polygon(Polygon(faces[0]), color)
        if width and (p3 - base).length():
            self._commit_polygon(Polygon(faces[1]), color)
        if width and delta_y:
            self._commit_polygon(Polygon(faces[2]), color)
        if delta_y and (p3 - base).length():
            self._commit_polygon(Polygon(faces[3]), color)
            self._commit_polygon(Polygon(faces[4]), color)

        return self
Ejemplo n.º 17
0
 def add_to(self, geom_builder):
     rot = LRotationf(*self.hpr)
     geom_builder.add_dome(self.color, self.center, self.radius,
                           self.samples, self.planes, rot)
Ejemplo n.º 18
0
 def add_to(self, geom_builder):
     rot = LRotationf()
     rot.set_hpr(self.hpr)
     geom_builder.add_block(self.color, self.center, self.size, rot)
Ejemplo n.º 19
0
    def add_wedge(self, color, base, top, width, rot=None):
        delta_y = top.get_y() - base.get_y()
        midpoint = Point3((top + base) / 2.0)
        rot = LRotationf(0, 0, 0) if rot is None else rot

        # Temporarily move `base` and `top` to positions relative to a midpoint
        # at (0, 0, 0).
        if midpoint != Point3(0, 0, 0):
            base = Point3(base - (midpoint - Point3(0, 0, 0)))
            top = Point3(top - (midpoint - Point3(0, 0, 0)))
        p3 = Point3(top.get_x(), base.get_y(), top.get_z())

        # Use three points to calculate an offset vector we can apply to `base`
        # and `top` in order to find the required vertices. Ideally we'd use
        # `p3` as the third point, but `p3` can potentially be the same as `top`
        # if delta_y is 0, so we'll just calculate a new point relative to top
        # that differs in elevation by 1000, because that sure seems unlikely.
        # The "direction" of that point relative to `top` does depend on whether
        # `base` or `top` is higher. Honestly, I don't know why that's important
        # for wedges but not for ramps.
        if base.get_y() > top.get_y():
            direction = Vec3(0, 1000, 0)
        else:
            direction = Vec3(0, -1000, 0)
        offset = (Point3(top + direction) - base).cross(top - base)
        offset.normalize()
        offset *= (width / 2.0)

        vertices = (
            Point3(top - offset),
            Point3(base - offset),
            Point3(base + offset),
            Point3(top + offset),
            Point3(p3 + offset),
            Point3(p3 - offset),
        )
        vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]

        faces = (
            # The slope.
            [vertices[0], vertices[1], vertices[2], vertices[3]],
            # The bottom.
            [vertices[5], vertices[4], vertices[2], vertices[1]],
            # The back.
            [vertices[0], vertices[3], vertices[4], vertices[5]],
            # The sides.
            [vertices[5], vertices[1], vertices[0]],
            [vertices[4], vertices[3], vertices[2]],
        )

        if width or delta_y:
            self._commit_polygon(Polygon(faces[0]), color)
        if width and (p3 - base).length():
            self._commit_polygon(Polygon(faces[1]), color)
        if width and delta_y:
            self._commit_polygon(Polygon(faces[2]), color)
        if delta_y and (p3 - base).length():
            self._commit_polygon(Polygon(faces[3]), color)
            self._commit_polygon(Polygon(faces[4]), color)

        return self
Ejemplo n.º 20
0
 def add_to(self, geom_builder):
     geom_builder.add_ramp(self.color, self.base, self.top, self.width,
                           self.thickness, LRotationf(*self.hpr))
Ejemplo n.º 21
0
    def rollTask(self, task):
        # Standard technique for finding the amount of time since the last
        # frame
        dt = globalClock.getDt()

        # If dt is large, then there has been a # hiccup that could cause the ball
        # to leave the field if this functions runs, so ignore the frame
        if dt > .2:
            return Task.cont

        # if base.mouseWatcherNode.is_button_down('a'):
        #     self.holeRoot.setH(self.holeRoot.getH() + 1)
        #     print(self.holeRoot.getHpr())
        #     pass
        # if base.mouseWatcherNode.is_button_down('s'):
        #     self.holeRoot.setP(self.holeRoot.getP() + 1)
        #     print(self.holeRoot.getHpr())
        #     pass
        # if base.mouseWatcherNode.is_button_down('d'):
        #     self.holeRoot.setR(self.holeRoot.getR() + 1)
        #     print(self.holeRoot.getHpr())
        #     pass

        # go through different visualizations
        if base.mouseWatcherNode.is_button_down(
                'space') and self.showing == 'none':
            self.showing = 'parts'
            self.showingProgress = 0
            pass
        #print(self.showing)
        #print(self.showing)
        if self.showing == 'none':
            return Task.cont
        if self.showing == 'parts':
            self.showingProgress += 0.01
            #self.showingProgress += 1
            #print(self.showingProgress)
            scale = 2 - self.showingProgress
            scaleY = 1 + (scale - 1) * 0.5
            for planeIndex, planeNP in enumerate(self.planeNPs):
                center = self.planeCenters[planeIndex]
                planeNP.setPos(center[0] * scale, center[1] * scaleY,
                               center[2] * scale)
                planeNP.reparentTo(self.render)
                planeNP.setTwoSided(True)
                continue
            if self.showingProgress > 1:
                self.showing = 'moving'
                for planeIndex, planeNP in enumerate(self.planeNPs):
                    planeNP.removeNode()
                    continue
                self.planeScene.show()
                self.showingProgress = 1
            return Task.cont
        if self.showing == 'moving':
            self.showingProgress += 0.005
            #self.showingProgress += 1
            #print(self.showingProgress, np.sign(self.showingProgress - 0.5) * min(self.showingProgress % 0.5, 0.5 - self.showingProgress % 0.5) * 4)
            self.camera.setPos(
                np.sign(self.showingProgress - 0.5) *
                min(self.showingProgress % 0.5,
                    0.5 - self.showingProgress % 0.5) * 3, 0, 0)
            #self.camera.setHpr(angleDegrees, 0, 0)
            #self.camera.lookAt(0, 0, 0)
            self.camera.lookAt(0, 3, 0)
            if self.showingProgress > 1:
                self.showing = 'geometry'
                self.camera.setPos(0, 0, 0)
                #self.planeScene.removeNode()
                # for triNP in self.triNPs:
                #     triNP.show()
                #     continue
                self.showingProgress = 1
            return Task.cont
        if self.showing == 'geometry':
            self.showingProgress += 0.02
            if self.showingProgress > 1:
                #self.showing = 'image'
                self.showing = 'placement'
                self.showingProgress = 0
                self.holeRoot.show()
                self.inPortalRoot.show()
                self.outPortalRoot.show()
                self.inPortalTube.show()
                self.outPortalTube.show()
                for ballRoot in self.ballRoots:
                    ballRoot.show()
                    continue
                self.showingProgress = 0
                pass
            return Task.cont
        # if self.showing == 'placement':
        #     self.showingProgress += 0.005
        #         continue

        # mouse pose
        if self.mouseWatcherNode.hasMouse():
            mpos = self.mouseWatcherNode.getMouse()
            self.mpos = mpos
            self.pickerRay.setFromLens(self.camNode, mpos.getX(), mpos.getY())
            pass

        #if base.mouseWatcherNode.is_button_down('space') and self.showing == 'placement':
        if self.showing == 'placement':
            self.card.show()
            self.planeScene.removeNode()
            self.showing = 'image'
            pass
        # if base.mouseWatcherNode.is_button_down('space') and self.showing == 'image':
        #     for triNP in self.triNPs:
        #         triNP.hide()
        #         continue
        #     self.showing = 'start'
        #     pass

        # for each plane, check which horizontal plane it is sitting on
        self.ballGroundMap = {}
        for i in range(self.cHandler.getNumEntries()):
            entry = self.cHandler.getEntry(i)
            ballName = entry.getFromNode().getName()
            groundName = entry.getIntoNode().getName()
            if 'ball_ray_' not in ballName:
                continue
            if 'ground_' not in groundName:
                continue
            ballIndex = int(ballName[9:])
            groundIndex = int(groundName[7:])
            norm = -entry.getSurfaceNormal(render)
            if norm.length() == 0:
                continue
            norm = norm / norm.length()
            distance = norm.dot(
                entry.getSurfacePoint(render) -
                self.ballRoots[ballIndex].getPos())
            #print(distance)
            if distance < 0:
                continue
            if ballIndex not in self.ballGroundMap or distance < self.ballGroundMap[
                    ballIndex][1]:
                self.ballGroundMap[ballIndex] = (groundIndex, distance)
                pass
            continue

        # The collision handler collects the collisions. We dispatch which function
        # to handle the collision based on the name of what was collided into
        for i in range(self.cHandler.getNumEntries()):
            entry = self.cHandler.getEntry(i)
            fromName = entry.getFromNode().getName()
            #if 'mouseRay' in fromName:
            #continue
            name = entry.getIntoNode().getName()
            #if name == "plane_collide":
            if 'tri_' in name:
                self.planeCollideHandler(entry)
            #elif name == "wall_collide":
            #self.wallCollideHandler(entry)
            #elif name == "ground_collide":
            #self.groundCollideHandler(entry)
            elif 'ball_' in name:
                self.ballCollideHandler(entry)
            elif 'ground_' in name:
                self.groundCollideHandler(entry)
            elif 'hole' in name:
                self.score(entry)
            elif 'portal_' in name:
                self.portal(entry)
                pass
            continue

        # Read the mouse position and tilt the maze accordingly
        if base.mouseWatcherNode.hasMouse():
            mpos = base.mouseWatcherNode.getMouse()  # get the mouse position
            #self.maze.setP(mpos.getY() * -10)
            #self.maze.setR(mpos.getX() * 10)
            pass

        # if base.mouseWatcherNode.is_button_down('mouse1'):
        #     print(base.mouseWatcherNode.getMouseX())
        #     print(base.mouseWatcherNode.getMouseY())
        #     exit(1)

        # Finally, we move the ball
        # Update the velocity based on acceleration
        for ballIndex in xrange(len(self.balls)):
            if self.ballVs[ballIndex].length(
            ) < 1e-4 and self.ballVs[ballIndex].dot(
                    self.accelVs[ballIndex]) < -1e-4:
                self.ballVs[ballIndex] = LVector3(0, 0, 0)
                self.accelVs[ballIndex] = LVector3(0, 0, 0)
            else:
                self.ballVs[ballIndex] += self.accelVs[ballIndex] * dt * ACCEL
                pass
            #print('current speed', self.ballVs[ballIndex], self.accelVs[ballIndex])
            # Clamp the velocity to the maximum speed
            if self.ballVs[ballIndex].lengthSquared() > MAX_SPEED_SQ:
                self.ballVs[ballIndex].normalize()
                self.ballVs[ballIndex] *= MAX_SPEED
                pass
            #print(self.ballVs[ballIndex], self.accelVs[ballIndex], self.ballRoots[ballIndex].getPos())

            # Update the position based on the velocity
            self.ballRoots[ballIndex].setPos(
                self.ballRoots[ballIndex].getPos() +
                (self.ballVs[ballIndex] * dt))

            # This block of code rotates the ball. It uses something called a quaternion
            # to rotate the ball around an arbitrary axis. That axis perpendicular to
            # the balls rotation, and the amount has to do with the size of the ball
            # This is multiplied on the previous rotation to incrimentally turn it.
            prevRot = LRotationf(self.balls[ballIndex].getQuat())
            axis = LVector3.up().cross(self.ballVs[ballIndex])
            newRot = LRotationf(
                axis,
                np.rad2deg(dt * self.ballVs[ballIndex].length() /
                           self.ballSize))
            self.balls[ballIndex].setQuat(prevRot * newRot)
            continue

        self.cueRoot.setPos(self.cuePos[0], self.cuePos[1], self.cuePos[2])
        return Task.cont  # Continue the task indefinitely
Ejemplo n.º 22
0
 def add_to(self, geom_builder):
     rot = LRotationf()
     rot.set_hpr(self.hpr)
     geom_builder.add_block(self.color, self.center, self.size, rot)
Ejemplo n.º 23
0
Archivo: main.py Proyecto: MarcMDE/LASO
    def rollTask(self, task):
        # Standard technique for finding the amount of time since the last
        # frame
        #print("\r",self.maze.getR(), self.maze.getP(), self.ballRoot.getPos(), end="")

        dt = globalClock.getDt()
        print("\r{:.3} fps      ".format(1 / dt), end="")

        # If dt is large, then there has been a # hiccup that could cause the ball
        # to leave the field if this functions runs, so ignore the frame
        if dt > .2:
            return Task.cont

        #print(action)
        if action == "start":
            a = 1

        elif action == "stop":
            while action == "stop":
                a = 0

    # elif action == "restart": in Line 531

        elif action == "coord":
            a = 1
            #ALGUNA CRIDA A METODE DE NARCIS/MARC

        key_down = base.mouseWatcherNode.is_button_down

        if self.ready_to_solve:

            if key_down(KeyboardButton.ascii_key('d')):
                screenshot = self.camera2_buffer.getScreenshot()
                if screenshot:
                    v = memoryview(screenshot.getRamImage()).tolist()
                    img = np.array(v, dtype=np.uint8)
                    img = img.reshape(
                        (screenshot.getYSize(), screenshot.getXSize(), 4))
                    img = img[::-1]
                    #self.digitizer.set_src_img(img)
                    #self.digitizer.digitalize_source()
                    cv2.imshow('img', img)
                    #cv2.waitKey(0)

            if key_down(KeyboardButton.ascii_key('s')):
                print("Screenshot!")
                self.camera2_buffer.saveScreenshot("screenshot.jpg")

            # The collision handler collects the collisions. We dispatch which function
            # to handle the collision based on the name of what was collided into
            for i in range(self.cHandler.getNumEntries()):
                entry = self.cHandler.getEntry(i)
                name = entry.getIntoNode().getName()
                if action == "restart":
                    self.loseGame(entry)
                if name == "wall_col":
                    self.wallCollideHandler(entry)
                elif name == "ground_col":
                    self.groundCollideHandler(entry)
                elif name == "loseTrigger":
                    vr.restart = 1
                    global th
                    th = threading.Thread(target=listenVoice)
                    th.start()
                    self.loseGame(entry)

            # Read the mouse position and tilt the maze accordingly
            # Rotation axes use (roll, pitch, heave)
            """
            if base.mouseWatcherNode.hasMouse():
                mpos = base.mouseWatcherNode.getMouse()  # get the mouse position
                self.maze.setP(mpos.getY() * -10)
                self.maze.setR(mpos.getX() * 10)
            """

            ballPos = self.get_ball_position()
            #print("BALL POS: ", ballPos)

            posFPixel = self.path[self.indexPuntActual]

            xFinal = posFPixel[1]  #posFPixel[1]/np.shape(laberint)[0]*13 - 6.5
            yFinal = posFPixel[
                0]  #-(posFPixel[0]/np.shape(laberint)[1]*13.5 - 6.8)

            dist = math.sqrt((xFinal - ballPos[1])**2 +
                             (yFinal - ballPos[0])**2)

            if (dist < self.minDist):
                if (self.indexPuntActual == len(self.path) - 1):
                    print("SOLVED!!", end="")

                while (self.aStar.distance(
                    (ballPos[0], ballPos[1]), self.path[self.indexPuntActual])
                       < self.pas):
                    if (self.indexPuntActual < len(self.path) - 1):
                        self.indexPuntActual += 1
                    else:
                        break

            # ball pos (y,x)

            #print("END POS: ", self.digitizer.endPos)

            if voice_solving:
                p_rotation = dir_veu[0]
                r_rotation = dir_veu[1]
                if p_rotation == 0 and r_rotation == 0 and ballPos is not None:
                    p_rotation, r_rotation = self.pid.getPR(
                        ballPos[1], ballPos[0], ballPos[1], ballPos[0],
                        self.maze.getP(), self.maze.getR(), dt)

            else:
                p_rotation = 0
                r_rotation = 0
                #print(ballPos, dist)

                #print(ballPos)

                if ballPos is not None:

                    p_rotation, r_rotation = self.pid.getPR(
                        ballPos[1], ballPos[0], xFinal, yFinal,
                        self.maze.getP(), self.maze.getR(), dt)
                    #print(p_rotation, r_rotation)

            if key_down(KeyboardButton.up()):
                p_rotation = -1
            elif key_down(KeyboardButton.down()):
                p_rotation = 1

            if key_down(KeyboardButton.left()):
                r_rotation = -1
            elif key_down(KeyboardButton.right()):
                r_rotation = 1

            self.rotateMaze(p_rotation, r_rotation)

            # Finally, we move the ball
            # Update the velocity based on acceleration
            self.ballV += self.accelV * dt * ACCEL
            # Clamp the velocity to the maximum speed
            if self.ballV.lengthSquared() > MAX_SPEED_SQ:
                self.ballV.normalize()
                self.ballV *= MAX_SPEED
            # Update the position based on the velocity
            self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))
            #print(self.ballRoot.getPos())

            # This block of code rotates the ball. It uses something called a quaternion
            # to rotate the ball around an arbitrary axis. That axis perpendicular to
            # the balls rotation, and the amount has to do with the size of the ball
            # This is multiplied on the previous rotation to incrimentally turn it.
            prevRot = LRotationf(self.ball.getQuat())
            axis = LVector3.up().cross(self.ballV)
            newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
            self.ball.setQuat(prevRot * newRot)

        elif key_down(KeyboardButton.ascii_key('1')):
            self.solve()

        if key_down(KeyboardButton.ascii_key('i')):
            self.light.setY(self.light.getY() + 10 * dt)
        elif key_down(KeyboardButton.ascii_key('k')):
            self.light.setY(self.light.getY() - 10 * dt)

        if key_down(KeyboardButton.ascii_key('j')):
            self.light.setX(self.light.getX() - 10 * dt)
        elif key_down(KeyboardButton.ascii_key('l')):
            self.light.setX(self.light.getX() + 10 * dt)

        if key_down(KeyboardButton.ascii_key('u')):
            self.lightColor += 10000 * dt
            self.light.node().setColor(
                (self.lightColor, self.lightColor, self.lightColor, 1))
        elif key_down(KeyboardButton.ascii_key('o')):
            self.lightColor -= 10000 * dt
            self.light.node().setColor(
                (self.lightColor, self.lightColor, self.lightColor, 1))

        if key_down(KeyboardButton.ascii_key('8')):
            self.alightColor += 1 * dt
            self.ambientL.node().setColor(
                (self.alightColor, self.alightColor, self.alightColor, 1))
        elif key_down(KeyboardButton.ascii_key('9')):
            self.alightColor -= 1 * dt
            self.ambientL.node().setColor(
                (self.alightColor, self.alightColor, self.alightColor, 1))

        if key_down(KeyboardButton.ascii_key('r')):
            base.trackball.node().set_pos(0, 200, 0)
            base.trackball.node().set_hpr(0, 60, 0)

        return Task.cont  # Continue the task indefinitely