home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 3 / RISC_DISC_3.iso / resources / etexts / gems / gemsi / matrixinvert.c < prev    next >
C/C++ Source or Header  |  1992-09-15  |  5KB  |  184 lines
  1. /*
  2. Matrix Inversion
  3. by Richard Carling
  4. from "Graphics Gems", Academic Press, 1990
  5. */
  6.  
  7. #define SMALL_NUMBER    1.e-8
  8. /* 
  9.  *   inverse( original_matrix, inverse_matrix )
  10.  * 
  11.  *    calculate the inverse of a 4x4 matrix
  12.  *
  13.  *     -1     
  14.  *     A  = ___1__ adjoint A
  15.  *         det A
  16.  */
  17.  
  18. #include "GraphicsGems.h"
  19. #include <math.h>
  20. inverse( in, out ) Matrix4 *in, *out;
  21. {
  22.     int i, j;
  23.     double det, det4x4();
  24.  
  25.     /* calculate the adjoint matrix */
  26.  
  27.     adjoint( in, out );
  28.  
  29.     /*  calculate the 4x4 determinant
  30.      *  if the determinant is zero, 
  31.      *  then the inverse matrix is not unique.
  32.      */
  33.  
  34.     det = det4x4( in );
  35.  
  36.     if ( fabs( det ) < SMALL_NUMBER) {
  37.         printf("Non-singular matrix, no inverse!\n");
  38.         exit(1);
  39.     }
  40.  
  41.     /* scale the adjoint matrix to get the inverse */
  42.  
  43.     for (i=0; i<4; i++)
  44.         for(j=0; j<4; j++)
  45.         out->element[i][j] = out->element[i][j] / det;
  46. }
  47.  
  48.  
  49. /* 
  50.  *   adjoint( original_matrix, inverse_matrix )
  51.  * 
  52.  *     calculate the adjoint of a 4x4 matrix
  53.  *
  54.  *      Let  a   denote the minor determinant of matrix A obtained by
  55.  *           ij
  56.  *
  57.  *      deleting the ith row and jth column from A.
  58.  *
  59.  *                    i+j
  60.  *     Let  b   = (-1)    a
  61.  *          ij            ji
  62.  *
  63.  *    The matrix B = (b  ) is the adjoint of A
  64.  *                     ij
  65.  */
  66.  
  67. adjoint( in, out ) Matrix4 *in; Matrix4 *out;
  68. {
  69.     double a1, a2, a3, a4, b1, b2, b3, b4;
  70.     double c1, c2, c3, c4, d1, d2, d3, d4;
  71.     double det3x3();
  72.  
  73.     /* assign to individual variable names to aid  */
  74.     /* selecting correct values  */
  75.  
  76.     a1 = in->element[0][0]; b1 = in->element[0][1]; 
  77.     c1 = in->element[0][2]; d1 = in->element[0][3];
  78.  
  79.     a2 = in->element[1][0]; b2 = in->element[1][1]; 
  80.     c2 = in->element[1][2]; d2 = in->element[1][3];
  81.  
  82.     a3 = in->element[2][0]; b3 = in->element[2][1];
  83.     c3 = in->element[2][2]; d3 = in->element[2][3];
  84.  
  85.     a4 = in->element[3][0]; b4 = in->element[3][1]; 
  86.     c4 = in->element[3][2]; d4 = in->element[3][3];
  87.  
  88.  
  89.     /* row column labeling reversed since we transpose rows & columns */
  90.  
  91.     out->element[0][0]  =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
  92.     out->element[1][0]  = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
  93.     out->element[2][0]  =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
  94.     out->element[3][0]  = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  95.         
  96.     out->element[0][1]  = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
  97.     out->element[1][1]  =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
  98.     out->element[2][1]  = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
  99.     out->element[3][1]  =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
  100.         
  101.     out->element[0][2]  =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
  102.     out->element[1][2]  = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
  103.     out->element[2][2]  =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
  104.     out->element[3][2]  = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
  105.         
  106.     out->element[0][3]  = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
  107.     out->element[1][3]  =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
  108.     out->element[2][3]  = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
  109.     out->element[3][3]  =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
  110. }
  111. /*
  112.  * double = det4x4( matrix )
  113.  * 
  114.  * calculate the determinant of a 4x4 matrix.
  115.  */
  116. double det4x4( m ) Matrix4 *m;
  117. {
  118.     double ans;
  119.     double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,             d4;
  120.     double det3x3();
  121.  
  122.     /* assign to individual variable names to aid selecting */
  123.     /*  correct elements */
  124.  
  125.     a1 = m->element[0][0]; b1 = m->element[0][1]; 
  126.     c1 = m->element[0][2]; d1 = m->element[0][3];
  127.  
  128.     a2 = m->element[1][0]; b2 = m->element[1][1]; 
  129.     c2 = m->element[1][2]; d2 = m->element[1][3];
  130.  
  131.     a3 = m->element[2][0]; b3 = m->element[2][1]; 
  132.     c3 = m->element[2][2]; d3 = m->element[2][3];
  133.  
  134.     a4 = m->element[3][0]; b4 = m->element[3][1]; 
  135.     c4 = m->element[3][2]; d4 = m->element[3][3];
  136.  
  137.     ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
  138.         - b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
  139.         + c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
  140.         - d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  141.     return ans;
  142. }
  143.  
  144.  
  145.  
  146. /*
  147.  * double = det3x3(  a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  148.  * 
  149.  * calculate the determinant of a 3x3 matrix
  150.  * in the form
  151.  *
  152.  *     | a1,  b1,  c1 |
  153.  *     | a2,  b2,  c2 |
  154.  *     | a3,  b3,  c3 |
  155.  */
  156.  
  157. double det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  158. double a1, a2, a3, b1, b2, b3, c1, c2, c3;
  159. {
  160.     double ans;
  161.     double det2x2();
  162.  
  163.     ans = a1 * det2x2( b2, b3, c2, c3 )
  164.         - b1 * det2x2( a2, a3, c2, c3 )
  165.         + c1 * det2x2( a2, a3, b2, b3 );
  166.     return ans;
  167. }
  168.  
  169. /*
  170.  * double = det2x2( double a, double b, double c, double d )
  171.  * 
  172.  * calculate the determinant of a 2x2 matrix.
  173.  */
  174.  
  175. double det2x2( a, b, c, d)
  176. double a, b, c, d; 
  177. {
  178.     double ans;
  179.     ans = a * d - b * c;
  180.     return ans;
  181. }
  182.  
  183.  
  184.