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*
Look at the \FIXME's in the code. I have tried to separate the CForest code from the pruning code, but there's one case where I need your help to disentangle them.