home *** CD-ROM | disk | FTP | other *** search
/ World of Graphics / WOGRAPH.BIN / 729.SOURCE.ZIP / VECT.C < prev    next >
C/C++ Source or Header  |  1993-04-28  |  13KB  |  538 lines
  1. #include <math.h>
  2. #include <string.h>
  3. #include "vect.h"
  4.  
  5. #define EPSILON 1e-6
  6.  
  7. void   adjoint (Matrix mat);
  8. double det4x4 (Matrix mat);
  9. double det3x3 (double a1, double a2, double a3, double b1, double b2,
  10.            double b3, double c1, double c2, double c3);
  11. double det2x2 (double a, double b, double c, double d);
  12.  
  13.  
  14. void vect_init (Vector v, float  x, float  y, float  z)
  15. {
  16.     v[X] = x;
  17.     v[Y] = y;
  18.     v[Z] = z;
  19. }
  20.  
  21.  
  22. void vect_copy (Vector v1, Vector v2)
  23. {
  24.     v1[X] = v2[X];
  25.     v1[Y] = v2[Y];
  26.     v1[Z] = v2[Z];
  27. }
  28.  
  29.  
  30. int vect_equal (Vector v1, Vector v2)
  31. {
  32.     if (v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z])
  33.     return 1;
  34.     else
  35.     return 0;
  36. }
  37.  
  38.  
  39. void vect_add (Vector v1, Vector v2, Vector v3)
  40. {
  41.     v1[X] = v2[X] + v3[X];
  42.     v1[Y] = v2[Y] + v3[Y];
  43.     v1[Z] = v2[Z] + v3[Z];
  44. }
  45.  
  46.  
  47. void vect_sub (Vector v1, Vector v2, Vector v3)
  48. {
  49.     v1[X] = v2[X] - v3[X];
  50.     v1[Y] = v2[Y] - v3[Y];
  51.     v1[Z] = v2[Z] - v3[Z];
  52. }
  53.  
  54.  
  55. void vect_scale (Vector v1, Vector v2, float  k)
  56. {
  57.     v1[X] = k * v2[X];
  58.     v1[Y] = k * v2[Y];
  59.     v1[Z] = k * v2[Z];
  60. }
  61.  
  62.  
  63. float vect_mag (Vector v)
  64. {
  65.     float mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
  66.  
  67.     return mag;
  68. }
  69.  
  70.  
  71. void vect_normalize (Vector v)
  72. {
  73.     float mag = vect_mag (v);
  74.  
  75.     if (mag > 0.0)
  76.     vect_scale (v, v, 1.0/mag);
  77. }
  78.  
  79.  
  80. float vect_dot (Vector v1, Vector v2)
  81. {
  82.     return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]);
  83. }
  84.  
  85.  
  86. void vect_cross (Vector v1, Vector v2, Vector v3)
  87. {
  88.     v1[X] = (v2[Y] * v3[Z]) - (v2[Z] * v3[Y]);
  89.     v1[Y] = (v2[Z] * v3[X]) - (v2[X] * v3[Z]);
  90.     v1[Z] = (v2[X] * v3[Y]) - (v2[Y] * v3[X]);
  91. }
  92.  
  93. void vect_min (Vector v1, Vector v2, Vector v3)
  94. {
  95.     v1[X] = (v2[X] < v3[X]) ? v2[X] : v3[X];
  96.     v1[Y] = (v2[Y] < v3[Y]) ? v2[Y] : v3[Y];
  97.     v1[Z] = (v2[Z] < v3[Z]) ? v2[Z] : v3[Z];
  98. }
  99.  
  100.  
  101. void vect_max (Vector v1, Vector v2, Vector v3)
  102. {
  103.     v1[X] = (v2[X] > v3[X]) ? v2[X] : v3[X];
  104.     v1[Y] = (v2[Y] > v3[Y]) ? v2[Y] : v3[Y];
  105.     v1[Z] = (v2[Z] > v3[Z]) ? v2[Z] : v3[Z];
  106. }
  107.  
  108.  
  109. /* Return the angle between two vectors */
  110. float vect_angle (Vector v1, Vector v2)
  111. {
  112.     float  mag1, mag2, angle, cos_theta;
  113.  
  114.     mag1 = vect_mag(v1);
  115.     mag2 = vect_mag(v2);
  116.  
  117.     if (mag1 * mag2 == 0.0)
  118.     angle = 0.0;
  119.     else {
  120.     cos_theta = vect_dot(v1,v2) / (mag1 * mag2);
  121.  
  122.     if (cos_theta <= -1.0)
  123.         angle = 180.0;
  124.     else if (cos_theta >= +1.0)
  125.         angle = 0.0;
  126.     else
  127.         angle = (180.0/M_PI) * acos(cos_theta);
  128.     }
  129.  
  130.     return angle;
  131. }
  132.  
  133.  
  134. void vect_print (FILE *f, Vector v, int dec, char sep)
  135. {
  136.     char fstr[] = "%.4f, %.4f, %.4f";
  137.  
  138.     if (dec < 0) dec = 0;
  139.     if (dec > 9) dec = 9;
  140.  
  141.     fstr[2]  = '0' + dec;
  142.     fstr[8]  = '0' + dec;
  143.     fstr[14] = '0' + dec;
  144.  
  145.     fstr[4]  = sep;
  146.     fstr[10] = sep;
  147.  
  148.     fprintf (f, fstr, v[X], v[Y], v[Z]);
  149. }
  150.  
  151.  
  152. /* Rotate a vector about the X, Y or Z axis */
  153. void vect_rotate (Vector v1, Vector v2, int axis, float angle)
  154. {
  155.     float  cosa, sina;
  156.  
  157.     cosa = cos ((M_PI/180.0) * angle);
  158.     sina = sin ((M_PI/180.0) * angle);
  159.  
  160.     switch (axis) {
  161.     case X:
  162.         v1[X] =  v2[X];
  163.         v1[Y] =  v2[Y] * cosa + v2[Z] * sina;
  164.         v1[Z] =  v2[Z] * cosa - v2[Y] * sina;
  165.         break;
  166.  
  167.     case Y:
  168.         v1[X] = v2[X] * cosa - v2[Z] * sina;
  169.         v1[Y] = v2[Y];
  170.         v1[Z] = v2[Z] * cosa + v2[X] * sina;
  171.         break;
  172.  
  173.     case Z:
  174.         v1[X] = v2[X] * cosa + v2[Y] * sina;
  175.         v1[Y] = v2[Y] * cosa - v2[X] * sina;
  176.         v1[Z] = v2[Z];
  177.         break;
  178.     }
  179. }
  180.  
  181.  
  182. /* Rotate a vector about a specific axis */
  183. void vect_axis_rotate (Vector v1, Vector v2, Vector axis, float angle)
  184. {
  185.     float  cosa, sina;
  186.     Matrix mat;
  187.  
  188.     cosa = cos ((M_PI/180.0) * angle);
  189.     sina = sin ((M_PI/180.0) * angle);
  190.  
  191.     mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
  192.     mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
  193.     mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
  194.     mat[0][3] = 0.0;
  195.  
  196.     mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
  197.     mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
  198.     mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
  199.     mat[1][3] = 0.0;
  200.  
  201.     mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
  202.     mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
  203.     mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
  204.     mat[2][3] = 0.0;
  205.  
  206.     mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
  207.  
  208.     vect_transform (v1, v2, mat);
  209. }
  210.  
  211.  
  212. /* Transform the given vector */
  213. void vect_transform (Vector v1, Vector v2, Matrix mat)
  214. {
  215.     Vector tmp;
  216.  
  217.     tmp[X] = (v2[X] * mat[0][0]) + (v2[Y] * mat[1][0]) + (v2[Z] * mat[2][0]) + mat[3][0];
  218.     tmp[Y] = (v2[X] * mat[0][1]) + (v2[Y] * mat[1][1]) + (v2[Z] * mat[2][1]) + mat[3][1];
  219.     tmp[Z] = (v2[X] * mat[0][2]) + (v2[Y] * mat[1][2]) + (v2[Z] * mat[2][2]) + mat[3][2];
  220.  
  221.     vect_copy (v1, tmp);
  222. }
  223.  
  224.  
  225. /* Create an identity matrix */
  226. void mat_identity (Matrix mat)
  227. {
  228.     int i, j;
  229.  
  230.     for (i = 0; i < 4; i++)
  231.     for (j = 0; j < 4; j++)
  232.         mat[i][j] = 0.0;
  233.  
  234.     for (i = 0; i < 4; i++)
  235.     mat[i][i] = 1.0;
  236. }
  237.  
  238.  
  239. void mat_copy (Matrix mat1, Matrix mat2)
  240. {
  241.     int i, j;
  242.  
  243.     for (i = 0; i < 4; i++)
  244.     for (j = 0; j < 4; j++)
  245.         mat1[i][j] = mat2[i][j];
  246. }
  247.  
  248.  
  249. /* Rotate a matrix about the X, Y or Z axis */
  250. void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle)
  251. {
  252.     Matrix mat;
  253.     float  cosa, sina;
  254.  
  255.     cosa = cos ((M_PI/180.0) * angle);
  256.     sina = sin ((M_PI/180.0) * angle);
  257.  
  258.     mat_identity (mat);
  259.  
  260.     switch (axis) {
  261.     case X:
  262.         mat[1][1] = cosa;
  263.         mat[1][2] = sina;
  264.         mat[2][1] = -sina;
  265.         mat[2][2] = cosa;
  266.         break;
  267.  
  268.     case Y:
  269.         mat[0][0] = cosa;
  270.         mat[0][2] = -sina;
  271.         mat[2][0] = sina;
  272.         mat[2][2] = cosa;
  273.         break;
  274.  
  275.     case Z:
  276.         mat[0][0] = cosa;
  277.         mat[0][1] = sina;
  278.         mat[1][0] = -sina;
  279.         mat[1][1] = cosa;
  280.         break;
  281.     }
  282.  
  283.     mat_mult (mat1, mat2, mat);
  284. }
  285.  
  286.  
  287. void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle)
  288. {
  289.     float  cosa, sina;
  290.     Matrix mat;
  291.  
  292.     cosa = cos ((M_PI/180.0) * angle);
  293.     sina = sin ((M_PI/180.0) * angle);
  294.  
  295.     mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
  296.     mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
  297.     mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
  298.     mat[0][3] = 0.0;
  299.  
  300.     mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
  301.     mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
  302.     mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
  303.     mat[1][3] = 0.0;
  304.  
  305.     mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
  306.     mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
  307.     mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
  308.     mat[2][3] = 0.0;
  309.  
  310.     mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
  311.  
  312.     mat_mult (mat1, mat2, mat);
  313. }
  314.  
  315.  
  316. /*  mat1 <-- mat2 * mat3 */
  317. void mat_mult (Matrix mat1, Matrix mat2, Matrix mat3)
  318. {
  319.     float sum;
  320.     int   i, j, k;
  321.     Matrix result;
  322.  
  323.     for (i = 0; i < 4; i++) {
  324.     for (j = 0; j < 4; j++) {
  325.         sum = 0.0;
  326.  
  327.         for (k = 0; k < 4; k++)
  328.         sum = sum + mat2[i][k] * mat3[k][j];
  329.  
  330.         result[i][j] = sum;
  331.     }
  332.     }
  333.  
  334.     for (i = 0; i < 4; i++)
  335.     for (j = 0; j < 4; j++)
  336.         mat1[i][j] = result[i][j];
  337. }
  338.  
  339.  
  340. /*
  341.    Decodes a 3x4 transformation matrix into separate scale, rotation,
  342.    translation, and shear vectors. Based on a program by Spencer W.
  343.    Thomas (Graphics Gems II)
  344. */
  345. void mat_decode (Matrix mat, Vector scale,  Vector shear, Vector rotate,
  346.         Vector transl)
  347. {
  348.     int i;
  349.     Vector row[3], temp;
  350.  
  351.     for (i = 0; i < 3; i++)
  352.     transl[i] = mat[3][i];
  353.  
  354.     for (i = 0; i < 3; i++) {
  355.     row[i][X] = mat[i][0];
  356.     row[i][Y] = mat[i][1];
  357.     row[i][Z] = mat[i][2];
  358.     }
  359.  
  360.     scale[X] = vect_mag (row[0]);
  361.     vect_normalize (row[0]);
  362.  
  363.     shear[X] = vect_dot (row[0], row[1]);
  364.     row[1][X] = row[1][X] - shear[X]*row[0][X];
  365.     row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
  366.     row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
  367.  
  368.     scale[Y] = vect_mag (row[1]);
  369.     vect_normalize (row[1]);
  370.  
  371.     if (scale[Y] != 0.0)
  372.     shear[X] /= scale[Y];
  373.  
  374.     shear[Y] = vect_dot (row[0], row[2]);
  375.     row[2][X] = row[2][X] - shear[Y]*row[0][X];
  376.     row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
  377.     row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
  378.  
  379.     shear[Z] = vect_dot (row[1], row[2]);
  380.     row[2][X] = row[2][X] - shear[Z]*row[1][X];
  381.     row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
  382.     row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
  383.  
  384.     scale[Z] = vect_mag (row[2]);
  385.     vect_normalize (row[2]);
  386.  
  387.     if (scale[Z] != 0.0) {
  388.     shear[Y] /= scale[Z];
  389.     shear[Z] /= scale[Z];
  390.     }
  391.  
  392.     vect_cross (temp, row[1], row[2]);
  393.     if (vect_dot (row[0], temp) < 0.0) {
  394.     for (i = 0; i < 3; i++) {
  395.         scale[i]  *= -1.0;
  396.         row[i][X] *= -1.0;
  397.         row[i][Y] *= -1.0;
  398.         row[i][Z] *= -1.0;
  399.     }
  400.     }
  401.  
  402.     if (row[0][Z] < -1.0) row[0][Z] = -1.0;
  403.     if (row[0][Z] > +1.0) row[0][Z] = +1.0;
  404.  
  405.     rotate[Y] = asin(-row[0][Z]);
  406.  
  407.     if (fabs(cos(rotate[Y])) > EPSILON) {
  408.     rotate[X] = atan2 (row[1][Z], row[2][Z]);
  409.     rotate[Z] = atan2 (row[0][Y], row[0][X]);
  410.     }
  411.     else {
  412.     rotate[X] = atan2 (row[1][X], row[1][Y]);
  413.     rotate[Z] = 0.0;
  414.     }
  415.  
  416.     /* Convert the rotations to degrees */
  417.     rotate[X] = (180.0/M_PI)*rotate[X];
  418.     rotate[Y] = (180.0/M_PI)*rotate[Y];
  419.     rotate[Z] = (180.0/M_PI)*rotate[Z];
  420. }
  421.  
  422.  
  423. /* Matrix inversion code from Graphics Gems */
  424.  
  425. /* mat1 <-- mat2^-1 */
  426. float mat_inv (Matrix mat1, Matrix mat2)
  427. {
  428.     int i, j;
  429.     float det;
  430.  
  431.     if (mat1 != mat2) {
  432.     for (i = 0; i < 4; i++)
  433.         for (j = 0; j < 4; j++)
  434.         mat1[i][j] = mat2[i][j];
  435.     }
  436.  
  437.     det = det4x4 (mat1);
  438.  
  439.     if (fabs (det) < EPSILON)
  440.     return 0.0;
  441.  
  442.     adjoint (mat1);
  443.  
  444.     for (i = 0; i < 4; i++)
  445.     for(j = 0; j < 4; j++)
  446.         mat1[i][j] = mat1[i][j] / det;
  447.  
  448.     return det;
  449. }
  450.  
  451.  
  452. void adjoint (Matrix mat)
  453. {
  454.     double a1, a2, a3, a4, b1, b2, b3, b4;
  455.     double c1, c2, c3, c4, d1, d2, d3, d4;
  456.  
  457.     a1 = mat[0][0]; b1 = mat[0][1];
  458.     c1 = mat[0][2]; d1 = mat[0][3];
  459.  
  460.     a2 = mat[1][0]; b2 = mat[1][1];
  461.     c2 = mat[1][2]; d2 = mat[1][3];
  462.  
  463.     a3 = mat[2][0]; b3 = mat[2][1];
  464.     c3 = mat[2][2]; d3 = mat[2][3];
  465.  
  466.     a4 = mat[3][0]; b4 = mat[3][1];
  467.     c4 = mat[3][2]; d4 = mat[3][3];
  468.  
  469.     /* row column labeling reversed since we transpose rows & columns */
  470.     mat[0][0]  =  det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
  471.     mat[1][0]  = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
  472.     mat[2][0]  =  det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
  473.     mat[3][0]  = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  474.  
  475.     mat[0][1]  = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
  476.     mat[1][1]  =  det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
  477.     mat[2][1]  = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
  478.     mat[3][1]  =  det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
  479.  
  480.     mat[0][2]  =  det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
  481.     mat[1][2]  = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
  482.     mat[2][2]  =  det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
  483.     mat[3][2]  = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
  484.  
  485.     mat[0][3]  = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
  486.     mat[1][3]  =  det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
  487.     mat[2][3]  = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
  488.     mat[3][3]  =  det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
  489. }
  490.  
  491.  
  492. double det4x4 (Matrix mat)
  493. {
  494.     double ans;
  495.     double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,             d4;
  496.  
  497.     a1 = mat[0][0]; b1 = mat[0][1];
  498.     c1 = mat[0][2]; d1 = mat[0][3];
  499.  
  500.     a2 = mat[1][0]; b2 = mat[1][1];
  501.     c2 = mat[1][2]; d2 = mat[1][3];
  502.  
  503.     a3 = mat[2][0]; b3 = mat[2][1];
  504.     c3 = mat[2][2]; d3 = mat[2][3];
  505.  
  506.     a4 = mat[3][0]; b4 = mat[3][1];
  507.     c4 = mat[3][2]; d4 = mat[3][3];
  508.  
  509.     ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) -
  510.       b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) +
  511.       c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) -
  512.       d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  513.  
  514.     return ans;
  515. }
  516.  
  517.  
  518. double det3x3 (double a1, double a2, double a3, double b1, double b2,
  519.            double b3, double c1, double c2, double c3)
  520. {
  521.     double ans;
  522.  
  523.     ans = a1 * det2x2 (b2, b3, c2, c3)
  524.     - b1 * det2x2 (a2, a3, c2, c3)
  525.     + c1 * det2x2 (a2, a3, b2, b3);
  526.  
  527.     return ans;
  528. }
  529.  
  530.  
  531. double det2x2 (double a, double b, double c, double d)
  532. {
  533.     double ans;
  534.     ans = a * d - b * c;
  535.     return ans;
  536. }
  537.  
  538.