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 could keep track of the start states we add at the top of solve() in a separate vector of states. Those would be the roots we start from (technically, OMPL trees have multiple roots. You can think of this as having one virtual root for all start states, but we skip creating that). 
Then we can have pruneTree be a recursive function that gets called with a Motion* and a Cost.
We will call it for each start state. 
The function is recursive and goes to every child. If the cost of a child is above the Cost received as arg, we delete that Motion and descendants, including their states (probably by calling another recursive function). At the end, we simply reconstruct nn_;