Pridan algoritmus vypoctu diagonalni matice