Redesigned CForest to minimize changes to asymptotically optimal planner...

...s it uses. Fix bug in benchmarking that caused huge log files. Added option for tree pruning in RRT*
This is needed because shared paths use to have the same states. Let's say that we share a path p1 at time t1. Most probably, path p2 shared at time t2 would have most of the states the same. If this is not checked, final paths would have lots of states repeated and the kNN would "fail" since there would be more than 1 state representing the same state.