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*
why post increment? it was like that originally and I changed it to preincrement since it is supposed to be more efficient and there is no difference in this case.