Test and minor refactor findCloseNodes.

* Tesst ported from the drogulus project.