astar dubins fcns