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*
Most likely, vector::swap will just reassign two pointers and do no copying. When RRTstar::clear() is called, it removes its sampler, so we need to get rid of those sampler instances.