def find_path(self, x1, y1, x2, y2):
start_node = Node(x1, y1)
goal_node = Node(x2, y2)
self.add_node(goal_node)
self.compute_neighbours()
# check for direct los from start_node to goal_node
# if not intersect_multi([start_node, goal_node], self.polygons):
# return [start_node, goal_node]
closest_node = self.get_closest_node(start_node)
closed_set = set()
open_set = set()
open_queue = []
# key: node, value: previous_node_in_path
path_map = {}
heappush(open_queue, (0, closest_node))
while len(open_queue) > 0:
current_node_cost, current_node = heappop(open_queue)
if current_node == goal_node:
break
neighbors = current_node.neighbors
for neighbor in neighbors:
if current_node in closed_set:
continue
gx = distance_between(current_node,
neighbor) + current_node_cost
hx = distance_between(neighbor, goal_node)
neighbor_cost = gx + hx
if neighbor in open_set:
if gx < self.gx_map[neighbor]:
self.gx_map[neighbor] = gx
path_map[neighbor] = current_node
else:
self.gx_map[neighbor] = gx
self.hx_map[neighbor] = hx
path_map[neighbor] = current_node
heappush(open_queue, (gx + hx, neighbor))
open_set.add(neighbor)
closed_set.add(current_node)
path_node = goal_node
path = []
while path_node in path_map:
path.append(path_node)
path_node = path_map[path_node]
return path
def find_path(self, x1, y1, x2, y2):
start_node = Node(x1, y1)
goal_node = Node(x2, y2)
self.add_node(goal_node)
self.compute_neighbours()
# check for direct los from start_node to goal_node
# if not intersect_multi([start_node, goal_node], self.polygons):
# return [start_node, goal_node]
closest_node = self.get_closest_node(start_node)
closed_set = set()
open_set = set()
open_queue = []
# key: node, value: previous_node_in_path
path_map = {}
heappush(open_queue, (0, closest_node))
while len(open_queue) > 0:
current_node_cost, current_node = heappop(open_queue)
if current_node == goal_node:
break
neighbors = current_node.neighbors
for neighbor in neighbors:
if current_node in closed_set:
continue
gx = distance_between(current_node, neighbor) + current_node_cost
hx = distance_between(neighbor, goal_node)
neighbor_cost = gx + hx
if neighbor in open_set:
if gx < self.gx_map[neighbor]:
self.gx_map[neighbor] = gx
path_map[neighbor] = current_node
else:
self.gx_map[neighbor] = gx
self.hx_map[neighbor] = hx
path_map[neighbor] = current_node
heappush(open_queue, (gx + hx, neighbor))
open_set.add(neighbor)
closed_set.add(current_node)
path_node = goal_node
path = []
while path_node in path_map:
path.append(path_node)
path_node = path_map[path_node]
return path
def get_closest_node(self, node):
min_dist = distance_between(node, self.nodes[0])
min_node = self.nodes[0]
for current_node in self.nodes:
dist = distance_between(node, current_node)
if dist < min_dist:
min_dist = dist
min_node = node
return min_node
def get_closest_node(self, node):
min_dist = distance_between(node, self.nodes[0])
min_node = self.nodes[0]
for current_node in self.nodes:
dist = distance_between(node, current_node)
if dist < min_dist:
min_dist = dist
min_node = node
return min_node