home *** CD-ROM | disk | FTP | other *** search
/ Photo CD Demo 1 / Demo.bin / graphtal / trnsmtrx.c < prev    next >
C/C++ Source or Header  |  1992-10-22  |  10KB  |  375 lines
  1. /*
  2.  * TransMatrix.C - methods for general 4x3 transformation matrices.
  3.  *
  4.  * Copyright (C) 1992, Christoph Streit (streit@iam.unibe.ch)
  5.  * All rights reserved.
  6.  *
  7.  * This software may be freely copied, modified, and redistributed
  8.  * provided that this copyright notice is preserved on all copies.
  9.  *
  10.  * You may not distribute this software, in whole or in part, as part of
  11.  * any commercial product without the express consent of the authors.
  12.  *
  13.  * There is no warranty or other guarantee of fitness of this software
  14.  * for any purpose.  It is provided solely "as is".
  15.  *
  16.  */
  17.  
  18. #include "TransMatrix.h"
  19.  
  20. void SinCos(real alpha, real& sin_a, real& cos_a)
  21. {
  22.   const real tolerance = 1e-10;
  23.  
  24.   sin_a = sin(alpha);
  25.   cos_a = cos(alpha);
  26.  
  27.   if (cos_a > 1-tolerance) {
  28.     cos_a = 1; sin_a = 0;
  29.   }
  30.   else if (cos_a < -1+tolerance) {
  31.     cos_a = -1; sin_a = 0;
  32.   }
  33.  
  34.   if (sin_a > 1-tolerance) {
  35.     cos_a = 0; sin_a = 1;
  36.   }
  37.   else if (sin_a < -1+tolerance) {
  38.     cos_a = 0; sin_a = -1;
  39.   }
  40. }
  41.  
  42. //___________________________________________________________ TransMatrix
  43.  
  44.  
  45. TransMatrix::TransMatrix()
  46. {
  47.   for (register int i=0; i<4; i++)
  48.     for (register int j=0; j<3; j++)
  49.       m[i][j] = (i == j);
  50. }
  51.   
  52. TransMatrix::TransMatrix(const Vector& v1, const Vector& v2, 
  53.              const Vector& v3)
  54. {
  55.   m[0][0] = v1[0]; m[0][1] = v1[1]; m[0][2] = v1[2];
  56.   m[1][0] = v2[0]; m[1][1] = v2[1]; m[1][2] = v2[2];  
  57.   m[2][0] = v3[0]; m[2][1] = v3[1]; m[2][2] = v3[2];  
  58.   m[3][0] = m[3][1] = m[3][2] = 0;
  59. }
  60.  
  61. TransMatrix::TransMatrix(const Vector&v1, const Vector& v2, 
  62.              const Vector& v3, const Vector& v4)
  63. {
  64.   m[0][0] = v1[0]; m[0][1] = v1[1]; m[0][2] = v1[2];
  65.   m[1][0] = v2[0]; m[1][1] = v2[1]; m[1][2] = v2[2];  
  66.   m[2][0] = v3[0]; m[2][1] = v3[1]; m[2][2] = v3[2];  
  67.   m[3][0] = v4[0]; m[3][1] = v4[1]; m[3][2] = v4[2];  
  68. }
  69.  
  70. TransMatrix::TransMatrix(const TransMatrix& t)
  71. {
  72.   for (register int i=0; i<4; i++)
  73.     for (register int j=0; j<3; j++)
  74.       m[i][j] = t.m[i][j];
  75. }
  76.  
  77. const TransMatrix& TransMatrix::operator=(const TransMatrix& t)
  78. {
  79.   for (register int i=0; i<4; i++)
  80.     for (register int j=0; j<3; j++)
  81.       m[i][j] = t.m[i][j];
  82.   return *this;
  83. }
  84.   
  85. real& TransMatrix::operator()(int i, int j)
  86. {
  87.   if (i<0 || i>3 || j<0 || j>2)
  88.     Error(ERR_PANIC, "TransMatrix::operator(i,j) index out of range");
  89.  
  90.   return m[i][j];
  91. }
  92.  
  93. real TransMatrix::operator()(int i, int j) const
  94. {
  95.   if (i<0 || i>3 || j<0 || j>2)
  96.     Error(ERR_PANIC, "TransMatrix::operator(i,j) index out of range");
  97.  
  98.   return m[i][j];
  99. }
  100.  
  101. TransMatrix& TransMatrix::operator+=(const TransMatrix& t)
  102. {
  103.   for (register int i=0; i<4; i++)
  104.     for (register int j=0; j<3; j++)
  105.       m[i][j] += t.m[i][j];
  106.   return *this;
  107. }
  108.  
  109. TransMatrix& TransMatrix::operator-=(const TransMatrix& t)
  110. {
  111.   for (register int i=0; i<4; i++)
  112.     for (register int j=0; j<3; j++)
  113.       m[i][j] -= t.m[i][j];
  114.   return *this;
  115. }
  116.  
  117. TransMatrix& TransMatrix::operator*=(const TransMatrix& t)
  118. {
  119.   TransMatrix tmp(*this);
  120.  
  121.   for (register int i=0; i<3; i++) {
  122.     m[0][i] = tmp.m[0][0]*t.m[0][i] + tmp.m[0][1]*t.m[1][i] + tmp.m[0][2]*t.m[2][i];
  123.     m[1][i] = tmp.m[1][0]*t.m[0][i] + tmp.m[1][1]*t.m[1][i] + tmp.m[1][2]*t.m[2][i];
  124.     m[2][i] = tmp.m[2][0]*t.m[0][i] + tmp.m[2][1]*t.m[1][i] + tmp.m[2][2]*t.m[2][i];
  125.     m[3][i] = tmp.m[3][0]*t.m[0][i] + tmp.m[3][1]*t.m[1][i] + tmp.m[3][2]*t.m[2][i] +
  126.               t.m[3][i];
  127.   }
  128.  
  129.   return *this;
  130. }
  131.  
  132. TransMatrix TransMatrix::operator-()
  133. {
  134.   TransMatrix result;
  135.  
  136.   for (register int i=0; i<4; i++)
  137.     for (register int j=0; j<3; j++)
  138.       result.m[i][j] = m[i][j];
  139.   return result;
  140. }
  141.  
  142. TransMatrix TransMatrix::operator+(const TransMatrix& t)
  143. {
  144.   TransMatrix result;
  145.  
  146.   for (register int i=0; i<4; i++)
  147.     for (register int j=0; j<3; j++)
  148.       result.m[i][j] = m[i][j] + t.m[i][j];
  149.   return result;
  150. }
  151.  
  152. TransMatrix TransMatrix::operator-(const TransMatrix& t)
  153. {
  154.   TransMatrix result;
  155.  
  156.   for (register int i=0; i<4; i++)
  157.     for (register int j=0; j<3; j++)
  158.       result.m[i][j] = m[i][j] - t.m[i][j];
  159.   return result;
  160.  
  161. }
  162.  
  163. TransMatrix TransMatrix::operator*(const TransMatrix& t)
  164. {
  165.   TransMatrix result;
  166.  
  167.   for (register int i=0; i<3; i++) {
  168.     result.m[0][i] = m[0][0]*t.m[0][i] + m[0][1]*t.m[1][i] + m[0][2]*t.m[2][i];
  169.     result.m[1][i] = m[1][0]*t.m[0][i] + m[1][1]*t.m[1][i] + m[1][2]*t.m[2][i];
  170.     result.m[2][i] = m[2][0]*t.m[0][i] + m[2][1]*t.m[1][i] + m[2][2]*t.m[2][i];
  171.     result.m[3][i] = m[3][0]*t.m[0][i] + m[3][1]*t.m[1][i] + m[3][2]*t.m[2][i] 
  172.                      + t.m[3][i];
  173.   }
  174.  
  175.   return result;
  176. }
  177.  
  178. /*
  179.  * Compute the inverse of a 3D affine matrix; i.e. a matrix with a dimen-
  180.  * sionality of 4 where the right column has entries (0, 0, 0, 1).
  181.  *
  182.  * This procedures treats the 4 by 4 matrix as ablock matrix and calculates
  183.  * the inverse of one submatrix for a significant performance improvement
  184.  * over a general procedure that can invert any nonsingular matrix:
  185.  *        --      --       --         --
  186.  *        |        | -1    |   -1      |
  187.  *        | A    0 |       |  A      0 |
  188.  *   -1   |        |       |           |
  189.  *  M   = |        |    =  |     -1    |
  190.  *        | C    1 |       | -C A    1 |
  191.  *        |        |       |           |
  192.  *        --      --       --         --
  193.  * 
  194.  * where  M is a 4 by 4 matrix,
  195.  *        A is the 3 by 3 upper left submatrix of M,
  196.  *        C is the 1 by 3 lower left subnatrix of M.
  197.  *
  198.  * Returned value:
  199.  *   1   matrix is nonsingular
  200.  *   0   otherwise
  201.  *
  202.  * Reference GraphicsGems I.
  203.  */
  204.  
  205. int TransMatrix::invert()
  206. {
  207.   TransMatrix ttmp;
  208.   real det;
  209.  
  210.   /*
  211.    * Calculate the determinant of submatrix A (optimized version:
  212.    * don,t just compute the determinant of A)
  213.    */
  214.   ttmp.m[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
  215.   ttmp.m[1][0] = m[1][0]*m[2][2] - m[1][2]*m[2][0];
  216.   ttmp.m[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
  217.  
  218.   ttmp.m[0][1] = m[0][1]*m[2][2] - m[0][2]*m[2][1];
  219.   ttmp.m[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];
  220.   ttmp.m[2][1] = m[0][0]*m[2][1] - m[0][1]*m[2][0];
  221.  
  222.   ttmp.m[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];
  223.   ttmp.m[1][2] = m[0][0]*m[1][2] - m[0][2]*m[1][0];
  224.   ttmp.m[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
  225.  
  226.   det = m[0][0]*ttmp.m[0][0] - m[0][1]*ttmp.m[1][0] + m[0][2]*ttmp.m[2][0];
  227.  
  228.   /*
  229.    * singular matrix ?
  230.    */
  231.   if (fabs(det) < EPSILON*EPSILON)
  232.     return 0;
  233.  
  234.   det = 1/det;
  235.  
  236.   /*
  237.    * inverse(A) = adj(A)/det(A)
  238.    */
  239.   ttmp.m[0][0] *= det;
  240.   ttmp.m[0][2] *= det;
  241.   ttmp.m[1][1] *= det;
  242.   ttmp.m[2][0] *= det;
  243.   ttmp.m[2][2] *= det;
  244.  
  245.   det = -det;
  246.  
  247.   ttmp.m[0][1] *= det;
  248.   ttmp.m[1][0] *= det;
  249.   ttmp.m[1][2] *= det;
  250.   ttmp.m[2][1] *= det;
  251.  
  252.   ttmp.m[3][0] = -(ttmp.m[0][0]*m[3][0] + ttmp.m[1][0]*m[3][1] + ttmp.m[2][0]*m[3][2]);
  253.   ttmp.m[3][1] = -(ttmp.m[0][1]*m[3][0] + ttmp.m[1][1]*m[3][1] + ttmp.m[2][1]*m[3][2]);
  254.   ttmp.m[3][2] = -(ttmp.m[0][2]*m[3][0] + ttmp.m[1][2]*m[3][1] + ttmp.m[2][2]*m[3][2]);
  255.   
  256.   *this = ttmp;
  257.  
  258.   return 1;
  259. }
  260.  
  261. void TransMatrix::setRotate(const Vector& v, real alpha)
  262. {
  263.   real sin_a, cos_a, n1, n2, n3;
  264.   Vector n(v.normalized());
  265.  
  266.   SinCos(alpha, sin_a, cos_a);
  267.  
  268.   n1 = n[0]; n2 = n[1]; n3 = n[2];
  269.  
  270.   m[0][0] = (n1*n1 + (1. - n1*n1)*cos_a);
  271.   m[0][1] = (n1*n2*(1. - cos_a) + n3*sin_a);
  272.   m[0][2] = (n1*n3*(1. - cos_a) - n2*sin_a);
  273.   m[1][0] = (n1*n2*(1. - cos_a) - n3*sin_a);
  274.   m[1][1] = (n2*n2 + (1. - n2*n2)*cos_a);
  275.   m[1][2] = (n2*n3*(1. - cos_a) + n1*sin_a);
  276.   m[2][0] = (n1*n3*(1. - cos_a) + n2*sin_a);
  277.   m[2][1] = (n2*n3*(1. - cos_a) - n1*sin_a);
  278.   m[2][2] = (n3*n3 + (1. - n3*n3)*cos_a);
  279.   m[3][0] = m[3][1] = m[3][2] = 0;
  280. }
  281.  
  282. TransMatrix& TransMatrix::rotate(const Vector& v, real alpha)
  283. {
  284.   TransMatrix rot;
  285.   rot.setRotate(v, alpha);
  286.   return this->operator*=(rot);
  287. }
  288.  
  289. TransMatrix& TransMatrix::rotate(Axis a, const real sin_a, const real cos_a)
  290. {
  291.   static real m00, m01, m10, m11, m20, m21, m30, m31;
  292.  
  293.   switch(a) {
  294.   case X:
  295.     m01 = m[0][1]; m11 = m[1][1]; m21 = m[2][1]; m31 = m[3][1]; 
  296.  
  297.     m[0][1] = m01*cos_a-m[0][2]*sin_a; m[0][2] = m01*sin_a+m[0][2]*cos_a;
  298.     m[1][1] = m11*cos_a-m[1][2]*sin_a; m[1][2] = m11*sin_a+m[1][2]*cos_a;
  299.     m[2][1] = m21*cos_a-m[2][2]*sin_a; m[2][2] = m21*sin_a+m[2][2]*cos_a;
  300.     m[3][1] = m31*cos_a-m[3][2]*sin_a; m[3][2] = m31*sin_a+m[3][2]*cos_a;
  301.     break;
  302.  
  303.   case Y:
  304.     m00 = m[0][0]; m10 = m[1][0]; m20 = m[2][0]; m30 = m[3][0]; 
  305.  
  306.     m[0][0] = m00*cos_a-m[0][2]*sin_a; m[0][2] = m00*sin_a+m[0][2]*cos_a;
  307.     m[1][0] = m10*cos_a-m[1][2]*sin_a; m[1][2] = m10*sin_a+m[1][2]*cos_a;
  308.     m[2][0] = m20*cos_a-m[2][2]*sin_a; m[2][2] = m20*sin_a+m[2][2]*cos_a;
  309.     m[3][0] = m30*cos_a-m[3][2]*sin_a; m[3][2] = m30*sin_a+m[3][2]*cos_a;
  310.     break;
  311.  
  312.   case Z:
  313.     m00 = m[0][0]; m10 = m[1][0]; m20 = m[2][0]; m30 = m[3][0]; 
  314.  
  315.     m[0][0] = m00*cos_a-m[0][1]*sin_a; m[0][1] = m00*sin_a+m[0][1]*cos_a;
  316.     m[1][0] = m10*cos_a-m[1][1]*sin_a; m[1][1] = m10*sin_a+m[1][1]*cos_a;
  317.     m[2][0] = m20*cos_a-m[2][1]*sin_a; m[2][1] = m20*sin_a+m[2][1]*cos_a;
  318.     m[3][0] = m30*cos_a-m[3][1]*sin_a; m[3][1] = m30*sin_a+m[3][1]*cos_a;
  319.     break;
  320.  
  321.   default:
  322.     Error(ERR_PANIC, "TransMatrix::rotate illegal value for axis");
  323.   }
  324.   
  325.   return *this;
  326. }
  327.  
  328. TransMatrix& TransMatrix::rotate(Axis a, const real alpha)
  329. {
  330.   real sin_a, cos_a;
  331.  
  332.   SinCos(alpha, sin_a, cos_a);
  333.  
  334.   return this->rotate(a, sin_a, cos_a);
  335. }  
  336.  
  337. TransMatrix& TransMatrix::scale(const real Sx, const real Sy, const real Sz)
  338. {
  339.   for (register int i=0; i<4; i++) {
  340.     m[i][0] *= Sx;
  341.     m[i][1] *= Sy;
  342.     m[i][2] *= Sz;
  343.   }
  344.  
  345.   return *this;
  346. }  
  347.  
  348. TransMatrix& TransMatrix::translate(const Vector& T)
  349. {
  350.   m[3][0] += T[0];     
  351.   m[3][1] += T[1];     
  352.   m[3][2] += T[2];     
  353.  
  354.   return *this;
  355. }  
  356.  
  357. Vector operator*(const Vector &v, const TransMatrix& t)
  358. {
  359.   return Vector(t.m[0][0]*v[0] + t.m[1][0]*v[1] + t.m[2][0]*v[2] + t.m[3][0],
  360.         t.m[0][1]*v[0] + t.m[1][1]*v[1] + t.m[2][1]*v[2] + t.m[3][1],
  361.         t.m[0][2]*v[0] + t.m[1][2]*v[1] + t.m[2][2]*v[2] + t.m[3][2]);
  362. }
  363.  
  364. ostream& operator<<(ostream &os, const TransMatrix& t)
  365. {
  366.   for (register int i=0; i<4; i++) {
  367.     os << "\t( " << t.m[i][0] << ' ' << t.m[i][1] << ' ' <<t.m[i][2] << " )\n";
  368.   }
  369.  
  370.   return os;
  371. }
  372.  
  373.  
  374.  
  375.