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.
In RRTstar these functions are public, and they do not make any sense as private. The values are in fact protected, but these functions should be public, right?