home *** CD-ROM | disk | FTP | other *** search
/ Gold Fish 3 / goldfish_volume_3.bin / files / gfx / 3d / irit / misc_lib / genmat.c < prev    next >
Encoding:
C/C++ Source or Header  |  1994-10-13  |  24.7 KB  |  488 lines
  1. /*****************************************************************************
  2. * General matrix manipulation routines.                         *
  3. *                                         *
  4. * Written by:  Gershon Elber                   Ver 1.0,    June 1988    *
  5. *****************************************************************************/
  6.  
  7. #include <math.h>
  8. #include "irit_sm.h"
  9. #include "genmat.h"
  10.  
  11. /*****************************************************************************
  12. * DESCRIPTION:                                                               M
  13. * Routine to generate a 4*4 unit matrix:                                     M
  14. *                                                                            *
  15. * PARAMETERS:                                                                M
  16. *   Mat:      Matrix to initialize as a unit matrix.                         M
  17. *                                                                            *
  18. * RETURN VALUE:                                                              M
  19. *   void                                                                     M
  20. *                                                                            *
  21. * KEYWORDS:                                                                  M
  22. *   MatGenUnitMat, transformations, unit matrix                              M
  23. *****************************************************************************/
  24. void MatGenUnitMat(MatrixType Mat)
  25. {
  26.     int i, j;
  27.  
  28.     for (i = 0; i < 4; i++)
  29.     for (j = 0; j < 4; j++)
  30.         if (i == j)
  31.         Mat[i][j] = 1.0;
  32.         else
  33.         Mat[i][j] = 0.0;
  34. }
  35.  
  36. /*****************************************************************************
  37. * DESCRIPTION:                                                               M
  38. * Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       M
  39. *                                         *
  40. * PARAMETERS:                                                                M
  41. *   Tx, Ty, Tz:  Translational amounts requested.                            M
  42. *   Mat:         Matrix to initialize as a translation matrix.               M
  43. *                                                                            *
  44. * RETURN VALUE:                                                              M
  45. *   void                                                                     M
  46. *                                                                            *
  47. * KEYWORDS:                                                                  M
  48. *   MatGenMatTrans, transformations, translation                             M
  49. *****************************************************************************/
  50. void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
  51. {
  52.     MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  53.     Mat[3][0] = Tx;
  54.     Mat[3][1] = Ty;
  55.     Mat[3][2] = Tz;
  56. }
  57.  
  58. /*****************************************************************************
  59. * DESCRIPTION:                                                               M
  60. * Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   M
  61. *                                         *
  62. * PARAMETERS:                                                                M
  63. *   Sx, Sy, Sz:  Scaling factors requested.                                  M
  64. *   Mat:         Matrix to initialize as a scaling matrix.                   M
  65. *                                                                            *
  66. * RETURN VALUE:                                                              M
  67. *   void                                                                     M
  68. *                                                                            *
  69. * KEYWORDS:                                                                  M
  70. *   MatGenMatScale, transformations, scaling                                 M
  71. *****************************************************************************/
  72. void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
  73. {
  74.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  75.     Mat[0][0] = Sx;
  76.     Mat[1][1] = Sy;
  77.     Mat[2][2] = Sz;
  78. }
  79.  
  80. /*****************************************************************************
  81. * DESCRIPTION:                                                               M
  82. * Routine to generate a 4*4 matrix to uniformly scale Scale amount.         M
  83. *                                         *
  84. * PARAMETERS:                                                                M
  85. *   Scale:       Uniform scaling factor requested.                           M
  86. *   Mat:         Matrix to initialize as a scaling matrix.                   M
  87. *                                                                            *
  88. * RETURN VALUE:                                                              M
  89. *   void                                                                     M
  90. *                                                                            *
  91. * KEYWORDS:                                                                  M
  92. *   MatGenMatUnifScale, transformations, scaling                             M
  93. *****************************************************************************/
  94. void MatGenMatUnifScale(RealType Scale, MatrixType Mat)
  95. {
  96.     MatGenMatScale(Scale, Scale, Scale, Mat);
  97. }
  98.  
  99. /*****************************************************************************
  100. * DESCRIPTION:                                                               M
  101. * Routine to generate a 4*4 matrix to Rotate around the X axis by Teta       M
  102. * radians.                                                                   M
  103. *                                                                            *
  104. * PARAMETERS:                                                                M
  105. *   Teta:       Amount of rotation, in radians.                              M
  106. *   Mat:        Matrix to initialize as a rotation matrix.                   M
  107. *                                                                            *
  108. * RETURN VALUE:                                                              M
  109. *   void                                                                     M
  110. *                                                                            *
  111. * KEYWORDS:                                                                  M
  112. *   MatGenMatRotX1, transformations, rotation                                M
  113. *****************************************************************************/
  114. void MatGenMatRotX1(RealType Teta, MatrixType Mat)
  115. {
  116.     RealType CTeta, STeta;
  117.  
  118.     CTeta = cos(Teta);
  119.     STeta = sin(Teta);
  120.     MatGenMatRotX(CTeta, STeta, Mat);
  121. }
  122.  
  123. /*****************************************************************************
  124. * DESCRIPTION:                                                               M
  125. * Routine to generate a 4*4 matrix to Rotate around the X axis by Teta,      M
  126. * given the sin and cosine of Teta                                           M
  127. *                                                                            *
  128. * PARAMETERS:                                                                M
  129. *   SinTeta, CosTeta:  Amount of rotation, given as sine and cosine of Teta. M
  130. *   Mat:               Matrix to initialize as a rotation matrix.            M
  131. *                                                                            *
  132. * RETURN VALUE:                                                              M
  133. *   void                                                                     M
  134. *                                                                            *
  135. * KEYWORDS:                                                                  M
  136. *   MatGenMatRotX, transformations, rotation                                 M
  137. *****************************************************************************/
  138. void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  139. {
  140.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  141.     Mat[1][1] = CosTeta;
  142.     Mat[1][2] = SinTeta;
  143.     Mat[2][1] = -SinTeta;
  144.     Mat[2][2] = CosTeta;
  145. }
  146.  
  147. /*****************************************************************************
  148. * DESCRIPTION:                                                               M
  149. * Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta       M
  150. * radians.                                                                   M
  151. *                                                                            *
  152. * PARAMETERS:                                                                M
  153. *   Teta:       Amount of rotation, in radians.                              M
  154. *   Mat:        Matrix to initialize as a rotation matrix.                   M
  155. *                                                                            *
  156. * RETURN VALUE:                                                              M
  157. *   void                                                                     M
  158. *                                                                            *
  159. * KEYWORDS:                                                                  M
  160. *   MatGenMatRotY1, transformations, rotation                                M
  161. *****************************************************************************/
  162. void MatGenMatRotY1(RealType Teta, MatrixType Mat)
  163. {
  164.     RealType CTeta, STeta;
  165.  
  166.     CTeta = cos(Teta);
  167.     STeta = sin(Teta);
  168.     MatGenMatRotY(CTeta, STeta, Mat);
  169. }
  170.  
  171. /*****************************************************************************
  172. * DESCRIPTION:                                                               M
  173. * Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta,      M
  174. * given the sin and cosine of Teta                                           M
  175. *                                                                            *
  176. * PARAMETERS:                                                                M
  177. *   SinTeta, CosTeta:  Amount of rotation, given as sine and cosine of Teta. M
  178. *   Mat:               Matrix to initialize as a rotation matrix.            M
  179. *                                                                            *
  180. * RETURN VALUE:                                                              M
  181. *   void                                                                     M
  182. *                                                                            *
  183. * KEYWORDS:                                                                  M
  184. *   MatGenMatRotY, transformations, rotation                                 M
  185. *****************************************************************************/
  186. void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  187. {
  188.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  189.     Mat[0][0] = CosTeta;
  190.     Mat[0][2] = -SinTeta;
  191.     Mat[2][0] = SinTeta;
  192.     Mat[2][2] = CosTeta;
  193. }
  194.  
  195. /*****************************************************************************
  196. * DESCRIPTION:                                                               M
  197. * Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta       M
  198. * radians.                                                                   M
  199. *                                                                            *
  200. * PARAMETERS:                                                                M
  201. *   Teta:       Amount of rotation, in radians.                              M
  202. *   Mat:        Matrix to initialize as a rotation matrix.                   M
  203. *                                                                            *
  204. * RETURN VALUE:                                                              M
  205. *   void                                                                     M
  206. *                                                                            *
  207. * KEYWORDS:                                                                  M
  208. *   MatGenMatRotZ1, transformations, rotation                                M
  209. *****************************************************************************/
  210. void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
  211. {
  212.     RealType CTeta, STeta;
  213.  
  214.     CTeta = cos(Teta);
  215.     STeta = sin(Teta);
  216.     MatGenMatRotZ(CTeta, STeta, Mat);
  217. }
  218.  
  219. /*****************************************************************************
  220. * DESCRIPTION:                                                               M
  221. * Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta,      M
  222. * given the sin and cosine of Teta                                           M
  223. *                                                                            *
  224. * PARAMETERS:                                                                M
  225. *   SinTeta, CosTeta:  Amount of rotation, given as sine and cosine of Teta. M
  226. *   Mat:               Matrix to initialize as a rotation matrix.            M
  227. *                                                                            *
  228. * RETURN VALUE:                                                              M
  229. *   void                                                                     M
  230. *                                                                            *
  231. * KEYWORDS:                                                                  M
  232. *   MatGenMatRotZ, transformations, rotation                                 M
  233. *****************************************************************************/
  234. void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  235. {
  236.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  237.     Mat[0][0] = CosTeta;
  238.     Mat[0][1] = SinTeta;
  239.     Mat[1][0] = -SinTeta;
  240.     Mat[1][1] = CosTeta;
  241. }
  242.  
  243. /*****************************************************************************
  244. * DESCRIPTION:                                                               M
  245. * Routine to multiply 2 4by4 matrices.                                       M
  246. *  MatRes may be one of Mat1 or Mat2 - it is only updated in the end.        M
  247. *                                                                            *
  248. * PARAMETERS:                                                                M
  249. *   MatRes:      Result of matrix product.                                   M
  250. *   Mat1, Mat2:  The two operand of the matrix product.                      M
  251. *                                                                            *
  252. * RETURN VALUE:                                                              M
  253. *   void                                                                     M
  254. *                                                                            *
  255. * KEYWORDS:                                                                  M
  256. *   MatMultTwo4by4, transformations, matrix product                          M
  257. *****************************************************************************/
  258. void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  259. {
  260.     int i, j, k;
  261.     MatrixType MatResTemp;
  262.  
  263.     for (i = 0; i < 4; i++)
  264.     for (j = 0; j < 4; j++) {
  265.        MatResTemp[i][j] = 0;
  266.        for (k = 0; k < 4; k++)
  267.            MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
  268.     }
  269.     for (i = 0; i < 4; i++)
  270.     for (j = 0; j < 4; j++)
  271.         MatRes[i][j] = MatResTemp[i][j];
  272. }
  273.  
  274. /*****************************************************************************
  275. * DESCRIPTION:                                                               M
  276. * Routine to add 2 4by4 matrices.                         M
  277. *   MatRes may be one of Mat1 or Mat2.                         M
  278. *                                                                            *
  279. * PARAMETERS:                                                                M
  280. *   MatRes:      Result of matrix addition.                                  M
  281. *   Mat1, Mat2:  The two operand of the matrix addition.                     M
  282. *                                                                            *
  283. * RETURN VALUE:                                                              M
  284. *   void                                                                     M
  285. *                                                                            *
  286. * KEYWORDS:                                                                  M
  287. *   MatAddTwo4by4, transformations, matrix addition                          M
  288. *****************************************************************************/
  289. void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  290. {
  291.     int i, j;
  292.  
  293.     for (i = 0; i < 4; i++)
  294.         for (j = 0; j < 4; j++)
  295.             MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
  296. }
  297.  
  298. /*****************************************************************************
  299. * DESCRIPTION:                                                               M
  300. * Routine to subtract 2 4by4 matrices.                         M
  301. *   MatRes may be one of Mat1 or Mat2.                         M
  302. *                                                                            *
  303. * PARAMETERS:                                                                M
  304. *   MatRes:      Result of matrix subtraction.                               M
  305. *   Mat1, Mat2:  The two operand of the matrix subtraction.                  M
  306. *                                                                            *
  307. * RETURN VALUE:                                                              M
  308. *   void                                                                     M
  309. *                                                                            *
  310. * KEYWORDS:                                                                  M
  311. *   MatSubTwo4by4, transformations, matrix subtraction                       M
  312. *****************************************************************************/
  313. void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  314. {
  315.     int i, j;
  316.  
  317.     for (i = 0; i < 4; i++)
  318.         for (j = 0; j < 4; j++)
  319.         MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
  320. }
  321.  
  322. /*****************************************************************************
  323. * DESCRIPTION:                                                               M
  324. * Routine to scale a 4by4 matrix.                         M
  325. *   MatRes may be Mat.                                 M
  326. *                                                                            *
  327. * PARAMETERS:                                                                M
  328. *   MatRes:      Result of matrix scaling.                                   M
  329. *   Mat:         The two operand of the matrix scaling.                      M
  330. *   Scale:       Scalar value to multiple matrix with.                       M
  331. *                                                                            *
  332. * RETURN VALUE:                                                              M
  333. *   void                                                                     M
  334. *                                                                            *
  335. * KEYWORDS:                                                                  M
  336. *   MatScale4by4, transformations, matrix scaling                            M
  337. *****************************************************************************/
  338. void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
  339. {
  340.     int i, j;
  341.  
  342.     for (i = 0; i < 4; i++)
  343.         for (j = 0; j < 4; j++)
  344.         MatRes[i][j] = Mat[i][j] * (*Scale);
  345. }
  346.  
  347. /*****************************************************************************
  348. * DESCRIPTION:                                                               M
  349. * Routine to multiply an XYZ Vector by 4by4 matrix:                          M
  350. *   The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1  M
  351. *   VRes may be Vec as it is only updated in the end.                        M
  352. *                                                                            *
  353. * PARAMETERS:                                                                M
  354. *   VRes:      Result of vector - matrix product.                            M
  355. *   Vec:       Vector to transfrom using Matrix.                             M
  356. *   Mat:       Transformation matrix.                                        M
  357. *                                                                            *
  358. * RETURN VALUE:                                                              M
  359. *   void                                                                     M
  360. *                                                                            *
  361. * KEYWORDS:                                                                  M
  362. *   MatMultVecby4by4, transformations, vector matrix product                 M
  363. *****************************************************************************/
  364. void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
  365. {
  366.     int i, j;
  367.     RealType
  368.     CalcW = Mat[3][3];
  369.     VectorType VTemp;
  370.  
  371.     for (i = 0; i < 3; i++) {
  372.     VTemp[i] = Mat[3][i];         /* Initiate it with the weight factor. */
  373.     for (j = 0; j < 3; j++)
  374.         VTemp[i] += Vec[j] * Mat[j][i];
  375.     }
  376.  
  377.     for (i = 0; i < 3; i++)
  378.     CalcW += Vec[i] * Mat[i][3];
  379.     if (CalcW == 0)
  380.     CalcW = 1 / INFINITY;
  381.  
  382.     for (i = 0; i < 3; i++)
  383.     VRes[i] = VTemp[i] / CalcW;
  384. }
  385.  
  386. /*****************************************************************************
  387. * DESCRIPTION:                                                               M
  388. * Routine to multiply a WXYZ Vector by 4by4 matrix:                          M
  389. *   The Vector has only 4 components (W, X, Y, Z).                           M
  390. *   VRes may be Vec as it is only updated in the end.                        M
  391. *                                                                            *
  392. * PARAMETERS:                                                                M
  393. *   VRes:      Result of vector - matrix product.                            M
  394. *   Vec:       Vector to transfrom using Matrix.                             M
  395. *   Mat:       Transformation matrix.                                        M
  396. *                                                                            *
  397. * RETURN VALUE:                                                              M
  398. *   void                                                                     M
  399. *                                                                            *
  400. * KEYWORDS:                                                                  M
  401. *   MatMultWVecby4by4, transformations, vector matrix product                M
  402. *****************************************************************************/
  403. void MatMultWVecby4by4(RealType VRes[4], RealType Vec[4], MatrixType Mat)
  404. {
  405.     int i, j;
  406.     RealType VTemp[4];
  407.  
  408.     for (i = 0; i < 4; i++) {
  409.         VTemp[i] = 0.0;
  410.         for (j = 0; j < 4; j++)
  411.         VTemp[i] += Vec[j] * Mat[j][i];
  412.     }
  413.  
  414.     for (i = 0; i < 4; i++)
  415.     VRes[i] = VTemp[i];
  416. }
  417.  
  418. /*****************************************************************************
  419. * DESCRIPTION:                                                               M
  420. * Routine to compute the INVERSE of a given matrix M which is not modified.  M
  421. *   The matrix is assumed to be 4 by 4 (transformation matrix).             M
  422. *   Return TRUE if inverted matrix (InvM) do exists.                 M
  423. *                                                                            *
  424. * PARAMETERS:                                                                M
  425. *   M:          Original matrix to invert.                                   M
  426. *   InvM:       Inverted matrix will be placed here.                         M
  427. *                                                                            *
  428. * RETURN VALUE:                                                              M
  429. *   int:        TRUE if inverse exists, FALSE otherwise.                     M
  430. *                                                                            *
  431. * KEYWORDS:                                                                  M
  432. *   MatInverseMatrix, transformations, matrix inverse                        M
  433. *****************************************************************************/
  434. int MatInverseMatrix(MatrixType M, MatrixType InvM)
  435. {
  436.     MatrixType A;
  437.     int i, j, k;
  438.     RealType V;
  439.  
  440.     MAT_COPY(A, M);            /* Prepare temporary copy of M in A. */
  441.     MatGenUnitMat(InvM);                 /* Make it unit matrix. */
  442.  
  443.     for (i = 0; i < 4; i++) {
  444.     V = A[i][i];                      /* Find the new pivot. */
  445.     k = i;
  446.     for (j = i + 1; j < 4; j++)
  447.         if (ABS(A[j][i]) > ABS(V)) {
  448.             /* Find maximum on col i, row i+1..n */
  449.             V = A[j][i];
  450.             k = j;
  451.         }
  452.     j = k;
  453.  
  454.     if (i != j)
  455.         for (k = 0; k < 4; k++) {
  456.         SWAP(RealType, A[i][k], A[j][k]);
  457.         SWAP(RealType, InvM[i][k], InvM[j][k]);
  458.             }
  459.  
  460.     for (j = i + 1; j < 4; j++) {     /* Eliminate col i from row i+1..n. */
  461.             V = A[j][i] / A[i][i];
  462.         for (k = 0; k < 4; k++) {
  463.                 A[j][k]    -= V * A[i][k];
  464.                 InvM[j][k] -= V * InvM[i][k];
  465.         }
  466.     }
  467.     }
  468.  
  469.     for (i = 3; i >= 0; i--) {                   /* Back Substitution. */
  470.     if (A[i][i] == 0)
  471.         return FALSE;                       /* Error. */
  472.  
  473.     for (j = 0; j < i; j++) {     /* Eliminate col i from row 1..i-1. */
  474.             V = A[j][i] / A[i][i];
  475.         for (k = 0; k < 4; k++) {
  476.                 /* A[j][k] -= V * A[i][k]; */
  477.                 InvM[j][k] -= V * InvM[i][k];
  478.         }
  479.     }
  480.     }
  481.  
  482.     for (i = 0; i < 4; i++)            /* Normalize the inverse Matrix. */
  483.     for (j = 0; j < 4; j++)
  484.             InvM[i][j] /= A[i][i];
  485.  
  486.     return TRUE;
  487. }
  488.