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)
