Add tests and impl for nearest points.

Follow up on #147.