home *** CD-ROM | disk | FTP | other *** search
/ DP Tool Club 8 / CDASC08.ISO / NEWS / RADIANCE / SRC / COMMON / MAT4.C < prev    next >
C/C++ Source or Header  |  1993-10-07  |  3KB  |  118 lines
  1. /* Copyright (c) 1990 Regents of the University of California */
  2.  
  3. #ifndef lint
  4. static char SCCSid[] = "@(#)mat4.c 2.1 11/12/91 LBL";
  5. #endif
  6.  
  7. /*
  8.  *  mat4.c - routines dealing with 4 X 4 homogeneous transformation matrices.
  9.  *
  10.  *     10/19/85
  11.  */
  12.  
  13. #include  "mat4.h"
  14.  
  15. MAT4  m4ident = MAT4IDENT;
  16.  
  17. static MAT4  m4tmp;        /* for efficiency */
  18.  
  19.  
  20. multmat4(m4a, m4b, m4c)        /* multiply m4b X m4c and put into m4a */
  21. MAT4  m4a;
  22. register MAT4  m4b, m4c;
  23. {
  24.     register int  i, j;
  25.     
  26.     for (i = 4; i--; )
  27.         for (j = 4; j--; )
  28.             m4tmp[i][j] = m4b[i][0]*m4c[0][j] +
  29.                       m4b[i][1]*m4c[1][j] +
  30.                       m4b[i][2]*m4c[2][j] +
  31.                       m4b[i][3]*m4c[3][j];
  32.     
  33.     copymat4(m4a, m4tmp);
  34. }
  35.  
  36.  
  37. multv3(v3a, v3b, m4)    /* transform vector v3b by m4 and put into v3a */
  38. FVECT  v3a;
  39. register FVECT  v3b;
  40. register MAT4  m4;
  41. {
  42.     m4tmp[0][0] = v3b[0]*m4[0][0] + v3b[1]*m4[1][0] + v3b[2]*m4[2][0];
  43.     m4tmp[0][1] = v3b[0]*m4[0][1] + v3b[1]*m4[1][1] + v3b[2]*m4[2][1];
  44.     m4tmp[0][2] = v3b[0]*m4[0][2] + v3b[1]*m4[1][2] + v3b[2]*m4[2][2];
  45.     
  46.     v3a[0] = m4tmp[0][0];
  47.     v3a[1] = m4tmp[0][1];
  48.     v3a[2] = m4tmp[0][2];
  49. }
  50.  
  51.  
  52. multp3(p3a, p3b, m4)        /* transform p3b by m4 and put into p3a */
  53. register FVECT  p3a;
  54. FVECT  p3b;
  55. register MAT4  m4;
  56. {
  57.     multv3(p3a, p3b, m4);    /* transform as vector */
  58.     p3a[0] += m4[3][0];    /* translate */
  59.     p3a[1] += m4[3][1];
  60.     p3a[2] += m4[3][2];
  61. }
  62.  
  63.  
  64. /*
  65.  * invmat - computes the inverse of mat into inverse.  Returns 1
  66.  * if there exists an inverse, 0 otherwise.  It uses Gaussian Elimination
  67.  * method with partial pivoting.
  68.  */
  69.  
  70. invmat(inverse,mat)
  71. MAT4  mat, inverse;
  72. {
  73. #define SWAP(a,b,t) (t=a,a=b,b=t)
  74. #define ABS(x) (x>=0?x:-(x))
  75.  
  76.     register int i,j,k;
  77.     register double temp;
  78.  
  79.     copymat4(m4tmp, mat);
  80.     setident4(inverse);
  81.  
  82.     for(i = 0; i < 4; i++) {
  83.         /* Look for row with largest pivot and swap rows */
  84.         temp = 0; j = -1;
  85.         for(k = i; k < 4; k++)
  86.             if(ABS(m4tmp[k][i]) > temp) {
  87.                 temp = ABS(m4tmp[k][i]);
  88.                 j = k;
  89.                 }
  90.         if(j == -1)    /* No replacing row -> no inverse */
  91.             return(0);
  92.         if (j != i)
  93.             for(k = 0; k < 4; k++) {
  94.                 SWAP(m4tmp[i][k],m4tmp[j][k],temp);
  95.                 SWAP(inverse[i][k],inverse[j][k],temp);
  96.                 }
  97.  
  98.         temp = m4tmp[i][i];
  99.         for(k = 0; k < 4; k++) {
  100.             m4tmp[i][k] /= temp;
  101.             inverse[i][k] /= temp;
  102.             }
  103.         for(j = 0; j < 4; j++) {
  104.             if(j != i) {
  105.                 temp = m4tmp[j][i];
  106.                 for(k = 0; k < 4; k++) {
  107.                     m4tmp[j][k] -= m4tmp[i][k]*temp;
  108.                     inverse[j][k] -= inverse[i][k]*temp;
  109.                     }
  110.                 }
  111.             }
  112.         }
  113.     return(1);
  114.  
  115. #undef ABS
  116. #undef SWAP
  117. }
  118.