#include <stdio.h>
#include <math.h>

invert_matrix (double A[4][4], double Ainv[4][4]);
adjoint( double in[4][4], double out[4][4] );
double det4x4( double m[4][4] );
double det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 );
double det2x2( a, b, c, d);


main () {
  double A[4][4] = {1, 0, 0, 0,
		    0, 0.707, -0.707, 0,
		    0, 0.707, 0.707, 0,
		    0, 0, 0, 1};
  double Ainv[4][4];
  int i,j;

  invert_matrix(A, Ainv);

  printf("A is:\n");
  for (i=0; i<4; i++) {
    for (j=0; j<4; j++)
      printf("%6.2f ",A[i][j]);
    printf("\n");
  }

  printf("A^{-1} is:\n");
  for (i=0; i<4; i++) {
    for (j=0; j<4; j++)
      printf("%6.2f ",Ainv[i][j]);
    printf("\n");
  }
}   
  

/*------------------------------------------------------*/
/*
 * Invert a 4x4 matrix.  Changed slightly from Richard Carling's code
 * in "Graphics Gems I".
 */

#define SMALL_NUMBER    1.e-8
invert_matrix (double A[4][4], double Ainv[4][4]) {
  int i, j;
  double det, det4x4();

  adjoint( A, Ainv );

  det = det4x4( A );

  if ( fabs( det ) < SMALL_NUMBER) {
    fprintf(stderr,"invert_matrix: matrix is singular!");
    return;
  }

  for (i=0; i<4; i++)
    for(j=0; j<4; j++)
      Ainv[i][j] = Ainv[i][j] / det;
}

adjoint( double in[4][4], double out[4][4] ) {
  double a1, a2, a3, a4, b1, b2, b3, b4;
  double c1, c2, c3, c4, d1, d2, d3, d4;
  double det3x3();

  a1 = in[0][0]; b1 = in[0][1]; 
  c1 = in[0][2]; d1 = in[0][3];

  a2 = in[1][0]; b2 = in[1][1]; 
  c2 = in[1][2]; d2 = in[1][3];

  a3 = in[2][0]; b3 = in[2][1];
  c3 = in[2][2]; d3 = in[2][3];

  a4 = in[3][0]; b4 = in[3][1]; 
  c4 = in[3][2]; d4 = in[3][3];

  out[0][0]  =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
  out[1][0]  = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
  out[2][0]  =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
  out[3][0]  = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
        
  out[0][1]  = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
  out[1][1]  =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
  out[2][1]  = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
  out[3][1]  =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
        
  out[0][2]  =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
  out[1][2]  = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
  out[2][2]  =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
  out[3][2]  = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
        
  out[0][3]  = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
  out[1][3]  =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
  out[2][3]  = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
  out[3][3]  =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
}

double det4x4( double m[4][4] ) {
  double ans;
  double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
  double det3x3();

  a1 = m[0][0]; b1 = m[0][1]; 
  c1 = m[0][2]; d1 = m[0][3];

  a2 = m[1][0]; b2 = m[1][1]; 
  c2 = m[1][2]; d2 = m[1][3];

  a3 = m[2][0]; b3 = m[2][1]; 
  c3 = m[2][2]; d3 = m[2][3];

  a4 = m[3][0]; b4 = m[3][1]; 
  c4 = m[3][2]; d4 = m[3][3];

  ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
    - b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
    + c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
    - d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  return ans;
}

double det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
     double a1, a2, a3, b1, b2, b3, c1, c2, c3;
{
  double ans;
  double det2x2();

  ans = a1 * det2x2( b2, b3, c2, c3 )
    - b1 * det2x2( a2, a3, c2, c3 )
    + c1 * det2x2( a2, a3, b2, b3 );
  return ans;
}

double det2x2( a, b, c, d)
     double a, b, c, d; 
{
  double ans;
  ans = a * d - b * c;
  return ans;
}