Utils: add distance and distance2