def updateClick(self, click): if p.dist(self.menuPos, click) <= 25: return "menu" elif p.dist(self.restartPos, click) <= 25: return "restart" elif p.dist(self.volPos, click) <= 25: self.volCtrl = 1 - self.volCtrl return "volToggle"
def collision(self, wp): if not self.isActive: return False if wp.isParticle and p.dist(self.P, (wp.x, wp.y)) < self.r + wp.r: wp.health = 0 elif not wp.isParticle and p.dist(self.P, (wp.x, wp.y)) < self.r + wp.r: self.isActive = False self.img = self.imgInactive return False #collision with this does not constitute collision w block
def updateMove(self, move): self.selected = None for loc in [self.menuPos, self.restartPos, self.volPos]: if p.dist(move, loc) <= 25: self.selected = loc break if p.dist(move, self.helpPos) <= 25: self.help = True else: self.help = False
def update(self): if not self.isActive: return None if p.dist(self.P, self.pathList[self.index]) < 2: #then near this position, guard moves towards next target self.index = (self.index + 1) % self.L self.target = self.pathList[self.index] dist = p.dist(self.P, self.target) direction = p.vsub(self.target, self.P) if direction[0] < 0: self.imgDir = 0 elif direction[0] > 0: self.imgDir = 1 move = p.fixNorm(direction, min(self.stride, dist)) self.P = p.vadd(self.P, move)
def follow(self, other): partition = 4 factor = 0.01 allowance = 10 Hooke = factor * p.dist((self.camX, self.camY), (other.x, other.y)) #refocus on other if not moving if Hooke > allowance and (self.camX, self.camY) != (other.x, other.y): self.camX += (other.x - self.camX) / Hooke**2 self.camY += (other.y - self.camY) / Hooke**2 self.bound(self.camX, self.camY) return None #if moving but not going offscreen if (p.distX((self.camX, self.camY), (other.x, other.y)) < self.camWidth / partition and p.distY((self.camX, self.camY), (other.x, other.y)) < self.camHeight / partition): self.bound(self.camX, self.camY) return None #otherwise moving and going offscreen #follow x-dir if other.x > self.camX + self.camWidth / partition: self.camX = other.x - self.camWidth /partition elif other.x < self.camX - self.camWidth / partition: self.camX = other.x + self.camWidth / partition #follow y-dir if other.y > self.camY + self.camHeight / partition: self.camY = other.y - self.camHeight /partition elif other.y < self.camY - self.camHeight / partition: self.camY = other.y + self.camHeight / partition self.bound(self.camX, self.camY) return None
def collision(self, wp): if not self.lit: return False if p.dist(self.P, (wp.x, wp.y)) < 20: heal = 40 wp.health = min(wp.health + heal, wp.maxHealth) self.img = self.imgE self.lit = False return False
def posepub(xtg, ytg, x, y, ango, bx, by, ang, k):
pub = rospy.Publisher('robot1n1/pose', Pose, queue_size=10)
pubball = rospy.Publisher('ballpose', Pose, queue_size=10)
pose = Pose()
bpose = Pose()
k = k
rate = rospy.Rate(60)
pg.init()
ball = p.ball(x=bx, y=by)
mybot = p.robot(x=x, y=y, yaw=ango, ball=ball)
mybotpid = pid.pid(x=x, y=y, ball=ball, angle=ango)
ko = 0
while not rospy.is_shutdown():
for event in pg.event.get():
if event.type == pg.QUIT:
sys.exit()
mybotpid.gtg(xtg, ytg, mybot, ball, thtg=ang)
if mybot.dribble == 0:
p.collRb(mybot, ball)
bpose.position.x = ball.x
bpose.position.y = ball.y
p.walleffect(mybot)
p.walleffect(ball)
pose.position.x = mybot.x
pose.position.y = mybot.y
pose.orientation.z = m.tan(mybot.theta / 2)
pose.orientation.w = 1
bpose.orientation.w = 1
pub.publish(pose)
pubball.publish(bpose)
if (p.dist(mybot.x, mybot.y, xtg, ytg) < 10
and abs(mybot.theta - angle) < 0.1 and ball.speed <= 3
and ko == 0):
if k == 1 and mybot.dribble == 1:
mybot.kick(ball, 5)
ko = 1
#print ball.speed
if (p.dist(mybot.x, mybot.y, xtg, ytg) < 10
and abs(mybot.theta - angle) < 0.1 and ball.speed <= 3
and ko == 1):
return mybot.x, mybot.y, mybot.theta, ball.x, ball.y
oldx = mybot.x
oldy = mybot.y
rate.sleep()
def exitMe(self, wp, i): epsilon = 2 P = self.LoC[i] Q = self.LoC[(i + 1) % len(self.LoC)] R = (wp.x, wp.y) nextR = p.vadd(R, wp.v) det = p.det(P, Q, R) d = p.dist(P, Q) # check edge collision checkDist = (d * (wp.r - wp.maxSpeed - epsilon) <= det <= d * (wp.r + epsilon))
def collision(self, wp): #check if in bounding box if not (self.minX - wp.r <= wp.x <= self.maxX + wp.r and self.minY - wp.r <= wp.y <= self.maxY + wp.r): return False epsilon = 2 L = len(self.LoC) if self.inMe: inside = True #still inside for i in xrange(L): P = self.LoC[i] Q = self.LoC[(i + 1) % L] R = (wp.x, wp.y) nextR = p.vadd((wp.x, wp.y), wp.v) det = p.det(P, Q, nextR) #if moving away from wall, continue if p.det(P, Q, R) >= det: continue inside &= (det <= p.dist(P, Q) * (epsilon - wp.r)) if not inside: break #to retain values of P, Q if inside: return True #because collision #otherwise check for internal reflection base = p.vsub(Q, P) if not p.refract(wp.v, base, self.rIndex, 1): wp.v = p.reflect(wp.v, base) return True #otherwise if not complete exit, return collision true if det < p.dist(P, Q) * wp.r: return True #otherwise complete exit, change angle self.inMe = False wp.v = p.fixNorm(p.refract(wp.v, p.vsub((0, 0), base), self.rIndex, 1), wp.maxSpeed) return False else: #check each individual side for i in xrange(L): if self.checkEdgeCollision(wp, i): return True for i in xrange(L): if self.checkVertexCollision(wp, i): return True return False
def checkEdgeCollision(self, wp, i): epsilon = 2 P = self.LoC[i] Q = self.LoC[(i + 1) % len(self.LoC)] R = (wp.x, wp.y) nextR = p.vadd(R, wp.v) det = p.det(P, Q, R) d = p.dist(P, Q) # check edge collision checkDist = (d * (wp.r - wp.maxSpeed - epsilon) <= det <= d * (wp.r + epsilon)) checkAngle1 = (p.distSq(P, Q) + p.distSq(Q, R) - p.distSq(P, R)) / p.dist(P, Q) / p.dist(Q, R) > 0 checkAngle2 = (p.distSq(P, Q) + p.distSq(P, R) - p.distSq(Q, R)) / p.dist(P, Q) / p.dist(P, R) > 0 checkDir = p.det(P, Q, R) + epsilon >= p.det(P, Q, nextR) if checkDist and checkAngle1 and checkAngle2 and checkDir: #move particle back to proper location self.calibrateParticleEdge(wp, P, Q, R) #calculate new velocity/gravity if wp.isParticle: self.particleEdgeCollision(wp, P, Q) else: self.waveEdgeCollision(wp, P, Q) wp.collision = (P, Q) return True
def checkVertexCollision(self, wp, i): epsilon = 2 P = self.LoC[i] R = (wp.x, wp.y) # check vertex collision with P checkDist = p.dist(P, R) <= wp.r + epsilon if checkDist: #move particle back to proper location self.calibrateParticleVertex(wp, P, R) L = len(self.LoC) Q = self.LoC[(i + 1) % L] S = self.LoC[(i - 1) % L] u = p.fixNorm(p.vsub(Q, P)) v = p.fixNorm(p.vsub(P, S)) w = p.vadd(u, v) if wp.isParticle: self.particleVertexCollision(wp, P) else: self.waveVertexCollision(wp, w) wp.collision = ((0, 0), w)
def update(self): #if not in universe if not (0 < self.x < self.width and 0 < self.y < self.height): self.health = 0 #check health if self.health == 0: #play death sound subprocess.Popen(["python", self.path + os.sep + "aud" + os.sep + "death.py"]) self.health = self.health / self.mhu * self.mhu if self.isParticle: #no wave positions self.posList = None #velocity affected by gravity self.v = p.vadd(self.v, self.g) if p.dist(self.v) >= self.maxSpeed: #then transform to wave self.isParticle = False self.r = 12 self.g = (0, 0) self.v = p.fixNorm(self.v, self.maxSpeed) if not self.collision: #gravity has effect self.g = self.gravity #select image to draw #select particle image if self.v[0] < 0: self.img = self.imgL elif self.v[0] > 0: self.img = self.imgR #change position (self.x, self.y) = p.vadd((self.x, self.y), self.v) if not self.isParticle: #then draw ray if self.posList == None: self.posList = [(self.x, self.y)] else: self.posList.append(p.vadd(p.sprod(self.posList[-1], 2/3.0), p.sprod((self.x, self.y), 1/3.0))) self.posList.append(p.sprod(p.vadd(self.posList[-1], (self.x, self.y)), 0.5)) self.posList.append((self.x, self.y)) while len(self.posList) > self.mrl: del self.posList[0: 3]
def complete(self, wp): dist = 25 if p.dist(self.P, (wp.x, wp.y)) <= dist: subprocess.Popen(["python", self.path + os.sep + "aud" + os.sep + "end.py"]) return True return False
