check diagonal matrix in c++