Iterate over neighbours only

This increases the solution finding speed alot.

Signed-off-by: Christoph Höger <choeger@umpa-net.de>
Use sparsity for distance computation

Signed-off-by: Christoph Höger <choeger@umpa-net.de>