should now be correct A* implementation, implemented after the pseudocode on wikipedia
should now be correct A* implementation, implemented after the pseudocode on wikipedia

31415us/trajectory,31415us/trajectory,31415us/trajectory