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 probably where you are leaking memory. You use copyState, so you need to call freeState on statesToInclude_.back(). Maybe "rmotion->state=statesToInclude_.back()" is also ok, if not a bit dangerous.