home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 3 / RISC_DISC_3.iso / resources / etexts / gems / gemsii / inverse.c < prev    next >
C/C++ Source or Header  |  1991-10-02  |  5KB  |  142 lines
  1. #include "GraphicsGems.h"
  2. #include <stdio.h>
  3.  
  4. /****
  5.  *
  6.  * affine_matrix4_inverse
  7.  *
  8.  * Computes the inverse of a 3D affine matrix; i.e. a matrix with a dimen-
  9.  * sionality of 4 where the right column has the entries (0, 0, 0, 1).
  10.  *
  11.  * This procedure treats the 4 by 4 matrix as a block matrix and
  12.  * calculates the inverse of one submatrix for a significant perform-
  13.  * ance improvement over a general procedure that can invert any non-
  14.  * singular matrix:
  15.  *          --        --          --          --
  16.  *          |          | -1       |    -1      |
  17.  *          | A      0 |          |   A      0 |
  18.  *    -1    |          |          |            |
  19.  *   M   =  |          |     =    |     -1     |
  20.  *          | C      1 |          | -C A     1 |
  21.  *          |          |          |            |
  22.  *          --        --          --          --
  23.  *
  24.  *  where     M is a 4 by 4 matrix,
  25.  *            A is the 3 by 3 upper left submatrix of M,
  26.  *            C is the 1 by 3 lower left submatrix of M.
  27.  *
  28.  * Input:
  29.  *   in   - 3D affine matrix
  30.  *
  31.  * Output:
  32.  *   out  - inverse of 3D affine matrix
  33.  *
  34.  * Returned value:
  35.  *   TRUE   if input matrix is nonsingular
  36.  *   FALSE  otherwise
  37.  *
  38.  ***/
  39.  
  40. boolean
  41. affine_matrix4_inverse (in, out)
  42.     register  Matrix4  *in;
  43.     register  Matrix4  *out;
  44. {
  45.     register  double    det_1;
  46.               double    pos, neg, temp;
  47.  
  48. #define ACCUMULATE    \
  49.     if (temp >= 0.0)  \
  50.         pos += temp;  \
  51.     else              \
  52.         neg += temp;
  53.  
  54. #define PRECISION_LIMIT (1.0e-15)
  55.  
  56.     /*
  57.      * Calculate the determinant of submatrix A and determine if the
  58.      * the matrix is singular as limited by the double precision
  59.      * floating-point data representation.
  60.      */
  61.     pos = neg = 0.0;
  62.     temp =  in->element[0][0] * in->element[1][1] * in->element[2][2];
  63.     ACCUMULATE
  64.     temp =  in->element[0][1] * in->element[1][2] * in->element[2][0];
  65.     ACCUMULATE
  66.     temp =  in->element[0][2] * in->element[1][0] * in->element[2][1];
  67.     ACCUMULATE
  68.     temp = -in->element[0][2] * in->element[1][1] * in->element[2][0];
  69.     ACCUMULATE
  70.     temp = -in->element[0][1] * in->element[1][0] * in->element[2][2];
  71.     ACCUMULATE
  72.     temp = -in->element[0][0] * in->element[1][2] * in->element[2][1];
  73.     ACCUMULATE
  74.     det_1 = pos + neg;
  75.  
  76.     /* Is the submatrix A singular? */
  77.     if ((det_1 == 0.0) || (ABS(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
  78.  
  79.         /* Matrix M has no inverse */
  80.         fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
  81.         return FALSE;
  82.     }
  83.  
  84.     else {
  85.  
  86.         /* Calculate inverse(A) = adj(A) / det(A) */
  87.         det_1 = 1.0 / det_1;
  88.         out->element[0][0] =   ( in->element[1][1] * in->element[2][2] -
  89.                                  in->element[1][2] * in->element[2][1] )
  90.                                * det_1;
  91.         out->element[1][0] = - ( in->element[1][0] * in->element[2][2] -
  92.                                  in->element[1][2] * in->element[2][0] )
  93.                                * det_1;
  94.         out->element[2][0] =   ( in->element[1][0] * in->element[2][1] -
  95.                                  in->element[1][1] * in->element[2][0] )
  96.                                * det_1;
  97.         out->element[0][1] = - ( in->element[0][1] * in->element[2][2] -
  98.                                  in->element[0][2] * in->element[2][1] )
  99.                                * det_1;
  100.         out->element[1][1] =   ( in->element[0][0] * in->element[2][2] -
  101.                                  in->element[0][2] * in->element[2][0] )
  102.                                * det_1;
  103.         out->element[2][1] = - ( in->element[0][0] * in->element[2][1] -
  104.                                  in->element[0][1] * in->element[2][0] )
  105.                                * det_1;
  106.         out->element[0][2] =   ( in->element[0][1] * in->element[1][2] -
  107.                                  in->element[0][2] * in->element[1][1] )
  108.                                * det_1;
  109.         out->element[1][2] = - ( in->element[0][0] * in->element[1][2] -
  110.                                  in->element[0][2] * in->element[1][0] )
  111.                                * det_1;
  112.         out->element[2][2] =   ( in->element[0][0] * in->element[1][1] -
  113.                                  in->element[0][1] * in->element[1][0] )
  114.                                * det_1;
  115.  
  116.         /* Calculate -C * inverse(A) */
  117.         out->element[3][0] = - ( in->element[3][0] * out->element[0][0] +
  118.                                  in->element[3][1] * out->element[1][0] +
  119.                                  in->element[3][2] * out->element[2][0] );
  120.         out->element[3][1] = - ( in->element[3][0] * out->element[0][1] +
  121.                                  in->element[3][1] * out->element[1][1] +
  122.                                  in->element[3][2] * out->element[2][1] );
  123.         out->element[3][2] = - ( in->element[3][0] * out->element[0][2] +
  124.                                  in->element[3][1] * out->element[1][2] +
  125.                                  in->element[3][2] * out->element[2][2] );
  126.  
  127.         /* Fill in last column */
  128.         out->element[0][3] = out->element[1][3] = out->element[2][3] = 0.0;
  129.         out->element[3][3] = 1.0;
  130.  
  131.         return TRUE;
  132.     }
  133. }
  134.  
  135.  
  136.  
  137.  
  138.  
  139.  
  140.  
  141.  
  142.