Working preliminary algorithm.  TODO: tweak algorithms, implement maximum speed, generalize for different boundary sizes, fix distance for periodic boundaries