The runtime could be improved by implementing an efficient spatial data structure. At the moment, we have implemented an sorted ArrayList with access time in O(logn) and additions/removals in O(n), but n (number of vehicles) is quite small per lane. Implementations using a NavigableMap doesn't help.