def compute_neighbours(self): for node_a in self.nodes: for node_b in self.nodes: if node_a == node_b: continue node_within_polygon = False for polygon in self.polygons: if polygon.contains_point(node_a) or polygon.contains_point(node_b): node_within_polygon = True if node_within_polygon: continue if not intersect_polygons([node_a, node_b], self.polygons): self.neighbors[node_a].append(node_b)
def compute_neighbours(self): for node_a in self.nodes: for node_b in self.nodes: if node_a == node_b: continue node_within_polygon = False for polygon in self.polygons: if polygon.contains_point( node_a) or polygon.contains_point(node_b): node_within_polygon = True if node_within_polygon: continue if not intersect_polygons([node_a, node_b], self.polygons): self.neighbors[node_a].append(node_b)
def init_start_goal(self, start_node, goal_node): self.clean_start_node = True self.clean_goal_node = True for start_goal_node in [start_node, goal_node]: for node in self.nodes: if start_goal_node == node: if start_goal_node == start_node: self.clean_start_node = False elif start_goal_node == goal_node: self.clean_goal_node = False elif not intersect_polygons([start_goal_node, node], self.polygons): if node not in self.neighbors[start_goal_node]: self.neighbors[start_goal_node].append(node) if start_goal_node not in self.neighbors[node]: self.neighbors[node].append(start_goal_node)
def find_path(self, x1, y1, x2, y2): start_node = Vec2d(x1, y1) goal_node = Vec2d(x2, y2) if not intersect_polygons([start_node, goal_node], self.polygons): return [goal_node] for polygon in self.polygons: if polygon.contains_point(Vec2d(x2, y2)): return None self.init_start_goal(start_node, goal_node) closed_set = set() open_set = set([start_node]) path_map = {} gx_map = {start_node: 0} hx_map = {} while len(open_set) > 0: current_node = min( open_set, key=lambda node: gx_map[node] + hx_map.setdefault( node, distance_between(node, goal_node))) if current_node == goal_node: break open_set.remove(current_node) closed_set.add(current_node) neighbors = self.neighbors[current_node] for neighbor in neighbors: if neighbor in closed_set: continue gx = distance_between(neighbor, current_node) + gx_map[current_node] hx = hx_map.setdefault(neighbor, distance_between(neighbor, goal_node)) if neighbor in open_set: if gx < gx_map[neighbor]: gx_map[neighbor] = gx path_map[neighbor] = current_node else: gx_map[neighbor] = gx path_map[neighbor] = current_node open_set.add(neighbor) if current_node != goal_node: return None path = [] path_node = goal_node while path_node is not None: path.append(path_node.copy()) path_node = path_map.get(path_node) # Remove the starting node path.pop() path.reverse() self.cleanup_start_goal(start_node, goal_node) return path
def find_path(self, x1, y1, x2, y2): start_node = Vec2d(x1, y1) goal_node = Vec2d(x2, y2) if not intersect_polygons([start_node, goal_node], self.polygons): return [goal_node] for polygon in self.polygons: if polygon.contains_point(Vec2d(x2, y2)): return None self.init_start_goal(start_node, goal_node) closed_set = set() open_set = set([start_node]) path_map = {} gx_map = { start_node: 0 } hx_map = {} while len(open_set) > 0: current_node = min(open_set, key=lambda node:gx_map[node] + hx_map.setdefault(node, distance_between(node, goal_node))) if current_node == goal_node: break open_set.remove(current_node) closed_set.add(current_node) neighbors = self.neighbors[current_node] for neighbor in neighbors: if neighbor in closed_set: continue gx = distance_between(neighbor, current_node) + gx_map[current_node] hx = hx_map.setdefault(neighbor, distance_between(neighbor, goal_node)) if neighbor in open_set: if gx < gx_map[neighbor]: gx_map[neighbor] = gx path_map[neighbor] = current_node else: gx_map[neighbor] = gx path_map[neighbor] = current_node open_set.add(neighbor) if current_node != goal_node: return None path = [] path_node = goal_node while path_node is not None: path.append(path_node.copy()) path_node = path_map.get(path_node) # Remove the starting node path.pop() path.reverse() self.cleanup_start_goal(start_node, goal_node) return path