Fixed reachability distance issue

Reachability distances are correctly calculated. Still has an issue
with the ordering of points; currently orders points in ordered list
according to next closest point, instead of next closest/smallest
reachability distance :-/