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