Fix search for origin and destination points
Move loop invariants
Cleanup test_perfs
Use speed rule instead of traffic rules
Move cost calculator