Diagonal elems have to be scaled to 1s first before zeroing lower triangle elems (your second nested loop). It turns out that diagonal contains 0 => no inverse exists OR we get a row echelon form. With a reversed iteration on diagonal we can reduce it and get the inverse.
The code is far from optimal. When you have FPU, double could be a better choice than float for such numerical computations.
Note that zeroing submatrices, row swaps etc. could be replaced by far more optimal solutions. Matrix is a custom type and IsFloat0 is a custom function but all should be clear of naming and context. Enjoy code:
const uint sz = 4;
Matrix< double > mx;
mx.Resize( 2 * sz, sz );
mx.Zero();
for( uint rdx = 0; rdx < mx.NumRow(); ++rdx )
{
mx( rdx, rdx + mx.NumRow() ) = 1.0; // eye
}
mx( 0, 0 ) = 1.0; mx( 0, 1 ) = 1.0; mx( 0, 2 ) = 1.0; mx( 0, 3 ) = 0.0;
mx( 1, 0 ) = 1.0; mx( 1, 1 ) = 1.0; mx( 1, 2 ) = 2.0; mx( 1, 3 ) = 0.0;
mx( 2, 0 ) = 1.0; mx( 2, 1 ) = 2.0; mx( 2, 2 ) = 0.0; mx( 2, 3 ) = 1.0;
mx( 3, 0 ) = 1.0; mx( 3, 1 ) = 2.0; mx( 3, 2 ) = 0.0; mx( 3, 3 ) = 2.0;
// pivot iteration
uint idx;
for( idx = 0; idx < sz; ++idx )
{
// search for non-0 pivot
uint sdx = sz;
for( uint rdx = idx; rdx < sz; ++rdx )
{
if( !Util::IsFloat0( mx( rdx, idx ) ) ) { sdx = rdx; rdx = sz - 1; }
}
if( sdx < sz )
{
// swap rows
if( idx != sdx )
{
for( uint cdx = 0; cdx < ( sz << 1 ); ++cdx )
{
double swp;
swp = mx( idx, cdx );
mx( idx, cdx ) = mx( sdx, cdx );
mx( sdx, cdx ) = swp;
}
}
// 1 pivot and 0 col
{
double sc = 1.0 / mx( idx, idx );
for( uint cdx = 0; cdx < ( sz << 1 ); ++cdx )
{
mx( idx, cdx ) *= sc; // 1
}
for( uint rdx = 1 + idx; rdx < sz; ++rdx )
{
double sd = mx( rdx, idx );
for( uint cdx = 0; cdx < ( sz << 1 ); ++cdx )
{
mx( rdx, cdx ) -= sd * mx( idx, cdx ); // 0
}
}
}
}
else { idx = sz; }
}
if( sz < idx ) { mx.Zero(); }
else
{
for( idx = 0; idx < sz; ++idx )
{
uint ydx = sz - 1 - idx;
for( uint rdx = 0; rdx < ydx; ++rdx )
{
double sc = mx( rdx, ydx );
for( uint cdx = 0; cdx < ( sz << 1 ); ++cdx )
{
mx( rdx, cdx ) -= sc * mx( ydx, cdx ); // 0
}
}
}
}