home *** CD-ROM | disk | FTP | other *** search
/ Tricks of the Mac Game Programming Gurus / TricksOfTheMacGameProgrammingGurus.iso / More Source / C⁄C++ / Peter's Final Project / src / matrix.vector.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-05-10  |  12.4 KB  |  617 lines  |  [TEXT/KAHL]
  1. /*
  2.  *  Peter's Final Project -- A texture mapping demonstration
  3.  *  © 1995, Peter Mattis
  4.  *
  5.  *  E-mail:
  6.  *  petm@soda.csua.berkeley.edu
  7.  *
  8.  *  Snail-mail:
  9.  *   Peter Mattis
  10.  *   557 Fort Laramie Dr.
  11.  *   Sunnyvale, CA 94087
  12.  *
  13.  *  Avaible from:
  14.  *  http://www.csua.berkeley.edu/~petm/final.html
  15.  *
  16.  *  This program is free software; you can redistribute it and/or modify
  17.  *  it under the terms of the GNU General Public License as published by
  18.  *  the Free Software Foundation; either version 2 of the License, or
  19.  *  (at your option) any later version.
  20.  *
  21.  *  This program is distributed in the hope that it will be useful,
  22.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  23.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  24.  *  GNU General Public License for more details.
  25.  *
  26.  *  You should have received a copy of the GNU General Public License
  27.  *  along with this program; if not, write to the Free Software
  28.  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  29.  */
  30.  
  31. #include <math.h>
  32. #include <stdio.h>
  33. #include <stdlib.h>
  34. #include "matrix.vector.h"
  35. #include "utils.h"
  36.  
  37. /*
  38.  * Print a matrix.
  39.  */
  40.  
  41. void
  42. matrix_describe (m)
  43.     MATRIX m;
  44. {
  45.     printf ("( ");
  46.     printf ("%7.3f ", matrix_element (m, 0, 0));
  47.     printf ("%7.3f ", matrix_element (m, 0, 1));
  48.     printf ("%7.3f ", matrix_element (m, 0, 2));
  49.     printf ("%7.3f ", matrix_element (m, 0, 3));
  50.     printf ("\n");
  51.  
  52.     printf ("  ");
  53.     printf ("%7.3f ", matrix_element (m, 1, 0));
  54.     printf ("%7.3f ", matrix_element (m, 1, 1));
  55.     printf ("%7.3f ", matrix_element (m, 1, 2));
  56.     printf ("%7.3f ", matrix_element (m, 1, 3));
  57.     printf ("\n");
  58.  
  59.     printf ("  ");
  60.     printf ("%7.3f ", matrix_element (m, 2, 0));
  61.     printf ("%7.3f ", matrix_element (m, 2, 1));
  62.     printf ("%7.3f ", matrix_element (m, 2, 2));
  63.     printf ("%7.3f ", matrix_element (m, 2, 3));
  64.     printf ("\n");
  65.  
  66.     printf ("  ");
  67.     printf ("%7.3f ", matrix_element (m, 3, 0));
  68.     printf ("%7.3f ", matrix_element (m, 3, 1));
  69.     printf ("%7.3f ", matrix_element (m, 3, 2));
  70.     printf ("%7.3f ", matrix_element (m, 3, 3));
  71.     printf (")\n");
  72. }
  73.  
  74. /*
  75.  * Clear a matrix to the identity matrix.
  76.  */
  77.  
  78. void
  79. matrix_clear (m)
  80.     MATRIX m;
  81. {
  82.     register short i, j;
  83.  
  84.     for (i = 0; i < 4; i++)
  85.     {
  86.         for (j = 0; j < 4; j++)
  87.             set_matrix_element (m, i, j, NUM_ZERO);
  88.         set_matrix_element (m, i, i, NUM_ONE);
  89.     }
  90. }
  91.  
  92. /*
  93.  * Copy a matrix.
  94.  */
  95.  
  96. void
  97. matrix_copy (src, dest)
  98.     MATRIX src, dest;
  99. {
  100.     register short i, j;
  101.  
  102.     for (i = 0; i < 4; i++)
  103.         for (j = 0; j < 4; j++)
  104.             set_matrix_element (dest, i, j, matrix_element (src, i, j));
  105. }
  106.  
  107. /*
  108.  * Multiply a matrix by a scalar.
  109.  */
  110.  
  111. void
  112. matrix_multiply (m, k)
  113.     MATRIX m;
  114.     NUM k;
  115. {
  116.     register short i, j;
  117.  
  118.     if (k != 0)
  119.     {
  120.         for (i = 0; i < 4; i++)
  121.             for (j = 0; j < 4; j++)
  122.                 set_matrix_element (m, i, j, my_mul (matrix_element (m, i, j), k));
  123.     }
  124. }
  125.  
  126. /*
  127.  * Construct a translation matrix.
  128.  */
  129.  
  130. void
  131. matrix_translate (m, tx, ty, tz)
  132.     MATRIX m;
  133.     NUM tx, ty, tz;
  134. {
  135.     matrix_clear (m);
  136.     set_matrix_element (m, 0, 3, tx);
  137.     set_matrix_element (m, 1, 3, ty);
  138.     set_matrix_element (m, 2, 3, tz);
  139. }
  140.  
  141. /*
  142.  * Construct a scale matrix.
  143.  */
  144.  
  145. void
  146. matrix_scale (m, sx, sy, sz)
  147.     MATRIX m;
  148.     NUM sx, sy, sz;
  149. {
  150.     matrix_clear (m);
  151.     set_matrix_element (m, 0, 0, sx);
  152.     set_matrix_element (m, 1, 1, sy);
  153.     set_matrix_element (m, 2, 2, sz);
  154. }
  155.  
  156. /*
  157.  * Construct an x-axis rotation matrix.
  158.  */
  159.  
  160. void
  161. matrix_rotate_x (m, theta)
  162.     MATRIX m;
  163.     NUM theta;
  164. {
  165.     NUM sine, cosine;
  166.  
  167.     sine = my_float_to_num (sin (my_num_to_float (theta)));
  168.     cosine = my_float_to_num (cos (my_num_to_float (theta)));
  169.  
  170.     matrix_clear (m);
  171.     set_matrix_element (m, 1, 1, cosine);
  172.     set_matrix_element (m, 1, 2, -sine);
  173.     set_matrix_element (m, 2, 1, sine);
  174.     set_matrix_element (m, 2, 2, cosine);
  175. }
  176.  
  177. /*
  178.  * Construct an y-axis rotation matrix.
  179.  */
  180.  
  181. void
  182. matrix_rotate_y (m, theta)
  183.     MATRIX m;
  184.     NUM theta;
  185. {
  186.     NUM sine, cosine;
  187.  
  188.     sine = my_float_to_num (sin (my_num_to_float (theta)));
  189.     cosine = my_float_to_num (cos (my_num_to_float (theta)));
  190.  
  191.     matrix_clear (m);
  192.     set_matrix_element (m, 0, 0, cosine);
  193.     set_matrix_element (m, 0, 2, sine);
  194.     set_matrix_element (m, 2, 0, -sine);
  195.     set_matrix_element (m, 2, 2, cosine);
  196. }
  197.  
  198. /*
  199.  * Construct an z-axis rotation matrix.
  200.  */
  201.  
  202. void
  203. matrix_rotate_z (m, theta)
  204.     MATRIX m;
  205.     NUM theta;
  206. {
  207.     NUM sine, cosine;
  208.  
  209.     sine = my_float_to_num (sin (my_num_to_float (theta)));
  210.     cosine = my_float_to_num (cos (my_num_to_float (theta)));
  211.  
  212.     matrix_clear (m);
  213.     set_matrix_element (m, 0, 0, cosine);
  214.     set_matrix_element (m, 0, 1, -sine);
  215.     set_matrix_element (m, 1, 0, sine);
  216.     set_matrix_element (m, 1, 1, cosine);
  217. }
  218.  
  219. /*
  220.  * Construct a projection matrix.
  221.  */
  222.  
  223. void
  224. matrix_projection (m, d)
  225.     MATRIX m;
  226.     NUM d;
  227. {
  228.     matrix_clear (m);
  229.     set_matrix_element (m, 3, 2, my_div (NUM_ONE, d));
  230.     set_matrix_element (m, 3, 3, 0);
  231. }
  232.  
  233. /*
  234.  * Compose two matrices. (Matrix multiplication).
  235.  */ 
  236.  
  237. void
  238. matrix_compose (a, b, ab)
  239.     MATRIX a, b, ab;
  240. {
  241.     register short i, j;
  242.  
  243.     for (i = 0; i < 4; i++)
  244.     {
  245.         for (j = 0; j < 4; j++)
  246.         {
  247.             set_matrix_element (ab, i, j,
  248.                 (my_mul (matrix_element (a, i, 0), matrix_element (b, 0, j)) +
  249.                  my_mul (matrix_element (a, i, 1), matrix_element (b, 1, j)) +
  250.                  my_mul (matrix_element (a, i, 2), matrix_element (b, 2, j)) +
  251.                  my_mul (matrix_element (a, i, 3), matrix_element (b, 3, j))));
  252.         }
  253.     }
  254. }
  255.  
  256. /*
  257.  * Multiply a matrix and a vector.
  258.  */
  259.  
  260. void
  261. matrix_vector (m, src, dest)
  262.     MATRIX m;
  263.     VECTOR src, dest;
  264. {
  265.     set_vector_x (dest,
  266.         my_mul (matrix_element (m, 0, 0), vector_x (src)) +
  267.         my_mul (matrix_element (m, 0, 1), vector_y (src)) +
  268.         my_mul (matrix_element (m, 0, 2), vector_z (src)) +
  269.         my_mul (matrix_element (m, 0, 3), vector_w (src)));
  270.  
  271.     set_vector_y (dest,
  272.         my_mul (matrix_element (m, 1, 0), vector_x (src)) +
  273.         my_mul (matrix_element (m, 1, 1), vector_y (src)) +
  274.         my_mul (matrix_element (m, 1, 2), vector_z (src)) +
  275.         my_mul (matrix_element (m, 1, 3), vector_w (src)));
  276.  
  277.     set_vector_z (dest,
  278.         my_mul (matrix_element (m, 2, 0), vector_x (src)) +
  279.         my_mul (matrix_element (m, 2, 1), vector_y (src)) +
  280.         my_mul (matrix_element (m, 2, 2), vector_z (src)) +
  281.         my_mul (matrix_element (m, 2, 3), vector_w (src)));
  282.  
  283.     set_vector_w (dest,
  284.         my_mul (matrix_element (m, 3, 0), vector_x (src)) +
  285.         my_mul (matrix_element (m, 3, 1), vector_y (src)) +
  286.         my_mul (matrix_element (m, 3, 2), vector_z (src)) +
  287.         my_mul (matrix_element (m, 3, 3), vector_w (src)));
  288. }
  289.  
  290. /*
  291.  * Transpose a matrix.
  292.  */
  293.  
  294. void
  295. matrix_transpose (src, dest)
  296.     MATRIX src, dest;
  297. {
  298.     short i, j;
  299.  
  300.     for (i = 0; i < 4; i++)
  301.         for (j = 0; j < 4; j++)
  302.             set_matrix_element (dest, i, j, matrix_element (src, j, i));
  303. }
  304.  
  305. /*
  306.  * Invert a matrix.
  307.  */
  308.  
  309. void
  310. matrix_inverse (src, dest)
  311.     MATRIX src, dest;
  312. {
  313.     short i, j;
  314.     NUM det;
  315.  
  316.     matrix_adjoint (src, dest);
  317.     det = matrix_determinant4x4 (src);
  318.  
  319.     if (fabs (det) < 1e-12)
  320.         fatal_error ("Matrix is singular");
  321.  
  322.     matrix_multiply (dest, my_div (NUM_ONE, det));
  323. }
  324.  
  325. /*
  326.  * Construct the adjoint of a matrix (for use
  327.  *  in determing the inverse of a matrix).
  328.  */
  329.  
  330. void
  331. matrix_adjoint (src, dest)
  332.     MATRIX src, dest;
  333. {
  334.     MATRIX temp;
  335.     NUM a1, a2, a3, a4;
  336.     NUM b1, b2, b3, b4;
  337.     NUM c1, c2, c3, c4;
  338.     NUM d1, d2, d3, d4;
  339.  
  340.     a1 = src[0][0];
  341.     a2 = src[0][1];
  342.     a3 = src[0][2];
  343.     a4 = src[0][3];
  344.  
  345.     b1 = src[1][0];
  346.     b2 = src[1][1];
  347.     b3 = src[1][2];
  348.     b4 = src[1][3];
  349.  
  350.     c1 = src[2][0];
  351.     c2 = src[2][1];
  352.     c3 = src[2][2];
  353.     c4 = src[2][3];
  354.  
  355.     d1 = src[3][0];
  356.     d2 = src[3][1];
  357.     d3 = src[3][2];
  358.     d4 = src[3][3];
  359.  
  360.     temp[0][0] = matrix_determinant3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
  361.     temp[1][0] = -matrix_determinant3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
  362.     temp[2][0] = matrix_determinant3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
  363.     temp[3][0] = -matrix_determinant3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  364.  
  365.     temp[0][1] = -matrix_determinant3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
  366.     temp[1][1] = matrix_determinant3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
  367.     temp[2][1] = -matrix_determinant3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
  368.     temp[3][1] = matrix_determinant3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
  369.  
  370.     temp[0][2] = matrix_determinant3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
  371.     temp[1][2] = -matrix_determinant3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
  372.     temp[2][2] = matrix_determinant3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
  373.     temp[3][2] = -matrix_determinant3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
  374.  
  375.     temp[0][3] = -matrix_determinant3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
  376.     temp[1][3] = matrix_determinant3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
  377.     temp[2][3] = -matrix_determinant3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
  378.     temp[3][3] = matrix_determinant3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
  379.  
  380.     matrix_transpose (temp, dest);
  381. }
  382.  
  383. /*
  384.  * Calculate the determinant of a matrix.
  385.  */
  386.  
  387. NUM
  388. matrix_determinant4x4 (m)
  389.     MATRIX m;
  390. {
  391.     NUM ans;
  392.     NUM a1, a2, a3, a4;
  393.     NUM b1, b2, b3, b4;
  394.     NUM c1, c2, c3, c4;
  395.     NUM d1, d2, d3, d4;
  396.  
  397.     a1 = m[0][0];
  398.     a2 = m[0][1];
  399.     a3 = m[0][2];
  400.     a4 = m[0][3];
  401.  
  402.     b1 = m[1][0];
  403.     b2 = m[1][1];
  404.     b3 = m[1][2];
  405.     b4 = m[1][3];
  406.  
  407.     c1 = m[2][0];
  408.     c2 = m[2][1];
  409.     c3 = m[2][2];
  410.     c4 = m[2][3];
  411.  
  412.     d1 = m[3][0];
  413.     d2 = m[3][1];
  414.     d3 = m[3][2];
  415.     d4 = m[3][3];
  416.  
  417.     ans = (my_mul (a1, matrix_determinant3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4)) -
  418.            my_mul (b1, matrix_determinant3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4)) +
  419.            my_mul (c1, matrix_determinant3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4)) -
  420.            my_mul (d1, matrix_determinant3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4)));
  421.  
  422.     return ans;
  423. }
  424.  
  425. /*
  426.  * Calculate the determinant of a 3x3 matrix.
  427.  */
  428.  
  429. NUM
  430. matrix_determinant3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3)
  431.     NUM a1, a2, a3;
  432.     NUM b1, b2, b3;
  433.     NUM c1, c2, c3;
  434. {
  435.     NUM ans;
  436.  
  437.     ans = (my_mul (a1, matrix_determinant2x2 (b2, b3, c2, c3)) -
  438.            my_mul (b1, matrix_determinant2x2 (a2, a3, c2, c3)) +
  439.            my_mul (c1, matrix_determinant2x2 (a2, a3, b2, b3)));
  440.  
  441.     return ans;
  442. }
  443.  
  444. /*
  445.  * Calculate the determinant of a 2x2 matrix.
  446.  */
  447.  
  448. NUM
  449. matrix_determinant2x2 (a, b, c, d)
  450.     NUM a, b;
  451.     NUM c, d;
  452. {
  453.     return (my_mul (a, d) - my_mul (b, c));
  454. }
  455.  
  456. /*
  457.  * Clear a vector. The w coordinate is 1 by default.
  458.  */
  459.  
  460. void
  461. vector_clear (v)
  462.     VECTOR v;
  463. {
  464.     set_vector_x (v, NUM_ZERO);
  465.     set_vector_y (v, NUM_ZERO);
  466.     set_vector_z (v, NUM_ZERO);
  467.     set_vector_w (v, NUM_ONE);
  468. }
  469.  
  470. /*
  471.  * Add two vectors.
  472.  */
  473.  
  474. void
  475. vector_add (a, b, d)
  476.     VECTOR a, b, d;
  477. {
  478.     set_vector_x (d, vector_x (a) + vector_x (b));
  479.     set_vector_y (d, vector_y (a) + vector_y (b));
  480.     set_vector_z (d, vector_z (a) + vector_z (b));
  481.     set_vector_w (d, NUM_ONE);
  482. }
  483.  
  484. /*
  485.  * Subtract two vectors.
  486.  */
  487.  
  488. void
  489. vector_sub (a, b, d)
  490.     VECTOR a, b, d;
  491. {
  492.     set_vector_x (d, vector_x (a) - vector_x (b));
  493.     set_vector_y (d, vector_y (a) - vector_y (b));
  494.     set_vector_z (d, vector_z (a) - vector_z (b));
  495.     set_vector_w (d, NUM_ONE);
  496. }
  497.  
  498. /*
  499.  * Multiply a vector by a scalar.
  500.  */
  501.  
  502. void
  503. vector_mul (src, k, dest)
  504.     VECTOR src, dest;
  505.     NUM k;
  506. {
  507.     set_vector_x (dest, my_mul (vector_x (src), k));
  508.     set_vector_y (dest, my_mul (vector_y (src), k));
  509.     set_vector_z (dest, my_mul (vector_z (src), k));
  510.     set_vector_w (dest, NUM_ONE);
  511. }
  512.  
  513. /*
  514.  * Copy a vector.
  515.  */
  516.  
  517. void
  518. vector_copy (src, dest)
  519.     VECTOR src, dest;
  520. {
  521.     set_vector_x (dest, vector_x (src));
  522.     set_vector_y (dest, vector_y (src));
  523.     set_vector_z (dest, vector_z (src));
  524.     set_vector_w (dest, vector_w (src));
  525. }
  526.  
  527. /*
  528.  * Calculate the dot product of two vectors.
  529.  */
  530.  
  531. NUM
  532. vector_dot (a, b)
  533.     VECTOR a, b;
  534. {
  535.     return (my_mul (vector_x (a), vector_x (b)) +
  536.             my_mul (vector_y (a), vector_y (b)) +
  537.             my_mul (vector_z (a), vector_z (b)));
  538. }
  539.  
  540. /*
  541.  * Calculate the cross product of two vectors.
  542.  */
  543.  
  544. void
  545. vector_cross (a, b, d)
  546.     VECTOR a, b, d;
  547. {
  548.     set_vector_x (d, 
  549.         my_mul (vector_y (a), vector_z (b)) - 
  550.         my_mul (vector_z (a), vector_y (b)));
  551.     set_vector_y (d, 
  552.         my_mul (vector_z (a), vector_x (b)) - 
  553.         my_mul (vector_x (a), vector_z (b)));
  554.     set_vector_z (d, 
  555.         my_mul (vector_x (a), vector_y (b)) - 
  556.         my_mul (vector_y (a), vector_x (b)));
  557.     set_vector_w (d, NUM_ONE);
  558. }
  559.  
  560. /*
  561.  * Calculate the magnitude (or length) of a vector).
  562.  */
  563.  
  564. NUM
  565. vector_magnitude (v)
  566.     VECTOR v;
  567. {
  568.     return my_sqrt (vector_dot (v, v));
  569. }
  570.  
  571. /*
  572.  * Normalize a vector. (That is, make its length 1).
  573.  */
  574.  
  575. void
  576. vector_normalize (v)
  577.     VECTOR v;
  578. {
  579.     NUM norm;
  580.  
  581.     norm = vector_magnitude (v);
  582.  
  583.     if (norm != 0)
  584.     {
  585.         set_vector_x (v, my_div (vector_x (v), norm));
  586.         set_vector_y (v, my_div (vector_y (v), norm));
  587.         set_vector_z (v, my_div (vector_z (v), norm));
  588.         set_vector_w (v, NUM_ONE);
  589.     }
  590.     else
  591.     {
  592.         fatal_error ("Vector normalization of a 0 length vector");
  593.     }
  594. }
  595.  
  596. /*
  597.  * Homogenize a vector. (That is, divide each coordinate
  598.  *  by the w coordinate).
  599.  */
  600.  
  601. void
  602. vector_homogenize (v)
  603.     VECTOR v;
  604. {
  605.     if (vector_w (v) != 0)
  606.     {
  607.         set_vector_x (v, my_div (vector_x (v), vector_w (v)));
  608.         set_vector_y (v, my_div (vector_y (v), vector_w (v)));
  609.         set_vector_z (v, my_div (vector_z (v), vector_w (v)));
  610.         set_vector_w (v, NUM_ONE);
  611.     }
  612.     else
  613.     {
  614.         fatal_error ("Vector homogenization of a directional vector");
  615.     }
  616. }
  617.