Made RRT's getPathToGoal tail-recursive