def setOffset(self, offset): self.offset = offset self.partition.stop() self.partition = Partition( offset, self.container.points, self.obstacles )
def __init__(self, offset, xml_filename): self.areas = {} self.vertices = {} self.width = 0 self.height = 0 self.obstacles = [] self.dynamic_obstacles = [] self.offset = offset self.load_xml(xml_filename) self.container = Poly(((0,0),(self.width,0),(self.width,self.height),(0,self.height))) self.partition = Partition( offset, self.container.points, self.obstacles ) self.calc_areas() self.calc_vertex_graph() self.pathfinding_area = AStar(self.areas.values(), NAVGRAPH) self.pathfinding_vertices = AStar(self.vertices.values(), NAVGRAPH)
class NavGraph: def __init__(self, offset, xml_filename): self.areas = {} self.vertices = {} self.width = 0 self.height = 0 self.obstacles = [] self.dynamic_obstacles = [] self.offset = offset self.load_xml(xml_filename) self.container = Poly(((0,0),(self.width,0),(self.width,self.height),(0,self.height))) self.partition = Partition( offset, self.container.points, self.obstacles ) self.calc_areas() self.calc_vertex_graph() self.pathfinding_area = AStar(self.areas.values(), NAVGRAPH) self.pathfinding_vertices = AStar(self.vertices.values(), NAVGRAPH) def setOffset(self, offset): self.offset = offset self.partition.stop() self.partition = Partition( offset, self.container.points, self.obstacles ) def calc_areas(self): for k in tuple(self.areas.keys()): self.areas.pop(k) request = self.partition.obstacles_to_str(list(map(lambda p: p.points, self.dynamic_obstacles))) self.partition.calc(request) for i,poly in self.partition.polygons.items(): self.areas[i] = Area(poly.points) self.areas[i].color = 'black' self.areas[i].outline = 'white' for i,poly in self.partition.polygons.items(): neighbors = [ self.areas[neighbor] for neighbor in poly.neighbors ] self.areas[i].init(i, self.areas[i].middle, neighbors) def calc_vertex_graph(self): for k in tuple(self.vertices.keys()): self.vertices.pop(k) # création des vertices for area in self.areas.values(): for p in area.points: if p not in self.vertices: self.vertices[p] = VertexNode(p) # ajout des liaisons et des areas for area in self.areas.values(): for p in area.points: self.vertices[p].areas.append(area) for pp in area.points: if pp != p: self.vertices[p].neighbors.append(self.vertices[pp]) # tri des areas dans le sens trigo for vertex in self.vertices.values(): vertex.areas.sort(key=lambda a: (a.pos - vertex.pos).angle()) def load_xml(self, filename): """ Charger une carte version xml """ self.width, self.height, polys = load_xml(filename) self.obstacles = list([ poly.points for poly in polys]) def get_path(self, p_depart, p_arrive, enable_smooth=True): return self.get_path_vertex_mode(p_depart, p_arrive, enable_smooth) #return self.get_path_areas_mode(p_depart, p_arrive) def get_path_vertex_mode(self, p_depart, p_arrive, enable_smooth=True): p_depart = Vec(p_depart) p_arrive = Vec(p_arrive) area_depart = self.find_area_for_point(p_depart) area_arrive = self.find_area_for_point(p_arrive) remove_arrive_at_end = True # le point d'arrivé est inateignable if not area_arrive: return [],[],[] # le point de départ est déjà dans les vertices (très peut probable) if p_depart in self.vertices: vertex_depart = self.vertices[p_depart] # sinon, on doit le rajouter 'à la main', puis le supprimer après le traitement else: vertex_depart = Node() vertex_depart.init(p_depart.__hash__(),p_depart, []) # si le point est dans une zone if area_depart: for p in area_depart.points: vertex_depart.neighbors.append(self.vertices[p]) # sinon, il est dans une zone interdite, on va chercher le chemin le plus court pour sortir else: near_vertex = min(self.vertices.values(), key=lambda vertex: (p_depart - vertex.pos).norm2()) vertex_depart.neighbors.append(self.vertices[near_vertex.pos]) if p_arrive not in self.vertices: vertex_arrive = Node() vertex_arrive.init(p_arrive.__hash__(),p_arrive, []) for p in area_arrive.points: self.vertices[p].neighbors.append(vertex_arrive) remove_arrive_at_end = True # application du pathfinding vertices, raw_path = self.pathfinding_vertices.compute_path(vertex_depart, vertex_arrive) if not enable_smooth: return [], vertices, raw_path # remove des vertices ajoutées if remove_arrive_at_end: for p in area_arrive.points: self.vertices[p].neighbors.remove(vertex_arrive) # smooth path if len(vertices) > 2: smooth_path = raw_path id_areas = set() for vertex in vertices[1:-1]: for area in vertex.areas: id_areas.add(area.id) for area in self.areas.values(): if area.id not in id_areas: area.walkable = False if not area_depart: # le départ est en zone interdite area_depart = min(vertices[1].areas, key=lambda a: (a.middle - p_arrive).norm2()) _areas, _raw_path, smooth_path = self.get_path_areas_mode(p_depart, p_arrive, area_depart) for area in self.areas.values(): area.walkable = True elif len(vertices) == 2: smooth_path = raw_path else: raw_path = [] smooth_path = [] return vertices, raw_path, smooth_path def get_path_areas_mode(self, p_depart, p_arrive, area_depart=None): p_depart = Vec(p_depart) p_arrive = Vec(p_arrive) area_depart = self.find_area_for_point(p_depart) if not area_depart else area_depart area_arrive = self.find_area_for_point(p_arrive) # le point d'arrivé est inateignable if not area_arrive: return [],[],[] # aucune aire n'a été trouvée pour le point de départ, on va prendre la plus proche if not area_depart: area_depart = min(self.areas.values(), key=lambda a: (a.middle - p_depart).norm2()) #application du pathfinding areas, raw_path = self.pathfinding_area.compute_path(area_depart, area_arrive) # smooth path if len(areas) > 1: portal_edges = polys_to_portal_edges(areas) smooth_path = funnel(p_depart, p_arrive, portal_edges) else: raw_path = [p_depart, p_arrive] smooth_path = [p_depart, p_arrive] return areas,raw_path,smooth_path def find_area_for_point(self, p): for area in self.areas.values(): if p in area: return area def get_polygons(self): l = [] for n in self.vertices.values(): p = Poly().initFromCircle(n.pos, 20, 5) p.color = 'yellow' l.append(p) ll = [] for v in self.vertices.values(): for n in v.neighbors: p = Poly((n.pos,n.pos+(1,0),v.pos+(1,0),v.pos)) p.outline = 'green' ll.append(p) self.container.neighbors = [] return [self.container] + list(self.areas.values()) + l + ll def add_dynamic_obstacle(self, poly): self.dynamic_obstacles.append(poly) def pop_dynamic_obstable(self): self.dynamic_obstacles.pop() def update(self): """ fonction à appeller après avoir bougé un obstacle dynamic """ self.calc_areas() self.calc_vertex_graph() @property def edges(self): segments = set() for area in self.areas.values(): for edge in area.iedges(): segments.add(edge) return list(segments)