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.
If I write **, then in this method I would need to change z for &z and that would be a bit harder to follow (I think). In this way, z is used as if it was just a pointer.