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.
The value is not used here. We should simply use a std::set<Motion*>