def steer4(vertex_nearest, vertex_rand, goal_set, tree_num): # steer nearest vertex towards random Vertex, within radius if norm(vertex_nearest, vertex_rand) < Dimensions.tree_radius: # if already within radius vertex_new = vertex_rand # then no calculation needed else: ratio1 = (vertex_rand.x - vertex_nearest.x) / (vertex_rand.y - vertex_nearest.y) # calculate 4-Dimensional "slopes" ratio2 = (vertex_rand.x - vertex_nearest.x) / (vertex_rand.x_vel - vertex_nearest.x_vel) ratio3 = (vertex_rand.x - vertex_nearest.x) / (vertex_rand.y_vel - vertex_nearest.y_vel) new_delta_x = sqrt((Dimensions.tree_radius ** 2) / (1 + (1 / (ratio1 ** 2)) + (1 / (ratio2 ** 2)) + (1 / (ratio3 ** 2)))) # use Euclidean norm to solve if vertex_rand.x < vertex_nearest.x: new_delta_x = -new_delta_x y_new = vertex_nearest.y + (new_delta_x / ratio1) x_v_new = vertex_nearest.x_vel + (new_delta_x / ratio2) y_v_new = vertex_nearest.y_vel + (new_delta_x / ratio3) vertex_new = Vertex(vertex_nearest.x + new_delta_x, y_new, x_v_new, y_v_new) # initialize vertex if tree_num == 1: vertex_new_ = trace_inclusivity(vertex_nearest, vertex_new, goal_set) # if hits goal, adjust vertex to intersection pt vertex_new.tree_num = tree_num return vertex_new
def main(): pywindow, obstacles, axis, buttons = init_pywindow( 'i-Nash Trajectory') # set up pygame window, dimensions and obstacles start, goal_set, num_robots, robo_colors, sign, goal = user_prompt( pywindow) # prompt for num bots, start, end positions num_tests = input('how many tests to run? ') success_total = 0 while success_total < num_tests: print 'running test number:', success_total + 1 for i in range(len(start)): print 'start:', start[i].x, start[i].y, 'goal', goal[i].x, goal[ i].y nash_success = run_test(pywindow, obstacles, axis, buttons, start, goal_set, num_robots, robo_colors, sign, goal, success_total) if nash_success: success_total = success_total + 1 for i in range(len(goal)): goal[i] = Vertex(goal[i].x, goal[i].y, 0, 0) goal[i].tree_num = 2 start[i] = Vertex(start[i].x, goal[i].y, 0, 0) print 'Done the whole thing, wow, excellent, nice! Super cool!' return