Untitled

void matrix_inverse(matrix b, matrix a)
{
 int i, j, i2, pr;
 vector r;
 matrix aa;
 int used[4] = {-1, -1, -1, -1};

 matrix_copy(aa, a);
 matrix_identity(b);

 for(j = 0; j < 4; j++)
    {
     pr = -1;
     for(i = 0; i < 4; i++)
         if(used[i] < 0 && aa[i][j] != 0.0)
            {
             pr = i;
             break;
            }
     if(pr < 0) { fprintf(stderr, "No row found without a zero in column %d!\n", j); return; }
     used[pr] = j;
     for(i = 0; i < 4; i++)
        {
         if(i == pr) continue;
         vector_scale(r, b[pr], -aa[i][j]/aa[pr][j]);
         vector_add(b[i], b[i], r);
         vector_scale(r, aa[pr], -aa[i][j]/aa[pr][j]);
         vector_add(aa[i], aa[i], r);
        }
    }

 for(i = 0; i < 4; i++)
     for(i2 = i+1; i2 < 4; i2++)
         if(used[i2] == i)
            {
             vector_swap(aa[i], aa[i2]);
             vector_swap(b[i], b[i2]);
             used[i2] = used[i];
            }

 for(i = 0; i < 4; i++)
    {
     vector_scale(b[i], b[i], 1.0 / aa[i][i]);
     vector_scale(aa[i], aa[i], 1.0 / aa[i][i]);
    }
}