def h_walldist(state, fline, walls): """ The first time this function is called, for each gridpoint that's not inside a wall it will cache a rough estimate of the length of the shortest path to the finish line. The computation is done by a breadth-first search going backwards from the finish line, one gridpoint at a time. On all subsequent calls, this function will retrieve the cached value and add an estimate of how long it will take to stop. """ global g_fline, g_walls if fline != g_fline or walls != g_walls or grid == []: edist_grid(fline, walls) ((x,y),(u,v)) = state hval = float(grid[x][y]) # add a small penalty to favor short stopping distances au = abs(u); av = abs(v); sdu = au*(au-1)/2.0 sdv = av*(av-1)/2.0 sd = max(sdu,sdv) penalty = sd/10.0 # compute location after fastest stop, and add a penalty if it goes through a wall if u < 0: sdu = -sdu if v < 0: sdv = -sdv sx = x + sdu sy = y + sdv if racetrack.crash([(x,y),(sx,sy)],walls): penalty += math.sqrt(au**2 + av**2) hval = max(hval+penalty,sd) return hval
def edist_grid(fline, walls): global grid, g_fline, g_walls, xmax, ymax xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls]) ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls]) grid = [[edistw_to_finish((x, y), fline, walls) for y in range(ymax + 1)] for x in range(xmax + 1)] flag = True print('computing edist grid', end=' ') sys.stdout.flush() while flag: print('.', end='') sys.stdout.flush() flag = False for x in range(xmax + 1): for y in range(ymax + 1): for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)): for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)): if grid[x1][y1] != infinity and not racetrack.crash( ((x, y), (x1, y1)), walls): if x == x1 or y == y1: d = grid[x1][y1] + 1 else: # In principle, it seems like a taxicab metric should be just as # good, but Euclidean seems to work a little better in my tests. d = grid[x1][y1] + 1.4142135623730951 if d < grid[x][y]: grid[x][y] = d flag = True print(' done') g_fline = fline g_walls = walls return grid
def xymax_grid(fline, walls): global grid, g_metric, g_fline, g_walls, xmax, ymax xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls]) ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls]) grid = [[xymaxw_to_line((x, y), fline, walls) for y in range(ymax + 1)] for x in range(xmax + 1)] flag = True print('computing xymax grid', end=' ') sys.stdout.flush() while flag: print('.', end='') sys.stdout.flush() flag = False for x in range(xmax + 1): for y in range(ymax + 1): for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)): for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)): if grid[x1][y1] != infinity and not racetrack.crash( ((x, y), (x1, y1)), walls): d = grid[x1][y1] + max(abs(x - x1), abs(y - y1)) if d < grid[x][y]: grid[x][y] = d flag = True print(' done') g_metric = 'xymax' g_fline = fline g_walls = walls return grid
def edist_grid(fline,walls): global grid, g_fline, g_walls, xmax, ymax xmax = max([max(x,x1) for ((x,y),(x1,y1)) in walls]) ymax = max([max(y,y1) for ((x,y),(x1,y1)) in walls]) grid = [[edistw_to_line((x,y), fline, walls) for y in range(ymax+1)] for x in range(xmax+1)] flag = True print('computing edist grid', end=' '); sys.stdout.flush() while flag: print('.', end=''); sys.stdout.flush() flag = False for x in range(xmax+1): for y in range(ymax+1): for y1 in range(max(0,y-1),min(ymax+1,y+2)): for x1 in range(max(0,x-1),min(xmax+1,x+2)): if grid[x1][y1] != infinity and not racetrack.crash(((x,y),(x1,y1)),walls): if x == x1 or y == y1: d = grid[x1][y1] + 1 else: d = grid[x1][y1] + 1.4142135623730951 if d < grid[x][y]: grid[x][y] = d flag = True print(' done') g_fline = fline g_walls = walls return grid
def dist(pt1, pt2, walls): if not racetrack.crash((pt1, pt2), walls): (x1, y1) = pt1 (x2, y2) = pt2 return math.sqrt((x1 - x2)**2 + (y1 - y2)**2) else: return infinity
def cost_to_go(s, z, finish, walls): """ calculate cost to go Q(s,z) s:current state, z:velocity (u,v) """ (u, v) = z # next correct state without error: (x_correct, y_correct) = (s[0][0]+u, s[0][1]+v) # probability mass function: u_same = 0.6 if abs(u) > 1 else 1 u_diff = 0.2 v_same = 0.6 if abs(v) > 1 else 1 v_diff = 0.2 cost = 0 for ((x_next, y_next), z_next) in next_possible_states_with_z(s, z): weight = (u_same * (x_next == x_correct) + \ u_diff * (x_next != x_correct)) * \ (v_same * (y_next == y_correct) + \ v_diff * (y_next != y_correct)) s_next = ((x_next,y_next),z_next) # penalize not moving if not at the goal state if not racetrack.goal_test(s_next,finish): if z_next == (0,0): V[s_next] = 100 if racetrack.crash((s[0],s_next[0]), walls): V[s_next] = 100 cost += weight * V[s_next] return (1 + cost)
def h_proj1(state, fline, walls): ''' The first time this function is called, for points near all corners in the map it will cacluate its distance to the finish line. It does this by seeing what the nearest corner point that can reach the finish line with a euclidean distance without crashing and stores that distance in a stack. It will then find the closest corner point that can reach the point at the top of the stack and add it's euclidean distance to that point + the distance fromt that point to the finish line. By then end most/all of points near corner will have an estimate of it's distance to the finish line. At each state, the heurisitc will find the nearest corner point it can go to, and add it's euclidean distance to it + the points distance to the finish line. ''' global s_corners, xmax, ymax, stack corner = None if not corners: print("filling in corners -------------------------------------------") corn(walls, fline) ((x, y), (u, v)) = state ((x1, y1), (x2, y2)) = fline ''' If the state has a straight line distance to to the finish line without any walls between, use the euclidean heuristic provided ''' if heuristics.edistw_to_finish((x, y), fline, walls) != infinity: return heuristics.h_esdist(state, fline, walls) else: mininum = math.inf for i, c in enumerate(stack): distance_to_corner = heuristics.edistw_to_finish( (x, y), [(c[0][0], c[0][1]), (c[0][0], c[0][1])], walls) corner_to_fline = c[1] d = distance_to_corner + corner_to_fline if d <= mininum: corner = c mininum = d index = i # add a small penalty to favor short stopping distances au = abs(u) av = abs(v) sdu = au * (au - 1) / 2.0 sdv = av * (av - 1) / 2.0 sd = max(sdu, sdv) penalty = sd / 10.0 # compute location after fastest stop, and add a penalty if it goes through a wall if u < 0: sdu = -sdu if v < 0: sdv = -sdv sx = x + sdu sy = y + sdv if racetrack.crash([(x, y), (sx, sy)], walls): penalty += math.sqrt(au**2 + av**2) hval = max(mininum + penalty, sd) return hval
def edistw_to_line(point, edge, walls): """ straight-line distance from (x,y) to the line ((x1,y1),(x2,y2)). Return infinity if there's no way to do it without intersecting a wall """ # if min(x1,x2) <= x <= max(x1,x2) and min(y1,y2) <= y <= max(y1,y2): # return 0 (x,y) = point ((x1,y1),(x2,y2)) = edge if x1 == x2: ds = [math.sqrt((x1-x)**2 + (y3-y)**2) \ for y3 in range(min(y1,y2),max(y1,y2)+1) \ if not racetrack.crash(((x,y),(x1,y3)), walls)] else: ds = [math.sqrt((x3-x)**2 + (y1-y)**2) \ for x3 in range(min(x1,x2),max(x1,x2)+1) \ if not racetrack.crash(((x,y),(x3,y1)), walls)] ds.append(infinity) return min(ds)
def edistw_to_finish(point, fline, walls): """ straight-line distance from (x,y) to the finish line ((x1,y1),(x2,y2)). Return infinity if there's no way to do it without intersecting a wall """ (x, y) = point ds = infinity ((x1, y1), (x2, y2)) = fline # make a list of distances to each reachable point in fline if x1 == x2: # fline is vertical, so iterate over y for y3 in range(min(y1, y2), max(y1, y2) + 1): if not rt.crash(((x, y), (x1, y3)), walls): ds = min(ds, math.sqrt((x1 - x)**2 + (y3 - y)**2)) else: # fline is horizontal, so iterate over x for x3 in range(min(x1, x2), max(x1, x2) + 1): if not rt.crash(((x, y), (x3, y1)), walls): ds = min(ds, math.sqrt((x3 - x)**2 + (y1 - y)**2)) return ds
def edistw_to_finish(point, fline, walls): """ straight-line distance from (x,y) to the finish line ((x1,y1),(x2,y2)). Return infinity if there's no way to do it without intersecting a wall """ (x, y) = point ((x1, y1), (x2, y2)) = fline # make a list of distances to each reachable point in fline if x1 == x2: # fline is vertical, so iterate over y ds = [math.sqrt((x1 - x) ** 2 + (y3 - y) ** 2) \ for y3 in range(min(y1, y2), max(y1, y2) + 1) \ if not rt.crash(((x, y), (x1, y3)), walls)] else: # fline is horizontal, so iterate over x ds = [math.sqrt((x3 - x) ** 2 + (y1 - y) ** 2) \ for x3 in range(min(x1, x2), max(x1, x2) + 1) \ if not rt.crash(((x, y), (x3, y1)), walls)] ds.append(infinity) # for the case where ds is empty return min(ds)
def UCT(s, depth): global fline, g_walls, Q, t, seen if racetrack.goal_test(s, fline): return -50 if depth == 0: return heuristics.h_walldist(s, fline, g_walls) # if there are no applicable velocities at this state then that means we are going to crash if not applicable(s): return 200 if not s in seen: seen.append(s) t[s] = 0 for z in applicable(s): Q[(s, z)] = 0 t[(s, z)] = 0 untried = list(filter(lambda x: t[(s, x)] == 0, applicable(s))) z_prime = None if untried: z_prime = random.choice(untried) else: #after experminentation, choosing 6 for the value of k seems to converge better x = list( map( lambda x: Q[ (s, x)] - 6 * math.sqrt(math.log(t[s]) / t[(s, x)]), applicable(s))) min_index, min_val = 0, x[0] for i, y in enumerate(x): if y < min_val: min_index, min_val = i, y z_prime = applicable(s)[min_index] (loc, (vx, vy)) = s #(wx,wy) = (z_prime[0]+vx , z_prime[1]+vy) error = supervisor.steering_error(z_prime[0], z_prime[1]) new_state = ((loc[0] + z_prime[0] + error[0], loc[1] + z_prime[1] + error[1]), z_prime) if racetrack.crash((loc, new_state[0]), g_walls): cost = 50 #a penalty else: cost = 1 + UCT(new_state, depth - 1) Q[(s, z_prime)] = (t[(s, z_prime)] * Q[(s, z_prime)] + cost) / (1 + t[(s, z_prime)]) t[s] = t[s] + 1 t[(s, z_prime)] = t[(s, z_prime)] + 1 return cost
def edist_grid(fline, walls): """ This functions creates a grid to cache values in the graph. Walls and unreachable nodes are stored as infinity. The function uses a BFS to traverse the grid and find distance values to each node from the finish line. """ global grid, g_fline, g_walls, xmax, ymax xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls]) ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls]) visited = [] # initialize grid grid = [[infinity for y in range(ymax + 1)] for x in range(xmax + 1)] # get all reachable points from finish and mark as visited for x in range(xmax + 1): for y in range(ymax + 1): grid[x][y] = edistw_to_finish((x, y), fline, walls) if grid[x][y] != infinity: visited.append((x, y)) queue = visited[:] while queue: (x, y) = queue.pop(0) # for each neighbor of the first node in queue for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)): for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)): # if a neighbor is not a wall and not visited # add it to queue and mark as visited # then update grid with new value for (x, y) if not rt.crash(((x, y), (x1, y1)), walls): if (x1, y1) not in visited: queue.append((x1, y1)) visited.append((x1, y1)) if x == x1 or y == y1: d = grid[x1][y1] + 1 else: # In principle, it seems like a taxicab metric should be just as # good, but Euclidean seems to work a little better in my tests. d = grid[x1][y1] + 1.4142135623730951 if d < grid[x][y]: grid[x][y] = d flag = True g_fline = fline g_walls = walls return grid
def h_h2(state, fline, walls, metric='edist', crash_aware=True, fline_aware=True): global g_metric, g_fline, g_walls if fline != g_fline or walls != g_walls or metric != g_metric: # if fline != g_fline: print('fline = ', fline, '; g_fline =', g_fline) # if walls != g_walls: print('walls different') # if metric != g_metric: print('metric = ', metric, '; g_metric =', g_metric) if metric == 'edist': edist_grid(fline, walls) elif metric == 'xymax': xymax_grid(fline, walls) else: raise RuntimeError("'" + metric + "' is not a known metric") ((x, y), (u, v)) = state hval = float(grid[x][y]) if crash_aware or fline_aware: penalty = 0 # compute stopping distance au = abs(u) av = abs(v) sdu = au * (au - 1) / 2.0 sdv = av * (av - 1) / 2.0 sd = max(sdu, sdv) # compute location after fastest stop if u < 0: sdu = -sdu if v < 0: sdv = -sdv sx = x + sdu sy = y + sdv if crash_aware: if racetrack.crash([(x, y), (sx, sy)], walls): if metric == 'edist': penalty += math.sqrt(au**2 + av**2) elif metric == 'xymax': penalty += max(au, av) if fline_aware: penalty += sd / 10.0 hval = max(hval + penalty, sd) return hval
def edist_grid_1(fline, walls): """ This method stores the grid with straight line distance to finish line. This heurstic is used when there exists a wall between start point to finish line. :param fline: :param walls: :return: grid """ global grid, g_fline, g_walls, xmax, ymax xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls]) ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls]) grid = [[edistw_to_finish((x, y), fline, walls) for y in range(ymax + 1)] for x in range(xmax + 1)] flag = True print('computing edist grid_cross', end=' ') sys.stdout.flush() while flag: print('.', end='') sys.stdout.flush() flag = False for x in range(xmax + 1): for y in range(ymax + 1): for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)): for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)): if grid[x1][y1] != infinity and not rt.crash( ((x, y), (x1, y1)), walls): if x == x1 or y == y1: d = grid[x1][y1] + 1 else: d = grid[x1][y1] + 1.4142135623730951 if d < grid[x][y]: grid[x][y] = d flag = True print(' done') g_fline = fline g_walls = walls return grid
def lao_star(s0, finish, walls): """ LAO * algorithm """ Envelope.add(s0) if not next_possible_states(s0, walls): # starting at dead end V[s0] = 100 print("What !!!! The starting point is a dead end!!!") else: V[s0] = heuristics.h_walldist(s0, finish, walls) count = 0 init = False while leaves_not_goal(s0, policy, finish): old_policy = dict(policy) s = leaves_not_goal(s0, policy, finish).pop() if not next_possible_states(s, walls): # dead end V[s] = 100 for s1 in next_possible_states(s, walls) - Envelope: Envelope.add(s1) if racetrack.crash((s[0],s1[0]), walls): V[s1] = 100 else: V[s1] = heuristics.h_walldist(s1, finish, walls) # lao star lao_update(s0, s, finish, walls) # ao_update(s, finish, walls) if(init and old_policy[s0] == policy[s0]): # the new policy doesn't change the action taken at state s0 count += 1 if count > 15: print("Exit !!! ") break init = True return policy
def proj1(state, fline, walls): global g_fline, g_walls, best_pt, best_dis, ref_pts ((x, y), (u, v)) = state #if we have a direct path to the finish, just return the distance, no need for extra computations d = edistw_to_finish((x, y), fline, walls) if d != infinity: hval = float(d) # there is an obstacle in between the state and finish line else: if fline != g_fline or walls != g_walls or ref_pts == []: references(fline, walls) hval = infinity for (d1, x1, y1) in ref_pts: hval = min(hval, d1 + dist((x1, y1), (x, y), walls)) ref_pts.append((hval, x, y)) # add a small penalty to favor short stopping distances au = abs(u) av = abs(v) sdu = au * (au - 1) / 2.0 sdv = av * (av - 1) / 2.0 sd = max(sdu, sdv) penalty = sd / 10.0 # compute location after fastest stop, and add a penalty if it goes through a wall if u < 0: sdu = -sdu if v < 0: sdv = -sdv sx = x + sdu sy = y + sdv if racetrack.crash([(x, y), (sx, sy)], walls): penalty += math.sqrt(au**2 + av**2) hval = max(hval + penalty, sd) return hval
def h_proj1(state, fline, walls): """ This first time this function called will test if there is a striaght line from start to finish line. If straight line exists will use diagonal distance as heuristic. Otherwise will use straight line distance as heuristic. :param state: :param fline: :param walls: :return: hval """ global g_fline, g_walls, first_time, start_x, start_y, reached_fline, straight ((x, y), (u, v)) = state # find closest point from finish line to the current state x_f, y_f = cloest_point((x, y), fline) # calculate diagonal distance if no walls if not rt.crash(((x, y), (x_f, y_f)), walls) and first_time: edist_grid((x, y), fline, walls) start_x = x start_y = y straight = True first_time = False # calculate straight line distance if wall exist if fline != g_fline or walls != g_walls or grid == []: edist_grid_1(fline, walls) first_time = False hval = float(grid[x][y]) # add a small penalty to favor short stopping distances au = abs(u) av = abs(v) sdu = au * (au - 1) / 2.0 sdv = av * (av - 1) / 2.0 sd = max(sdu, sdv) penalty = sd / 10.0 # compute location after fastest stop, and add a penalty if it goes through a wall if u < 0: sdu = -sdu if v < 0: sdv = -sdv sx = x + sdu sy = y + sdv if rt.crash([(x, y), (sx, sy)], walls): penalty += math.sqrt(au**2 + av**2) hval = max(hval + penalty, sd) if not straight: # add reward to favor points that are closer to goal if not go through wall sdist = edist_diagonal((x - u, y - v), fline) tdist = edist_diagonal((x, y), fline) reward = 0 if not rt.crash(((x, y), (x - u, y - v)), walls) and sdist > tdist: reward = -math.sqrt(2) hval += reward if not reached_fline: if rt.intersect(((x - u, y - v), (x, y)), fline): reward = -math.sqrt(2) hval += reward elif straight: # use straight line if no walls in between # tie breaking for multiple similar heuristics cloest_x, cloest_y = cloest_point((x, y), fline) dx1 = x - cloest_x dy1 = y - cloest_y dx2 = start_x - cloest_x dy2 = start_y - cloest_y cross = abs(dx1 * dy2 - dx2 * dy1) hval += cross * 0.001 # if straight line reached goal expand other nodes will have penalty for early stopping if not reached_fline: if point_on_line((x - u, y - v), fline): reached_fline = True if reached_fline: return hval + math.sqrt(2) return hval
def main(state, f_line, walls): """ Uses a trained neural network to return the best velocity value for the program to find a path to the finish line. This function uses the A* algorithm to do this. :param state: The current state :param f_line: The finish line of the arena :param walls: The walls in the arena """ # This is the placeholder variable for the feed-forward inp = tf.placeholder(shape=[1, 16], dtype=tf.float32) L = tf.Variable(tf.random_uniform([16, 4], 0, 0.01)) # Helps the neural network choose actions out = tf.matmul(inp, L) predict = tf.argmax(out, 1) # Get loss next = tf.placeholder(shape=[1, 4], dtype=tf.float32) # Done by adding squares of next minus out loss = tf.reduce_sum(tf.square(next - out)) neur = tf.train.GradientDescentOptimizer(learning_rate=0.2) # Minimizes the loss of the new updated model updateModel = neur.minimize(loss) # Time to use tensorflow for training below (by session) with tf.Session() as sess: # init velocity Z_dict = {} # init global variables for tf sessions sess.run(tf.global_variables_initializer()) # Iterate through state vals i = 0 while i < len(racetrack.next_states(state, walls)): elem = racetrack.next_states(state, walls)[i] i += 1 (a, b) = elem # Set that val to 0 Z_dict[b] = 0 # Iterate through state vals again but for finding paths j = 0 while j < len(racetrack.next_states(state, walls)): elem = racetrack.next_states(state, walls)[j] j += 1 (a, b) = elem # A* algorithm to find route road = route(elem, f_line, walls) numcrashes = 0 # To train network, punish crashes by decrementing pts if road: for point in road: if (racetrack.crash(point, walls)): numcrashes -= 1 # To train network, punish crashes and reward not crashes if (not racetrack.crash(elem, walls)): Z_dict[b] += 2 else: Z_dict[b] -= 5 # Adjust score based on crashes Z_dict[b] += numcrashes # Use constant for finding the best value const = -9999999 (u, v) = (0, 0) # Get the best possible velocity by iterating through the elements in Z_dict keys = Z_dict.keys() for elem in keys: # Get the respective velocity from the appropriate key velocity = Z_dict[elem] # If the curr const value is not the best if const < velocity: (u, v) = elem # this is the best value so far const = velocity # higher const # Return the best velocity value return (u, v)
def corn(walls, fline): ''' This function will cacluate points near all corners of the maps and estimate it's distance to the finish line. ''' global corners, s_corners, xmax, ymax, target_corner, stack xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls]) ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls]) #adding all corners to a corners list for wall in walls: corners.add(wall[0]) corners.add(wall[1]) #add points to the left,right,up, and down of each corner point to s_corners list for c in corners: if not racetrack.crash( [(c[0] + 1, c[1]), (c[0] + 1, c[1])], walls ) and c[0] + 1 > 0 and c[0] + 1 < xmax and c[1] > 0 and c[1] < ymax: s_corners.append((c[0] + 1, c[1])) #right if not racetrack.crash( [(c[0] - 1, c[1]), (c[0] - 1, c[1])], walls ) and c[0] - 1 > 0 and c[0] - 1 < xmax and c[1] > 0 and c[1] < ymax: s_corners.append((c[0] - 1, c[1])) #left if not racetrack.crash( [(c[0], c[1] + 1), (c[0], c[1] + 1)], walls ) and c[0] > 0 and c[0] < xmax and c[1] + 1 > 0 and c[1] + 1 < ymax: s_corners.append((c[0], c[1] + 1)) #up if not racetrack.crash( [(c[0], c[1] - 1), (c[0], c[1] - 1)], walls ) and c[0] > 0 and c[0] < xmax and c[1] - 1 > 0 and c[1] - 1 < ymax: s_corners.append((c[0], c[1] - 1)) #down #Find closest point in s_corners that has straight line path to finish line for corner in s_corners: dist = heuristics.edistw_to_finish(corner, fline, walls) if dist != infinity: if not target_corner: target_corner = (corner, dist) elif dist < target_corner[1]: target_corner = (corner, dist) stack.append(target_corner) if target_corner: s_corners.remove(target_corner[0]) max_limit = len(s_corners) * len(s_corners) i = 0 #Working backwards from closest point in s_corners, estimates all points distance to finish line if s_corners: while i < max_limit: min_dist = infinity min_corner = None for corner in s_corners: temp = None dist = heuristics.edistw_to_finish(corner, [stack[0][0], stack[0][0]], walls) if dist != infinity and dist != 0: if dist < min_dist: min_dist = dist min_corner = corner if min_corner: stack.insert(0, (min_corner, min_dist + stack[0][1])) s_corners.remove(min_corner) i += 1
def bfs(fline, walls): """ Use a breath-first-search from the fline to compute costs for points on the grid (combine edistw_to_finish and edist_grid into one time traversal of the nodes; significantly reduce the runtime) :param: fline, walls :return: return the grid """ global grid, g_fline, g_walls, xmax, ymax xmax = max([max(x, x0) for ((x, y), (x0, y0)) in walls]) ymax = max([max(y, y0) for ((x, y), (x0, y0)) in walls]) grid = [[infinity for i in range(ymax + 1)] for j in range(xmax + 1)] ((x1, y1), (x2, y2)) = fline frontier = deque([]) visited = [] # set the points on fline as 0 in the grid # and put all neighbors of the fline in the frontier if x1 == x2: for y3 in range(min(y1, y2), max(y1, y2) + 1): grid[x1][y3] = 0 visited.append((x1, y3)) for y3 in range(min(y1, y2), max(y1, y2) + 1): for n in range(max(0, y3 - 1), min(ymax + 1, y3 + 2)): for m in range(max(0, x1 - 1), min(xmax + 1, x1 + 2)): if frontier.count((m, n)) == 0 and grid[m][n] == infinity: frontier.append((m, n)) else: for x3 in range(min(x1, x2), max(x1, x2) + 1): grid[x3][y1] = 0 visited.append((x3, y1)) for x3 in range(min(x1, x2), max(x1, x2) + 1): for n in range(max(0, y1 - 1), min(ymax + 1, y1 + 2)): for m in range(max(0, x3 - 1), min(xmax + 1, x3 + 2)): if frontier.count((m, n)) == 0 and grid[m][n] == infinity: frontier.append((m, n)) # use breath-first-search to compute the costs in the grid while frontier: (v1, v2) = frontier.popleft() visited.append((v1, v2)) grid[v1][v2] = edistw_to_finish((v1, v2), fline, walls) # check if edistw_to_finish is able to compute the cost # if not, compute the cost using its non-infinity neighbors if grid[v1][v2] == infinity: for g in range(max(0, v1 - 1), min(xmax + 1, v1 + 2)): for h in range(max(0, v2 - 1), min(ymax + 1, v2 + 2)): if grid[g][h] != infinity and not racetrack.crash( ((v1, v2), (g, h)), walls): if g == v1 or h == v2: d = grid[g][h] + 1 else: d = grid[g][h] + 1.4142135623730951 if d < grid[v1][v2]: grid[v1][v2] = d if grid[v1][v2] != infinity: for i in range(max(0, v1 - 1), min(xmax + 1, v1 + 2)): for j in range(max(0, v2 - 1), min(ymax + 1, v2 + 2)): if frontier.count((i, j)) == 0 and visited.count( (i, j)) == 0: ##and grid[i][j] == infinity: frontier.append((i, j)) g_fline = fline g_walls = walls return grid
def h_proj1(state, fline, walls): """ The first time this function is called, it will use optimized breath first search to find the cost for all the points on the grid. On all subsequent calls, this function will retrieve the cached value and add an estimate of how long it will take to stop. :param a: state, fline, walls :return: estimate cost of the step """ global g_fline, g_walls, find # if it has already found a solution path, stop exploring other nodes if find: return infinity if fline != g_fline or walls != g_walls or grid == []: bfs(fline, walls) ((x, y), (u, v)) = state ((h1, w1), (h2, w2)) = fline li = listfl(fline) hval = float(grid[x][y]) au = abs(u) av = abs(v) sdu = au * (au - 1) / 2.0 sdv = av * (av - 1) / 2.0 sd = max(sdu, sdv) if u < 0: sdu = -sdu if v < 0: sdv = -sdv sx = x + sdu sy = y + sdv # check if it has already found a solution path # if yes, stop exploring other nodes if li.count((x, y)) > 0 and u == 0 and v == 0: find = True return negainfinity # add a small penalty to favor short stopping distances penalty = sd / 10.0 # compute location after fastest stop, and add a penalty if it goes through a wall if racetrack.crash([(x, y), (sx, sy)], walls): penalty += math.sqrt(au**2 + av**2) else: penalty -= sd / 10.0 # compute the slowest stopping distance ssu = (au - 2) * (au - 1) / 2.0 ssv = (av - 2) * (av - 1) / 2.0 if u < 0: ssu = -ssu if v < 0: ssv = -ssv ssx = x + ssu ssy = y + ssv if not racetrack.crash([(x, y), (ssx, ssy)], walls): # check if the slowest stop point land on the fline # if yes, significantly reduce the return cost if li.count((ssx, ssy)) > 0: return (-100) * (grid[x][y] + math.sqrt(ssu**2 + ssv**2)) # check if the slowest stopping x or y point is on the fline and reduce the return cost elif li.count((h1, ssy)) > 0 or li.count((h2, ssy)) > 0 or li.count( (ssx, w1)) > 0 or li.count((ssx, w2)) > 0: penalty -= sd / 10.0 hval += penalty return hval