home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / general / raytrace / radiance / simplerd.lha / simplerad / FinalFTP / Light / geo.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-07-20  |  31.9 KB  |  1,115 lines
  1. /**********************************************************************/
  2. /* geo.c : Geometric operations                                       */
  3. /*                                                                    */
  4. /* Parts from "Illumination and Color in Computer Graphics", by Roy   */
  5. /* Hall.                                                              */
  6. /* Parts also adapted from Graphics Gems I (ed. James Arvo), 1989.    */
  7. /*                                                                    */
  8. /* Copyright (C) 1992, Bernard Kwok                                   */
  9. /* All rights reserved.                                               */
  10. /* Revision 1.0                                                       */
  11. /* May, 1992                                                          */
  12. /**********************************************************************/
  13. #include <stdio.h>
  14. #include <math.h>
  15. #include "misc.h"
  16. #include "geo.h"
  17.  
  18. /*====================================================================*/
  19. /* 2D operations */
  20. /*====================================================================*/
  21.  
  22. /**********************************************************************/
  23. /* 2D Dot product */
  24. /**********************************************************************/
  25. double vdot2(v) 
  26. Vector2 *v;
  27. { return((v->x * v->x) + (v->y * v->y)); }
  28.         
  29. /**********************************************************************/
  30. /* 2D length of vector */
  31. /**********************************************************************/
  32. double vlen2(a) 
  33.      Vector2 *a;
  34. { return(sqrt(vdot2(a))); }
  35.     
  36. /**********************************************************************/
  37. /* 2D negate vector */
  38. /**********************************************************************/
  39. Vector2 *vnegate2(v) 
  40.      Vector2 *v;
  41. { v->x = -v->x;  v->y = -v->y;  return(v); }
  42.  
  43. /**********************************************************************/
  44. /* 2D norm */
  45. /**********************************************************************/
  46. Vector2 *vnorm2(v) 
  47.      Vector2 *v;
  48. {
  49.   double len;
  50.   
  51.   if ((len=vlen2(v)) == 0.0)
  52.     fprintf(stderr,"Cannot normalize, 2d length is zero\n");
  53.   else { v->x /= len; v->y /= len; }
  54.   return(v);
  55. }
  56.  
  57. /**********************************************************************/
  58. /* 2D scale vector */
  59. /**********************************************************************/
  60. Vector2 *vscale2(v, newlen) 
  61.      Vector2 *v;
  62.      double newlen;
  63. {
  64.   double len;
  65.  
  66.   if ((len=vlen2(v)) == 0.0)
  67.     fprintf(stderr,"Cannot scale, 2d length is zero\n");
  68.   else { v->x *= newlen/len; v->y *= newlen/len; }
  69.   return(v);
  70. }
  71.  
  72. /**********************************************************************/
  73. /* 2d vector add */
  74. /**********************************************************************/
  75. Vector2 *vadd2(a, b, c)
  76.      Vector2 *a, *b, *c;
  77. {
  78.   c->x = a->x + b->x;  c->y = a->y + b->y;
  79.   return(c);
  80. }
  81.     
  82. /**********************************************************************/
  83. /* 2d vector subtract */
  84. /**********************************************************************/
  85. Vector2 *vsub2(a, b, c)
  86.      Vector2 *a, *b, *c;
  87. {
  88.   c->x = a->x - b->x;  c->y = a->y - b->y;
  89.   return(c);
  90. }
  91.  
  92. /**********************************************************************/
  93. /* 2d linearly interpolate between vectors by amount alpha */
  94. /**********************************************************************/
  95. Vector2 *vinterp2(lo, hi, alpha, result) 
  96.      Vector2 *lo, *hi, *result; 
  97.      double alpha;
  98. {
  99.   result->x = LERP(alpha, lo->x, hi->x);
  100.   result->y = LERP(alpha, lo->y, hi->y);
  101.   return(result);
  102. }
  103.  
  104. /**********************************************************************/
  105. /* 2d linear combination */
  106. /**********************************************************************/
  107. Vector2 *vcomb2(v1, v2, result, a, b) 
  108.      Vector2 *v1, *v2, *result;
  109.      double a, b;
  110. {
  111.   result->x = (a * v1->x) + (b * v2->x);
  112.   result->y = (a * v1->y) + (b * v2->y);
  113.   return(result);
  114. }
  115.  
  116. /**********************************************************************/
  117. /* 2d multiply two vectors together component-wise */
  118. /**********************************************************************/
  119. Vector2 *vmult2(a, b, result) 
  120. Vector2 *a, *b, *result;
  121. {
  122.   result->x = a->x * b->x;  result->y = a->y * b->y;
  123.   return(result);
  124. }
  125.  
  126. /**********************************************************************/
  127. /* 2D distance between two points */
  128. /**********************************************************************/
  129. double vdist2(a, b)
  130.      point2 *a, *b;
  131. {
  132.   double dx = a->x - b->x;
  133.   double dy = a->y - b->y;
  134.   return(sqrt((dx*dx)+(dy*dy)));
  135. }
  136.  
  137. /**********************************************************************/
  138. /* 2d perpendicular vector to the input vector a */
  139. /**********************************************************************/
  140. Vector2 *vperp2(a, ap)
  141. Vector2 *a, *ap;
  142. {
  143.   ap->x = -a->y; ap->y = a->x;
  144.   return(ap);
  145. }
  146.  
  147. /**********************************************************************/
  148. /* create, initialize, and return a new vector */
  149. /**********************************************************************/
  150. Vector2 *vnew2(x, y)
  151.      double x, y;
  152. {
  153.   Vector2 *v = NEWTYPE(Vector2);
  154.   v->x = x;  v->y = y; 
  155.   return(v);
  156. }
  157.     
  158. /**********************************************************************/
  159. /* create, initialize, and return a duplicate vector */
  160. /**********************************************************************/
  161. Vector2 *vdup2(a)
  162.      Vector2 *a;
  163. {
  164.   Vector2 *v = NEWTYPE(Vector2);
  165.   v->x = a->x;  v->y = a->y; 
  166.   return(v);
  167. }
  168.  
  169. /**********************************************************************/
  170. /* multiply a point by a matrix and return the transformed point */
  171. /**********************************************************************/
  172. point2 *vtransform2(p, m)
  173.      point2 *p;
  174.      Matrix3 *m;
  175. {
  176.   double x,y;
  177.   double w;
  178.     
  179.   x = (p->x * m->element[0][0]) + 
  180.     (p->y * m->element[1][0]) + m->element[2][0];
  181.   y = (p->x * m->element[0][1]) + 
  182.     (p->y * m->element[1][1]) + m->element[2][1];
  183.   w    = (p->x * m->element[0][2]) + 
  184.     (p->y * m->element[1][2]) + m->element[2][2];
  185.   p->x = x; p->y = y;
  186.   if (w != 0.0) { p->x /= w;  p->y /= w; }
  187.   else printf("V2MulpointByMatrix : point at infinity\n");
  188.   return(p);
  189. }
  190.  
  191. /**********************************************************************/
  192. /* multiply together matrices c = ab */
  193. /* note that c must not point to either of the input matrices */
  194. /**********************************************************************/
  195. Matrix3 *multmatrix2(a, b, c)
  196.      Matrix3 *a, *b, *c;
  197. {
  198.   int i,j,k;
  199.  
  200.   for (i=0; i<3; i++) {
  201.     for (j=0; j<3; j++) {
  202.       c->element[i][j] = 0;
  203.       for (k=0; k<3; k++) c->element[i][j] += 
  204.     a->element[i][k] * b->element[k][j];
  205.     }
  206.   }
  207. return(c);
  208. }
  209.  
  210. /*====================================================================*/
  211. /* 3D geometric routines */
  212. /*====================================================================*/
  213. /**********************************************************************/
  214. /* Two vertices are within some tolerance distance of each other */
  215. /**********************************************************************/
  216. int vsame(v1, v2, tolerance)
  217.      Vector *v1, *v2;
  218.      double tolerance;
  219. {
  220.   extern double vdist();
  221.  
  222.   if (_ABS(vdist(v1,v2)) < tolerance) 
  223.     return(1);
  224.   else
  225.     return(0);
  226. }
  227.  
  228. /**********************************************************************/
  229. /* find dot product of vectors v0 and v1 */
  230. /**********************************************************************/
  231. double dot(v0,v1)
  232.      Vector *v0, *v1;
  233. {  return (v0->x * v1->x) + (v0->y * v1->y) + (v0->z * v1->z); }
  234.  
  235. /**********************************************************************/
  236. /* Find cross product of vectors v0 and v1 */
  237. /**********************************************************************/
  238. Vector cross(v0, v1)
  239.      Vector *v0, *v1;
  240. {
  241.   Vector v2;
  242.  
  243.   v2.x = (v0->y * v1->z) - (v0->z * v1->y);
  244.   v2.y = (v0->z * v1->x) - (v0->x * v1->z);
  245.   v2.z = (v0->x * v1->y) - (v0->y * v1->x);
  246.   return v2;
  247. }
  248.  
  249. /**********************************************************************/
  250. /* Multiply a vector by a scalar */
  251. /**********************************************************************/
  252. Vector *vscalar(s, v)
  253.      double s;
  254.      Vector *v;
  255. {
  256.   Vector v1;
  257.  
  258.   v1.x = s * v->x; v1.y = s * v->y; v1.z = s * v->z;
  259.   return &v1;
  260. }
  261.  
  262. /**********************************************************************/
  263. /* Normalized vector v0 and return length, return negative length if */
  264. /* close to zero, or less than zero */
  265. /**********************************************************************/
  266. double norm(v0)
  267.      Vector *v0;
  268. {
  269.   double len;
  270.  
  271.   if ((len = dot(v0,v0)) <= 0.0) return FALSE;
  272.   len = sqrt((double)len);
  273.   if (len < MIN_VECTOR_LENGTH) return FALSE;
  274.   v0->x = v0->x / len;
  275.   v0->y = v0->y / len;
  276.   v0->z = v0->z / len;
  277.   return TRUE;
  278. }
  279.  
  280. /**********************************************************************/
  281. /* Create vector from 2 points, and return length */
  282. /**********************************************************************/
  283. double geo_line(p0, p1, v)
  284.      point3 *p0, *p1;
  285.      line *v;
  286. {
  287.   v->start = *p0;
  288.   v->dir.x = p1->x - p0->x;
  289.   v->dir.y = p1->y - p0->y;
  290.   v->dir.z = p1->z - p0->z;
  291.   return norm(&v->dir);
  292. }
  293.  
  294. /**********************************************************************/
  295. /* Add 2 vectors */
  296. /**********************************************************************/
  297. Vector *vadd(v0, v1)
  298.      Vector *v0, *v1;
  299. {
  300.   Vector add;
  301.   
  302.   add.x = v0->x + v1->x;
  303.   add.y = v0->y + v1->y;
  304.   add.z = v0->z + v1->z;
  305.   return &add;
  306. }
  307.  
  308. /**********************************************************************/
  309. /* Subtract vector v1 from v0 */
  310. /**********************************************************************/
  311. Vector *vsub(v0, v1)
  312.      Vector *v0, *v1;
  313. {
  314.   Vector sub;
  315.   
  316.   sub.x = v0->x - v1->x;
  317.   sub.y = v0->y - v1->y;
  318.   sub.z = v0->z - v1->z;
  319.   return ⊂
  320. }
  321.  
  322. /**********************************************************************/
  323. /* Vector negation */
  324. /**********************************************************************/
  325. Vector *vnegate(v) 
  326.      Vector *v;
  327. { 
  328.   v->x = -v->x;  v->y = -v->y;  v->z = -v->z;
  329.   return(v);
  330. }
  331.  
  332. /**********************************************************************/
  333. /* multiply two vectors together component-wise and return the result */
  334. /**********************************************************************/
  335. Vector *vmult(v1, v2, result) 
  336. Vector *v1, *v2, *result;
  337. {
  338.   result->x = v1->x * v2->x;
  339.   result->y = v1->y * v2->y;
  340.   result->z = v1->z * v2->z;
  341.   return(result);
  342. }
  343.  
  344. /**********************************************************************/
  345. /* return the distance between two points */
  346. /**********************************************************************/
  347. double vdist(a, b)
  348.      Vector *a, *b;
  349. {
  350.   double dx, dy, dz;
  351.  
  352.   dx = a->x - b->x; dy = a->y - b->y;  dz = a->z - b->z;
  353.   return(sqrt((dx*dx)+(dy*dy)+(dz*dz)));
  354. }
  355.  
  356. /**********************************************************************/
  357. /* Mid-point between 2 vectors */
  358. /**********************************************************************/
  359. Vector *vmiddle(v1,v2)
  360.      Vector *v1, *v2;
  361. {
  362.   Vector vmid;
  363.  
  364.   vmid.x = (v1->x + v2->x) / 2.0;
  365.   vmid.y = (v1->y + v2->y) / 2.0;
  366.   vmid.z = (v1->z + v2->z) / 2.0;
  367.   return(&vmid);
  368. }
  369.  
  370. /**********************************************************************/
  371. /* Length of a vector
  372. /**********************************************************************/
  373. double vlen(v) 
  374.      Vector *v;
  375. { 
  376.   double v_dot_v;
  377.   double len;
  378.  
  379.   v_dot_v = dot(v,v);
  380.   len = sqrt(v_dot_v);
  381.   return(len);
  382. }
  383.  
  384. /**********************************************************************/
  385. /* Vector scale */
  386. /**********************************************************************/
  387. Vector *vscale(v, newlen) 
  388.      Vector *v;
  389.      double newlen;
  390. {
  391.   double len = vlen(v);
  392.   if (len != 0.0) {
  393.     v->x *= newlen/len;   v->y *= newlen/len;  v->z *= newlen/len;
  394.   }
  395.   return(v);
  396. }
  397.  
  398. /**********************************************************************/
  399. /* Vector interpolation by alpha */
  400. /**********************************************************************/
  401. Vector *vinterp(lo, hi, alpha, result) 
  402.      Vector *lo, *hi, *result; 
  403.      double alpha;
  404. {
  405.   result->x = LERP(alpha, lo->x, hi->x);
  406.   result->y = LERP(alpha, lo->y, hi->y);
  407.   result->z = LERP(alpha, lo->z, hi->z);
  408.   return(result);
  409. }
  410.  
  411. /**********************************************************************/
  412. /* Vector linear combination: a*v0 + b*v1 */
  413. /**********************************************************************/
  414. Vector *vcomb(v0,v1,a,b)
  415.      Vector *v0, *v1;
  416.      double a,b;
  417. {
  418.   static Vector comb;
  419.   
  420.   comb.x = a * v0->x + b * v1->x;
  421.   comb.y = a * v0->y + b * v1->y;
  422.   comb.z = a * v0->z + b * v1->z;
  423.   return &comb;
  424. }
  425.  
  426. /**********************************************************************/
  427. /* New vector */
  428. /**********************************************************************/
  429. Vector *vnew(x, y, z)
  430.      double x, y, z;
  431. {
  432.   Vector *v = NEWTYPE(Vector);
  433.   v->x = x;  v->y = y;  v->z = z;
  434.   return(v);
  435. }
  436.  
  437. /**********************************************************************/
  438. /* New duplicate vector */
  439. /**********************************************************************/
  440. Vector *vdup(d)
  441. Vector *d;
  442. {
  443.   Vector *v = NEWTYPE(Vector);
  444.   v->x = d->x;  v->y = d->y;  v->z = d->z;
  445.   return(v);
  446. }
  447.  
  448. /**********************************************************************/
  449. /* Ideal reflection vector */
  450. /**********************************************************************/
  451. Vector *reflection(v,n)
  452.      Vector *v, *n;
  453. {
  454.   double n_dot_v;
  455.   static Vector rfl;
  456.  
  457.   n_dot_v = dot(n,v);
  458.   rfl.x = (2.0 * n_dot_v * n->x) - v->x;
  459.   rfl.y = (2.0 * n_dot_v * n->y) - v->y;
  460.   rfl.z = (2.0 * n_dot_v * n->z) - v->z;
  461.   return &rfl;
  462. }
  463.  
  464. /**********************************************************************/
  465. /* Ideal refraction vector (simple) */
  466. /**********************************************************************/
  467. Vector *refraction(v,n,ni,nt)
  468.      Vector *v, *n;
  469.      double ni, nt;
  470. {
  471.   static Vector t;
  472.   Vector sin_t, cos_v;
  473.   double len_sin_t, n_mult, n_dot_v, n_dot_t;
  474.   if ((n_dot_v = dot(n,v)) > 0.0) n_mult = ni / nt;
  475.   else n_mult = nt / ni;
  476.   cos_v.x = n_dot_v * n->x;
  477.   cos_v.y = n_dot_v * n->y;
  478.   cos_v.z = n_dot_v * n->z;
  479.   sin_t.x = (n_mult) * (cos_v.x = v->x);
  480.   sin_t.y = (n_mult) * (cos_v.y = v->y);
  481.   sin_t.z = (n_mult) * (cos_v.z = v->z);
  482.   if ((len_sin_t = dot(&sin_t, &sin_t)) > 1.0)
  483.     return NULL; /* internal reflection */
  484.   n_dot_t = sqrt(1.0 - len_sin_t);
  485.   if (n_dot_v < 0.0) n_dot_t = -n_dot_t;
  486.   t.x = sin_t.x - (n->x * n_dot_t);
  487.   t.y = sin_t.y - (n->y * n_dot_t);
  488.   t.z = sin_t.z - (n->z * n_dot_t);
  489.   return &t;
  490. }
  491.  
  492. /**********************************************************************/
  493. /* Halfway vector */
  494. /**********************************************************************/
  495. Vector *geo_h(l,v)
  496.      Vector *l, *v;
  497. {
  498.   static Vector h;
  499.   h.x = l->x + v->x;
  500.   h.y = l->y + v->y;
  501.   h.z = l->z + v->z;
  502.   if (!norm(&h)) return NULL;
  503.   return &h;
  504. }
  505.  
  506. /**********************************************************************/
  507. /* Halfway vector (refraction) */
  508. /**********************************************************************/
  509. Vector *geo_ht(l,t,ni,nt)
  510.      Vector *l, *t;
  511.      double ni, nt;
  512. {
  513.   float l_dot_t;
  514.   float divisor;
  515.   static Vector ht;
  516.  
  517.   l_dot_t = -(dot(l,t));
  518.   if (ni == nt) {
  519.     if (l_dot_t == 1.0) return l;
  520.     else return NULL;
  521.   }
  522.   if (ni < nt) {
  523.     if (l_dot_t < ni/nt) return NULL;
  524.     divisor = (nt / ni) - 1.0;
  525.     ht.x = -(((l->x + t->x) / divisor) + t->x);
  526.     ht.y = -(((l->y + t->y) / divisor) + t->y);
  527.     ht.z = -(((l->z + t->z) / divisor) + t->z);
  528.   } else {
  529.     if (l_dot_t < nt/ni) return NULL;
  530.     divisor = (ni / nt) - 1.0;
  531.     ht.x = ((l->x + t->x) / divisor) + l->x;
  532.     ht.y = ((l->y + t->y) / divisor) + l->y;
  533.     ht.z = ((l->z + t->z) / divisor) + l->z;
  534.   }
  535.   (void) norm(&ht);
  536.   return &ht;
  537. }
  538.  
  539. /* from optik originally */
  540. /**********************************************************************/
  541. /* This function is similar to pow() but works right only for */
  542. /* positive integer b (but a lot faster) */
  543. /**********************************************************************/
  544. double Power(a,b)
  545.      double a,b;
  546. {
  547.   register int power;
  548.   double result;
  549.   
  550.   result= 1.0;
  551.   power= (int) b;
  552.   while(power > 0) {
  553.     if(power & 1)
  554.       result *= a;
  555.     a *= a;
  556.     power >>= 1;
  557.   }
  558.   return(result);
  559. }
  560.  
  561. /**********************************************************************/
  562. /* Calculate normal for a polygon from its vertices */
  563. /**********************************************************************/
  564. Vector polynorm(vert,size)
  565.      register Vector vert[];
  566.      register int size;
  567. {
  568.   Vector n;
  569.   register int i,j;
  570.   
  571.   n.x = n.y = n.z= 0.0;
  572.   for(i=0;i<size;i++) {
  573.     j= (i+1)%size;
  574.     n.x += (vert[i].y-vert[j].y) * (vert[i].z+vert[j].z);
  575.     n.y += (vert[i].z-vert[j].z) * (vert[i].x+vert[j].x);
  576.     n.z += (vert[i].x-vert[j].x) * (vert[i].y+vert[j].y);
  577.   }
  578.   if(norm(&(n)) < 0.) {
  579.     /*    if(Option.debug)
  580.     printf("Sorry, a normal cannot be computed for one of the polygons.\n");
  581.     */
  582.     n.x = n.y = 0.;
  583.     n.z= 1.;
  584.   }
  585.   return(n);
  586. }
  587.  
  588. /**********************************************************************/
  589. /* Copy matrix M0 to M1 */
  590. /**********************************************************************/
  591. void copy_matrix(M0,M1)
  592.      register Matrix M0, M1;
  593. {
  594.   register int i,j;
  595.   
  596.   for(i=0;i<4;i++) for(j=0;j<4;j++) M1[i][j]= M0[i][j];
  597. }
  598.  
  599. /**********************************************************************/
  600. /* Are matrix M0 to M1 the same matrix */
  601. /**********************************************************************/
  602. int same_matrix(M0,M1)
  603.      register Matrix M0, M1;
  604. {
  605.   register int i,j;
  606.   
  607.   for(i=0;i<4;i++) for(j=0;j<4;j++) 
  608.     if (M1[i][j] != M0[i][j]) return 0;
  609.   return 1;
  610. }
  611.  
  612. /**********************************************************************/
  613. /* Are matrix M0 to M1 the same matrix */
  614. /**********************************************************************/
  615. int same_matrix3x3(M0,M1)
  616.      register Matrix M0, M1;
  617. {
  618.   register int i,j;
  619.   
  620.   for(i=0;i<3;i++) for(j=0;j<3;j++) 
  621.     if (M1[i][j] != M0[i][j]) return 0;
  622.   return 1;
  623. }
  624.  
  625. /**********************************************************************/
  626. /* Multiply matrix M2 = M0 * M1 */
  627. /**********************************************************************/
  628. void mult_matrix(M0, M1, M2)
  629.      register Matrix M0, M1, M2;
  630. {
  631.   double sum;
  632.   register int i, j, k;
  633.   
  634.   for(i=0;i<4;i++) for(j=0;j<4;j++) {
  635.     sum= 0.0;
  636.     for(k=0;k<4;k++)
  637.       sum += M0[i][k] * M1[k][j];
  638.     M2[i][j] = sum;
  639.   }
  640. }
  641.  
  642. /**********************************************************************/
  643. /* Scalar Multiply matrix M1 = s * M0 */
  644. /**********************************************************************/
  645. void smult_matrix(s, M0, M1)
  646.      double s;
  647.      register Matrix M0, M1;
  648. {
  649.   register int i, j;
  650.   
  651.   for(i=0;i<4;i++) for(j=0;j<4;j++) {
  652.     M1[i][j] = s * M0[i][j];
  653.   }
  654. }
  655. /**********************************************************************/
  656. /* 4x4 Matrix * vector */
  657. /**********************************************************************/
  658. Vector vrotate(v,M)
  659.      Vector v;
  660.      register Matrix M;
  661. {
  662.   Vector vPrime;
  663.   
  664.   vPrime.x= v.x * M[0][0] + v.y * M[1][0] + v.z * M[2][0]; 
  665.   vPrime.y= v.x * M[0][1] + v.y * M[1][1] + v.z * M[2][1]; 
  666.   vPrime.z= v.x * M[0][2] + v.y * M[1][2] + v.z * M[2][2]; 
  667.  
  668.   return(vPrime);
  669. }
  670.  
  671. /**********************************************************************/
  672. /* 4x4 Matrix * vector */
  673. /**********************************************************************/
  674. Vector vtransform(v,M)
  675.      Vector v;
  676.      register Matrix M;
  677. {
  678.   Vector vPrime;
  679.   
  680.   vPrime.x= v.x * M[0][0] + v.y * M[1][0] + v.z * M[2][0] + M[3][0];
  681.   vPrime.y= v.x * M[0][1] + v.y * M[1][1] + v.z * M[2][1] + M[3][1];
  682.   vPrime.z= v.x * M[0][2] + v.y * M[1][2] + v.z * M[2][2] + M[3][2];
  683.   return(vPrime);
  684. }
  685.  
  686. /**********************************************************************/
  687. /* Point transform */
  688. /**********************************************************************/
  689. point3 *ptransform(p, m)
  690.      point3 *p;
  691.      Matrix m;
  692. {
  693.   double x,y,z;
  694.   double w;
  695.   x = (p->x * m[0][0]) + (p->y * m[1][0]) + (p->z * m[2][0]) + m[3][0];
  696.   y = (p->x * m[0][1]) + (p->y * m[1][1]) + (p->z * m[2][1]) + m[3][1];
  697.   z = (p->x * m[0][2]) + (p->y * m[1][2]) + (p->z * m[2][2]) + m[3][2];
  698.   w = (p->x * m[0][3]) + (p->y * m[1][3]) + (p->z * m[2][3]) + m[3][3];
  699.   p->x = x; p->y = y; p->z = z;
  700.   if (w != 0.0) {
  701.     p->x /= w;  p->y /= w;  p->z /= w;
  702.   }
  703.   else printf("ptransform : point at Infinity.\n");
  704.   return(p);
  705. }
  706.  
  707. /**********************************************************************/
  708. /* Transpose a matrix */
  709. /**********************************************************************/
  710. void transpose_matrix(M,m)
  711.      Matrix M, m;
  712. {
  713.   int i,j;
  714.  
  715.   for (i=0;i<4;i++) for (j=0;j<4;j++)
  716.     m[j][i] = M[i][j];
  717. }
  718.  
  719. /**********************************************************************/
  720. /* Invert matrix a -> b (using determinant method) */
  721. /**********************************************************************/
  722. int invert_matrix(a,b)
  723.      Matrix a,b;
  724. {
  725.   double det;
  726.  
  727.   b[0][0]= a[1][1]*a[2][2] - a[1][2]*a[2][1];
  728.   b[1][0]= a[1][0]*a[2][2] - a[1][2]*a[2][0];
  729.   b[2][0]= a[1][0]*a[2][1] - a[1][1]*a[2][0];
  730.   
  731.   b[0][1]= a[0][1]*a[2][2] - a[0][2]*a[2][1];
  732.   b[1][1]= a[0][0]*a[2][2] - a[0][2]*a[2][0];
  733.   b[2][1]= a[0][0]*a[2][1] - a[0][1]*a[2][0];
  734.   
  735.   b[0][2]= a[0][1]*a[1][2] - a[0][2]*a[1][1];
  736.   b[1][2]= a[0][0]*a[1][2] - a[0][2]*a[1][0];
  737.   b[2][2]= a[0][0]*a[1][1] - a[0][1]*a[1][0];
  738.   
  739.   det= a[0][0]*b[0][0] - a[0][1]*b[1][0] + a[0][2]*b[2][0];
  740.   
  741.   /* matrix is singular */
  742.   if(det < DAMN_SMALL && det > -DAMN_SMALL) return(ERROR);
  743.  
  744.   b[0][0] /= det;
  745.   b[0][1] /= -det;
  746.   b[0][2] /= det;
  747.   b[1][0] /= -det;
  748.   b[1][1] /= det;
  749.   b[1][2] /= -det;
  750.   b[2][0] /= det;
  751.   b[2][1] /= -det;
  752.   b[2][2] /= det;
  753.   
  754.   b[3][0]= -(b[0][0]*a[3][0] + b[1][0]*a[3][1] + b[2][0]*a[3][2]);
  755.   b[3][1]= -(b[0][1]*a[3][0] + b[1][1]*a[3][1] + b[2][1]*a[3][2]);
  756.   b[3][2]= -(b[0][2]*a[3][0] + b[1][2]*a[3][1] + b[2][2]*a[3][2]);
  757.   b[3][3]= 1.;
  758.   b[0][3]= 0.;
  759.   b[1][3]= 0.;
  760.   b[2][3]= 0.;
  761.   return(OK);
  762. }
  763.  
  764. /**********************************************************************/
  765. /* create rotation by theta in z-axis matrix */
  766. /**********************************************************************/
  767. void cr_rotatez(theta,M)
  768.      double theta;
  769.      register Matrix M;
  770. {
  771.   double sine, cosine;
  772.   
  773.   sine= sin(theta);
  774.   cosine= cos(theta);
  775.   
  776.   copy_matrix(Identity,M);
  777.   M[0][0]= cosine;
  778.   M[1][0]= -sine;
  779.   M[0][1]= sine;
  780.   M[1][1]= cosine;
  781. }
  782.  
  783. /**********************************************************************/
  784. /* Rotate in x by angle a */ 
  785. /**********************************************************************/
  786. void rotatex_matrix(M,a)
  787.      Matrix M;   /* Current transformation matrix*/
  788.      double a;     /* Rotation angle        */
  789. {
  790.   double cosine, sine;
  791.   double t;
  792.   int i;
  793.  
  794.   cosine = cos(a);    
  795.   sine = sin(a);
  796.   for (i=0; i<4; i++) {
  797.     t = M[i][1];
  798.     M[i][1] = t*cosine - M[i][2]*sine;
  799.     M[i][2] = t*sine + M[i][2]*cosine;
  800.   }
  801. }
  802.  
  803. /**********************************************************************/
  804. /* Rotate in y by angle a */ 
  805. /**********************************************************************/
  806. void rotatey_matrix(M,a)
  807.      Matrix M;  /* Current transformation matrix*/
  808.      double a;    /* Rotation angle        */
  809. {
  810.   double cosine, sine;
  811.   double t;
  812.   int i;
  813.  
  814.   cosine = cos(a);
  815.   sine = sin(a);
  816.   /* printf("cosine(%g) = %g\n", a, cosine);
  817.   printf("sine(%g) = %g\n", a, sine); */
  818.  
  819.   for (i=0; i<4; i++) {
  820.     t = M[i][0];
  821.     M[i][0] = t*cosine + M[i][2]*sine;
  822.     M[i][2] = M[i][2]*cosine - t*sine;
  823.   }
  824. }
  825.  
  826.  
  827. /**********************************************************************/
  828. /* rotate in z */
  829. /**********************************************************************/
  830. void rotatez_matrix(M,a)
  831.      Matrix    M;            /* Current transformation matrix*/
  832.      double    a;            /* Rotation angle        */
  833. {
  834.   double cosine, sine;
  835.   double t;
  836.   int i;
  837.  
  838.   cosine = cos(a);    
  839.   sine = sin(a);
  840.   
  841.   for (i=0; i<4; i++) {
  842.     t = M[i][0];
  843.     M[i][0] = t*cosine - M[i][1]*sine;
  844.     M[i][1] = t*sine + M[i][1]*cosine;
  845.   }
  846. }
  847.  
  848. /**********************************************************************/
  849. /* scale matrix by sx, sy, sz */
  850. /**********************************************************************/
  851. void scale_matrix(M,sx,sy,sz)
  852.      Matrix    M;        /* Current transformation matrix */
  853.      double    sx, sy, sz;    /* Scale factors about x,y,z */
  854. {
  855.   int i;
  856.  
  857.   for (i=0; i<4; i++) {
  858.     M[i][0] *= sx;
  859.     M[i][1] *= sy;
  860.     M[i][2] *= sz;
  861.   }
  862. }
  863.  
  864. /**********************************************************************/
  865. /* translate matrix */
  866. /**********************************************************************/
  867. void translate_matrix(M,tx,ty,tz)
  868.      Matrix    M;            /* Current transformation matrix */
  869.      double    tx, ty, tz;    /* Translation distance */
  870. {
  871.   int i;
  872.  
  873.   for (i=0; i<4; i++) {
  874.     M[i][0] += M[i][3] * tx;
  875.     M[i][1] += M[i][3] * ty;
  876.     M[i][2] += M[i][3] * tz;
  877.   }
  878. }
  879.  
  880. /**********************************************************************/
  881. /* subtract matrices: m2 = m0 - m1 */
  882. /**********************************************************************/
  883. void subtract_matrix(m0, m1, m2)
  884.      Matrix    m0, m1, m2;
  885. {
  886.   int i,j;
  887.   
  888.   for (i=0; i<4; i++) for (j=0; j<4; j++) 
  889.     m2[i][j] = m0[i][j] - m1[i][j];
  890. }
  891.  
  892. /**********************************************************************/
  893. /* Add 2 matrices */
  894. /**********************************************************************/
  895. void add_matrix(m0, m1, m2)
  896.      Matrix    m0, m1, m2;
  897. {
  898.   int i,j;
  899.   
  900.   for (i=0; i<4; i++) for (j=0; j<4; j++) 
  901.     m2[i][j] = m0[i][j] + m1[i][j];
  902. }
  903.  
  904. /**********************************************************************/
  905. /* Perspetive transformation along z */
  906. /**********************************************************************/
  907. /* Perspective is along the z-axis with the eye at +z.   */
  908. void ZPerspective_matrix(M,d)
  909.      Matrix    M;            /* Current transformation matrix  */
  910.      double    d;            /* Perspective distance        */
  911. {
  912.   int i;
  913.   double f = 1.0 / d;
  914.  
  915.   for (i = 0; i < 4; i++) {
  916.     M[i][3] += M[i][2] * f;
  917.     M[i][2]  = 0.0;
  918.   }
  919. }
  920.  
  921. /*********************************************************************
  922.  * Reorthogonalize matrix R - that is find an orthogonal matrix that is
  923.  * "close" to R by computing an approximation to the orthogonal matrix
  924.  *
  925.  *           T  -1/2
  926.  *   RC = R(R R)
  927.  *                           T-1
  928.  * [RC is orthogonal because (RC) = (RC) ]
  929.  *                                                       -1/2
  930.  * To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
  931.  * (where x = C - I) about x=0.
  932.  * This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
  933.  *
  934.  * Reorthogonalize matrix R - that is find an orthogonal matrix that is
  935.  * "close" to R by computing an approximation to the orthogonal matrix
  936.  *
  937.  *           T  -1/2
  938.  *   RC = R(R R)
  939.  *                        T-1
  940.  * [RC is orthogonal because (RC) = (RC) ]
  941.  *                                                       -1/2
  942.  * To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
  943.  * (where x = C - I) about x=0.
  944.  * This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
  945.  *********************************************************************/
  946. static float coef[10] = {            /* From mathematica */
  947.   1, -1./2., 3/8., -5/16., 35/128., -63/256.,
  948.   231/1024., -429/2048., 6435/32768., -12155/65536. 
  949.   };
  950.  
  951. void reorthogonalize_matrix(R, r, limit)
  952.      Matrix R, r;
  953.      int limit;
  954. {
  955.   Matrix I, Temp, Xx, X_power, Sum;
  956.   int power;
  957.   
  958.   limit = MAX(limit, 10);
  959.  
  960.   transpose_matrix(R, Temp);    /* Rt */
  961.   mult_matrix(Temp, R, Temp);    /* RtR */
  962.   copy_matrix(Identity,I);      /* I */
  963.   subtract_matrix(Temp, I, Xx);    /* RtR - I */
  964.   copy_matrix(Identity,X_power);/* X^0 */
  965.   copy_matrix(Identity,Sum);    /* coef[0] * X^0 */
  966.  
  967.   for (power=1; power<limit; ++power) {
  968.     mult_matrix(X_power, Xx, X_power);
  969.     smult_matrix(coef[power], X_power, Temp);
  970.     add_matrix(Sum, Temp, Sum);
  971.   }
  972.   mult_matrix(R, Sum, r);
  973. }
  974.  
  975. /**********************************************************************/
  976. /* Find matrix adjoint */
  977. /**********************************************************************/
  978. void adjoint(in,out) 
  979.      Matrix in; Matrix out;
  980. {
  981.   double a1, a2, a3, a4, b1, b2, b3, b4;
  982.   double c1, c2, c3, c4, d1, d2, d3, d4;
  983.   double det3x3();
  984.  
  985.   /* assign to individual variable names to aid  */
  986.   /* selecting correct values  */
  987.  
  988.   a1 = in[0][0]; b1 = in[0][1]; 
  989.   c1 = in[0][2]; d1 = in[0][3];
  990.   a2 = in[1][0]; b2 = in[1][1]; 
  991.   c2 = in[1][2]; d2 = in[1][3];
  992.   a3 = in[2][0]; b3 = in[2][1];
  993.   c3 = in[2][2]; d3 = in[2][3];
  994.   a4 = in[3][0]; b4 = in[3][1]; 
  995.   c4 = in[3][2]; d4 = in[3][3];
  996.  
  997.   /* row column labeling reversed since we transpose rows & columns */
  998.   out[0][0]  =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
  999.   out[1][0]  = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
  1000.   out[2][0]  =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
  1001.   out[3][0]  = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  1002.   out[0][1]  = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
  1003.   out[1][1]  =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
  1004.   out[2][1]  = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
  1005.   out[3][1]  =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
  1006.   out[0][2]  =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
  1007.   out[1][2]  = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
  1008.   out[2][2]  =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
  1009.   out[3][2]  = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
  1010.   out[0][3]  = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
  1011.   out[1][3]  =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
  1012.   out[2][3]  = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
  1013.   out[3][3]  =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
  1014. }
  1015.  
  1016. /**********************************************************************/
  1017. /* calculate the determinent of a 4x4 matrix. */
  1018. /**********************************************************************/
  1019. double det4x4( m ) 
  1020.      Matrix m;
  1021. {
  1022.   double ans;
  1023.   double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
  1024.   double det3x3();
  1025.  
  1026.   /* assign to individual variable names to aid selecting */
  1027.   /*  correct elements */
  1028.  
  1029.   a1 = m[0][0]; b1 = m[0][1]; 
  1030.   c1 = m[0][2]; d1 = m[0][3];
  1031.   a2 = m[1][0]; b2 = m[1][1]; 
  1032.   c2 = m[1][2]; d2 = m[1][3];
  1033.   a3 = m[2][0]; b3 = m[2][1]; 
  1034.   c3 = m[2][2]; d3 = m[2][3];
  1035.   a4 = m[3][0]; b4 = m[3][1]; 
  1036.   c4 = m[3][2]; d4 = m[3][3];
  1037.  
  1038.   ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4) -
  1039.     b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4) +
  1040.       c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4) -
  1041.     d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  1042.   return ans;
  1043. }
  1044.  
  1045. /**********************************************************************/
  1046. /* calculate the determinent of a 3x3 matrix */
  1047. /**********************************************************************/
  1048. double det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  1049.      double a1, a2, a3, b1, b2, b3, c1, c2, c3;
  1050. {
  1051.   double ans;
  1052.   double det2x2();
  1053.  
  1054.   ans = a1 * det2x2( b2, b3, c2, c3 ) -
  1055.      b1 * det2x2( a2, a3, c2, c3 ) +
  1056.        c1 * det2x2( a2, a3, b2, b3 );
  1057.   return ans;
  1058. }
  1059.  
  1060. /**********************************************************************/
  1061. /* calculate the determinent of a 2x2 matrix. */
  1062. /**********************************************************************/
  1063. double det2x2( a, b, c, d)
  1064. double a, b, c, d; 
  1065. {
  1066.   return (a * d - b * c);
  1067. }
  1068.  
  1069.  
  1070. /*====================================================================*/
  1071. /* Output routines */
  1072. /*====================================================================*/
  1073.  
  1074. /**********************************************************************/
  1075. /* Print a vector */
  1076. /**********************************************************************/
  1077. void print_vector(fp, label,v)
  1078.      FILE *fp;
  1079.      char *label;
  1080.      Vector *v;
  1081. {
  1082.   fprintf(fp, "\t%s %g,%g,%g\n", label, v->x, v->y, v->z);
  1083. }
  1084.  
  1085. /**********************************************************************/
  1086. /* Print a vector */
  1087. /**********************************************************************/
  1088. void print_vector2(fp, label,v)
  1089.      FILE *fp;
  1090.      char *label;
  1091.      Vector2 *v;
  1092. {
  1093.   fprintf(fp, "\tVector: %s = %g,%g\n", label, v->x, v->y);
  1094. }
  1095.  
  1096. /**********************************************************************/
  1097. /* Print a matrix */
  1098. /**********************************************************************/
  1099. void print_matrix(fp, label, a)
  1100.      FILE *fp;
  1101.      char *label;
  1102.      register Matrix a;
  1103. {
  1104.   int i,j;
  1105.  
  1106.   fprintf(fp,"\tMatrix: %s\n", label);
  1107.   for(i=0;i<4;i++) {
  1108.     fprintf(fp,"\t");
  1109.     for(j=0;j<4;j++)
  1110.       fprintf(fp,"%10.6g  ",a[i][j]);
  1111.     fprintf(fp,"\n");
  1112.   }
  1113.   fprintf(fp,"\n");
  1114. }
  1115.