home *** CD-ROM | disk | FTP | other *** search
/ Current Shareware 1994 January / SHAR194.ISO / graphuti / rawpov18.zip / SOURCE.ZIP / VECT.C < prev    next >
C/C++ Source or Header  |  1993-04-24  |  13KB  |  528 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. /* Rotate a matrix about the X, Y or Z axis */
  240. void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle)
  241. {
  242.     Matrix mat;
  243.     float  cosa, sina;
  244.  
  245.     cosa = cos ((M_PI/180.0) * angle);
  246.     sina = sin ((M_PI/180.0) * angle);
  247.  
  248.     mat_identity (mat);
  249.  
  250.     switch (axis) {
  251.     case X:
  252.         mat[1][1] = cosa;
  253.         mat[1][2] = sina;
  254.         mat[2][1] = -sina;
  255.         mat[2][2] = cosa;
  256.         break;
  257.  
  258.     case Y:
  259.         mat[0][0] = cosa;
  260.         mat[0][2] = -sina;
  261.         mat[2][0] = sina;
  262.         mat[2][2] = cosa;
  263.         break;
  264.  
  265.     case Z:
  266.         mat[0][0] = cosa;
  267.         mat[0][1] = sina;
  268.         mat[1][0] = -sina;
  269.         mat[1][1] = cosa;
  270.         break;
  271.     }
  272.  
  273.     mat_mult (mat1, mat2, mat);
  274. }
  275.  
  276.  
  277. void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle)
  278. {
  279.     float  cosa, sina;
  280.     Matrix mat;
  281.  
  282.     cosa = cos ((M_PI/180.0) * angle);
  283.     sina = sin ((M_PI/180.0) * angle);
  284.  
  285.     mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
  286.     mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
  287.     mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
  288.     mat[0][3] = 0.0;
  289.  
  290.     mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
  291.     mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
  292.     mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
  293.     mat[1][3] = 0.0;
  294.  
  295.     mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
  296.     mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
  297.     mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
  298.     mat[2][3] = 0.0;
  299.  
  300.     mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
  301.  
  302.     mat_mult (mat1, mat2, mat);
  303. }
  304.  
  305.  
  306. /*  mat1 <-- mat2 * mat3 */
  307. void mat_mult (Matrix mat1, Matrix mat2, Matrix mat3)
  308. {
  309.     float sum;
  310.     int   i, j, k;
  311.     Matrix result;
  312.  
  313.     for (i = 0; i < 4; i++) {
  314.     for (j = 0; j < 4; j++) {
  315.         sum = 0.0;
  316.  
  317.         for (k = 0; k < 4; k++)
  318.         sum = sum + mat2[i][k] * mat3[k][j];
  319.  
  320.         result[i][j] = sum;
  321.     }
  322.     }
  323.  
  324.     for (i = 0; i < 4; i++)
  325.     for (j = 0; j < 4; j++)
  326.         mat1[i][j] = result[i][j];
  327. }
  328.  
  329.  
  330. /*
  331.    Decodes a 3x4 transformation matrix into separate scale, rotation,
  332.    translation, and shear vectors. Based on a program by Spencer W.
  333.    Thomas (Graphics Gems II)
  334. */
  335. void mat_decode (Matrix mat, Vector scale,  Vector shear, Vector rotate,
  336.         Vector transl)
  337. {
  338.     int i;
  339.     Vector row[3], temp;
  340.  
  341.     for (i = 0; i < 3; i++)
  342.     transl[i] = mat[3][i];
  343.  
  344.     for (i = 0; i < 3; i++) {
  345.     row[i][X] = mat[i][0];
  346.     row[i][Y] = mat[i][1];
  347.     row[i][Z] = mat[i][2];
  348.     }
  349.  
  350.     scale[X] = vect_mag (row[0]);
  351.     vect_normalize (row[0]);
  352.  
  353.     shear[X] = vect_dot (row[0], row[1]);
  354.     row[1][X] = row[1][X] - shear[X]*row[0][X];
  355.     row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
  356.     row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
  357.  
  358.     scale[Y] = vect_mag (row[1]);
  359.     vect_normalize (row[1]);
  360.  
  361.     if (scale[Y] != 0.0)
  362.     shear[X] /= scale[Y];
  363.  
  364.     shear[Y] = vect_dot (row[0], row[2]);
  365.     row[2][X] = row[2][X] - shear[Y]*row[0][X];
  366.     row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
  367.     row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
  368.  
  369.     shear[Z] = vect_dot (row[1], row[2]);
  370.     row[2][X] = row[2][X] - shear[Z]*row[1][X];
  371.     row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
  372.     row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
  373.  
  374.     scale[Z] = vect_mag (row[2]);
  375.     vect_normalize (row[2]);
  376.  
  377.     if (scale[Z] != 0.0) {
  378.     shear[Y] /= scale[Z];
  379.     shear[Z] /= scale[Z];
  380.     }
  381.  
  382.     vect_cross (temp, row[1], row[2]);
  383.     if (vect_dot (row[0], temp) < 0.0) {
  384.     for (i = 0; i < 3; i++) {
  385.         scale[i]  *= -1.0;
  386.         row[i][X] *= -1.0;
  387.         row[i][Y] *= -1.0;
  388.         row[i][Z] *= -1.0;
  389.     }
  390.     }
  391.  
  392.     if (row[0][Z] < -1.0) row[0][Z] = -1.0;
  393.     if (row[0][Z] > +1.0) row[0][Z] = +1.0;
  394.  
  395.     rotate[Y] = asin(-row[0][Z]);
  396.  
  397.     if (fabs(cos(rotate[Y])) > EPSILON) {
  398.     rotate[X] = atan2 (row[1][Z], row[2][Z]);
  399.     rotate[Z] = atan2 (row[0][Y], row[0][X]);
  400.     }
  401.     else {
  402.     rotate[X] = atan2 (row[1][X], row[1][Y]);
  403.     rotate[Z] = 0.0;
  404.     }
  405.  
  406.     /* Convert the rotations to degrees */
  407.     rotate[X] = (180.0/M_PI)*rotate[X];
  408.     rotate[Y] = (180.0/M_PI)*rotate[Y];
  409.     rotate[Z] = (180.0/M_PI)*rotate[Z];
  410. }
  411.  
  412.  
  413. /* Matrix inversion code from Graphics Gems */
  414.  
  415. /* mat1 <-- mat2^-1 */
  416. float mat_inv (Matrix mat1, Matrix mat2)
  417. {
  418.     int i, j;
  419.     float det;
  420.  
  421.     if (mat1 != mat2) {
  422.     for (i = 0; i < 4; i++)
  423.         for (j = 0; j < 4; j++)
  424.         mat1[i][j] = mat2[i][j];
  425.     }
  426.  
  427.     det = det4x4 (mat1);
  428.  
  429.     if (fabs (det) < EPSILON)
  430.     return 0.0;
  431.  
  432.     adjoint (mat1);
  433.  
  434.     for (i = 0; i < 4; i++)
  435.     for(j = 0; j < 4; j++)
  436.         mat1[i][j] = mat1[i][j] / det;
  437.  
  438.     return det;
  439. }
  440.  
  441.  
  442. void adjoint (Matrix mat)
  443. {
  444.     double a1, a2, a3, a4, b1, b2, b3, b4;
  445.     double c1, c2, c3, c4, d1, d2, d3, d4;
  446.  
  447.     a1 = mat[0][0]; b1 = mat[0][1];
  448.     c1 = mat[0][2]; d1 = mat[0][3];
  449.  
  450.     a2 = mat[1][0]; b2 = mat[1][1];
  451.     c2 = mat[1][2]; d2 = mat[1][3];
  452.  
  453.     a3 = mat[2][0]; b3 = mat[2][1];
  454.     c3 = mat[2][2]; d3 = mat[2][3];
  455.  
  456.     a4 = mat[3][0]; b4 = mat[3][1];
  457.     c4 = mat[3][2]; d4 = mat[3][3];
  458.  
  459.     /* row column labeling reversed since we transpose rows & columns */
  460.     mat[0][0]  =  det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
  461.     mat[1][0]  = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
  462.     mat[2][0]  =  det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
  463.     mat[3][0]  = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  464.  
  465.     mat[0][1]  = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
  466.     mat[1][1]  =  det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
  467.     mat[2][1]  = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
  468.     mat[3][1]  =  det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
  469.  
  470.     mat[0][2]  =  det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
  471.     mat[1][2]  = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
  472.     mat[2][2]  =  det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
  473.     mat[3][2]  = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
  474.  
  475.     mat[0][3]  = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
  476.     mat[1][3]  =  det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
  477.     mat[2][3]  = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
  478.     mat[3][3]  =  det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
  479. }
  480.  
  481.  
  482. double det4x4 (Matrix mat)
  483. {
  484.     double ans;
  485.     double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,             d4;
  486.  
  487.     a1 = mat[0][0]; b1 = mat[0][1];
  488.     c1 = mat[0][2]; d1 = mat[0][3];
  489.  
  490.     a2 = mat[1][0]; b2 = mat[1][1];
  491.     c2 = mat[1][2]; d2 = mat[1][3];
  492.  
  493.     a3 = mat[2][0]; b3 = mat[2][1];
  494.     c3 = mat[2][2]; d3 = mat[2][3];
  495.  
  496.     a4 = mat[3][0]; b4 = mat[3][1];
  497.     c4 = mat[3][2]; d4 = mat[3][3];
  498.  
  499.     ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) -
  500.       b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) +
  501.       c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) -
  502.       d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  503.  
  504.     return ans;
  505. }
  506.  
  507.  
  508. double det3x3 (double a1, double a2, double a3, double b1, double b2,
  509.            double b3, double c1, double c2, double c3)
  510. {
  511.     double ans;
  512.  
  513.     ans = a1 * det2x2 (b2, b3, c2, c3)
  514.     - b1 * det2x2 (a2, a3, c2, c3)
  515.     + c1 * det2x2 (a2, a3, b2, b3);
  516.  
  517.     return ans;
  518. }
  519.  
  520.  
  521. double det2x2 (double a, double b, double c, double d)
  522. {
  523.     double ans;
  524.     ans = a * d - b * c;
  525.     return ans;
  526. }
  527.  
  528.