Replaced distance compare in loop with distance squared (and test commit)