drug this up from an old college project... Maybe it'll help
typedef struct matrix
{
flag type; /* type of matrix: {RECTANGULAR, SQUARE, BANDED } */
ushort num_rows; /* number of rows in th matrix */
union cols
{
ushort num_cols; /* Number of cols in Rectanular matrix */
ushort band; /* Bandwidth in a square, symmetric, banded matrix */
}
double *m_val; /* ptr to start of storage of actual matrix values */
} MAT;
/* row_swap ---- swap two rows of an nxn UNBANDED matrix */
void row_swap(mat, r1, r2)
MAT *mat;
ushort r1, r2;
{
double *m = mat->m_val;
ushort n = m->num_row;
INDEX i;
double temp;
for (i=0; i < n; i++)
SWAP(*(m + n*r1 +i), *(m + n*r2 +i), temp);
}
/* reduce --- function to do Gauss- Jordon Reduction on matrix */
static void reduce(m, in, ref)
MAT *m *in;
double *ref;
{
ushort n = mat->num_row;
INDEX r,c;
int sr,sc;
double key, mat = m->m_val, inv = in->m_val;
sr =(ref-mat)/n;
sc=(ref-mat) % n;
scalprod(1.0/(*ref), inv+n*sr, n);
scalprod(1.0/(*ref), ref-sc, n);
for ( r=0; r LT n; r++)
{
if ( r != sr && *(mat+n*r+sc) != 0.0 )
{
key = *(mat+n*r+sc);
for (c=0; c < n; c++)
{
*(inv+n*r+c) -= key * *(inv+n*sr+c);
*(mat+n*r+c) -= key * *(mat+n*sr+c);
}
}
}
}
/* find_non_zero -- this finds and returns a ptr to next non-zero entry
* below and to the right of the last one */
static double *find_non_zero(start, last, n)
ushort n;
double *start, *last;
{
double *i = last; /* i is ptr to matrix entries */
while (*i == 0.0 && (i-start) < n*n)
{
if (n+i > start+n*n) /* last row without non-zero entry, */
return (-1); /* Matrix must be singular */
else
i += n; /* this is not the last row, goto next row */
}
if (i >= start + n*n) /* we have departed the matrix */
return (0);
else /* we found a non-zero entry, return it */
return(i);
}
/* invert -- function to invert nxn matrix */
flag invert(m, i)
MAT *m, *i;
{
double *ref, *new, *mat = m->m_val, *inv = i->m_val;
ushort n = mat->num_row;
INDEX i, j, row;
ushort new_row;
if (det(mat,n) == 0.0) return 0;
for (i=0; i < n; i++)
for (j=0; j < n; j++)
*(inv+n*i+j) = (i == j)? 1.0: 0.0;
ref = mat;
for (row=0; row < n; row++)
{
new = find_non_zero(mat,ref,n);
if ( new == -1)
{
scr_print(" This matrix is singular, not invertible ");
break;
}
new_row = (new-mat)/n;
if (new_row != row)
{
row_swap(mat, new_row, row, n);
row_swap(inv, new_row, row, n);
}
reduce(mat,inv,ref,n);
ref += n+1;
}
}