improved AStar and RRT