Forgot to update include directory. Added ExtractPath method for RRT search.