Implements RRT* and gets it to build