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.
I think we can put this logic at the top of the CForest solve function, no? then we can avoid having this function.