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 function is a bit inefficient in terms of memory allocation. We should free memory only when states.size() < statesToInclude_.size(); otherwise, we allocate memory from statesToInclude_.size() to states.size() -1 (allocState). Then we can use copyState() instead of cloneState() and we save a lot of memory alloc / free.