Можно взять любой алгоритм сортировки, например, ту же пузырьковую сортировку, и в процедуре обмена двух значений вместо, скажем, a[i] писать a[i,i], a[j] - a[j,j]. Так отсортируется главная диагональ.