some tweaks
more improvements to CForest - RRTstar decoupling
Merge branch 'MarkCForest' into CForest

Putting CForest work together into one branch