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.
We probably could, but it would be a bit more complex because FMT* stores the neighborhoods. Also, in my opinion, that would make the code much more complex while not giving any clear advantage. If you really think I should do it, I would ask you to merge this PR and I would do it once all these modifications are approved.