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.
Honestly, I do not see how to do this in a clean way. Now we have
    typedef ompl::BinaryHeap<Motion*, MotionCompare> MotionBinHeap;
    MotionBinHeap Open_;

What you propose is something like
    typedef ompl::BinaryHeap<Motion*, MotionCompareHeuristics> MotionBinHeapHeuristics;
    MotionBinHeap OpenHeuristics_;

And that would require a lot more if statemets in the cpp. How would you do it then?