Better data struct

[raviBhadeshiya]

Need grid based spatial indexing for finding the nearest neighbours and nodes at a specific location more efficiently. A simple 2D grid or more efficient Kd-tree.