RRTstar-CForest heuristic updated