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
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.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))
