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 think we can get rid of this logic around goal motions and the requirement we have a single start state:
- when we get a new path from another thread, always ignore the first state. That is a start state and we already have it.
- ignore the last state, as that is a goal state, and we likely have it. This is not a guarantee for lazy goal regions, but I think that is a caveat we can live with. Even if we don't have the goal state at this iteration, we will get it at upcoming iterations.