C   7

geometry.c

Guest on 20th July 2021 03:08:59 PM

  1. /* geometry.c
  2.    Copyright (C)  Eugene K. Ressler, Jr.
  3.  
  4. This file is part of Sketch, a small, simple system for making
  5. 3d drawings with LaTeX and the PSTricks or TikZ package.
  6.  
  7. Sketch is free software; you can redistribute it and/or modify
  8. it under the terms of the GNU General Public License as published by
  9. the Free Software Foundation; either version 3, or (at your option)
  10. any later version.
  11.  
  12. Sketch is distributed in the hope that it will be useful,
  13. but WITHOUT ANY WARRANTY; without even the implied warranty of
  14. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15. GNU General Public License for more details.
  16.  
  17. You should have received a copy of the GNU General Public License
  18. along with Sketch; see the file COPYING.txt.  If not, see
  19. http://www.gnu.org/copyleft */
  20.  
  21. #include <stdio.h>
  22. #include <stdlib.h>
  23. #include <string.h>
  24. #include <math.h>
  25. #include "geometry.h"
  26. #include "error.h"
  27. #include "memutil.h"
  28.  
  29. // global constants
  30. POINT_2D origin_2d = { 0, 0 };
  31. POINT_3D origin_3d = { 0, 0, 0 };
  32. VECTOR_2D I_2d = { 1, 0 };
  33. VECTOR_2D J_2d = { 0, 1 };
  34. VECTOR_3D I_3d = { 1, 0, 0 };
  35. VECTOR_3D J_3d = { 0, 1, 0 };
  36. VECTOR_3D K_3d = { 0, 0, 1 };
  37. TRANSFORM identity = {
  38.   1, 0, 0, 0,
  39.   0, 1, 0, 0,
  40.   0, 0, 1, 0,
  41.   0, 0, 0, 1
  42. };
  43.  
  44. // numerics
  45.  
  46. FLOAT
  47. max_float (FLOAT x, FLOAT y)
  48. {
  49.   return x > y ? x : y;
  50. }
  51.  
  52. FLOAT
  53. min_float (FLOAT x, FLOAT y)
  54. {
  55.   return x < y ? x : y;
  56. }
  57.  
  58. // points
  59.  
  60. void
  61. copy_pt_2d (POINT_2D r, POINT_2D s)
  62. {
  63.   r[X] = s[X];
  64.   r[Y] = s[Y];
  65. }
  66.  
  67. void
  68. copy_pt_3d (POINT_3D r, POINT_3D s)
  69. {
  70.   r[X] = s[X];
  71.   r[Y] = s[Y];
  72.   r[Z] = s[Z];
  73. }
  74.  
  75. void
  76. find_pt_3d_from_2d (POINT_3D r, POINT_2D pt)
  77. {
  78.   r[X] = pt[X];
  79.   r[Y] = pt[Y];
  80.   r[Z] = 0;
  81. }
  82.  
  83. // polyline initialization and cleanup
  84.  
  85. #define SET_NEXT_NULL a->next = NULL;
  86.  
  87. DECLARE_DYNAMIC_2D_ARRAY_FUNCS (POLYLINE_2D, POINT_2D, FLOAT, polyline_2d,
  88.                                 v, n_vertices, SET_NEXT_NULL)
  89. DECLARE_DYNAMIC_2D_ARRAY_FUNCS (POLYLINE_3D, POINT_3D, FLOAT, polyline_3d,
  90.                                 v, n_vertices, SET_NEXT_NULL)
  91. // polygon initialization and cleanup
  92.   DECLARE_DYNAMIC_2D_ARRAY_FUNCS (POLYGON_2D, POINT_2D, FLOAT, polygon_2d, v,
  93.                                 n_sides, SET_NEXT_NULL)
  94. DECLARE_DYNAMIC_2D_ARRAY_FUNCS (POLYGON_3D, POINT_3D, FLOAT, polygon_3d, v,
  95.                                 n_sides, SET_NEXT_NULL)
  96. // rudimentary vectors of variable size
  97.      void init_vec (VECTOR * v)
  98. {
  99.   *v = 0;
  100. }
  101.  
  102. void
  103. clear_vec (VECTOR * v)
  104. {
  105.   safe_free (*v);
  106.   init_vec (v);
  107. }
  108.  
  109. void
  110. setup_vec (VECTOR * v, SIZE n)
  111. {
  112.   clear_vec (v);
  113.   *v = safe_malloc (n * sizeof (FLOAT));
  114. }
  115.  
  116. void
  117. init_and_setup_vec (VECTOR * v, SIZE n)
  118. {
  119.   *v = safe_malloc (n * sizeof (FLOAT));
  120. }
  121.  
  122. void
  123. zero_vec (VECTOR r, SIZE n)
  124. {
  125.   INDEX i;
  126.  
  127.   for (i = 0; i < n; i++)
  128.     r[i] = 0;
  129. }
  130.  
  131. void
  132. copy_vec (VECTOR r, VECTOR v, SIZE n)
  133. {
  134.   INDEX i;
  135.  
  136.   for (i = 0; i < n; i++)
  137.     r[i] = v[i];
  138. }
  139.  
  140. FLOAT
  141. length_vec_2d (VECTOR_2D v)
  142. {
  143.   return sqrt (dot_2d (v, v));
  144. }
  145.  
  146. FLOAT
  147. length_vec_3d (VECTOR_3D v)
  148. {
  149.   return sqrt (dot_3d (v, v));
  150. }
  151.  
  152. FLOAT
  153. dist_2d (POINT_2D p1, POINT_2D p2)
  154. {
  155.   VECTOR_2D dif;
  156.   sub_pts_2d (dif, p1, p2);
  157.   return length_vec_2d (dif);
  158. }
  159.  
  160. FLOAT
  161. dist_3d (POINT_3D p1, POINT_3D p2)
  162. {
  163.   VECTOR_3D dif;
  164.   sub_pts_3d (dif, p1, p2);
  165.   return length_vec_3d (dif);
  166. }
  167.  
  168. FLOAT
  169. length_vec_2d_sqr (VECTOR_2D v)
  170. {
  171.   return dot_2d (v, v);
  172. }
  173.  
  174. FLOAT
  175. length_vec_3d_sqr (VECTOR_3D v)
  176. {
  177.   return dot_3d (v, v);
  178. }
  179.  
  180. FLOAT
  181. dist_2d_sqr (POINT_2D p1, POINT_2D p2)
  182. {
  183.   VECTOR_2D dif;
  184.   sub_pts_2d (dif, p1, p2);
  185.   return length_vec_2d_sqr (dif);
  186. }
  187.  
  188. FLOAT
  189. dist_3d_sqr (POINT_3D p1, POINT_3D p2)
  190. {
  191.   VECTOR_3D dif;
  192.   sub_pts_3d (dif, p1, p2);
  193.   return length_vec_3d_sqr (dif);
  194. }
  195.  
  196. void
  197. zero_vec_2d (VECTOR_2D v)
  198. {
  199.   v[X] = v[Y] = 0;
  200. }
  201.  
  202. void
  203. zero_vec_3d (VECTOR_3D v)
  204. {
  205.   v[X] = v[Y] = v[Z] = 0;
  206. }
  207.  
  208. void
  209. negate_vec_2d (VECTOR_2D r, VECTOR_2D v)
  210. {
  211.   r[X] = -v[X];
  212.   r[Y] = -v[Y];
  213. }
  214.  
  215. void
  216. negate_vec_3d (VECTOR_3D r, VECTOR_3D v)
  217. {
  218.   r[X] = -v[X];
  219.   r[Y] = -v[Y];
  220.   r[Z] = -v[Z];
  221. }
  222.  
  223. void
  224. copy_vec_2d (VECTOR_2D r, VECTOR_2D s)
  225. {
  226.   r[X] = s[X];
  227.   r[Y] = s[Y];
  228. }
  229.  
  230. void
  231. copy_vec_3d (VECTOR_3D r, VECTOR_3D s)
  232. {
  233.   r[X] = s[X];
  234.   r[Y] = s[Y];
  235.   r[Z] = s[Z];
  236. }
  237.  
  238. void
  239. scale_vec_2d (VECTOR_2D r, VECTOR_2D v, FLOAT s)
  240. {
  241.   r[X] = v[X] * s;
  242.   r[Y] = v[Y] * s;
  243. }
  244.  
  245. void
  246. scale_vec_3d (VECTOR_3D r, VECTOR_3D v, FLOAT s)
  247. {
  248.   r[X] = v[X] * s;
  249.   r[Y] = v[Y] * s;
  250.   r[Z] = v[Z] * s;
  251. }
  252.  
  253. int
  254. find_unit_vec_2d (VECTOR_2D r, VECTOR_2D v)
  255. {
  256.   FLOAT len = length_vec_2d (v);
  257.   if (len <= FLT_EPSILON)
  258.     {
  259.       r[X] = 1;
  260.       r[Y] = 0;
  261.       return 0;
  262.     }
  263.   else
  264.     {
  265.       scale_vec_2d (r, v, 1 / len);
  266.       return 1;
  267.     }
  268. }
  269.  
  270. int
  271. find_unit_vec_3d (VECTOR_3D r, VECTOR_3D v)
  272. {
  273.   FLOAT len = length_vec_3d (v);
  274.   if (len == FLT_EPSILON)
  275.     {
  276.       r[X] = 1;
  277.       r[Y] = r[Z] = 0;
  278.       return 0;
  279.     }
  280.   else
  281.     {
  282.       scale_vec_3d (r, v, 1 / len);
  283.       return 1;
  284.     }
  285. }
  286.  
  287. void
  288. add_vecs_2d (VECTOR_2D r, VECTOR_2D a, VECTOR_2D b)
  289. {
  290.   r[X] = a[X] + b[X];
  291.   r[Y] = a[Y] + b[Y];
  292. }
  293.  
  294. void
  295. add_vecs_3d (VECTOR_3D r, VECTOR_3D a, VECTOR_3D b)
  296. {
  297.   r[X] = a[X] + b[X];
  298.   r[Y] = a[Y] + b[Y];
  299.   r[Z] = a[Z] + b[Z];
  300. }
  301.  
  302. void
  303. sub_vecs_2d (VECTOR_2D r, VECTOR_2D a, VECTOR_2D b)
  304. {
  305.   r[X] = a[X] - b[X];
  306.   r[Y] = a[Y] - b[Y];
  307. }
  308.  
  309. void
  310. sub_vecs_3d (VECTOR_3D r, VECTOR_3D a, VECTOR_3D b)
  311. {
  312.   r[X] = a[X] - b[X];
  313.   r[Y] = a[Y] - b[Y];
  314.   r[Z] = a[Z] - b[Z];
  315. }
  316.  
  317. void
  318. add_vec_to_pt_2d (POINT_2D r, POINT_2D pt, VECTOR_2D v)
  319. {
  320.   r[X] = pt[X] + v[X];
  321.   r[Y] = pt[Y] + v[Y];
  322. }
  323.  
  324. void
  325. add_vec_to_pt_3d (POINT_3D r, POINT_3D pt, VECTOR_3D v)
  326. {
  327.   r[X] = pt[X] + v[X];
  328.   r[Y] = pt[Y] + v[Y];
  329.   r[Z] = pt[Z] + v[Z];
  330. }
  331.  
  332. void
  333. add_scaled_vec_to_pt_2d (POINT_2D r, POINT_2D pt, VECTOR_2D v, FLOAT s)
  334. {
  335.   r[X] = pt[X] + v[X] * s;
  336.   r[Y] = pt[Y] + v[Y] * s;
  337. }
  338.  
  339. void
  340. add_scaled_vec_to_pt_3d (POINT_3D r, POINT_3D pt, VECTOR_3D v, FLOAT s)
  341. {
  342.   r[X] = pt[X] + v[X] * s;
  343.   r[Y] = pt[Y] + v[Y] * s;
  344.   r[Z] = pt[Z] + v[Z] * s;
  345. }
  346.  
  347. void
  348. sub_pts_2d (VECTOR_2D r, POINT_2D a, POINT_2D b)
  349. {
  350.   r[X] = a[X] - b[X];
  351.   r[Y] = a[Y] - b[Y];
  352. }
  353.  
  354. void
  355. sub_pts_3d (VECTOR_3D r, POINT_3D a, POINT_3D b)
  356. {
  357.   r[X] = a[X] - b[X];
  358.   r[Y] = a[Y] - b[Y];
  359.   r[Z] = a[Z] - b[Z];
  360. }
  361.  
  362. void
  363. fold_min_pt_2d (POINT_2D min, POINT_2D new_pt)
  364. {
  365.   int i;
  366.  
  367.   for (i = 0; i < 2; i++)
  368.     if (new_pt[i] < min[i])
  369.       min[i] = new_pt[i];
  370. }
  371.  
  372. void
  373. fold_min_pt_3d (POINT_3D min, POINT_3D new_pt)
  374. {
  375.   int i;
  376.  
  377.   for (i = 0; i < 3; i++)
  378.     if (new_pt[i] < min[i])
  379.       min[i] = new_pt[i];
  380. }
  381.  
  382. void
  383. fold_max_pt_2d (POINT_2D max, POINT_3D new_pt)
  384. {
  385.   int i;
  386.  
  387.   for (i = 0; i < 2; i++)
  388.     if (new_pt[i] > max[i])
  389.       max[i] = new_pt[i];
  390. }
  391.  
  392. void
  393. fold_max_pt_3d (POINT_3D max, POINT_3D new_pt)
  394. {
  395.   int i;
  396.  
  397.   for (i = 0; i < 3; i++)
  398.     if (new_pt[i] > max[i])
  399.       max[i] = new_pt[i];
  400. }
  401.  
  402. FLOAT
  403. dot_2d (VECTOR_2D a, VECTOR_2D b)
  404. {
  405.   return a[X] * b[X] + a[Y] * b[Y];
  406. }
  407.  
  408. FLOAT
  409. dot_3d (VECTOR_3D a, VECTOR_3D b)
  410. {
  411.   return a[X] * b[X] + a[Y] * b[Y] + a[Z] * b[Z];
  412. }
  413.  
  414. void
  415. cross (VECTOR_3D r, VECTOR_3D a, VECTOR_3D b)
  416. {
  417.   r[X] = a[Y] * b[Z] - a[Z] * b[Y];
  418.   r[Y] = a[Z] * b[X] - a[X] * b[Z];
  419.   r[Z] = a[X] * b[Y] - a[Y] * b[X];
  420. }
  421.  
  422. void
  423. lerp_2d (POINT_2D r, FLOAT t, POINT_2D p1, POINT_2D p2)
  424. {
  425.   r[0] = p1[0] + t * (p2[0] - p1[0]);
  426.   r[1] = p1[1] + t * (p2[1] - p1[1]);
  427. }
  428.  
  429. void
  430. lerp_3d (POINT_3D r, FLOAT t, POINT_3D p1, POINT_3D p2)
  431. {
  432.   r[0] = p1[0] + t * (p2[0] - p1[0]);
  433.   r[1] = p1[1] + t * (p2[1] - p1[1]);
  434.   r[2] = p1[2] + t * (p2[2] - p1[2]);
  435. }
  436.  
  437. int
  438. line_intersect_2d (POINT_2D a, POINT_2D b, POINT_2D c, POINT_2D d,
  439.                    FLOAT eps, FLOAT * t_ab, FLOAT * t_cd)
  440. {
  441.   FLOAT dx_ab, dy_ab, dx_dc, dy_dc, det, dx_ac, dy_ac;
  442.  
  443.   dx_ab = b[X] - a[X];
  444.   dy_ab = b[Y] - a[Y];
  445.   dx_dc = c[X] - d[X];
  446.   dy_dc = c[Y] - d[Y];
  447.   det = dx_ab * dy_dc - dx_dc * dy_ab;
  448.   if (-eps < det && det < eps)
  449.     return 1;
  450.   dx_ac = c[X] - a[X];
  451.   dy_ac = c[Y] - a[Y];
  452.   *t_ab = (dx_ac * dy_dc - dx_dc * dy_ac) / det;
  453.   *t_cd = (dx_ab * dy_ac - dx_ac * dy_ab) / det;
  454.   return 0;
  455. }
  456.  
  457. void
  458. find_polygon_plane (PLANE * plane, POLYGON_3D * polygon)
  459. {
  460.   int i, j;
  461.   VECTOR_3D sum, dif;
  462.  
  463.   zero_vec_3d (plane->p);
  464.   zero_vec_3d (plane->n);
  465.   for (i = 0, j = polygon->n_sides - 1; i < polygon->n_sides; j = i++)
  466.     {
  467.       add_vecs_3d (plane->p, plane->p, polygon->v[i]);
  468.       add_vecs_3d (sum, polygon->v[j], polygon->v[i]);
  469.       sub_vecs_3d (dif, polygon->v[j], polygon->v[i]);
  470.       plane->n[X] += dif[Y] * sum[Z];
  471.       plane->n[Y] += dif[Z] * sum[X];
  472.       plane->n[Z] += dif[X] * sum[Y];
  473.     }
  474.   scale_vec_3d (plane->p, plane->p, 1.0 / polygon->n_sides);
  475.   find_unit_vec_3d (plane->n, plane->n);
  476.   plane->c = -dot_3d (plane->p, plane->n);
  477. }
  478.  
  479. int
  480. pt_side_of_plane (PLANE * plane, POINT_3D p)
  481. {
  482.   FLOAT d = dot_3d (p, plane->n) + plane->c;
  483.   return d < -PLANE_HALF_THICKNESS ? S_IN :
  484.     d > PLANE_HALF_THICKNESS ? S_OUT :
  485.     d < 0 ? S_IN_ON : d > 0 ? S_OUT_ON : S_ON;
  486. }
  487.  
  488. int
  489. polygon_side_of_plane (POLYGON_3D * polygon, PLANE * plane)
  490. {
  491.   int i, j, i_side, j_side, n_in, n_out;
  492.  
  493.   // initialize with last point in polygon
  494.   // scan for OUT-IN or IN-OUT pair
  495.   j = polygon->n_sides - 1;
  496.   j_side = pt_side_of_plane (plane, polygon->v[j]);
  497.   n_in = n_out = 0;
  498.   for (i = 0; i < polygon->n_sides; i++)
  499.     {
  500.  
  501.       // advance to next vertex
  502.       i_side = pt_side_of_plane (plane, polygon->v[i]);
  503.  
  504.       if ((i_side | j_side) == (S_IN | S_OUT))
  505.         // found a straddling pair
  506.         return S_SPLIT;
  507.  
  508.       if (i_side & (S_IN | S_OUT))
  509.         // found an IN or an OUT; remember it
  510.         j_side = i_side;
  511.  
  512.       // keep counts for polygons entirely inside the thick plane
  513.       if (i_side == S_OUT_ON)
  514.         n_out++;
  515.       if (i_side == S_IN_ON)
  516.         n_in++;
  517.     }
  518.   return
  519.     j_side & (S_IN | S_OUT) ? j_side :
  520.     (n_out > n_in) ? S_OUT : (n_in > n_out) ? S_IN : S_ON;
  521. }
  522.  
  523. # if TREAT_POLYLINE_POINTS_ON_PLANE_AS_IN_OR_OUT
  524.  
  525. // this will work only with BSPs, not with depth sort
  526. // it causes polylines that end  on a plane to be split into a line and a point
  527. int
  528. polyline_side_of_plane (POLYLINE_3D * polyline, PLANE * plane)
  529. {
  530.   int i, j, i_side, j_side, n_in, n_out;
  531.   //  predicate for "if more than one bit set..."
  532.   //                       0  1  2  3  4  5  6  7
  533.   static int is_split_p[] = { 0, 0, 0, 1, 0, 1, 1, 1 };
  534.  
  535.   // initialize with first point in polyline
  536.   // scan for OUT-IN or IN-OUT pair
  537.   j = 0;
  538.   i_side = pt_side_of_plane (plane, polyline->v[j]);
  539.   n_in = n_out = 0;
  540.   for (i = 1; i < polyline->n_vertices; i++)
  541.     {
  542.       // advance to next vertex, remembering side of last
  543.       j_side = i_side;
  544.       i_side = pt_side_of_plane (plane, polyline->v[i]);
  545.  
  546.       if (is_split_p[(i_side | j_side) & 7])
  547.         return S_SPLIT;
  548.  
  549.       // keep counts for polylines entirely inside the thick plane
  550.       if (i_side == S_OUT_ON)
  551.         n_out++;
  552.       if (i_side == S_IN_ON)
  553.         n_in++;
  554.     }
  555.   return
  556.     i_side & (S_IN | S_OUT) ? i_side :
  557.     (n_out > n_in) ? S_OUT : (n_in > n_out) ? S_IN : S_ON;
  558. }
  559.  
  560. #else
  561.  
  562. int
  563. polyline_side_of_plane (POLYLINE_3D * polyline, PLANE * plane)
  564. {
  565.   int i, j, i_side, j_side, n_in, n_out;
  566.  
  567.   // initialize with last point in polygon
  568.   // scan for OUT-IN or IN-OUT pair
  569.   j = polyline->n_vertices - 1;
  570.   j_side = pt_side_of_plane (plane, polyline->v[j]);
  571.   n_in = n_out = 0;
  572.   for (i = 0; i < polyline->n_vertices; i++)
  573.     {
  574.  
  575.       // advance to next vertex
  576.       i_side = pt_side_of_plane (plane, polyline->v[i]);
  577.  
  578.       if ((i_side | j_side) == (S_IN | S_OUT))
  579.         // found a straddling pair
  580.         return S_SPLIT;
  581.  
  582.       if (i_side & (S_IN | S_OUT))
  583.         // found an IN or an OUT; remember it
  584.         j_side = i_side;
  585.  
  586.       // keep counts for polylines entirely inside the thick plane
  587.       if (i_side == S_OUT_ON)
  588.         n_out++;
  589.       if (i_side == S_IN_ON)
  590.         n_in++;
  591.     }
  592.   return
  593.     j_side & (S_IN | S_OUT) ? j_side :
  594.     (n_out > n_in) ? S_OUT : (n_in > n_out) ? S_IN : S_ON;
  595. }
  596.  
  597. #endif
  598.  
  599. void
  600. init_box_2d (BOX_2D * b)
  601. {
  602.   b->min[X] = b->min[Y] = FLOAT_MAX;
  603.   b->max[X] = b->max[Y] = -FLOAT_MAX;
  604. }
  605.  
  606. void
  607. init_box_3d (BOX_3D * b)
  608. {
  609.   b->min[X] = b->min[Y] = b->min[Z] = FLOAT_MAX;
  610.   b->max[X] = b->max[Y] = b->max[Z] = -FLOAT_MAX;
  611. }
  612.  
  613. void
  614. fold_min_max_pt_2d (BOX_2D * b, POINT_2D p)
  615. {
  616.   fold_min_pt_2d (b->min, p);
  617.   fold_max_pt_2d (b->max, p);
  618. }
  619.  
  620. void
  621. fold_min_max_pt_3d (BOX_3D * b, POINT_3D p)
  622. {
  623.   fold_min_pt_3d (b->min, p);
  624.   fold_max_pt_3d (b->max, p);
  625. }
  626.  
  627. void
  628. fold_min_max_polygon_2d (BOX_2D * b, POLYGON_2D * polygon)
  629. {
  630.   int i;
  631.  
  632.   for (i = 0; i < polygon->n_sides; i++)
  633.     fold_min_max_pt_2d (b, polygon->v[i]);
  634. }
  635.  
  636. void
  637. fold_min_max_polygon_3d (BOX_3D * b, POLYGON_3D * polygon)
  638. {
  639.   int i;
  640.  
  641.   for (i = 0; i < polygon->n_sides; i++)
  642.     fold_min_max_pt_3d (b, polygon->v[i]);
  643. }
  644.  
  645. void
  646. fold_min_max_polyline_2d (BOX_2D * b, POLYLINE_2D * polyline)
  647. {
  648.   int i;
  649.  
  650.   for (i = 0; i < polyline->n_vertices; i++)
  651.     fold_min_max_pt_2d (b, polyline->v[i]);
  652. }
  653.  
  654. void
  655. fold_min_max_polyline_3d (BOX_3D * b, POLYLINE_3D * polyline)
  656. {
  657.   int i;
  658.  
  659.   for (i = 0; i < polyline->n_vertices; i++)
  660.     fold_min_max_pt_3d (b, polyline->v[i]);
  661. }
  662.  
  663. void
  664. copy_box_2d (BOX_2D * r, BOX_2D * s)
  665. {
  666.   *r = *s;
  667. }
  668.  
  669. void
  670. copy_box_3d (BOX_3D * r, BOX_3D * s)
  671. {
  672.   *r = *s;
  673. }
  674.  
  675. int
  676. boxes_2d_intersect_p (BOX_2D * a, BOX_2D * b)
  677. {
  678.   if (a->max[X] < b->min[X])    // a left of b
  679.     return 0;
  680.   if (a->min[X] > b->max[X])    // a right of b
  681.     return 0;
  682.   if (a->max[Y] < b->min[Y])    // a below  b
  683.     return 0;
  684.   if (a->min[Y] > b->max[Y])    // a above b
  685.     return 0;
  686.   return 1;
  687. }
  688.  
  689. int
  690. boxes_3d_intersect_p (BOX_2D * a, BOX_2D * b)
  691. {
  692.   if (a->max[X] < b->min[X])    // a left of b
  693.     return 0;
  694.   if (a->min[X] > b->max[X])    // a right of b
  695.     return 0;
  696.   if (a->max[Y] < b->min[Y])    // a below  b
  697.     return 0;
  698.   if (a->min[Y] > b->max[Y])    // a above b
  699.     return 0;
  700.   if (a->max[Z] < b->min[Z])    // a behind  b
  701.     return 0;
  702.   if (a->min[Z] > b->max[Z])    // a in front of b
  703.     return 0;
  704.   return 1;
  705. }
  706.  
  707. void
  708. copy_transform (TRANSFORM r, TRANSFORM s)
  709. {
  710.   int i;
  711.  
  712.   for (i = 0; i < 16; i++)
  713.     r[i] = s[i];
  714. }
  715.  
  716. #define R(I,J) r[IT(I,J)]
  717.  
  718. void
  719. set_ident (TRANSFORM r)
  720. {
  721.   R (1, 1) = 1;                 // hard code for speed
  722.   R (2, 1) = 0;
  723.   R (3, 1) = 0;
  724.   R (4, 1) = 0;
  725.  
  726.   R (1, 2) = 0;
  727.   R (2, 2) = 1;
  728.   R (3, 2) = 0;
  729.   R (4, 2) = 0;
  730.  
  731.   R (1, 3) = 0;
  732.   R (2, 3) = 0;
  733.   R (3, 3) = 1;
  734.   R (4, 3) = 0;
  735.  
  736.   R (1, 4) = 0;
  737.   R (2, 4) = 0;
  738.   R (3, 4) = 0;
  739.   R (4, 4) = 1;
  740. }
  741.  
  742. void
  743. set_scale (TRANSFORM r, FLOAT sx, FLOAT sy, FLOAT sz)
  744. {
  745.   set_ident (r);
  746.   R (1, 1) = sx;
  747.   R (2, 2) = sy;
  748.   R (3, 3) = sz;
  749. }
  750.  
  751. void
  752. set_translation (TRANSFORM r, FLOAT dx, FLOAT dy, FLOAT dz)
  753. {
  754.   set_ident (r);
  755.   R (1, 4) = dx;
  756.   R (2, 4) = dy;
  757.   R (3, 4) = dz;
  758. }
  759.  
  760. #define SQR(A) ((A) * (A))
  761.  
  762. void
  763. set_angle_axis_rot (TRANSFORM r, FLOAT theta, VECTOR_3D u)
  764. {
  765.   FLOAT c = cos (theta);
  766.   FLOAT s = sin (theta);
  767.   FLOAT d = 1 - c;
  768.  
  769.   R (1, 1) = d * (SQR (u[X]) - 1) + 1;
  770.   R (1, 2) = d * u[X] * u[Y] - u[Z] * s;
  771.   R (1, 3) = d * u[X] * u[Z] + u[Y] * s;
  772.  
  773.   R (2, 1) = d * u[X] * u[Y] + u[Z] * s;
  774.   R (2, 2) = d * (SQR (u[Y]) - 1) + 1;
  775.   R (2, 3) = d * u[Y] * u[Z] - u[X] * s;
  776.  
  777.   R (3, 1) = d * u[X] * u[Z] - u[Y] * s;
  778.   R (3, 2) = d * u[Y] * u[Z] + u[X] * s;
  779.   R (3, 3) = d * (SQR (u[Z]) - 1) + 1;
  780.  
  781.   R (1, 4) = R (4, 1) = R (2, 4) = R (4, 2) = R (3, 4) = R (4, 3) = 0;
  782.   R (4, 4) = 1;
  783. }
  784.  
  785. void
  786. set_angle_axis_rot_about_point (TRANSFORM r, FLOAT theta, POINT_3D p,
  787.                                 VECTOR_3D u)
  788. {
  789.   VECTOR_3D u_unit;
  790.   TRANSFORM tmp;
  791.  
  792.   if (u)
  793.     {
  794.       find_unit_vec_3d (u_unit, u);
  795.     }
  796.   else
  797.     {
  798.       u_unit[X] = u_unit[Y] = 0;
  799.       u_unit[Z] = 1;
  800.     }
  801.   set_angle_axis_rot (r, theta, u_unit);
  802.   if (p)
  803.     {
  804.       set_translation (tmp, -p[X], -p[Y], -p[Z]);
  805.       compose (r, r, tmp);
  806.       set_translation (tmp, p[X], p[Y], p[Z]);
  807.       compose (r, tmp, r);
  808.     }
  809. }
  810.  
  811. void
  812. set_perspective_projection (TRANSFORM r, FLOAT p)
  813. {
  814.   set_scale (r, p, p, p);
  815.   R (4, 4) = 0;
  816.   R (4, 3) = -1;
  817. }
  818.  
  819. void
  820. set_perspective_transform (TRANSFORM r, FLOAT p)
  821. {
  822.   set_scale (r, p, p, 1);
  823.   R (3, 4) = 1;
  824.   R (4, 3) = -1;
  825.   R (4, 4) = 0;
  826. }
  827.  
  828. void
  829. set_parallel_projection (TRANSFORM r)
  830. {
  831.   set_scale (r, 1, 1, 0);
  832. }
  833.  
  834. void
  835. set_view_transform (TRANSFORM r, POINT_3D eye, VECTOR_3D vd, VECTOR_3D up)
  836. {
  837.   static VECTOR_3D default_up = { 0, 1, 0 };
  838.  
  839.   VECTOR_3D unit_vd, unit_up, h, v;
  840.   TRANSFORM tmp;
  841.  
  842.   if (vd)
  843.     {
  844.       find_unit_vec_3d (unit_vd, vd);
  845.     }
  846.   else
  847.     {
  848.       negate_vec_3d (unit_vd, eye);     // assumes point and vector are compatible
  849.       find_unit_vec_3d (unit_vd, unit_vd);
  850.     }
  851.  
  852.   if (up)
  853.     find_unit_vec_3d (unit_up, up);
  854.   else
  855.     copy_vec_3d (unit_up, default_up);
  856.  
  857.   cross (h, unit_vd, unit_up);
  858.   cross (v, h, unit_vd);
  859.  
  860.   R (1, 1) = h[X];
  861.   R (1, 2) = h[Y];
  862.   R (1, 3) = h[Z];
  863.   R (1, 4) = 0;
  864.   R (2, 1) = v[X];
  865.   R (2, 2) = v[Y];
  866.   R (2, 3) = v[Z];
  867.   R (2, 4) = 0;
  868.   R (3, 1) = -unit_vd[X];
  869.   R (3, 2) = -unit_vd[Y];
  870.   R (3, 3) = -unit_vd[Z];
  871.   R (3, 4) = 0;
  872.   R (4, 1) = 0;
  873.   R (4, 2) = 0;
  874.   R (4, 3) = 0;
  875.   R (4, 4) = 1;
  876.  
  877.   if (eye)
  878.     {
  879.       set_translation (tmp, -eye[X], -eye[Y], -eye[Z]);
  880.       compose (r, r, tmp);
  881.     }
  882. }
  883.  
  884. void
  885. set_view_transform_with_look_at (TRANSFORM r, POINT_3D eye,
  886.                                  POINT_3D look_at, VECTOR_3D up)
  887. {
  888.   VECTOR_3D vd;
  889.   sub_vecs_3d (vd, look_at, eye);
  890.   set_view_transform (r, eye, vd, up);
  891. }
  892.  
  893. #define M(I,J) m[IT(I,J)]
  894.  
  895. // invert a transform using the method of cofactors
  896. // this code was generated by the Perl program geninv.pl
  897. void
  898. invert (TRANSFORM r, FLOAT * det_rtn, TRANSFORM m, FLOAT min_det)
  899. {
  900.   int i;
  901.   FLOAT det;
  902.   FLOAT t001, t002, t003, t004, t005, t006, t007, t008,
  903.     t009, t010, t011, t012, t013, t014, t015, t016,
  904.     t017, t018, t019, t020, t021, t022, t023, t024,
  905.     t025, t026, t027, t028, t029, t030, t031, t032,
  906.     t033, t034, t035, t036, t037, t038, t039, t040,
  907.     t057, t058, t061, t062, t065, t066, t072, t073,
  908.     t076, t077, t085, t086, t097, t098, t101, t102,
  909.     t105, t106, t112, t113, t116, t117, t125, t126;
  910.   t001 = M (3, 3) * M (4, 4);
  911.   t002 = M (3, 4) * M (4, 3);
  912.   t003 = t001 - t002;
  913.   t004 = M (2, 2) * t003;
  914.   t005 = M (3, 2) * M (4, 4);
  915.   t006 = M (3, 4) * M (4, 2);
  916.   t007 = t006 - t005;
  917.   t008 = M (2, 3) * t007;
  918.   t009 = M (3, 2) * M (4, 3);
  919.   t010 = M (3, 3) * M (4, 2);
  920.   t011 = t009 - t010;
  921.   t012 = M (2, 4) * t011;
  922.   t013 = t004 + t008 + t012;
  923.   R (1, 1) = t013;
  924.   t014 = t002 - t001;
  925.   t015 = M (2, 1) * t014;
  926.   t016 = M (3, 1) * M (4, 4);
  927.   t017 = M (3, 4) * M (4, 1);
  928.   t018 = t016 - t017;
  929.   t019 = M (2, 3) * t018;
  930.   t020 = M (3, 1) * M (4, 3);
  931.   t021 = M (3, 3) * M (4, 1);
  932.   t022 = t021 - t020;
  933.   t023 = M (2, 4) * t022;
  934.   t024 = t015 + t019 + t023;
  935.   R (2, 1) = t024;
  936.   t025 = t005 - t006;
  937.   t026 = M (2, 1) * t025;
  938.   t027 = t017 - t016;
  939.   t028 = M (2, 2) * t027;
  940.   t029 = M (3, 1) * M (4, 2);
  941.   t030 = M (3, 2) * M (4, 1);
  942.   t031 = t029 - t030;
  943.   t032 = M (2, 4) * t031;
  944.   t033 = t026 + t028 + t032;
  945.   R (3, 1) = t033;
  946.   t034 = t010 - t009;
  947.   t035 = M (2, 1) * t034;
  948.   t036 = t020 - t021;
  949.   t037 = M (2, 2) * t036;
  950.   t038 = t030 - t029;
  951.   t039 = M (2, 3) * t038;
  952.   t040 = t035 + t037 + t039;
  953.   R (4, 1) = t040;
  954.   det =
  955.     (M (1, 1) * t013) + (M (1, 2) * t024) + (M (1, 3) * t033) +
  956.     (M (1, 4) * t040);
  957.   R (1, 2) = (M (1, 2) * t014) + (M (1, 3) * t025) + (M (1, 4) * t034);
  958.   R (2, 2) = (M (1, 1) * t003) + (M (1, 3) * t027) + (M (1, 4) * t036);
  959.   R (3, 2) = (M (1, 1) * t007) + (M (1, 2) * t018) + (M (1, 4) * t038);
  960.   R (4, 2) = (M (1, 1) * t011) + (M (1, 2) * t022) + (M (1, 3) * t031);
  961.   t057 = M (2, 3) * M (4, 4);
  962.   t058 = M (2, 4) * M (4, 3);
  963.   t061 = M (2, 2) * M (4, 4);
  964.   t062 = M (2, 4) * M (4, 2);
  965.   t065 = M (2, 2) * M (4, 3);
  966.   t066 = M (2, 3) * M (4, 2);
  967.   R (1, 3) =
  968.     ((t057 - t058) * M (1, 2)) + ((t062 - t061) * M (1,
  969.                                                      3)) + ((t065 -
  970.                                                              t066) * M (1,
  971.                                                                         4));
  972.   t072 = M (2, 1) * M (4, 4);
  973.   t073 = M (2, 4) * M (4, 1);
  974.   t076 = M (2, 1) * M (4, 3);
  975.   t077 = M (2, 3) * M (4, 1);
  976.   R (2, 3) =
  977.     ((t058 - t057) * M (1, 1)) + ((t072 - t073) * M (1,
  978.                                                      3)) + ((t077 -
  979.                                                              t076) * M (1,
  980.                                                                         4));
  981.   t085 = M (2, 1) * M (4, 2);
  982.   t086 = M (2, 2) * M (4, 1);
  983.   R (3, 3) =
  984.     ((t061 - t062) * M (1, 1)) + ((t073 - t072) * M (1,
  985.                                                      2)) + ((t085 -
  986.                                                              t086) * M (1,
  987.                                                                         4));
  988.   R (4, 3) =
  989.     ((t066 - t065) * M (1, 1)) + ((t076 - t077) * M (1,
  990.                                                      2)) + ((t086 -
  991.                                                              t085) * M (1,
  992.                                                                         3));
  993.   t097 = M (2, 3) * M (3, 4);
  994.   t098 = M (2, 4) * M (3, 3);
  995.   t101 = M (2, 2) * M (3, 4);
  996.   t102 = M (2, 4) * M (3, 2);
  997.   t105 = M (2, 2) * M (3, 3);
  998.   t106 = M (2, 3) * M (3, 2);
  999.   R (1, 4) =
  1000.     ((t098 - t097) * M (1, 2)) + ((t101 - t102) * M (1,
  1001.                                                      3)) + ((t106 -
  1002.                                                              t105) * M (1,
  1003.                                                                         4));
  1004.   t112 = M (2, 1) * M (3, 4);
  1005.   t113 = M (2, 4) * M (3, 1);
  1006.   t116 = M (2, 1) * M (3, 3);
  1007.   t117 = M (2, 3) * M (3, 1);
  1008.   R (2, 4) =
  1009.     ((t097 - t098) * M (1, 1)) + ((t113 - t112) * M (1,
  1010.                                                      3)) + ((t116 -
  1011.                                                              t117) * M (1,
  1012.                                                                         4));
  1013.   t125 = M (2, 1) * M (3, 2);
  1014.   t126 = M (2, 2) * M (3, 1);
  1015.   R (3, 4) =
  1016.     ((t102 - t101) * M (1, 1)) + ((t112 - t113) * M (1,
  1017.                                                      2)) + ((t126 -
  1018.                                                              t125) * M (1,
  1019.                                                                         4));
  1020.   R (4, 4) =
  1021.     ((t105 - t106) * M (1, 1)) + ((t117 - t116) * M (1,
  1022.                                                      2)) + ((t125 -
  1023.                                                              t126) * M (1,
  1024.                                                                         3));
  1025.   if (-min_det <= det && det <= min_det)
  1026.     {
  1027.       *det_rtn = 0;
  1028.     }
  1029.   else
  1030.     {
  1031.       *det_rtn = det;
  1032.       for (i = 0; i < 16; i++)
  1033.         r[i] *= 1 / det;
  1034.     }
  1035. }
  1036.  
  1037. #define A(I,J) a[IT(I,J)]
  1038. #define B(I,J) b[IT(I,J)]
  1039. void
  1040. compose_unsafe (TRANSFORM r, TRANSFORM a, TRANSFORM b)
  1041. {
  1042.   int i, j;
  1043.   FLOAT *p = r;
  1044.  
  1045.   for (j = 1; j <= 4; j++)
  1046.     for (i = 1; i <= 4; i++)
  1047.       *p++ =
  1048.         A (i, 1) * B (1, j) + A (i, 2) * B (2, j) + A (i, 3) * B (3,
  1049.                                                                   j) +
  1050.         A (i, 4) * B (4, j);
  1051. }
  1052.  
  1053. void
  1054. compose (TRANSFORM r, TRANSFORM a, TRANSFORM b)
  1055. {
  1056.   TRANSFORM t;
  1057.   compose_unsafe (t, a, b);
  1058.   copy_transform (r, t);
  1059. }
  1060.  
  1061. void
  1062. transform_pt_3d (POINT_3D r, TRANSFORM m, POINT_3D p)
  1063. {
  1064.   POINT_3D t;
  1065.   FLOAT wi;
  1066.  
  1067.   wi = 1 / (M (4, 1) * p[X] + M (4, 2) * p[Y] + M (4, 3) * p[Z] + M (4, 4));
  1068.   t[X] =
  1069.     (M (1, 1) * p[X] + M (1, 2) * p[Y] + M (1, 3) * p[Z] + M (1, 4)) * wi;
  1070.   t[Y] =
  1071.     (M (2, 1) * p[X] + M (2, 2) * p[Y] + M (2, 3) * p[Z] + M (2, 4)) * wi;
  1072.   t[Z] =
  1073.     (M (3, 1) * p[X] + M (3, 2) * p[Y] + M (3, 3) * p[Z] + M (3, 4)) * wi;
  1074.   copy_pt_3d (r, t);
  1075. }
  1076.  
  1077. void
  1078. transform_vec_3d (VECTOR_3D r, TRANSFORM m, VECTOR_3D v)
  1079. {
  1080.   VECTOR_3D t;
  1081.  
  1082.   t[X] = M (1, 1) * v[X] + M (1, 2) * v[Y] + M (1, 3) * v[Z];
  1083.   t[Y] = M (2, 1) * v[X] + M (2, 2) * v[Y] + M (2, 3) * v[Z];
  1084.   t[Z] = M (3, 1) * v[X] + M (3, 2) * v[Y] + M (3, 3) * v[Z];
  1085.   copy_vec_3d (r, t);
  1086. }
  1087.  
  1088. void
  1089. set_ident_quat (QUATERNION q)
  1090. {
  1091.   q[W] = 1;
  1092.   q[X] = q[Y] = q[Z] = 0;
  1093. }
  1094.  
  1095. void
  1096. set_angle_axis_quat (QUATERNION q, FLOAT theta, VECTOR_3D axis)
  1097. {
  1098.   VECTOR_3D v;
  1099.  
  1100.   find_unit_vec_3d (v, axis);
  1101.   scale_vec_3d (&q[X], v, sin (theta));
  1102.   q[W] = cos (theta);
  1103. }
  1104.  
  1105. void
  1106. mult_quat (QUATERNION r, QUATERNION a, QUATERNION b)
  1107. {
  1108.   r[W] = a[W] * b[W] - a[X] * b[X] - a[Y] * b[Y] - a[Z] * b[Z];
  1109.   r[X] = a[W] * b[X] + a[X] * b[W] + a[Y] * b[Z] - a[Z] * b[Y];
  1110.   r[Y] = a[W] * b[Y] - a[X] * b[Z] + a[Y] * b[W] + a[Z] * b[X];
  1111.   r[Z] = a[W] * b[Z] + a[X] * b[Y] - a[Y] * b[X] + a[Z] * b[W];
  1112. }
  1113.  
  1114. #define R(I,J) r[IT(I,J)]
  1115. #define SQR(A) ((A) * (A))
  1116.  
  1117. void
  1118. find_rot_from_quat (TRANSFORM r, QUATERNION q)
  1119. {
  1120.   FLOAT len2 = SQR (q[W]) + SQR (q[X]) + SQR (q[Y]) + SQR (q[Z]);
  1121.   FLOAT s = len2 > 0 ? 2 / len2 : 0;
  1122.  
  1123.   R (1, 1) = 1 - s * (SQR (q[Y]) + SQR (q[Z]));
  1124.   R (1, 2) = s * (q[X] * q[Y] - q[W] * q[Z]);
  1125.   R (1, 3) = s * (q[X] * q[Z] + q[W] * q[Y]);
  1126.  
  1127.   R (2, 1) = s * (q[X] * q[Y] + q[W] * q[Z]);
  1128.   R (2, 2) = 1 - s * (SQR (q[X]) + SQR (q[Z]));
  1129.   R (2, 3) = s * (q[Y] * q[Z] - q[W] * q[X]);
  1130.  
  1131.   R (3, 1) = s * (q[X] * q[Z] - q[W] * q[Y]);
  1132.   R (3, 2) = s * (q[Y] * q[Z] + q[W] * q[X]);
  1133.   R (3, 3) = 1 - s * (SQR (q[X]) + SQR (q[Y]));
  1134.  
  1135.   R (1, 4) = R (4, 1) = R (2, 4) = R (4, 2) = R (3, 4) = R (4, 3) = 0;
  1136.   R (4, 4) = 1;
  1137. }
  1138.  
  1139. void
  1140. find_quat_from_rot (QUATERNION q, TRANSFORM r)
  1141. {
  1142.   if (R (1, 1) + R (2, 2) + R (3, 3) >= 0)
  1143.     {                           // w first
  1144.       FLOAT w2 = sqrt (R (1, 1) + R (2, 2) + R (3, 3) + 1);
  1145.       q[W] = 0.5 * w2;          // 1st
  1146.       q[X] = (0.5 / w2) * (R (3, 2) - R (2, 3));        // (f)
  1147.       q[Y] = (0.5 / w2) * (R (1, 3) - R (3, 1));        // (d)
  1148.       q[Z] = (0.5 / w2) * (R (2, 1) - R (1, 2));        // (b)
  1149.       return;
  1150.     }
  1151.   // x, y, or z first
  1152.   if (R (1, 1) > R (2, 2))
  1153.     if (R (1, 1) > R (3, 3))
  1154.       goto x_first;
  1155.     else
  1156.       goto z_first;
  1157.   else                          // R(2,2) >= R(1,1)
  1158.   if (R (2, 2) > R (3, 3))
  1159.     goto y_first;
  1160.   else
  1161.     goto z_first;
  1162.  
  1163. x_first:{
  1164.     FLOAT x2 = sqrt (R (1, 1) - R (2, 2) - R (3, 3) + 1);
  1165.     q[W] = (0.5 / x2) * (R (3, 2) - R (2, 3));  // (f)
  1166.     q[X] = 0.5 * x2;            // 1st
  1167.     q[Y] = (0.5 / x2) * (R (2, 1) + R (1, 2));  // (a)
  1168.     q[Z] = (0.5 / x2) * (R (1, 3) + R (3, 1));  // (c)
  1169.     return;
  1170.   }
  1171.  
  1172. y_first:{
  1173.     FLOAT y2 = sqrt (-R (1, 1) + R (2, 2) - R (3, 3) + 1);
  1174.     q[W] = (0.5 / y2) * (R (1, 3) - R (3, 1));  // (d)
  1175.     q[X] = (0.5 / y2) * (R (2, 1) + R (1, 2));  // (a)
  1176.     q[Y] = 0.5 * y2;            // 1st
  1177.     q[Z] = (0.5 / y2) * (R (3, 2) + R (2, 3));  // (e)
  1178.     return;
  1179.   }
  1180.  
  1181. z_first:{
  1182.     FLOAT z2 = sqrt (-R (1, 1) - R (2, 2) + R (3, 3) + 1);
  1183.     q[W] = (0.5 / z2) * (R (2, 1) - R (1, 2));  // (b)
  1184.     q[X] = (0.5 / z2) * (R (1, 3) + R (3, 1));  // (c)
  1185.     q[Y] = (0.5 / z2) * (R (3, 2) + R (2, 3));  // (e)
  1186.     q[Z] = 0.5 * z2;            // 1st
  1187.     return;
  1188.   }
  1189. }
  1190.  
  1191. #undef R
  1192.  
  1193. void
  1194. make_cso_polygon_2d (POLYGON_2D * r, POLYGON_2D * a, POINT_2D p,
  1195.                      POLYGON_2D * b)
  1196. {
  1197.   int j, ia, ja, ib, jb, ir, nb;
  1198.   FLOAT x, y, dx_a, dy_a, dx_b, dy_b;
  1199.  
  1200.   setup_polygon_2d (r, a->n_sides + b->n_sides);
  1201.   r->n_sides = a->n_sides + b->n_sides;
  1202.  
  1203.   ja = 0;
  1204.   x = a->v[ja][X];
  1205.   for (j = 1; j < a->n_sides; j++)
  1206.     if (a->v[j][X] < x)
  1207.       {
  1208.         x = a->v[j][X];
  1209.         ja = j;
  1210.       }
  1211.  
  1212.   jb = 0;
  1213.   x = b->v[0][X];
  1214.   for (j = 1; j < b->n_sides; j++)
  1215.     if (b->v[j][X] > x)
  1216.       {
  1217.         x = b->v[j][X];
  1218.         jb = j;
  1219.       }
  1220.   // this point is certain to be an extreme point of the cso
  1221.   x = b->v[jb][X] + (p[X] - a->v[ja][X]);
  1222.   y = b->v[jb][Y] + (p[Y] - a->v[ja][Y]);
  1223.  
  1224.   ia = (ja + 1) % a->n_sides;
  1225.   dx_a = a->v[ja][X] - a->v[ia][X];
  1226.   dy_a = a->v[ja][Y] - a->v[ia][Y];
  1227.   ib = (jb + 1) % b->n_sides;
  1228.   dx_b = b->v[ib][X] - b->v[jb][X];
  1229.   dy_b = b->v[ib][Y] - b->v[jb][Y];
  1230.   nb = b->n_sides;
  1231.   ir = 0;
  1232.   for (;;)
  1233.     {
  1234.  
  1235.       // record obstacle polygon point and quit if done
  1236.       r->v[ir][X] = x;
  1237.       r->v[ir][Y] = y;
  1238.       if (++ir == r->n_sides)
  1239.         break;
  1240.  
  1241.       // merge next edge of lowest theta. */
  1242.       if (nb == 0 || dx_a * dy_b - dy_a * dx_b > 0)
  1243.         {
  1244.           x += dx_a;
  1245.           y += dy_a;
  1246.           ja = ia;
  1247.           ia = (ja + 1) % a->n_sides;
  1248.           dx_a = a->v[ja][X] - a->v[ia][X];
  1249.           dy_a = a->v[ja][Y] - a->v[ia][Y];
  1250.         }
  1251.       else
  1252.         {
  1253.           x += dx_b;
  1254.           y += dy_b;
  1255.           jb = ib;
  1256.           ib = (jb + 1) % b->n_sides;
  1257.           dx_b = b->v[ib][X] - b->v[jb][X];
  1258.           dy_b = b->v[ib][Y] - b->v[jb][Y];
  1259.           nb--;
  1260.         }
  1261.     }
  1262. }
  1263.  
  1264. int
  1265. point_near_convex_polygon_2d_p (POINT_2D p, POLYGON_2D * a, FLOAT eps)
  1266. {
  1267.   int i, j;
  1268.   VECTOR_2D vji_perp, vjp;
  1269.  
  1270.   // if the point is more than eps right of any edge, we're outside
  1271.   for (i = 0, j = a->n_sides - 1; i < a->n_sides; j = i++)
  1272.     {
  1273.       vji_perp[X] = a->v[j][Y] - a->v[i][Y];
  1274.       vji_perp[Y] = a->v[i][X] - a->v[j][X];
  1275.       find_unit_vec_2d (vji_perp, vji_perp);
  1276.       sub_pts_2d (vjp, p, a->v[j]);
  1277.       if (dot_2d (vjp, vji_perp) <= eps)
  1278.         return 0;
  1279.     }
  1280.   // else we're inside!
  1281.   return 1;
  1282. }
  1283.  
  1284. int
  1285. point_inside_convex_polygon_2d_p (POINT_2D p, POLYGON_2D * a)
  1286. {
  1287.   int i, j;
  1288.  
  1289.   // if the point is right of any edge, we're outside
  1290.   for (i = 0, j = a->n_sides - 1; i < a->n_sides; j = i++)
  1291.     if ((p[X] - a->v[j][X]) * (a->v[i][Y] - a->v[j][Y]) -
  1292.         (p[Y] - a->v[j][Y]) * (a->v[i][X] - a->v[j][X]) >= 0)
  1293.       return 0;
  1294.  
  1295.   // else we're inside!
  1296.   return 1;
  1297. }
  1298.  
  1299. // The Franklin code...
  1300. int
  1301. point_inside_polygon_2d_p (POINT_2D p, POLYGON_2D * a)
  1302. {
  1303.   int i, j, r = 0;
  1304.   for (i = 0, j = a->n_sides - 1; i < a->n_sides; j = i++)
  1305.     {
  1306.       if (((a->v[i][Y] <= p[Y] && p[Y] < a->v[j][Y]) ||
  1307.            (a->v[j][Y] <= p[Y] && p[Y] < a->v[i][Y])) &&
  1308.           (p[X] < (a->v[j][X] - a->v[i][X]) * (p[Y] - a->v[i][Y]) /
  1309.            (a->v[j][Y] - a->v[i][Y]) + a->v[i][X]))
  1310.         r ^= 1;
  1311.     }
  1312.   return r;
  1313. }
  1314.  
  1315. #ifdef TEST_INVERT
  1316.  
  1317. void
  1318. print_transform (TRANSFORM m)
  1319. {
  1320.   int i, j;
  1321.   printf ("[\n");
  1322.   for (i = 1; i <= 4; i++)
  1323.     {
  1324.       printf ("[");
  1325.       for (j = 1; j <= 4; j++)
  1326.         {
  1327.           printf (" %8.3g", m[IT (i, j)]);
  1328.         }
  1329.       printf ("]\n");
  1330.     }
  1331.   printf ("]\n");
  1332. }
  1333.  
  1334. int
  1335. main (void)
  1336. {
  1337.   TRANSFORM m = { 1, 0, 1, 1, 2, 4, 0, 19, 3, 5, 6, 57, 14, -3, 34, 1 }, r;
  1338.   FLOAT det;
  1339.   VECTOR_3D axis = { 1, 2, 3 };
  1340.   POINT_3D pt = { -10, 2, 41 };
  1341.  
  1342.   // set_angle_axis_rot_about_point(m, 30, pt, axis);
  1343.   print_transform (m);
  1344.   invert (r, &det, m, 1e-4);
  1345.   printf ("det=%.3g\n", det);
  1346.   print_transform (r);
  1347.   invert (m, &det, r, 1e-4);
  1348.   printf ("det=%.3g\n", det);
  1349.   print_transform (m);
  1350. }
  1351.  
  1352. #endif
  1353.  
  1354. #ifdef TEST_DYNARRAY_H
  1355.  
  1356. // we need a dynamic arrao of these things
  1357. typedef struct foo_t
  1358. {
  1359.   char *name;
  1360.   int count;
  1361. }
  1362. FOO;
  1363.  
  1364. typedef struct foo_array_t
  1365. {
  1366.   DYNAMIC_ARRAY_FIELDS (FOO, val, n_vals);
  1367. }
  1368. FOO_ARRAY;
  1369.  
  1370. // do the prototypes for the constructor, destructor, and accessor functions
  1371. DECLARE_DYNAMIC_ARRAY_PROTOS (FOO_ARRAY, FOO, foo_list, val, n_vals)
  1372. // ---- in foo.c ----
  1373. // create the bodies for the constructor, destructor, and accessor functions
  1374.   DECLARE_DYNAMIC_ARRAY_FUNCS (FOO_ARRAY, FOO, foo_list, val, n_vals)
  1375. // use all the new stuff!
  1376.      void do_stuff_with_foos (void)
  1377. {
  1378.   int i;
  1379.   char buf[100];
  1380.   FOO_ARRAY list[1];            // or FOO_ARRAY list; but then we're forever &'ing
  1381.   FOO_ARRAY copy[1];
  1382.  
  1383.   init_foo_list (list);         // do this JUST ONCE right after declaration
  1384.   init_foo_list (copy);         // (not necessary for static/global decls)
  1385.  
  1386.   setup_foo_list (list, 10);    // allow for 10 elements
  1387.  
  1388.   // read some data and push it on the list tail
  1389.   while (scanf ("%d %s", &i, buf) == 2)
  1390.     {
  1391.       // get pointer to new (empty) element at the end of array
  1392.       FOO *p = pushed_foo_list_val (list);
  1393.       // fill in field values
  1394.       p->name = strdup (buf);
  1395.       p->count = i;
  1396.     }
  1397.  
  1398.   // shows unsafe access to elements
  1399.   printf ("forward listing:\n");
  1400.   for (i = 0; i < list->n_vals; i++)
  1401.     printf ("name=%s count=%d (%d)\n", list->val[i].name,       // fast unsafe access
  1402.             foo_list_val_ptr (list, i)->count,  // slower safe pointer access
  1403.             foo_list_val (list, i).count);      // copying access
  1404.  
  1405.   copy_foo_list_filled (copy, list);    // copies only filled elements
  1406.  
  1407.   // print in reverse order by popping from tail
  1408.   printf ("backward listing:\n");
  1409.   while (copy->n_vals > 0)
  1410.     {
  1411.       FOO *p = popped_foo_list_val (copy);
  1412.       printf ("name=%s count=%d\n", p->name, p->count);
  1413.     }
  1414.  
  1415.   // clear out all the allocated storage for the ilst
  1416.   clear_foo_list (list);
  1417.   clear_foo_list (copy);
  1418. }
  1419.  
  1420. #endif

Raw Paste


Login or Register to edit or fork this paste. It's free.