def get_centroid_farthest(grid, start, idds):
"""
===========================================================================
Description: Return Centroid Idd of Set of Idds (farhets if many).
===========================================================================
Arguments:
---------------------------------------------------------------------------
1. grid : Grid.
2. idds : set of int (Set of Nodes Idds).
===========================================================================
Return:
---------------------------------------------------------------------------
1. int : Centroid.
2. int : Total Distances from Centroid to Idds.
===========================================================================
"""
centroid = None
total_min = float('Infinity')
idds = set(idds)
for idd in idds:
total = get_total_distances_to(grid, idd, idds - {idd})
if (total < total_min):
centroid = idd
total_min = total
if (total == total_min):
if (u_grid.manhattan_distance(grid, start, idd) >
u_grid.manhattan_distance(grid, start, centroid)):
centroid = idd
return centroid, total_min
def get_far_node(grid, src, dist_min, dist_max=float('Infinity')):
"""
===========================================================================
Description:
---------------------------------------------------------------------------
1. Get Grid, Source Node and Distances Boundaries.
2. Return Random Valid Idd within the Boundaries.
===========================================================================
Arguments:
---------------------------------------------------------------------------
1. grid : Grid
2. src : Node
3. dist_min : int
4. dist_max : float
===========================================================================
Return: Random and Valid Node Idd (int).
===========================================================================
"""
idds = u_grid.get_valid_idds(grid)
idds_valid = list()
for idd in idds:
dist = u_grid.manhattan_distance(grid, src, idd)
if (dist >= dist_min) and (dist <= dist_max):
idds_valid.append(idd)
random.shuffle(idds_valid)
return idds_valid[0]
def get_farthest(grid, src, dests):
"""
===========================================================================
Description: Return the Farthest Idd to the Src (random if there are many).
===========================================================================
Arguments:
---------------------------------------------------------------------------
1. grid : Grid.
2. src : int (Node Idd).
3. dests : set of int (Nodes Idd).
===========================================================================
Return:
---------------------------------------------------------------------------
1. nearest : int (Farthest Node Idd).
2. distance_farthest : int (Distance from Src to the Farthest Dest).
===========================================================================
"""
farthest = None
distance_farthest = 0
for dest in dests:
distance = u_grid.manhattan_distance(grid, src, dest)
if (distance >= distance_farthest):
farthest = dest
distance_farthest = distance
return farthest, distance_farthest
def gen_random_pairs(grid, distance_min, epochs):
"""
===========================================================================
Description: Generate Random Pairs with Epochs where Min Distance.
===========================================================================
Arguments:
---------------------------------------------------------------------------
1. grid : Grid.
2. distance_min : int (Min Distance between the Pair Idds).
3. epochs : int (Amount of Attempts).
===========================================================================
Return: set of tuples (int,int) : Random Pairs.
===========================================================================
"""
idds = u_grid.get_valid_idds(grid)
pairs = set()
if len(idds) < 2:
return pairs
for i in range(epochs):
random.shuffle(idds)
idd_1 = idds[0]
idd_2 = idds[1]
if (u_grid.manhattan_distance(grid, idd_1, idd_2) >= distance_min):
pairs.add((idd_1, idd_2))
return pairs
def tester_get_path():
grid = u_grid.gen_symmetric_grid(4)
start = 0
goal = 12
astar = KAStar(grid, start, {goal})
astar.run()
optimal_path = [0, 4, 8, 12]
p1 = astar.get_path(goal) == optimal_path
grid = u_grid.gen_symmetric_grid(4)
grid[1][1] = -1
grid[2][1] = -1
start = 8
goal = 10
astar = KAStar(grid, start, {goal})
astar.run()
optimal_path = [8, 12, 13, 14, 10]
p2 = astar.get_path(goal) == optimal_path
p3 = True
for i in range(1000):
n = u_random.get_random_int(4, 4)
grid = u_grid.gen_symmetric_grid(n)
idds_valid = u_grid.get_valid_idds(grid)
random.shuffle(idds_valid)
start = idds_valid[0]
goals = idds_valid[1:3]
kastar = KAStar(grid, start, goals)
kastar.run()
for goal in goals:
len_optimal = u_grid.manhattan_distance(grid, start, goal) + 1
if len(kastar.get_path(goal)) != len_optimal:
p3 = False
print('start={0}'.format(start))
print('goal={0}'.format(goals))
print('grid:')
for row in range(grid.shape[0]):
li = list()
for col in range(grid.shape[1]):
li.append(grid[row][col])
li = [str(x) for x in li]
print(','.join(li))
print('goal[{0}]: {1}'.format(goal, kastar.get_path(goal)))
fname = sys._getframe().f_code.co_name[7:]
if (p1 and p2 and p3):
print('OK: {0}'.format(fname))
else:
print('Failed: {0}'.format(fname))
def _update_node(self, node, father, g):
"""
=======================================================================
Description: Update Node.
=======================================================================
Attributes:
-----------------------------------------------------------------------
1. node : Node (node to update)
2. father : Node
3. g : int
4. goals : set of int (active goals)
=======================================================================
"""
node.father = father
node.g = g
h = u_grid.manhattan_distance(self.grid,node.idd,self.goal)
node.f = node.g + h
def get_total_distances_to(grid, src, dests):
"""
===========================================================================
Description: Return a Total Distances from Source to Destinations.
===========================================================================
Arguments:
---------------------------------------------------------------------------
1. grid : Grid.
2. src : int (Node Idd).
3. dests : set of int (Nodes Idds).
===========================================================================
Return: int (Total Distances from Source to Destinations).
===========================================================================
"""
total = 0
for dest in dests:
total += u_grid.manhattan_distance(grid, src, dest)
return total
def tester_get_path():
grid = u_grid.gen_symmetric_grid(4)
start = 0
goal = 12
astar = AStar(grid,start,goal)
astar.run()
optimal_path = [0,4,8,12]
p1 = astar.get_path() == optimal_path
grid = u_grid.gen_symmetric_grid(4)
grid[1][1] = -1
grid[2][1] = -1
start = 8
goal = 10
astar = AStar(grid,start,goal)
astar.run()
optimal_path = [8,12,13,14,10]
p2 = astar.get_path() == optimal_path
p3 = True
for i in range(1000):
n = u_random.get_random_int(3,10)
grid = u_grid.gen_symmetric_grid(n)
idds_valid = u_grid.get_valid_idds(grid)
random.shuffle(idds_valid)
start = idds_valid[0]
goal = idds_valid[1]
astar = AStar(grid,start,goal)
astar.run()
len_optimal = u_grid.manhattan_distance(grid,start,goal)+1
if len(astar.get_path()) != len_optimal:
p3 = False
fname = sys._getframe().f_code.co_name[7:]
if (p1 and p2 and p3):
print('OK: {0}'.format(fname))
else:
print('Failed: {0}'.format(fname))
def _update_node(self, node, father, g, goals):
"""
=======================================================================
Description: Update Node.
=======================================================================
Attributes:
-----------------------------------------------------------------------
1. node : Node (node to update)
2. father : Node
3. g : int
4. goals : set of int (active goals)
=======================================================================
"""
node.father = father
node.g = g
h = float('Infinity')
for goal in goals:
h_cur = u_grid.manhattan_distance(self._grid, node.idd, goal)
self.counter_heuristic += 1
if (h_cur < h):
h = h_cur
node.h = h
node.f = node.g + h
def get_expanded_nodes(grid, grid_g, grid_h, goal):
"""
===========================================================================
Description: Return the Number of Expanded Nodes of A* Algorithm.
===========================================================================
Arguments:
---------------------------------------------------------------------------
1. grid_g : Grid of G (of Start).
2. grid_h : Grid of H (of Goal).
3. goal : int (Idd number).
===========================================================================
Return: Set of Idds (expanded nodes of A* algorithm).
===========================================================================
"""
grid_f = grid_g + grid_h
row, col = u_grid.to_row_col(grid_f, goal)
f = grid_f[row][col]
grid_f_less = grid * (grid_f < f)
expanded_nodes = set(np.unique(grid_f_less)) - {-1}
grid_f_equal = grid * (grid_f == f)
candidate_idds = list(np.unique(grid_f_equal))
queue_nodes = queue.PriorityQueue()
for idd in candidate_idds:
row, col = u_grid.to_row_col(grid, idd)
queue_nodes.put((-grid_g[row][col], idd))
idd_cur = queue_nodes.get()
expanded_nodes.add(idd_cur[1])
while not queue_nodes.empty():
idd = queue_nodes.get()
if u_grid.manhattan_distance(grid, idd_cur[1], idd[1]) == 1:
expanded_nodes.add(idd[1])
idd_cur = idd
expanded_nodes.remove(0)
return expanded_nodes
def gen_grid_h(grid, goal, lookups=set(), grid_lookup=None):
"""
===========================================================================
Description: Generate Grid of Heuristic values for Idd_1.
===========================================================================
Arguments:
---------------------------------------------------------------------------
1. grid : Serialized Grid.
2. goal : int (Node's Id).
3. lookup : Set of Idds.
4. grid_lookup : Grid with True Heuristic.
===========================================================================
Return: Grid of Heuristic values for Idd_1.
===========================================================================
"""
grid_h = np.full([grid.shape[0], grid.shape[1]], -1, dtype=int)
for idd in range(grid.size):
row, col = u_grid.to_row_col(grid, idd)
if grid[row][col] >= 0:
if idd in lookups:
grid_h[row][col] = grid_lookup[row][col]
else:
grid_h[row][col] = u_grid.manhattan_distance(grid, idd, goal)
return grid_h