Made a small improvement over the RRT search, pick nodes that are closers to the tree structure.