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
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
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
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
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
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
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
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
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
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
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
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
def add_to(self, geom_builder): geom_builder.add_wedge(self.color, self.base, self.top, self.width, LRotationf(*self.hpr))
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
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
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)
def add_to(self, geom_builder): rot = LRotationf() rot.set_hpr(self.hpr) geom_builder.add_block(self.color, self.center, self.size, rot)
def add_to(self, geom_builder): geom_builder.add_ramp(self.color, self.base, self.top, self.width, self.thickness, LRotationf(*self.hpr))
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
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