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 would move this inside the for loop, so that a new / empty / cleared object is used. Relative to the rest, constructing this object is negligible computational cost.