Subject: inversion code
From:  	Peter Schroeder  <ps@cs.caltech.edu>
Date:  	Fri, November 19, 2004 10:16 am
To:  	"Weiwei Yang" <weiweiy@cs.caltech.edu>

This has unrolled Gaussian elimination. Now that I look at it I can see 
that one could be even more careful, but this bit of code should do. 
Note that it solves only for a single rhs. If you want to use this to 
invert a matrix simply solve for 4 rhs sides at once (the columns of the 
identiy matrix). This can of course be done with a single pass through 
this code simply replacing the 4x1 matrix b with a 4x4 matrix.

peter

int
invert( const float A[4][4], float x[4], const float b[4] )
{
  float wtemp[4][5];
  register float m1, m2, m3, s;
  // make r[0-3] hold pointers to the rows for row swaps
  register float *r0 = wtemp[0], *r1 = wtemp[1];
  register float *r2 = wtemp[2], *r3 = wtemp[3];
  register float *rtemp;
 
  // build the tmp matrix with the single rhs
  r0[0] = A[0][0]; r0[1] = A[0][1]; r0[2] = A[0][2]; r0[3] = A[0][3];
  r0[4] = b[0];
  r1[0] = A[1][0]; r1[1] = A[1][1]; r1[2] = A[1][2]; r1[3] = A[1][3];
  r1[4] = b[1];
  r2[0] = A[2][0]; r2[1] = A[2][1]; r2[2] = A[2][2]; r2[3] = A[2][3];
  r2[4] = b[2];
  r3[0] = A[3][0]; r3[1] = A[3][1]; r3[2] = A[3][2]; r3[3] = A[3][3];
  r3[4] = b[3];
 
  // find largest pivot
  float max = abs( r0[0] ); int maxrow = 0;
  if( ( s = abs( r1[0] ) ) > max ){ max = s; maxrow = 1; }
  if( ( s = abs( r2[0] ) ) > max ){ max = s; maxrow = 2; }
  if( ( s = abs( r3[0] ) ) > max ){ max = s; maxrow = 3; }

  // only do the following if you have a non-zero pivot
  if( !zero( max ) ){
    // actually do the swap if necessary
    switch( maxrow ){
    case 0: break;
    case 1: rtemp = r0; r0 = r1; r1 = rtemp; break;
    case 2: rtemp = r0; r0 = r2; r2 = rtemp; break;
    case 3: rtemp = r0; r0 = r3; r3 = rtemp; break;
    default: die();
    }
    // zero out first column
    // get the multipliers for rows 1-3
    m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
    // subtract A entries in column 1 (column zero does not have to be
    // done since it will be zero by assumption)
    s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
    // column 2
    s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
    // column 3
    s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
    // and the b
    s = r0[4]; r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s;
  }
  else return 0;
 
  // find largest pivot
  max = abs( r1[1] ); maxrow = 1;
  if( ( s = abs( r2[1] ) ) > max ){ max = s; maxrow = 2; }
  if( ( s = abs( r3[1] ) ) > max ){ max = s; maxrow = 3; }

  // only do the following if you have a non-zero pivot
  if( !zero( max ) ){
    // actually do the swap if necessary
    switch( maxrow ){
    case 1: break;
    case 2: rtemp = r1; r1 = r2; r2 = rtemp; break;
    case 3: rtemp = r1; r1 = r3; r3 = rtemp; break;
    default: die();
    }
    // zero out second column
    // get the multipliers for rows 2-3
    m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
    // subtract A entries in column 2 (column one does not have to be
    // done since it will be zero by assumption)
    s = r1[2]; r2[2] -= m2 * s; r3[2] -= m3 * s;
    // column 3
    s = r1[3]; r2[3] -= m2 * s; r3[3] -= m3 * s;
    // and the b
    s = r1[4]; r2[4] -= m2 * s; r3[4] -= m3 * s;
  }
  else return 0;

  // find largest pivot
  max = abs( r2[2] ); maxrow = 2;
  if( ( s = abs( r3[2] ) ) > max ){ max = s; maxrow = 3; }

  // only do the following if you have a non-zero pivot
  if( !zero( max ) ){
    // actually do the swap if necessary
    switch( maxrow ){
    case 2: break;
    case 3: rtemp = r2; r2 = r3; r3 = rtemp; break;
    default: die();
    }
    // zero out second column
    // get the multiplier for row 3
    m3 = r3[2]/r2[2];
    // subtract A entries in column 3 (column two does not have to be
    // done since it will be zero by assumption)
    r3[3] -= m3 * r2[3];
    // and the b
    r3[4] -= m3 * r2[4];
  }
  else return 0;

  if ( zero(r3[3]) ) return 0;
 
  // back substitute column 3 (x3)
    // r3[4] now contains x3
    s = r3[4] /= r3[3];
    // now back substitute this value in rows 2-0
    r2[4] -= r2[3] * s;
    r1[4] -= r1[3] * s;
    r0[4] -= r0[3] * s;

  // back substitute column 2 (x2)
    // r2[4] now contains x2
    s = r2[4] /= r2[2];
    // now back substitute this value in rows 1-0
    r1[4] -= r1[2] * s;
    r0[4] -= r0[2] * s;

  // back substitute column 1 (x1)
    // r1[4] now contains x1
    r1[4] /= r1[1];
    // now back substitute this value in rows 1-0
    r0[4] -= r0[1] * r1[4];
 
  // now back substitute row 0 (x0)
    r0[4] /= r0[0];
 
  x[0] = r0[4]; x[1] = r1[4]; x[2] = r2[4]; x[3] = r3[4];

  return 1;
}