Improvements on the RoutingEngine

[behrisch]

By looking at the code, I had some ideas on how the performance of the route calculation might be improved and would like to hear your opinion on that:

1. Locating the start and end points should cache the results at least per agent. The previously used faces are probably a good start point for looking for the current start and end face.
2. Recalculating the heap at every step seems wasteful. Instead of invalidating the heap by using pop and then rebuilding it, pop_heap could be used and a rebuild is only needed for the changed part (if we only decrease one value). Alternatively skip the whole heap thing and search for the minimum directly.
3. SearchState objects could be associated directly with the faces using the "custom face class" mechanism which is already used to store the "in_domain" flag. No need for costly lookups then.
4. SearchStates could be permanent objects. Instead of creating and destroying them every time, they could be permanently attached to the faces and just need to be reset. (Of course this needs profiling, maybe resetting all faces is more costly. OTOH we could track the ones we touched and just reset those.)

[behrisch]

There is more than one search state for every face which makes 3 obsolete and 4 much harder to implement so probably not worth the effort.