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 init(self):
if self.parameter is None:
self.parameter = 10
self.agent.position = Vec2d(200, 200)
self.agent.goal = Vec2d(400, 200)
self.agent.angle = 0
shapes = [
[
Vec2d(100, 100),
Vec2d(100, 380),
Vec2d(500, 380),
Vec2d(500, 100),
Vec2d(100, 100),
],
[
Vec2d(300, 100),
Vec2d(300, 250),
],
[
Vec2d(200, 250),
Vec2d(400, 250),
]
]
for shape in shapes:
last = shape[0]
for point in shape:
self.world.add_obstacle(obstacle.Line(last, point))
last = point
for m in xrange(self.parameter):
good = False
u = RandomWalkingAvoider(random_seed=self.random.random())
# generate random positions for pedestrians
# that are not inside obstacles...
while not good:
init_position = Vec2d(
self.random.randrange(100 + u.radius + 1, 500 - u.radius - 1),
self.random.randrange(100 + u.radius + 1, 380 - u.radius - 1),
)
good = init_position.distance_to(self.agent.position) > 20
for shape in shapes:
last = shape[0]
for point in shape:
if linesegdist2(
last, point,
init_position
) < u.radius ** 2:
good = False
break
last = point
if not good:
break
u.position = init_position
self.world.add_unit(u)
def freeprob_turn(self, position, a1, a2, view, starttime):
""" Probability that a turn started at starttime at position between angles a1 and a2 will be collision free """
dur = abs(angle_diff(a1, a2)) / self.turningspeed
for p in view.pedestrians:
p1 = p.position
p2 = self.future_position(p, dur) # extrapolate
if linesegdist2(p1, p2, position) < (self.radius + p.radius + self.FREEMARGIN) ** 2:
self.freeprob_fail_pedestrian = p
return 0
return 1
def turn_safeness(self, position, a1, a2, view, starttime):
""" Probability that a turn is collision free
Started at starttime at position between
angles a1 and a2.
"""
dur = abs(angle_diff(a1, a2)) / self.turningspeed
for p in view.pedestrians:
# extrapolate to start of turn
p1 = self.future_position(p, starttime)
# extrapolate to end of turn
p2 = self.future_position(p, starttime + dur)
safedist2 = (self.radius + p.radius + self.FREEMARGIN) ** 2
if linesegdist2(p1, p2, position) < safedist2:
self.safeness_fail_pedestrian = p
return 0
return 1
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