Extract diagonal elements (like spdiags in matlab) - c++

Question

I want to implement ,as possible , spdiags function in C.
(I prefer C to C++ and I don't want to use C++ algorithms for now)
Having as input matrix:
inMx =
1 0 0
4 5 6
0 7 9
you should obtain (running that using spdiags in Matlab) :
ouMx =
4 1 0
7 5 0
0 9 6
(one thing I can't understand though , is even though at the documentation it says that zeros are inserted at the top if you are below the main diagonal , here you we can see that happens the opposite , but in the example in the link it's ok )
With my code below , I am taking as output:
ouMx =
4 7 0
7 5 9
0 9 6
so I think I am close!
I zeroed out the output matrix instead of having to insert zeros at the bottom or top of columns.
But I can't completeley handled the above/below the main diagonal.
I used if ( j > i ) swap rows , but it doesn't work , so I just use swap rows.
( I am assuming square matrix for this example ,but it should work for any matrix )
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
void Diag( int Rows , int Cols , float * inMx , float * ouMx );
void swapRows( int Rows , int Cols , float * Mx );
int main( int argc, const char* argv[] ){
int Rows = 3 , Cols = 3;
float *inMx = (float *) malloc ( Rows * Cols * sizeof (float) );
float *ouMx = (float *) malloc ( Rows * Cols * sizeof (float) );
// assume row major order
inMx[0] = 1.0;
inMx[1] = 0.0;
inMx[2] = 0.0;
inMx[3] = 4.0;
inMx[4] = 5.0;
inMx[5] = 6.0;
inMx[6] = 0.0;
inMx[7] = 7.0;
inMx[8] = 9.0;
// print input matrix ( row major )
printf("\n Input matrix \n\n");
for ( int i = 0; i < Rows; i++ )
for ( int j = 0; j < Cols; j++ ) {
printf("%f\t",inMx[ i * Cols + j ]);
if( j == Cols-1 )
printf("\n");
}
printf("\n");
// extract diagonals
Diag( Rows , Cols , inMx , ouMx );
// print Diagonal matrix
printf("\n Diagonal matrix \n\n");
for ( int i = 0; i < Rows; i++ )
for (int j = 0; j < Cols; j++ ) {
printf("%f\t",ouMx[ i * Cols + j ]);
if( j == Cols-1 )
printf("\n");
}
printf("\n");
free( inMx );
free( ouMx );
return 0;
}
void Diag( int Rows , int Cols , float * inMx , float * ouMx )
{
//zero out ouMx
memset( ouMx , 0 , Rows * Cols * sizeof(float) );
// scan from the last line to the first -1 for each column
for ( int j = 0; j < Cols; j++ )
{
for ( int i = ( Rows - 1 ); i > 0 ; i-- )
{
// neglect the zero elements
if ( inMx[ i * Cols + j ] != 0 )
{
ouMx[ i * Cols + j ] = inMx[ i * Cols + j ];
//if the element in the next colulmn is !=0
if ( inMx[ ( i + 1 ) * Cols + ( j + 1 ) ] != 0 )
{
ouMx[ ( i + 1 ) * Cols + j ] = inMx[ ( i + 1 ) * Cols + ( j + 1 ) ];
}
}
//if we are above the main diagonal
//swap elements of a row (in each column) in order to have the zeros at bottom/top
// if ( i > j ) doesn't work
swapRows( Rows , Cols , ouMx );
}
}
}
void swapRows( int Rows , int Cols , float * Mx )
{
float temp;
for ( int j = 0; j < Cols; j++ )
{
for ( int i = ( Rows - 1 ); i > 0 ; i-- )
{
temp = Mx[ ( i - 1 ) * Cols + j ];
Mx[ ( i - 1 ) * Cols + j ] = Mx[ i * Cols + j ];
Mx[ i * Cols + j ] = temp;
}
}
}