def main():
start_pos = Vec2(0.25,1.75)
start_speed = Vec2(0.0,-0.1)
start = RobotState(start_pos,start_speed,0.0)
goal_pos = Vec2(2.75,1.75)
goal_speed = Vec2(0.0,0.0)
goal = RobotState(goal_pos,goal_speed,0.0)
square_vertices = [Vec2(0,0),Vec2(0.5,0),Vec2(0.5,0.5),Vec2(0,0.5)]
square = Polygon(square_vertices,True,True)
obs1 = Affine.translation(Vec2(0.5,1.0)) * square
obs2 = Affine.translation(Vec2(2.0,0.5)) * square
env = Environment(Globals.PLAYGROUND_BORDER,[],[obs1,obs2])
#if poly_collides_poly(obs1,obs2):
# print "(="
#else:
# print ")="
#with PyCallGraph(output = GraphvizOutput()):
path = aStar(start,goal,env)
#print "start:\n {s}".format(s = start)
#print "goal:\n {g}".format(g = goal)
#for node in path:
# print node
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
screen.fill(black)
pygame.draw.lines(screen,white,True,convert_to_px_coords(env.border_as_tuple_list()),1)
for obs in env.obstacles_as_tuples():
pygame.draw.lines(screen,red,True,convert_to_px_coords(obs),1)
pygame.draw.lines(screen,green,False,convert_to_px_coords(Util.path_to_tuples(path)),1)
for node in path:
draw_state(node)
pygame.display.update()
