Consider maxspeed increases performance by 33%
Using a binary heap should help with lots of nodes