CForest first version

Sorry about the (void) convention in functions, this is what happens when you copy and paste ignoring the other changes :)  Other RRTstar tweaks have been also overwritten.
Another idea to separate out some of the CForest-specific stuff:
- Create a virtual member function called addMotion(Motion*)
- Create a RRTstar-derived class that overrides addMotion(Motion*) that CForest will use.