Support of "via" points

I have started with an implementation of "via" points. The final goal is that one may not specify only "From" and "To", but also an ordered list of "via" points. Unfortunately the current implementation does not work as intended with CH. The test DijkstraBidirectionCHTest is failing.

Could you please take a look and provide feedback: Are the current problems with CH caused by a wrong approach or can it be solved by clearing some temporary data within the routing algorithms?