Cleaned up the rrt implementation, added rrt drawing, reformatted unittests