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.
This is a minor thing, but the spacing is a bit odd. You probably have some tabs in your code. Replace those with spaces. See http://ompl.kavrakilab.org/styleGuide.html.