Generate straighter lines with map::route by checking points closer to the target first.

Before:
![](http://i.imgur.com/WAUlOcY.png)

After:
![](http://i.imgur.com/tHgBPSW.png)

PS: Maybe another solution for this would be to increase the crose of traveling diagonal.