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