if dist[u] == float('inf') or u == goal: break for v in u.neighbors.values(): alt = dist[u] + u.move_cost(v) if alt < dist[v]: dist[v] = alt previous[v] = u s, u = deque(), goal while previous[u]: s.appendleft(u) u = previous[u] s.appendleft(u) return s pp.register_search_method('Dijkstra', search_dijkstra) def search_dijkstra_mesh(origin, goal, mesh, heur=None): """ Executes the Dijkstra path planning algorithm over a mesh. Inputs: - origin: node at which to start. - goal: node that needs to be reached. - mesh: mesh over which to perform the algorithm. - heur: reference to a string representing an heuristic. Unused, kept to standarize input. Outputs: - ordered list of nodes representing the path found from origin to goal. """ dist = {v: float('inf') for v in mesh.values()}
if current.G + current.move_cost(node) < node.G: node.G = current.G + current.move_cost(node) node.parent = current else: #If it isn't in the open set, calculate the G and H score for the node node.G = current.G + current.move_cost(node) node.H = pp.heuristic[heur](node, goal) #Set the parent to our current item node.parent = current #Add it to the set openset.add(node) #Throw an exception if there is no path raise ValueError('No Path Found') pp.register_search_method('theta*', thetaStar) def lineOfSight(current, node, grid): """ Determina si dos puntos tienen linea de vision. Inputs: - current: punto 1. - node: punto 2. Outputs: - si hay una linea dee vision disponible entre ambos puntos. """ x0, y0 = current.grid_point x1, y1 = node.grid_point dist_y = y1 - y0 dist_x = x1 - x0
if node.G > new_g: #If so, update the node to have a new parent node.G = new_g node.parent = current else: #If it isn't in the open set, calculate the G and H score for the node node.G = current.G + current.move_cost(node) node.H = pp.heuristic[heur](node, goal) #Set the parent to our current item node.parent = current #Add it to the set openset.add(node) #Throw an exception if there is no path raise ValueError('No Path Found') pp.register_search_method('A*', aStar) def aStar_mesh(start, goal, grid, heur='naive'): """ Executes the A* path planning algorithm over a given nav mesh. Inputs: - start: node from which to start. - goal: node to which it is desired to arrive. - grid: mesh over which to execute the algorithm - heur: heuristic function to use for the algorithm, expressed as a string. Results will vary depending on it. Must be implemented separatedly. Outputs: - ordered list of nodes representing the shortest path found from start to goal. """
node.H = pp.heuristic[heur](node, goal, scale) node.parent = current.parent opened.add(node) else: new_g = current.G + current.move_cost(node) if node in opened: if node.G > new_g: node.G = new_g node.parent = current else: node.G = new_g node.H = pp.heuristic[heur](node, goal, scale) node.parent = current opened.add(node) raise ValueError('No Path Found') pp.register_search_method('theta', theta) def is_obstacle(x, y): return pp.npdata[int(x)][int(y)]==0 def line_of_sight (n0, n1): x0, y0 = n0.point x1, y1 = n1.point dx = x1 - x0 dy = y1 - y0 f = 0 if dy < 0: dy = -dy sy = -1 else: sy = 1
# If so, update the node to have a new parent node.G = new_g node.parent = current_node else: # If it isn't in the open set, calculate the G and H score for the node node.G = current_node.G + current_node.move_cost(node) node.H = pp.heuristic[heur](node, goal) # Set the parent to our current_node item node.parent = current_node # Add it to the set open_set.add(node) # Throw an exception if there is no path raise ValueError('No Path Found') pp.register_search_method('T*', tStar) def tStar_mesh(start, goal, grid, heur='naive'): """ Executes the A* path planning algorithm over a given nav mesh. Inputs: - start: node from which to start. - goal: node to which it is desired to arrive. - grid: mesh over which to execute the algorithm - heur: heuristic function to use for the algorithm, expressed as a string. Results will vary depending on it. Must be implemented separately. Outputs: - ordered list of nodes representing the shortest path found from start to goal.
if node.G > new_g: node.G = new_g node.parent = current else: #If it isn't in the open set, calculate the G and H score for the node node.G = current.G + current.move_cost(node) node.H = pp.heuristic[heur](node, goal) #Set the parent to our current item node.parent = current #Add it to the set openset.add(node) #Throw an exception if there is no path raise ValueError('No Path Found') pp.register_search_method('Theta*', thetaStar) def thetaStar_mesh(start, goal, grid, heur='naive'): #The open and closed sets openset = set() closedset = set() #Current point is the starting point current = start #Add the starting point to the open set openset.add(current) #While the open set is not empty while openset: #Find the item in the open set with the lowest G + H score current = min(openset, key=lambda o: o.G + o.H)