Fix inconsistency with checking diagonal distance