FMT updates: improves code, heuristics, kNN and collision check caching

Please, note that the new parameter cacheCC_ is intentionally not included as a PlannerParam because we want the general users to take advantage of it and not disable it accidentally. An advanced user could use the setCacheCC() method.
I do not think so. I was going to include this as a planner progress property (ppp). But, since FMT* works "in batchs", it is not actually a ppp per se. Therefore, I leave it public as other ppp getters (RRTstar for instance) but not include it as ppp.