Changed opt2 to work with indicies

This adds a bit of indirection but causes less copying o(only one in instead of 2 flots + 1 int) and makes it possible to more easily do optimizations llike caching the distances since they do not need to be stored with the nodes.
Merge remote-tracking branch 'origin/master'