def think(self, dt, view, debugsurface): if not self.goal: # have no goal? return # debugsurface.fill((255, 0, 0, 100)) ccourse = False last = self.position for i in xrange(self.waypoint_len()): for o in view.pedestrians: if linesegdist2(last, self.waypoint(i).position, o.position) <= (self.radius + o.radius) ** 2: ccourse = True break last = self.waypoint(i).position if ccourse: break gb = graphbuilder.GraphBuilder(self.speed, self.turningspeed) safe_distance = self.radius + 1 # some margin is nice start = gb.node(self.position, self.angle) end = gb.node(self.goal, None) if graphbuilder.free_path(self.position, self.goal, view, safe_distance): gb.connect(self.position, self.goal) else: for i in xrange(self.NODES / 2): # global planning newpos = Vec2d(640 * random.random(), 480 * random.random()) for pos in gb.positions(): if graphbuilder.free_path(Vec2d(*pos), newpos, view, safe_distance): gb.connect(pos, newpos) pygame.draw.aaline(debugsurface, (0, 255, 0, 255), pos, newpos) for i in xrange(self.NODES - self.NODES / 2): # some extra local points to handle the crowd newpos = self.position + Vec2d( (2 * random.random() - 1) * self.view_range, (2 * random.random() - 1) * self.view_range ) for pos in gb.positions(): if graphbuilder.free_path(Vec2d(*pos), newpos, view, safe_distance): gb.connect(pos, newpos) pygame.draw.aaline(debugsurface, (0, 255, 0, 255), pos, newpos) for p in gb.positions(): pygame.draw.circle(debugsurface, (0, 0, 0), map(int, p), 2, 0) nodes = gb.all_nodes() result = astar.shortest_path(start, end, nodes) if result.success: result.path = [tuple(self.position)] for i in result.indices: if result.path[-1] != nodes[i].position: result.path.append(nodes[i].position) if ccourse is True: self.waypoint_clear() if result.success is True and (self.waypoint_len() == 0 or result.total_cost < self.cdist): self.waypoint_clear() for p in result.path: self.waypoint_push(Vec2d(*p)) self.cdist = result.total_cost
def think(self, dt, view, debugsurface): if not self.goal: # have no goal? return for p in view.pedestrians: debugsurface.circle(p.position, p.radius + 2, "green", 2) #pygame.draw.aaline(debugsurface, pygame.Color("yellow"), map(int, self.position), map(int, p.position)) #generating static voronoi points (only performed once) if len(self.staticDPoints) == 0: for o in view.obstacles: already = False for p in self.staticDPoints: if o.p1 == p: already = True break if not already: self.staticDPoints.append(o.p1) already = False for p in self.staticDPoints: if o.p2 == p: already = True break if not already: self.staticDPoints.append(o.p2) obstacle_length = o.p1.distance_to(o.p2) num_midpoints = int(obstacle_length/(7.5*self.radius)) for m in xrange(num_midpoints): self.staticDPoints.append(o.p1+(o.p2-o.p1)*(m+1)/(num_midpoints+1)) safe_distance = self.radius + self.FREEMARGIN # some margin is nice diff = self.goal - self.position # in order to ignore obstacles just beyond the goal if graphbuilder.free_path(self.position, self.goal - diff.norm()*self.radius, view, safe_distance): self.waypoint_clear() self.waypoint_push(self.position) self.waypoint_push(self.goal) else: dPoints = [self.position, self.goal] for o in view.pedestrians: dPoints.append(o.position) for dP in self.staticDPoints: if(self.position.distance_to(dP) <= self.view_range): dPoints.append(dP) if(0): # to get a global best path (avoids known obstacles) vPoints = [self.position, self.goal] for vP in self.staticDPoints: vPoints.append(vP) gb = graphbuilder.SimpleGraphBuilder() vD = computeVoronoiDiagram(vPoints) vDNodes = vD[0] vDEdges = vD[2] for e in vDEdges: # build the voronoi map if((e[1] != -1) and (e[2] != -1)): # indicates line to infinity if graphbuilder.free_path_obstacles_only(Vec2d(*vDNodes[e[1]]), Vec2d(*vDNodes[e[2]]), view, safe_distance): gb.connect(vDNodes[e[1]], vDNodes[e[2]]) pygame.draw.aaline(debugsurface, (0, 255, 0, 255), vDNodes[e[1]], vDNodes[e[2]]) for pos in gb.positions(): # get off the voronoi map diff = Vec2d(*pos) - self.position if(graphbuilder.free_path(self.position + diff.norm()*self.radius, Vec2d(*pos), view, safe_distance) and (diff.length() <= self.view_range)): #so agent is not blocked by pedestrian next to it gb.connect(self.position, pos) pygame.draw.aaline(debugsurface, (0, 255, 0, 255), self.position, pos) diff = self.goal - Vec2d(*pos) if(graphbuilder.free_path(self.goal, Vec2d(*pos), view, 0) and (diff.length() <= self.view_range)): gb.connect(self.goal, pos) pygame.draw.aaline(debugsurface, (0, 255, 0, 255), self.goal, pos) start = gb.node(self.position, self.angle) end = gb.node(self.goal, None) nodes = gb.all_nodes() for p in gb.positions(): pygame.draw.circle(debugsurface, (0, 0, 0), map(int, p), 2, 0) result = astar.shortest_path(start, end, nodes) for r in xrange(len(result.indices)-1): pygame.draw.aaline(debugsurface, (255, 0, 255, 255), nodes[result.indices[r]].position, nodes[result.indices[r+1]].position) if len(result.indices) > 1: bestHeading = Vec2d(*nodes[result.indices[1]].position) - self.position else: bestHeading = self.goal - self.position else: bestHeading = self.goal - self.position neighborPairs = [] dD = computeDelaunayTriangulation(dPoints) for d in dD: if(d[0] == 0): debugsurface.line(dPoints[d[0]], dPoints[d[1]], "red") debugsurface.line(dPoints[d[0]], dPoints[d[2]], "red") neighborPairs.append([dPoints[d[1]], dPoints[d[2]]]) if(d[1] == 0): debugsurface.line(dPoints[d[1]], dPoints[d[0]], "red") debugsurface.line(dPoints[d[1]], dPoints[d[2]], "red") neighborPairs.append([dPoints[d[0]], dPoints[d[2]]]) if(d[2] == 0): debugsurface.line(dPoints[d[2]], dPoints[d[1]], "red") debugsurface.line(dPoints[d[2]], dPoints[d[0]], "red") neighborPairs.append([dPoints[d[0]], dPoints[d[1]]]) maxArea = 0 for n in neighborPairs: A = tuple(self.position) B = n[0] C = n[1] midPoint = [(B[0]+C[0])/2, (B[1]+C[1])/2] midPointHeading = Vec2d(*midPoint) - self.position area = abs(A[0]*B[1]+B[0]*C[1]+C[0]*A[1]-A[1]*B[0]-B[1]*C[0]-C[1]*A[0]) * pow((1+math.cos(bestHeading.angle(midPointHeading)))/2, 1) #the higher the power, the more focused the weighting is on the goalpoint if area > maxArea: maxArea = area maxMidPoint = midPoint self.waypoint_clear() self.waypoint_push(self.position) self.waypoint_push(Vec2d(*maxMidPoint))
def think(self, dt, view, debugsurface): if not self.goal: #have no goal? return for p in view.pedestrians: pygame.draw.circle(debugsurface, pygame.Color("green"), map(int, p.position), int(p.radius+2), 2) #pygame.draw.aaline(debugsurface, pygame.Color("yellow"), map(int, self.position), map(int, p.position)) #generating static voronoi points (only performed once) if len(self.staticVPoints) == 0: for o in view.obstacles: already = False for p in self.staticVPoints: if o.p1 == p: already = True break if not already: self.staticVPoints.append(o.p1) already = False for p in self.staticVPoints: if o.p2 == p: already = True break if not already: self.staticVPoints.append(o.p2) obstacle_length = o.p1.distance_to(o.p2) num_midpoints = int(obstacle_length/(7.5*self.radius)) for m in xrange(num_midpoints): self.staticVPoints.append(o.p1+(o.p2-o.p1)*(m+1)/(num_midpoints+1)) #debugsurface.fill((255, 0, 0, 100)) ccourse = False last = self.position for i in xrange(self.waypoint_len()): for o in view.pedestrians: if linesegdist2(last, self.waypoint(i).position, o.position) <= (self.radius + o.radius + self.FREEMARGIN)**2: ccourse = True break last = self.waypoint(i).position if ccourse: break gb = graphbuilder.SimpleGraphBuilder() safe_distance = self.radius + self.FREEMARGIN #some margin is nice diff = self.goal - self.position # in order to ignore obstacles just beyond the goal if graphbuilder.free_path(self.position, self.goal - diff.norm()*self.radius, view, safe_distance): gb.connect(self.position, self.goal) else: vPoints = [self.position, self.goal] for o in view.pedestrians: vPoints.append(o.position) for vP in self.staticVPoints: #if(self.position.distance_to(vP) <= self.view_range): vPoints.append(vP) if len(vPoints): vD = computeVoronoiDiagram(vPoints) vDNodes = vD[0] vDEdges = vD[2] for e in vDEdges: # build the voronoi map if((e[1] != -1) and (e[2] != -1)): #indicates line to infinity if graphbuilder.free_path_obstacles_only(Vec2d(*vDNodes[e[1]]), Vec2d(*vDNodes[e[2]]), view, safe_distance): gb.connect(vDNodes[e[1]], vDNodes[e[2]]) pygame.draw.aaline(debugsurface, (0,255,0,255), vDNodes[e[1]], vDNodes[e[2]]) for pos in gb.positions(): # get off the voronoi map diff = Vec2d(*pos) - self.position if(graphbuilder.free_path(self.position + diff.norm()*self.radius, Vec2d(*pos), view, safe_distance) and (diff.length() <= self.view_range)): #so agent is not blocked by pedestrian next to it gb.connect(self.position, pos) pygame.draw.aaline(debugsurface, (0,255,0,255), self.position, pos) diff = self.goal - Vec2d(*pos) if(graphbuilder.free_path(self.goal, Vec2d(*pos), view, 0) and (diff.length() <= self.view_range)): gb.connect(self.goal, pos) pygame.draw.aaline(debugsurface, (0,255,0,255), self.goal, pos) start = gb.node(self.position, self.angle) end = gb.node(self.goal, None) nodes = gb.all_nodes() for p in gb.positions(): pygame.draw.circle(debugsurface, (0,0,0), map(int, p), 2, 0) result = astar.shortest_path(start, end, nodes) if result.success: result.path = [tuple(self.position)] for i in result.indices: if result.path[-1] != nodes[i].position: # gets rid of duplicate pathpoints (?) result.path.append(nodes[i].position) if ccourse is True: self.waypoint_clear() if result.success is True and (self.waypoint_len() == 0 or result.total_cost < self.cdist): self.waypoint_clear() for p in result.path: self.waypoint_push(Vec2d(*p)) self.cdist = result.total_cost
def think(self, dt, view, debugsurface): if not self.goal: # have no goal? return #debugsurface.fill((255, 0, 0, 100)) ccourse = False last_pos = self.position for i in xrange(self.waypoint_len()): for pedestrian in view.pedestrians: safe_dist2 = (self.radius + pedestrian.radius + self.FREEMARGIN) ** 2 closest_dist2 = linesegdist2( last_pos, self.waypoint(i).position, pedestrian.position ) if closest_dist2 <= safe_dist2: ccourse = True break last_pos = self.waypoint(i).position if ccourse: break gb = graphbuilder.GraphBuilder(self.speed, self.turningspeed) safe_distance = self.radius + self.FREEMARGIN # some margin is nice start = gb.node(self.position, self.angle) end = gb.node(self.goal, None) if graphbuilder.free_path( self.position, self.goal, view, safe_distance ): gb.connect(self.position, self.goal) else: # using half of the points for global planning world_size = view.world_bounds[1::2] for i in xrange(self.NODES / 2): newpos = Vec2d( world_size[0] * self.random.random(), world_size[1] * self.random.random() ) for pos in gb.positions(): if graphbuilder.free_path( Vec2d(*pos), newpos, view, safe_distance ): gb.connect(pos, newpos) debugsurface.line(pos, newpos, "green") # some extra local points (within view range) to handle the crowd for i in xrange(self.NODES - self.NODES / 2): random_offset = Vec2d( (2 * self.random.random() - 1) * self.view_range, (2 * self.random.random() - 1) * self.view_range ) newpos = self.position + random_offset for pos in gb.positions(): if graphbuilder.free_path( Vec2d(*pos), newpos, view, safe_distance ): gb.connect(pos, newpos) debugsurface.line(pos, newpos, "green") for p in gb.positions(): debugsurface.circle(p, 2, "black", 0) nodes = gb.all_nodes() result = astar.shortest_path(start, end, nodes) if result.success: result.path = [tuple(self.position)] for i in result.indices: if result.path[-1] != nodes[i].position: result.path.append(nodes[i].position) if ccourse is True: self.waypoint_clear() # replace current waypoints if we have a better path # FIXME: self.cdist is not updated to take into account current # position which will penalize the current solution over # the last unjustly if result.success and (self.waypoint_len() == 0 or result.total_cost < self.cdist): self.waypoint_clear() for p in result.path: self.waypoint_push(Vec2d(*p)) self.cdist = result.total_cost
def think(self, dt, view, debugsurface): if not self.goal: #have no goal? return for p in view.pedestrians: pygame.draw.circle(debugsurface, pygame.Color("green"), map(int, p.position), int(p.radius+2), 2) #pygame.draw.aaline(debugsurface, pygame.Color("yellow"), map(int, self.position), map(int, p.position)) #generating static voronoi points (only performed once) if len(self.staticDPoints) == 0: for o in view.obstacles: already = False for p in self.staticDPoints: if o.p1 == p: already = True break if not already: self.staticDPoints.append(o.p1) already = False for p in self.staticDPoints: if o.p2 == p: already = True break if not already: self.staticDPoints.append(o.p2) obstacle_length = o.p1.distance_to(o.p2) num_midpoints = int(obstacle_length/(7.5*self.radius)) for m in xrange(num_midpoints): self.staticDPoints.append(o.p1+(o.p2-o.p1)*(m+1)/(num_midpoints+1)) safe_distance = self.radius + self.FREEMARGIN #some margin is nice diff = self.goal - self.position # in order to ignore obstacles just beyond the goal if graphbuilder.free_path(self.position, self.goal - diff.norm()*self.radius, view, safe_distance): self.waypoint_clear() self.waypoint_push(self.position) self.waypoint_push(self.goal) else: dPoints = [self.position, self.goal] numPeds = len(view.pedestrians) for o in view.pedestrians: dPoints.append(o.position) for dP in self.staticDPoints: if(self.position.distance_to(dP) <= self.view_range): dPoints.append(dP) if(0): # to get a global best path (avoids known obstacles) vPoints = [self.position, self.goal] for vP in self.staticDPoints: vPoints.append(vP) gb = graphbuilder.SimpleGraphBuilder() vD = computeVoronoiDiagram(vPoints) vDNodes = vD[0] vDEdges = vD[2] for e in vDEdges: # build the voronoi map if((e[1] != -1) and (e[2] != -1)): #indicates line to infinity if graphbuilder.free_path_obstacles_only(Vec2d(*vDNodes[e[1]]), Vec2d(*vDNodes[e[2]]), view, safe_distance): gb.connect(vDNodes[e[1]], vDNodes[e[2]]) pygame.draw.aaline(debugsurface, (0,255,0,255), vDNodes[e[1]], vDNodes[e[2]]) for pos in gb.positions(): # get off the voronoi map diff = Vec2d(*pos) - self.position if(graphbuilder.free_path(self.position + diff.norm()*self.radius, Vec2d(*pos), view, safe_distance) and (diff.length() <= self.view_range)): #so agent is not blocked by pedestrian next to it gb.connect(self.position, pos) pygame.draw.aaline(debugsurface, (0,255,0,255), self.position, pos) diff = self.goal - Vec2d(*pos) if(graphbuilder.free_path(self.goal, Vec2d(*pos), view, 0) and (diff.length() <= self.view_range)): gb.connect(self.goal, pos) pygame.draw.aaline(debugsurface, (0,255,0,255), self.goal, pos) start = gb.node(self.position, self.angle) end = gb.node(self.goal, None) nodes = gb.all_nodes() for p in gb.positions(): pygame.draw.circle(debugsurface, (0,0,0), map(int, p), 2, 0) result = astar.shortest_path(start, end, nodes) for r in xrange(len(result.indices)-1): pygame.draw.aaline(debugsurface, (255,0,255,255), nodes[result.indices[r]].position, nodes[result.indices[r+1]].position) if len(result.indices) > 1: bestHeading = Vec2d(*nodes[result.indices[1]].position) - self.position else: bestHeading = self.goal - self.position else: bestHeading = self.goal - self.position neighborData = [] dD = computeDelaunayTriangulation(dPoints) for d in dD: nbr = False if(d[0] == 0): nbr = True nbrIndexB = d[1] nbrIndexC = d[2] elif(d[1] == 0): nbr = True nbrIndexB = d[0] nbrIndexC = d[2] elif(d[2] == 0): nbr = True nbrIndexB = d[0] nbrIndexC = d[1] if(nbr): pygame.draw.aaline(debugsurface, (255,0,0,255), dPoints[0], dPoints[nbrIndexB]) pygame.draw.aaline(debugsurface, (255,0,0,255), dPoints[0], dPoints[nbrIndexC]) if(nbrIndexB - 2 < numPeds): # offset by 2 because first two dPoints are agent pos and goal velB = tuple(view.pedestrians[nbrIndexB-2].velocity) else: velB = [0,0] #fixed obstacle (or goal) if(nbrIndexC - 2 < numPeds): velC = tuple(view.pedestrians[nbrIndexC-2].velocity) else: velC = [0,0] neighborData.append([dPoints[nbrIndexB], dPoints[nbrIndexC], velB, velC]) maxAreaDot = -1e100 Ax = self.position.x Ay = self.position.y AxDot = self.velocity.x AyDot = self.velocity.y for n in neighborData: Bx = n[0][0] By = n[0][1] BxDot = n[2][0] ByDot = n[2][1] Cx = n[1][0] Cy = n[1][1] CxDot = n[3][0] CyDot = n[3][1] midPoint = [(Bx+Cx)/2,(By+Cy)/2] midPointHeading = Vec2d(*midPoint) - self.position areaDot = abs(AxDot*By + Ax*ByDot + BxDot*Cy + Bx*CyDot + CxDot*Ay + Cx*AyDot - AyDot*Bx - Ay*BxDot - ByDot*Cx - By*CxDot - CyDot*Ax - Cy*AxDot) * pow((1+math.cos(bestHeading.angle(midPointHeading)))/2,1) #the higher the power, the more focused the weighting is on the goalpoint pygame.draw.polygon(debugsurface, (200, 100, 130, int(min(areaDot/4,255))), [[Ax, Ay], [Bx, By], [Cx, Cy]]) if areaDot > maxAreaDot: maxAreaDot = areaDot maxMidPoint = midPoint self.waypoint_clear() self.waypoint_push(self.position) self.waypoint_push(Vec2d(*maxMidPoint))