home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 3 / RISC_DISC_3.iso / resources / etexts / gems / gemsii / unmatrix.c < prev    next >
C/C++ Source or Header  |  1993-10-07  |  5KB  |  171 lines
  1. /* unmatrix.c - given a 4x4 matrix, decompose it into standard operations.
  2.  *
  3.  * Author:    Spencer W. Thomas
  4.  *         University of Michigan
  5.  */
  6. #include <math.h>
  7. #include "GraphicsGems.h"
  8. #include "unmatrix.h"
  9.  
  10. /* unmatrix - Decompose a non-degenerate 4x4 transformation matrix into
  11.  *     the sequence of transformations that produced it.
  12.  * [Sx][Sy][Sz][Shearx/y][Sx/z][Sz/y][Rx][Ry][Rz][Tx][Ty][Tz][P(x,y,z,w)]
  13.  *
  14.  * The coefficient of each transformation is returned in the corresponding
  15.  * element of the vector tran.
  16.  *
  17.  * Returns 1 upon success, 0 if the matrix is singular.
  18.  */
  19. int
  20. unmatrix( mat, tran )
  21. Matrix4 *mat;
  22. double tran[16];
  23. {
  24.      register int i, j;
  25.      Matrix4 locmat;
  26.      Matrix4 pmat, invpmat, tinvpmat;
  27.      /* Vector4 type and functions need to be added to the common set. */
  28.      Vector4 prhs, psol;
  29.      Point3 row[3], pdum3;
  30.  
  31.      locmat = *mat;
  32.      /* Normalize the matrix. */
  33.      if ( pmat.element[3][3] = 0 )
  34.          return 0;
  35.      for ( i=0; i<4;i++ )
  36.          for ( j=0; j<4; j++ )
  37.              locmat.element[i][j] /= locmat.element[3][3];
  38.      /* pmat is used to solve for perspective, but it also provides
  39.       * an easy way to test for singularity of the upper 3x3 component.
  40.       */
  41.      pmat = locmat;
  42.      for ( i=0; i<3; i++ )
  43.          pmat.element[i][3] = 0;
  44.      pmat.element[3][3] = 1;
  45.  
  46.      if ( det4x4(pmat) == 0.0 )
  47.          return 0;
  48.  
  49.      /* First, isolate perspective.  This is the messiest. */
  50.      if ( locmat.element[0][3] != 0 || locmat.element[1][3] != 0 ||
  51.          locmat.element[2][3] != 0 ) {
  52.          /* prhs is the right hand side of the equation. */
  53.          prhs.x = locmat.element[0][3];
  54.          prhs.y = locmat.element[1][3];
  55.          prhs.z = locmat.element[2][3];
  56.          prhs.w = locmat.element[3][3];
  57.  
  58.          /* Solve the equation by inverting pmat and multiplying
  59.           * prhs by the inverse.  (This is the easiest way, not
  60.           * necessarily the best.)
  61.           * inverse function (and det4x4, above) from the Matrix
  62.           * Inversion gem in the first volume.
  63.           */
  64.          inverse( &pmat, &invpmat );
  65.         TransposeMatrix4( &invpmat, &tinvpmat );
  66.          V4MulPointByMatrix(&prhs, &tinvpmat, &psol);
  67.  
  68.          /* Stuff the answer away. */
  69.          tran[U_PERSPX] = psol.x;
  70.          tran[U_PERSPY] = psol.y;
  71.          tran[U_PERSPZ] = psol.z;
  72.          tran[U_PERSPW] = psol.w;
  73.          /* Clear the perspective partition. */
  74.          locmat.element[0][3] = locmat.element[1][3] =
  75.              locmat.element[2][3] = 0;
  76.          locmat.element[3][3] = 1;
  77.      } else        /* No perspective. */
  78.          tran[U_PERSPX] = tran[U_PERSPY] = tran[U_PERSPZ] =
  79.              tran[U_PERSPW] = 0;
  80.  
  81.      /* Next take care of translation (easy). */
  82.      for ( i=0; i<3; i++ ) {
  83.          tran[U_TRANSX + i] = locmat.element[3][i];
  84.          locmat.element[3][i] = 0;
  85.      }
  86.  
  87.      /* Now get scale and shear. */
  88.      for ( i=0; i<3; i++ ) {
  89.          row[i].x = locmat.element[i][0];
  90.          row[i].y = locmat.element[i][1];
  91.          row[i].z = locmat.element[i][2];
  92.      }
  93.  
  94.      /* Compute X scale factor and normalize first row. */
  95.      tran[U_SCALEX] = V3Length(&row[0]);
  96.      row[0] = *V3Scale(&row[0], 1.0);
  97.  
  98.      /* Compute XY shear factor and make 2nd row orthogonal to 1st. */
  99.      tran[U_SHEARXY] = V3Dot(&row[0], &row[1]);
  100.      (void)V3Combine(&row[1], &row[0], &row[1], 1.0, -tran[U_SHEARXY]);
  101.  
  102.      /* Now, compute Y scale and normalize 2nd row. */
  103.      tran[U_SCALEY] = V3Length(&row[1]);
  104.      V3Scale(&row[1], 1.0);
  105.      tran[U_SHEARXY] /= tran[U_SCALEY];
  106.  
  107.      /* Compute XZ and YZ shears, orthogonalize 3rd row. */
  108.      tran[U_SHEARXZ] = V3Dot(&row[0], &row[2]);
  109.      (void)V3Combine(&row[2], &row[0], &row[2], 1.0, -tran[U_SHEARXZ]);
  110.      tran[U_SHEARYZ] = V3Dot(&row[1], &row[2]);
  111.      (void)V3Combine(&row[2], &row[1], &row[2], 1.0, -tran[U_SHEARYZ]);
  112.  
  113.      /* Next, get Z scale and normalize 3rd row. */
  114.      tran[U_SCALEZ] = V3Length(&row[2]);
  115.      V3Scale(&row[2], 1.0);
  116.      tran[U_SHEARXZ] /= tran[U_SCALEZ];
  117.      tran[U_SHEARYZ] /= tran[U_SCALEZ];
  118.  
  119.      /* At this point, the matrix (in rows[]) is orthonormal.
  120.       * Check for a coordinate system flip.  If the determinant
  121.       * is -1, then negate the matrix and the scaling factors.
  122.       */
  123.      if ( V3Dot( &row[0], V3Cross( &row[1], &row[2], &pdum3) ) < 0 )
  124.          for ( i = 0; i < 3; i++ ) {
  125.              tran[U_SCALEX+i] *= -1;
  126.              row[i].x *= -1;
  127.              row[i].y *= -1;
  128.              row[i].z *= -1;
  129.          }
  130.  
  131.      /* Now, get the rotations out, as described in the gem. */
  132.      tran[U_ROTATEY] = asin(-row[0].z);
  133.      if ( cos(tran[U_ROTATEY]) != 0 ) {
  134.          tran[U_ROTATEX] = atan2(row[1].z, row[2].z);
  135.          tran[U_ROTATEZ] = atan2(row[0].y, row[0].x);
  136.      } else {
  137.          tran[U_ROTATEX] = atan2(row[1].x, row[1].y);
  138.          tran[U_ROTATEZ] = 0;
  139.      }
  140.      /* All done! */
  141.      return 1;
  142. }
  143.  
  144.  
  145. /* transpose rotation portion of matrix a, return b */
  146. Matrix4 *TransposeMatrix4(a, b)
  147. Matrix4 *a, *b;
  148. {
  149. int i, j;
  150.     for (i=0; i<4; i++)
  151.         for (j=0; j<4; j++)
  152.             b->element[i][j] = a->element[j][i];
  153.     return(b);
  154. }
  155.  
  156. /* multiply a hom. point by a matrix and return the transformed point */
  157. Vector4 *V4MulPointByMatrix(pin, m, pout)
  158. Vector4 *pin, *pout;
  159. Matrix4 *m;
  160. {
  161.     pout->x = (pin->x * m->element[0][0]) + (pin->y * m->element[1][0]) +
  162.         (pin->z * m->element[2][0]) + (pin->w * m->element[3][0]);
  163.     pout->y = (pin->x * m->element[0][1]) + (pin->y * m->element[1][1]) +
  164.         (pin->z * m->element[2][1]) + (pin->w * m->element[3][1]);
  165.     pout->z = (pin->x * m->element[0][2]) + (pin->y * m->element[1][2]) +
  166.         (pin->z * m->element[2][2]) + (pin->w * m->element[3][2]);
  167.     pout->w = (pin->x * m->element[0][3]) + (pin->y * m->element[1][3]) +
  168.         (pin->z * m->element[2][3]) + (pin->w * m->element[3][3]);
  169.         return(pout);
  170. }
  171.