Improved RRT's goal directed behavior