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.
For consistency purposes, we associate & to the type for return types (and to the var name for args / variables). Also no space before (