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.
if you want to get fancy, you could create a new boost::function that first calls prevSolutionCallback and then CForest::newSolutionFound