BRL-CAD
plane.c
Go to the documentation of this file.
1 /* P L A N E . C
2  * BRL-CAD
3  *
4  * Copyright (c) 2004-2014 United States Government as represented by
5  * the U.S. Army Research Laboratory.
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
9  * version 2.1 as published by the Free Software Foundation.
10  *
11  * This library is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this file; see the file named COPYING for more
18  * information.
19  */
20 /** @addtogroup plane */
21 /** @{ */
22 /** @file libbn/plane.c
23  *
24  * @brief
25  * Some useful routines for dealing with planes and lines.
26  *
27  */
28 
29 #include "common.h"
30 
31 #include <stdlib.h>
32 #include <string.h>
33 #include <stdio.h>
34 #include <math.h>
35 
36 #include "bu/debug.h"
37 #include "bu/log.h"
38 #include "vmath.h"
39 #include "bn/mat.h"
40 #include "bn/plane_struct.h"
41 #include "bn/plane_calc.h"
42 #include "bn/tol.h"
43 
44 #define UNIT_SQ_TOL 1.0e-13
45 
46 
47 double
48 bn_dist_pt3_pt3(const fastf_t *a, const fastf_t *b)
49 {
50  vect_t diff;
51 
52  VSUB2(diff, a, b);
53  return MAGNITUDE(diff);
54 }
55 
56 
57 int
58 bn_pt3_pt3_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
59 {
60  register fastf_t tmp = tol->dist_sq;
61  register fastf_t ab, abx, aby, abz;
62 
63  abx = a[X]-b[X];
64  ab = abx * abx;
65  if (ab > tmp) {
66  return 0;
67  }
68  aby = a[Y]-b[Y];
69  ab += (aby * aby);
70  if (ab > tmp) {
71  return 0;
72  }
73  abz = a[Z]-b[Z];
74  ab += (abz * abz);
75  if (ab > tmp) {
76  return 0;
77  }
78 
79  return 1;
80 }
81 
82 
83 /**
84  * @return 1 if the two points are equal, within the tolerance
85  * @return 0 if the two points are not "the same"
86  */
87 int
88 bn_pt2_pt2_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
89 {
90  vect_t diff;
91 
92  BN_CK_TOL(tol);
93  V2SUB2(diff, b, a);
94  if (MAG2SQ(diff) < tol->dist_sq) return 1;
95  return 0;
96 }
97 
98 
99 int
100 bn_3pts_collinear(fastf_t *a, fastf_t *b, fastf_t *c, const struct bn_tol *tol)
101 {
102  fastf_t mag_ab, mag_bc, mag_ca, max_len, dist_sq;
103  fastf_t cos_a, cos_b, cos_c;
104  vect_t ab, bc, ca;
105  int max_edge_no;
106 
107  VSUB2(ab, b, a);
108  VSUB2(bc, c, b);
109  VSUB2(ca, a, c);
110  mag_ab = MAGNITUDE(ab);
111  mag_bc = MAGNITUDE(bc);
112  mag_ca = MAGNITUDE(ca);
113 
114  /* If two or more points are the same, by definition we're collinear */
115  if (NEAR_ZERO(mag_ab,tol->dist_sq) || NEAR_ZERO(mag_bc,tol->dist_sq) || NEAR_ZERO(mag_ca,tol->dist_sq))
116  return 1;
117 
118  /* find longest edge */
119  max_len = mag_ab;
120  max_edge_no = 1;
121 
122  if (mag_bc > max_len) {
123  max_len = mag_bc;
124  max_edge_no = 2;
125  }
126 
127  if (mag_ca > max_len) {
128  max_edge_no = 3;
129  }
130 
131  switch (max_edge_no) {
132  default:
133  case 1:
134  cos_b = (-VDOT(ab, bc))/(mag_ab * mag_bc);
135  dist_sq = mag_bc*mag_bc*(1.0 - cos_b*cos_b);
136  break;
137  case 2:
138  cos_c = (-VDOT(bc, ca))/(mag_bc * mag_ca);
139  dist_sq = mag_ca*mag_ca*(1.0 - cos_c*cos_c);
140  break;
141  case 3:
142  cos_a = (-VDOT(ca, ab))/(mag_ca * mag_ab);
143  dist_sq = mag_ab*mag_ab*(1.0 - cos_a*cos_a);
144  break;
145  }
146 
147  if (dist_sq <= tol->dist_sq)
148  return 1;
149  else
150  return 0;
151 }
152 
153 
154 int
155 bn_3pts_distinct(const fastf_t *a, const fastf_t *b, const fastf_t *c, const struct bn_tol *tol)
156 {
157  vect_t B_A;
158  vect_t C_A;
159  vect_t C_B;
160 
161  BN_CK_TOL(tol);
162  VSUB2(B_A, b, a);
163  if (MAGSQ(B_A) <= tol->dist_sq) return 0;
164  VSUB2(C_A, c, a);
165  if (MAGSQ(C_A) <= tol->dist_sq) return 0;
166  VSUB2(C_B, c, b);
167  if (MAGSQ(C_B) <= tol->dist_sq) return 0;
168  return 1;
169 }
170 
171 
172 int
173 bn_npts_distinct(const int npt, const point_t *pts, const struct bn_tol *tol)
174 {
175  int i, j;
176  point_t r;
177 
178  BN_CK_TOL(tol);
179 
180  for (i=0;i<npt;i++)
181  for (j=i+1;j<npt;j++) {
182  VSUB2(r, pts[i], pts[j]);
183  if (MAGSQ(r) <= tol->dist_sq)
184  return 0;
185  }
186  return 1;
187 }
188 
189 
190 int
192  const fastf_t *a,
193  const fastf_t *b,
194  const fastf_t *c,
195  const struct bn_tol *tol)
196 {
197  vect_t B_A;
198  vect_t C_A;
199  vect_t C_B;
200  register fastf_t mag;
201 
202  BN_CK_TOL(tol);
203 
204  VSUB2(B_A, b, a);
205  if (MAGSQ(B_A) <= tol->dist_sq) return -1;
206  VSUB2(C_A, c, a);
207  if (MAGSQ(C_A) <= tol->dist_sq) return -1;
208  VSUB2(C_B, c, b);
209  if (MAGSQ(C_B) <= tol->dist_sq) return -1;
210 
211  VCROSS(plane, B_A, C_A);
212 
213  /* Ensure unit length normal */
214  if ((mag = MAGNITUDE(plane)) <= SMALL_FASTF)
215  return -1; /* FAIL */
216  mag = 1/mag;
217  VSCALE(plane, plane, mag);
218 
219  /* Find distance from the origin to the plane */
220  /* XXX Should do with pt that has smallest magnitude (closest to origin) */
221  plane[3] = VDOT(plane, a);
222 
223  return 0; /* OK */
224 }
225 
226 
227 int
228 bn_mkpoint_3planes(fastf_t *pt, const fastf_t *a, const fastf_t *b, const fastf_t *c)
229 {
230  vect_t v1;
231  fastf_t dot;
232 
233  /* Find a vector perpendicular to vectors b and c (parallel to planes B
234  * and C).
235  */
236  VCROSS(v1, b, c);
237 
238  /* If vector a is perpendicular to that vector, then two of the three
239  * planes are parallel. We test by examining their dot product, which is
240  * also the determinant of the matrix M^T:
241  * [ a[X] a[Y] a[Z] ]
242  * [ b[X] b[Y] b[Z] ]
243  * [ c[X] c[Y] c[Z] ]
244  */
245  dot = VDOT(a, v1);
246 
247  if (ZERO(dot)) {
248  return -1;
249  } else {
250  vect_t v2, v3;
251  fastf_t det, aH, bH, cH;
252 
253  VCROSS(v2, a, c);
254  VCROSS(v3, a, b);
255 
256  /* Since this algorithm assumes unit-length direction vectors, we need
257  * to calculate the scale factors associated with the unitized
258  * equivalents of the planes.
259  */
260  aH = MAGNITUDE(a) * a[H];
261  bH = MAGNITUDE(b) * b[H];
262  cH = MAGNITUDE(c) * c[H];
263 
264  /* We use the fact that det(M) = 1 / det(M^T) to calculate the
265  * determinant of matrix M:
266  * [ a[X] b[X] c[X] ]
267  * [ a[Y] b[Y] c[Y] ]
268  * [ a[Z] b[Z] c[Z] ]
269  */
270  det = 1 / dot;
271 
272  pt[X] = det * (aH * v1[X] - bH * v2[X] + cH * v3[X]);
273  pt[Y] = det * (aH * v1[Y] - bH * v2[Y] + cH * v3[Y]);
274  pt[Z] = det * (aH * v1[Z] - bH * v2[Z] + cH * v3[Z]);
275  }
276  return 0;
277 }
278 
279 
280 int
282  const fastf_t *d1,
283  const fastf_t *p2,
284  const fastf_t *d2,
285  double range,
286  const struct bn_tol *tol)
287 {
288  fastf_t mag1;
289  fastf_t mag2;
290  point_t tail;
291 
292  BN_CK_TOL(tol);
293 
294  if (!p1 || !d1 || !p2 || !d2) {
295  goto fail;
296  }
297 
298  if ((mag1 = MAGNITUDE(d1)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag1 zero\n");
299  if ((mag2 = MAGNITUDE(d2)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag2 zero\n");
300 
301  /* Impose a general angular tolerance to reject "obviously" non-parallel lines */
302  /* tol->para and RT_DOT_TOL are too tight a tolerance. 0.1 is 5 degrees */
303  if (fabs(VDOT(d1, d2)) < 0.9 * mag1 * mag2) goto fail;
304 
305  /* See if start points are within tolerance of other line */
306  if (bn_distsq_line3_pt3(p1, d1, p2) > tol->dist_sq) goto fail;
307  if (bn_distsq_line3_pt3(p2, d2, p1) > tol->dist_sq) goto fail;
308 
309  VJOIN1(tail, p1, range/mag1, d1);
310  if (bn_distsq_line3_pt3(p2, d2, tail) > tol->dist_sq) goto fail;
311 
312  VJOIN1(tail, p2, range/mag2, d2);
313  if (bn_distsq_line3_pt3(p1, d1, tail) > tol->dist_sq) goto fail;
314 
315  if (bu_debug & BU_DEBUG_MATH) {
316  bu_log("bn_2line3colinear(range=%g) ret=1\n", range);
317  }
318  return 1;
319 fail:
320  if (bu_debug & BU_DEBUG_MATH) {
321  bu_log("bn_2line3colinear(range=%g) ret=0\n", range);
322  }
323  return 0;
324 }
325 
326 
327 int
328 bn_dist_pt3_line3(fastf_t *dist, fastf_t *pca, const fastf_t *a, const fastf_t *dir, const fastf_t *p, const struct bn_tol *tol)
329 {
330  vect_t AtoP; /* P-A */
331  vect_t unit_dir; /* unitized dir vector */
332  fastf_t A_P_sq; /* |P-A|**2 */
333  fastf_t t; /* distance along ray of projection of P */
334  fastf_t dsq; /* square of distance from p to line */
335 
337  bu_log("bn_dist_pt3_line3(a=(%f %f %f), dir=(%f %f %f), p=(%f %f %f)\n" ,
338  V3ARGS(a), V3ARGS(dir), V3ARGS(p));
339 
340  BN_CK_TOL(tol);
341 
342  /* Check proximity to endpoint A */
343  VSUB2(AtoP, p, a);
344  A_P_sq = MAGSQ(AtoP);
345  if (A_P_sq < tol->dist_sq) {
346  /* P is within the tol->dist radius circle around A */
347  VMOVE(pca, a);
348  *dist = 0.0;
349  return 0;
350  }
351 
352  VMOVE(unit_dir, dir);
353  VUNITIZE(unit_dir);
354 
355  /* compute distance (in actual units) along line to PROJECTION of
356  * point p onto the line: point pca
357  */
358  t = VDOT(AtoP, unit_dir);
359 
360  VJOIN1(pca, a, t, unit_dir);
361  dsq = A_P_sq - t*t;
362  if (dsq < tol->dist_sq) {
363  /* P is within tolerance of the line */
364  *dist = 0.0;
365  return 1;
366  } else {
367  /* P is off line */
368  *dist = sqrt(dsq);
369  return 2;
370  }
371 }
372 
373 
374 int
375 bn_dist_line3_line3(fastf_t *dist, const fastf_t *p1, const fastf_t *d1, const fastf_t *p2, const fastf_t *d2, const struct bn_tol *tol)
376 {
377  fastf_t d1_d2;
378  point_t a1, a2;
379  vect_t a1_to_a2;
380  vect_t p2_to_p1;
381  fastf_t min_dist;
382  fastf_t tol_dist_sq;
383  fastf_t tol_dist;
384 
385  BN_CK_TOL(tol);
386 
387  if (tol->dist > 0.0)
388  tol_dist = tol->dist;
389  else
390  tol_dist = BN_TOL_DIST;
391 
392  if (tol->dist_sq > 0.0)
393  tol_dist_sq = tol->dist_sq;
394  else
395  tol_dist_sq = tol_dist * tol_dist;
396 
397  if (!NEAR_EQUAL(MAGSQ(d1), 1.0, tol_dist_sq)) {
398  bu_log("bn_dist_line3_line3: non-unit length direction vector (%f %f %f)\n", V3ARGS(d1));
399  bu_bomb("bn_dist_line3_line3\n");
400  }
401 
402  if (!NEAR_EQUAL(MAGSQ(d2), 1.0, tol_dist_sq)) {
403  bu_log("bn_dist_line3_line3: non-unit length direction vector (%f %f %f)\n", V3ARGS(d2));
404  bu_bomb("bn_dist_line3_line3\n");
405  }
406 
407  d1_d2 = VDOT(d1, d2);
408 
409  if (BN_VECT_ARE_PARALLEL(d1_d2, tol)) {
410  if (bn_dist_line3_pt3(p1, d1, p2) > tol_dist)
411  return -2; /* parallel, but not collinear */
412  else
413  return -1; /* parallel and collinear */
414  }
415 
416  VSUB2(p2_to_p1, p1, p2);
417  dist[0] = (d1_d2 * VDOT(p2_to_p1, d2) - VDOT(p2_to_p1, d1))/(1.0 - d1_d2 * d1_d2);
418  dist[1] = dist[0] * d1_d2 + VDOT(p2_to_p1, d2);
419 
420  VJOIN1(a1, p1, dist[0], d1);
421  VJOIN1(a2, p2, dist[1], d2);
422 
423  VSUB2(a1_to_a2, a2, a1);
424  min_dist = MAGNITUDE(a1_to_a2);
425  if (min_dist < tol_dist)
426  return 0;
427  else
428  return 1;
429 }
430 
431 
432 int
433 bn_dist_line3_lseg3(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
434 {
435  vect_t a_to_b;
436  vect_t a_dir;
437  fastf_t len_ab;
438  int outside_segment;
439  int ret;
440 
441  BN_CK_TOL(tol);
442 
443  VSUB2(a_to_b, b, a);
444  len_ab = MAGNITUDE(a_to_b);
445  VSCALE(a_dir, a_to_b, (1.0/len_ab));
446 
447  ret = bn_dist_line3_line3(dist, p, d, a, a_dir, tol);
448 
449  if (ret < 0) {
450  vect_t to_a, to_b;
451  fastf_t dist_to_a, dist_to_b;
452 
453  VSUB2(to_a, a, p);
454  VSUB2(to_b, b, p);
455  dist_to_a = VDOT(to_a, d);
456  dist_to_b = VDOT(to_b, d);
457 
458  if (dist_to_a <= dist_to_b) {
459  dist[0] = dist_to_a;
460  dist[1] = 0.0;
461  } else {
462  dist[0] = dist_to_b;
463  dist[1] = 1.0;
464  }
465  return ret;
466  }
467 
468  if (dist[1] >= (-tol->dist) && dist[1] <= len_ab + tol->dist) {
469  /* intersect or closest approach between a and b */
470  outside_segment = 0;
471  dist[1] = dist[1]/len_ab;
472  CLAMP(dist[1], 0.0, 1.0);
473  } else {
474  outside_segment = 1;
475  dist[1] = dist[1]/len_ab;
476  }
477 
478  return 2*ret + outside_segment;
479 }
480 
481 
482 int
484  const fastf_t *pt,
485  const fastf_t *dir,
486  const fastf_t *plane,
487  const struct bn_tol *tol)
488 {
489  register fastf_t slant_factor;
490  register fastf_t norm_dist;
491  register fastf_t dot;
492  vect_t local_dir;
493 
494  BN_CK_TOL(tol);
495 
496  norm_dist = plane[3] - VDOT(plane, pt);
497  slant_factor = VDOT(plane, dir);
498  VMOVE(local_dir, dir);
499  VUNITIZE(local_dir);
500  dot = VDOT(plane, local_dir);
501 
502  if (slant_factor < -SMALL_FASTF && dot < -tol->perp) {
503  *dist = norm_dist/slant_factor;
504  return 1; /* HIT, entering */
505  } else if (slant_factor > SMALL_FASTF && dot > tol->perp) {
506  *dist = norm_dist/slant_factor;
507  return 2; /* HIT, leaving */
508  }
509 
510  /*
511  * Ray is parallel to plane when dir.N == 0.
512  */
513  *dist = 0; /* sanity */
514  if (norm_dist < -tol->dist)
515  return -2; /* missed, outside */
516  if (norm_dist > tol->dist)
517  return -1; /* missed, inside */
518  return 0; /* Ray lies in the plane */
519 }
520 
521 
522 int
524  fastf_t *dir,
525  const fastf_t *a,
526  const fastf_t *b,
527  const fastf_t *rpp_min,
528  const struct bn_tol *tol)
529 {
530  vect_t abs_dir;
531  plane_t pl;
532  int i;
533 
534  VSETALL(pt, 0.0); /* sanity */
535  VSETALL(dir, 0.0); /* sanity */
536 
537  if ((i = bn_coplanar(a, b, tol)) != 0) {
538  if (i > 0) {
539  return -1; /* planes are coplanar */
540  }
541  return -2; /* planes are parallel but not coplanar */
542  }
543 
544  /* Direction vector for ray is perpendicular to both plane
545  * normals.
546  */
547  VCROSS(dir, a, b);
548 
549  /* Select an axis-aligned plane which has its normal pointing
550  * along the same axis as the largest magnitude component of the
551  * direction vector. If the largest magnitude component is
552  * negative, reverse the direction vector, so that model is "in
553  * front" of start point.
554  */
555  abs_dir[X] = fabs(dir[X]);
556  abs_dir[Y] = fabs(dir[Y]);
557  abs_dir[Z] = fabs(dir[Z]);
558 
559  if (ZERO(abs_dir[X])) {
560  abs_dir[X] = 0.0;
561  }
562  if (ZERO(abs_dir[Y])) {
563  abs_dir[Y] = 0.0;
564  }
565  if (ZERO(abs_dir[Z])) {
566  abs_dir[Z] = 0.0;
567  }
568 
569  if (abs_dir[X] >= abs_dir[Y]) {
570  if (abs_dir[X] >= abs_dir[Z]) {
571  VSET(pl, 1, 0, 0); /* X */
572  pl[W] = rpp_min[X];
573  if (dir[X] < -SMALL_FASTF) {
574  VREVERSE(dir, dir);
575  }
576  } else {
577  VSET(pl, 0, 0, 1); /* Z */
578  pl[W] = rpp_min[Z];
579  if (dir[Z] < -SMALL_FASTF) {
580  VREVERSE(dir, dir);
581  }
582  }
583  } else {
584  if (abs_dir[Y] >= abs_dir[Z]) {
585  VSET(pl, 0, 1, 0); /* Y */
586  pl[W] = rpp_min[Y];
587  if (dir[Y] < -SMALL_FASTF) {
588  VREVERSE(dir, dir);
589  }
590  } else {
591  VSET(pl, 0, 0, 1); /* Z */
592  pl[W] = rpp_min[Z];
593  if (dir[Z] < -SMALL_FASTF) {
594  VREVERSE(dir, dir);
595  }
596  }
597  }
598 
599  /* Intersection of the 3 planes defines ray start point */
600  if (bn_mkpoint_3planes(pt, pl, a, b) < 0) {
601  return -3; /* error, should be intersection but unable to find */
602  }
603 
604  /* success, line of intersection stored in 'pt' and 'dir' */
605  return 0;
606 }
607 
608 
609 int
610 bn_isect_line2_line2(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *c, const struct bn_tol *tol)
611 /* dist[2] */
612 
613 
614 {
615  fastf_t hx, hy; /* A - P */
616  register fastf_t det;
617  register fastf_t det1;
618  vect_t unit_d;
619  vect_t unit_c;
620  vect_t unit_h;
621  int parallel;
622  int parallel1;
623 
624  BN_CK_TOL(tol);
625  if (bu_debug & BU_DEBUG_MATH) {
626  bu_log("bn_isect_line2_line2() p=(%g, %g), d=(%g, %g)\n\t\t\ta=(%g, %g), c=(%g, %g)\n",
627  V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c));
628  }
629 
630  /*
631  * From the two components q and r, form a system of 2 equations
632  * in 2 unknowns. Solve for t and u in the system:
633  *
634  * Px + t * Dx = Ax + u * Cx
635  * Py + t * Dy = Ay + u * Cy
636  * or
637  * t * Dx - u * Cx = Ax - Px
638  * t * Dy - u * Cy = Ay - Py
639  *
640  * Let H = A - P, resulting in:
641  *
642  * t * Dx - u * Cx = Hx
643  * t * Dy - u * Cy = Hy
644  *
645  * or
646  *
647  * [ Dx -Cx ] [ t ] [ Hx ]
648  * [ ] * [ ] = [ ]
649  * [ Dy -Cy ] [ u ] [ Hy ]
650  *
651  * This system can be solved by direct substitution, or by finding
652  * the determinants by Cramer's rule:
653  *
654  * [ Dx -Cx ]
655  * det(M) = det [ ] = -Dx * Cy + Cx * Dy
656  * [ Dy -Cy ]
657  *
658  * If det(M) is zero, then the lines are parallel (perhaps
659  * collinear). Otherwise, exactly one solution exists.
660  */
661  det = c[X] * d[Y] - d[X] * c[Y];
662 
663  /*
664  * det(M) is non-zero, so there is exactly one solution. Using
665  * Cramer's rule, det1(M) replaces the first column of M with the
666  * constant column vector, in this case H. Similarly, det2(M)
667  * replaces the second column. Computation of the determinant is
668  * done as before.
669  *
670  * Now,
671  *
672  * [ Hx -Cx ]
673  * det [ ]
674  * det1(M) [ Hy -Cy ] -Hx * Cy + Cx * Hy
675  * t = ------- = --------------- = ------------------
676  * det(M) det(M) -Dx * Cy + Cx * Dy
677  *
678  * and
679  *
680  * [ Dx Hx ]
681  * det [ ]
682  * det2(M) [ Dy Hy ] Dx * Hy - Hx * Dy
683  * u = ------- = --------------- = ------------------
684  * det(M) det(M) -Dx * Cy + Cx * Dy
685  */
686  hx = a[X] - p[X];
687  hy = a[Y] - p[Y];
688  det1 = (c[X] * hy - hx * c[Y]);
689 
690  unit_d[0] = d[0];
691  unit_d[1] = d[1];
692  unit_d[2] = 0.0;
693  VUNITIZE(unit_d);
694  unit_c[0] = c[0];
695  unit_c[1] = c[1];
696  unit_c[2] = 0.0;
697  VUNITIZE(unit_c);
698  unit_h[0] = hx;
699  unit_h[1] = hy;
700  unit_h[2] = 0.0;
701  VUNITIZE(unit_h);
702 
703  if (fabs(VDOT(unit_d, unit_c)) >= tol->para)
704  parallel = 1;
705  else
706  parallel = 0;
707 
708  if (fabs(VDOT(unit_h, unit_c)) >= tol->para)
709  parallel1 = 1;
710  else
711  parallel1 = 0;
712 
713  /* XXX This zero tolerance here should actually be
714  * XXX determined by something like
715  * XXX max(c[X], c[Y], d[X], d[Y]) / MAX_FASTF_DYNAMIC_RANGE
716  * XXX In any case, nothing smaller than 1e-16
717  */
718 #define DETERMINANT_TOL 1.0e-14 /* XXX caution on non-IEEE machines */
719  if (parallel || NEAR_ZERO(det, DETERMINANT_TOL)) {
720  /* Lines are parallel */
721  if (!parallel1 && !NEAR_ZERO(det1, DETERMINANT_TOL)) {
722  /* Lines are NOT collinear, just parallel */
723  if (bu_debug & BU_DEBUG_MATH) {
724  bu_log("\tparallel, not co-linear. det=%e, det1=%g\n", det, det1);
725  }
726  return -1; /* parallel, no intersection */
727  }
728 
729  /*
730  * Lines are collinear.
731  * Determine t as distance from P to A.
732  * Determine u as distance from P to (A+C). [special!]
733  * Use largest direction component, for numeric stability
734  * (and avoiding division by zero).
735  */
736  if (fabs(d[X]) >= fabs(d[Y])) {
737  dist[0] = hx/d[X];
738  dist[1] = (hx + c[X]) / d[X];
739  } else {
740  dist[0] = hy/d[Y];
741  dist[1] = (hy + c[Y]) / d[Y];
742  }
743  if (bu_debug & BU_DEBUG_MATH) {
744  bu_log("\tcollinear, t = %g, u = %g\n", dist[0], dist[1]);
745  }
746  return 0; /* Lines collinear */
747  }
748  if (bu_debug & BU_DEBUG_MATH) {
749  /* XXX This print is temporary */
750  bu_log("\thx=%g, hy=%g, det=%g, det1=%g, det2=%g\n", hx, hy, det, det1, (d[X] * hy - hx * d[Y]));
751  }
752  det = 1/det;
753  dist[0] = det * det1;
754  dist[1] = det * (d[X] * hy - hx * d[Y]);
755  if (bu_debug & BU_DEBUG_MATH) {
756  bu_log("\tintersection, t = %g, u = %g\n", dist[0], dist[1]);
757  }
758 
759  return 1; /* Intersection found */
760 }
761 
762 
763 int
765  const fastf_t *p,
766  const fastf_t *d,
767  const fastf_t *a,
768  const fastf_t *c,
769  const struct bn_tol *tol)
770 {
771  register fastf_t f;
772  fastf_t ctol;
773  int ret;
774  point_t b;
775 
776  BN_CK_TOL(tol);
777  if (bu_debug & BU_DEBUG_MATH) {
778  bu_log("bn_isect_line2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\t\ta=(%g, %g), adir=(%g, %g)\n",
779  V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c));
780  }
781 
782  /* To keep the values of u between 0 and 1. C should NOT be
783  * scaled to have unit length. However, it is a good idea to make
784  * sure that C is a non-zero vector, (i.e., that A and B are
785  * distinct).
786  */
787  if ((ctol = MAG2SQ(c)) <= tol->dist_sq) {
788  ret = -4; /* points A and B are not distinct */
789  goto out;
790  }
791 
792  /* Detecting collinearity is difficult, and very very important.
793  * As a first step, check to see if both points A and B lie within
794  * tolerance of the line. If so, then the line segment AC is ON
795  * the line.
796  */
797  V2ADD2(b, a, c);
798  if (bn_distsq_line2_point2(p, d, a) <= tol->dist_sq &&
799  (ctol=bn_distsq_line2_point2(p, d, b)) <= tol->dist_sq) {
800  if (bu_debug & BU_DEBUG_MATH) {
801  bu_log("b=(%g, %g), b_dist_sq=%g\n", V2ARGS(b), ctol);
802  bu_log("bn_isect_line2_lseg2() pts A and B within tol of line\n");
803  }
804  /* Find the parametric distance along the ray */
805  dist[0] = bn_dist_pt2_along_line2(p, d, a);
806  dist[1] = bn_dist_pt2_along_line2(p, d, b);
807  ret = 0; /* Collinear */
808  goto out;
809  }
810 
811  if ((ret = bn_isect_line2_line2(dist, p, d, a, c, tol)) < 0) {
812  /* Lines are parallel, non-collinear */
813  ret = -3; /* No intersection found */
814  goto out;
815  }
816  if (ret == 0) {
817  fastf_t dtol;
818  /* Lines are collinear */
819  /* If P within tol of either endpoint (0, 1), make exact. */
820  dtol = tol->dist / sqrt(MAG2SQ(d));
821  if (bu_debug & BU_DEBUG_MATH) {
822  bu_log("bn_isect_line2_lseg2() dtol=%g, dist[0]=%g, dist[1]=%g\n",
823  dtol, dist[0], dist[1]);
824  }
825  if (dist[0] > -dtol && dist[0] < dtol) dist[0] = 0;
826  else if (dist[0] > 1-dtol && dist[0] < 1+dtol) dist[0] = 1;
827 
828  if (dist[1] > -dtol && dist[1] < dtol) dist[1] = 0;
829  else if (dist[1] > 1-dtol && dist[1] < 1+dtol) dist[1] = 1;
830  ret = 0; /* Collinear */
831  goto out;
832  }
833 
834  /* The two lines are claimed to intersect at a point. First,
835  * validate that hit point represented by dist[0] is in fact on
836  * and between A--B. (Nearly parallel lines can result in odd
837  * situations here). The performance hit of doing this is vastly
838  * preferable to returning wrong answers. Know a faster
839  * algorithm?
840  */
841  {
842  fastf_t ab_dist = 0;
843  point_t hit_pt;
844  point_t hit2;
845 
846  V2JOIN1(hit_pt, p, dist[0], d);
847  V2JOIN1(hit2, a, dist[1], c);
848  /* Check both hit point value calculations */
849  if (bn_pt2_pt2_equal(a, hit_pt, tol) ||
850  bn_pt2_pt2_equal(a, hit2, tol)) {
851  dist[1] = 0;
852  }
853  if (bn_pt2_pt2_equal(b, hit_pt, tol) ||
854  bn_pt2_pt2_equal(b, hit_pt, tol)) {
855  dist[1] = 1;
856  }
857 
858  ret = bn_isect_pt2_lseg2(&ab_dist, a, b, hit_pt, tol);
859  if (bu_debug & BU_DEBUG_MATH) {
860  /* XXX This is temporary */
861  V2PRINT("a", a);
862  V2PRINT("hit", hit_pt);
863  V2PRINT("b", b);
864  bu_log("bn_isect_pt2_lseg2() hit2d=(%g, %g) ab_dist=%g, ret=%d\n", hit_pt[X], hit_pt[Y], ab_dist, ret);
865  bu_log("\tother hit2d=(%g, %g)\n", hit2[X], hit2[Y]);
866  }
867  if (ret <= 0) {
868  if (ab_dist < 0) {
869  ret = -2; /* Intersection < A */
870  } else {
871  ret = -1; /* Intersection >B */
872  }
873  goto out;
874  }
875  if (ret == 1) {
876  dist[1] = 0;
877  ret = 1; /* Intersect is at A */
878  goto out;
879  }
880  if (ret == 2) {
881  dist[1] = 1;
882  ret = 2; /* Intersect is at B */
883  goto out;
884  }
885  /* ret == 3, hit_pt is between A and B */
886 
887  if (!bn_between(a[X], hit_pt[X], b[X], tol) ||
888  !bn_between(a[Y], hit_pt[Y], b[Y], tol)) {
889  bu_bomb("bn_isect_line2_lseg2() hit_pt not between A and B!\n");
890  }
891  }
892 
893  /* If the dist[1] parameter is outside the range (0..1), reject
894  * the intersection, because it falls outside the line segment
895  * A--B.
896  *
897  * Convert the tol->dist into allowable deviation in terms of
898  * (0..1) range of the parameters.
899  */
900  ctol = tol->dist / sqrt(ctol);
901  if (bu_debug & BU_DEBUG_MATH) {
902  bu_log("bn_isect_line2_lseg2() ctol=%g, dist[1]=%g\n", ctol, dist[1]);
903  }
904  if (dist[1] < -ctol) {
905  ret = -2; /* Intersection < A */
906  goto out;
907  }
908  if ((f=(dist[1]-1)) > ctol) {
909  ret = -1; /* Intersection > B */
910  goto out;
911  }
912 
913  /* Check for ctoly intersection with one of the vertices */
914  if (dist[1] < ctol) {
915  dist[1] = 0;
916  ret = 1; /* Intersection at A */
917  goto out;
918  }
919  if (f >= -ctol) {
920  dist[1] = 1;
921  ret = 2; /* Intersection at B */
922  goto out;
923  }
924  ret = 3; /* Intersection between A and B */
925 out:
926  if (bu_debug & BU_DEBUG_MATH) {
927  bu_log("bn_isect_line2_lseg2() dist[0]=%g, dist[1]=%g, ret=%d\n",
928  dist[0], dist[1], ret);
929  }
930  return ret;
931 }
932 
933 
934 int
936  const fastf_t *p,
937  const fastf_t *pdir,
938  const fastf_t *q,
939  const fastf_t *qdir,
940  const struct bn_tol *tol)
941 {
942  fastf_t ptol;
943  fastf_t qtol; /* length in parameter space == tol->dist */
944  int status;
945 
946  BN_CK_TOL(tol);
947  if (bu_debug & BU_DEBUG_MATH) {
948  bu_log("bn_isect_lseg2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n",
949  V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir));
950  }
951 
952  status = bn_isect_line2_line2(dist, p, pdir, q, qdir, tol);
953  if (status < 0) {
954  /* Lines are parallel, non-collinear */
955  return -1; /* No intersection */
956  }
957  if (status == 0) {
958  int nogood = 0;
959  /* Lines are collinear */
960  /* If P within tol of either endpoint (0, 1), make exact. */
961  ptol = tol->dist / sqrt(MAG2SQ(pdir));
962  if (bu_debug & BU_DEBUG_MATH) {
963  bu_log("ptol=%g\n", ptol);
964  }
965 
966  if (NEAR_ZERO(dist[0], ptol))
967  dist[0] = 0.0;
968  else if (NEAR_EQUAL(dist[0], 1.0, ptol))
969  dist[0] = 1.0;
970 
971  if (NEAR_ZERO(dist[1], ptol))
972  dist[1] = 0.0;
973  else if (NEAR_EQUAL(dist[1], 1.0, ptol))
974  dist[1] = 1.0;
975 
976  if (dist[1] < 0 || dist[1] > 1) nogood = 1;
977  if (dist[0] < 0 || dist[0] > 1) nogood++;
978  if (nogood >= 2)
979  return -1; /* collinear, but not overlapping */
980  if (bu_debug & BU_DEBUG_MATH) {
981  bu_log(" HIT collinear!\n");
982  }
983  return 0; /* collinear and overlapping */
984  }
985  /* Lines intersect */
986  /* If within tolerance of an endpoint (0, 1), make exact. */
987  ptol = tol->dist / sqrt(MAG2SQ(pdir));
988 
989  if (NEAR_ZERO(dist[0], ptol))
990  dist[0] = 0;
991  else if (NEAR_EQUAL(dist[0], 1.0, ptol))
992  dist[0] = 1;
993 
994  qtol = tol->dist / sqrt(MAG2SQ(qdir));
995  if (NEAR_ZERO(dist[1], ptol))
996  dist[1] = 0;
997  else if (NEAR_EQUAL(dist[1], 1.0, ptol))
998  dist[1] = 1;
999 
1000  if (bu_debug & BU_DEBUG_MATH) {
1001  bu_log("ptol=%g, qtol=%g\n", ptol, qtol);
1002  }
1003  if (dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1) {
1004  if (bu_debug & BU_DEBUG_MATH) {
1005  bu_log(" MISS\n");
1006  }
1007  return -1; /* missed */
1008  }
1009  if (bu_debug & BU_DEBUG_MATH) {
1010  bu_log(" HIT!\n");
1011  }
1012  return 1; /* hit, normal intersection */
1013 }
1014 
1015 
1016 int
1018  const fastf_t *p,
1019  const fastf_t *pdir,
1020  const fastf_t *q,
1021  const fastf_t *qdir,
1022  const struct bn_tol *tol)
1023 {
1024  fastf_t ptol;
1025  fastf_t qtol; /* length in parameter space == tol->dist */
1026  fastf_t pmag, qmag;
1027  int status;
1028  int ret;
1029 
1030  BN_CK_TOL(tol);
1031  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1032  bu_log("bn_isect_lseg3_lseg3() p=(%g, %g, %g), pdir=(%g, %g, %g)\n\t\tq=(%g, %g, %g), qdir=(%g, %g, %g)\n",
1033  V3ARGS(p), V3ARGS(pdir), V3ARGS(q), V3ARGS(qdir));
1034  }
1035 
1036  status = bn_isect_line3_line3(&dist[0], &dist[1], p, pdir, q, qdir, tol);
1037 
1038  /* It is expected that dist[0] and dist[1] returned from
1039  * 'bn_isect_line3_line3' are the actual distance to the
1040  * intersect, i.e. not scaled. Distances in the opposite of the
1041  * line direction vector result in a negative distance.
1042  */
1043 
1044  /* sanity check */
1045  if (UNLIKELY(status < -2 || status > 1)) {
1046  bu_bomb("bn_isect_lseg3_lseg3() function 'bn_isect_line3_line3' returned an invalid status\n");
1047  }
1048 
1049  if (status == -1) {
1050  /* Infinite lines do not intersect and are not parallel
1051  * therefore line segments do not intersect and are not
1052  * parallel.
1053  */
1054  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1055  bu_log("bn_isect_lseg3_lseg3(): MISS, line segments do not intersect and are not parallel\n");
1056  }
1057  ret = -3; /* missed */
1058  goto out;
1059  }
1060 
1061  if (status == -2) {
1062  /* infinite lines do not intersect, they are parallel */
1063  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1064  bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are parallel, i.e. do not intersect\n");
1065  }
1066  ret = -2; /* missed (line segments are parallel) */
1067  goto out;
1068  }
1069 
1070  pmag = MAGNITUDE(pdir);
1071  qmag = MAGNITUDE(qdir);
1072 
1073  if (UNLIKELY(pmag < SMALL_FASTF)) {
1074  bu_bomb("bn_isect_lseg3_lseg3(): |p|=0\n");
1075  }
1076 
1077  if (UNLIKELY(qmag < SMALL_FASTF)) {
1078  bu_bomb("bn_isect_lseg3_lseg3(): |q|=0\n");
1079  }
1080 
1081  ptol = tol->dist / pmag;
1082  qtol = tol->dist / qmag;
1083  dist[0] = dist[0] / pmag;
1084  if (status == 0) { /* infinite lines are collinear */
1085  /* When line segments are collinear, dist[1] has an alternate
1086  * interpretation: it's the parameter along p (not q)
1087  * therefore dist[1] must be scaled by pmag not qmag.
1088  */
1089  dist[1] = dist[1] / pmag;
1090  } else {
1091  dist[1] = dist[1] / qmag;
1092  }
1093 
1094  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1095  bu_log("ptol=%g, qtol=%g\n", ptol, qtol);
1096  }
1097 
1098  /* If 'p' within tol of either endpoint (0.0, 1.0), make exact. */
1099  if (NEAR_ZERO(dist[0], ptol)) {
1100  dist[0] = 0.0;
1101  } else if (NEAR_EQUAL(dist[0], 1.0, ptol)) {
1102  dist[0] = 1.0;
1103  }
1104 
1105  if (status == 0) { /* infinite lines are collinear */
1106  /* When line segments are collinear, dist[1] has an alternate
1107  * interpretation: it's the parameter along p (not q)
1108  * therefore dist[1] must use tolerance ptol not qtol. If 'q'
1109  * within tol of either endpoint (0.0, 1.0), make exact.
1110  */
1111  if (NEAR_ZERO(dist[1], ptol)) {
1112  dist[1] = 0.0;
1113  } else if (NEAR_EQUAL(dist[1], 1.0, ptol)) {
1114  dist[1] = 1.0;
1115  }
1116  } else {
1117  /* If 'q' within tol of either endpoint (0.0, 1.0), make exact. */
1118  if (NEAR_ZERO(dist[1], qtol)) {
1119  dist[1] = 0.0;
1120  } else if (NEAR_EQUAL(dist[1], 1.0, qtol)) {
1121  dist[1] = 1.0;
1122  }
1123  }
1124 
1125  if (status == 0) { /* infinite lines are collinear */
1126  /* Lines are collinear */
1127  if ((dist[0] > 1.0+ptol && dist[1] > 1.0+ptol) || (dist[0] < -ptol && dist[1] < -ptol)) {
1128  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1129  bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are collinear but not overlapping!\n");
1130  }
1131  ret = -1; /* line segments are collinear but not overlapping */
1132  goto out;
1133  }
1134 
1135  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1136  bu_log("bn_isect_lseg3_lseg3(): HIT, line segments are collinear and overlapping!\n");
1137  }
1138 
1139  ret = 0; /* line segments are collinear and overlapping */
1140  goto out;
1141  }
1142 
1143  /* At this point we know the infinite lines intersect and are not
1144  * collinear.
1145  */
1146 
1147  if (dist[0] < -ptol || dist[0] > 1.0+ptol || dist[1] < -qtol || dist[1] > 1.0+qtol) {
1148  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1149  bu_log("bn_isect_lseg3_lseg3(): MISS, infinite lines intersect but line segments do not!\n");
1150  }
1151  ret = -3; /* missed, infinite lines intersect but line segments do not */
1152  goto out;
1153  }
1154 
1155  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1156  bu_log("bn_isect_lseg3_lseg3(): HIT, line segments intersect!\n");
1157  }
1158 
1159  /* sanity check */
1160  if (UNLIKELY(dist[0] < -SMALL_FASTF || dist[0] > 1.0 || dist[1] < -SMALL_FASTF || dist[1] > 1.0)) {
1161  bu_bomb("bn_isect_lseg3_lseg3(): INTERNAL ERROR, intersect distance values must be in the range 0-1\n");
1162  }
1163 
1164  ret = 1; /* hit, line segments intersect */
1165 
1166 out:
1167 
1168  return ret;
1169 }
1170 
1171 
1172 int
1173 bn_isect_line3_line3(fastf_t *pdist, /* see above */
1174  fastf_t *qdist, /* see above */
1175  const fastf_t *p0, /* line p start point */
1176  const fastf_t *pdir_i, /* line p direction, must not be unit vector */
1177  const fastf_t *q0, /* line q start point */
1178  const fastf_t *qdir_i, /* line q direction, must not be unit vector */
1179  const struct bn_tol *tol)
1180 {
1181  fastf_t b, d, e, sc, tc, sc_numerator, tc_numerator, denominator;
1182  vect_t w0, qc_to_pc, u_scaled, v_scaled, v_scaled_to_u_scaled, tmp_vec, p0_to_q1;
1183  point_t p1, q1;
1184  fastf_t pdir_mag_sq;
1185  fastf_t qdir_mag_sq;
1186 
1187  int parallel = 0;
1188  int colinear = 0;
1189  fastf_t dot, d1, d2, d3, d4;
1190  vect_t pdir, qdir;
1191 
1192  VMOVE(pdir, pdir_i);
1193  VMOVE(qdir, qdir_i);
1194 
1195  pdir_mag_sq = MAGSQ(pdir);
1196  qdir_mag_sq = MAGSQ(qdir);
1197 
1198  if (UNLIKELY((pdir_mag_sq < tol->dist_sq) || (qdir_mag_sq < tol->dist_sq))) {
1199  bu_log(" p0 = %g %g %g\n", V3ARGS(p0));
1200  bu_log("pdir = %g %g %g\n", V3ARGS(pdir));
1201  bu_log(" q0 = %g %g %g\n", V3ARGS(q0));
1202  bu_log("qdir = %g %g %g\n", V3ARGS(qdir));
1203  bu_bomb("bn_isect_line3_line3(): input vector(s) 'pdir' and/or 'qdir' is zero magnitude.\n");
1204  }
1205 
1206  *pdist = 0.0;
1207  *qdist = 0.0;
1208 
1209  /* assumes pdir & qdir are not unit vectors */
1210  VADD2(p1, p0, pdir);
1211  VADD2(q1, q0, qdir);
1212 
1213  VSUB2(p0_to_q1, q1, p0);
1214 
1215  if (bn_lseg3_lseg3_parallel(p0, p1, q0, q1, tol)) {
1216  parallel = 1;
1217  d1 = bn_distsq_line3_pt3(q0,qdir,p0);
1218  d2 = bn_distsq_line3_pt3(q0,qdir,p1);
1219  d3 = bn_distsq_line3_pt3(p0,pdir,q0);
1220  d4 = bn_distsq_line3_pt3(p0,pdir,q1);
1221  if (NEAR_ZERO(d1, tol->dist_sq) && NEAR_ZERO(d2, tol->dist_sq) &&
1222  NEAR_ZERO(d3, tol->dist_sq) && NEAR_ZERO(d4, tol->dist_sq)) {
1223  colinear = 1;
1224  }
1225  }
1226 
1227  VSUB2(w0, p0, q0);
1228  b = VDOT(pdir, qdir);
1229  d = VDOT(pdir, w0);
1230  e = VDOT(qdir, w0);
1231  denominator = pdir_mag_sq * qdir_mag_sq - b * b;
1232 
1233 
1234  if (UNLIKELY(!parallel && colinear)) {
1235  bu_bomb("bn_isect_line3_line3(): logic error, lines colinear but not parallel\n");
1236  }
1237 
1238  if (parallel && !colinear)
1239  return -2; /* no intersection, lines are parallel */
1240 
1241  if (parallel && colinear) {
1242 
1243  /* when collinear pdist has a different meaning, it is the
1244  * distance from p0 to q0
1245  */
1246  *pdist = MAGNITUDE(w0); /* w0 is opposite direction of p0 to q0 */
1247  dot = VDOT(pdir, w0);
1248  if (dot > SMALL_FASTF) {
1249  *pdist = -(*pdist);
1250  }
1251 
1252  /* when collinear qdist has a different meaning, it is the
1253  * distance from p0 to q1
1254  */
1255  *qdist = MAGNITUDE(p0_to_q1);
1256 
1257  /* if vectors pdir and p0_to_q1 are not the same direction
1258  * then make the distance negative
1259  */
1260  dot = VDOT(pdir, p0_to_q1);
1261  if (dot < -SMALL_FASTF) {
1262  *qdist = -(*qdist);
1263  }
1264 
1265  return 0; /* collinear intersection */
1266  }
1267 
1268  sc_numerator = (b * e - qdir_mag_sq * d);
1269  tc_numerator = (pdir_mag_sq * e - b * d);
1270 
1271  if (ZERO(denominator) && !ZERO(sc_numerator)) {
1272  denominator = 0.0;
1273  sc = MAX_FASTF;
1274  } else if (!ZERO(denominator) && ZERO(sc_numerator)) {
1275  sc_numerator = 0.0;
1276  sc = 0.0;
1277  } else if (ZERO(denominator) && ZERO(sc_numerator)) {
1278  sc_numerator = 0.0;
1279  denominator = 0.0;
1280  sc = 1.0;
1281  } else {
1282  sc = sc_numerator / denominator;
1283  }
1284  if (ZERO(denominator) && !ZERO(tc_numerator)) {
1285  denominator = 0.0;
1286  tc = MAX_FASTF;
1287  } else if (!ZERO(denominator) && ZERO(tc_numerator)) {
1288  tc_numerator = 0.0;
1289  tc = 0.0;
1290  } else if (ZERO(denominator) && ZERO(tc_numerator)) {
1291  tc_numerator = 0.0;
1292  denominator = 0.0;
1293  tc = 1.0;
1294  } else {
1295  tc = tc_numerator / denominator;
1296  }
1297 
1298  VSCALE(u_scaled, pdir, sc_numerator);
1299  VSCALE(v_scaled, qdir, tc_numerator);
1300  VSUB2(v_scaled_to_u_scaled, u_scaled, v_scaled);
1301 
1302  if (ZERO(denominator)) {
1303  VSCALE(tmp_vec, v_scaled_to_u_scaled, MAX_FASTF);
1304  } else {
1305  VSCALE(tmp_vec, v_scaled_to_u_scaled, 1.0/denominator);
1306  }
1307 
1308  VADD2(qc_to_pc, w0, tmp_vec);
1309 
1310  if (MAGSQ(qc_to_pc) <= tol->dist_sq) {
1311  *pdist = sc * sqrt(pdir_mag_sq);
1312  *qdist = tc * sqrt(qdir_mag_sq);
1313  return 1; /* intersection */
1314  } else {
1315  return -1; /* no intersection */
1316  }
1317 }
1318 
1319 
1320 int
1321 bn_isect_line_lseg(fastf_t *t, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
1322 {
1323  vect_t ab, pa, pb; /* direction vectors a->b, p->a, p->b */
1324  fastf_t ab_mag;
1325  fastf_t pa_mag_sq;
1326  fastf_t pb_mag_sq;
1327  fastf_t d_mag_sq;
1328  int code;
1329  fastf_t dist1, dist2, d1, d2;
1330  int colinear = 0;
1331  fastf_t dot;
1332 
1333  BN_CK_TOL(tol);
1334 
1335  *t = 0.0;
1336 
1337  d_mag_sq = MAGSQ(d);
1338  if (UNLIKELY(NEAR_ZERO(d_mag_sq, tol->dist_sq))) {
1339  bu_bomb("bn_isect_line_lseg(): ray direction vector zero magnitude\n");
1340  }
1341 
1342  VSUB2(ab, b, a);
1343  ab_mag = MAGNITUDE(ab);
1344  if (ab_mag < tol->dist) {
1345  /* points A and B are not distinct */
1346  return -4;
1347  }
1348 
1349  VSUB2(pa, a, p);
1350  pa_mag_sq = MAGSQ(pa);
1351  if (pa_mag_sq < tol->dist_sq) {
1352  /* Intersection at vertex A */
1353  *t = sqrt(pa_mag_sq);
1354  return 1;
1355  }
1356 
1357  VSUB2(pb, b, p);
1358  pb_mag_sq = MAGSQ(pb);
1359  if (pb_mag_sq < tol->dist_sq) {
1360  /* Intersection at vertex B */
1361  *t = sqrt(pb_mag_sq);
1362  return 2;
1363  }
1364 
1365  /* Just check that the vertices of the line segment are
1366  * within distance tolerance of the ray. It may cause problems
1367  * to also require the ray start and end points to be within
1368  * distance tolerance of the infinite line associated with
1369  * the line segment.
1370  */
1371  d1 = bn_distsq_line3_pt3(p,d,a); /* distance of point a to ray */
1372  d2 = bn_distsq_line3_pt3(p,d,b); /* distance of point b to ray */
1373 
1374  colinear = 0;
1375  if (NEAR_ZERO(d1, tol->dist_sq) && NEAR_ZERO(d2, tol->dist_sq)) {
1376  colinear = 1;
1377 
1378  dist1 = sqrt(pa_mag_sq);
1379  dist2 = sqrt(pb_mag_sq);
1380 
1381  /* if the direction of the pa vector is in the
1382  * opposite direction of the ray, then make the
1383  * distance negative
1384  */
1385  dot = VDOT(pa, d);
1386  if (dot < -SMALL_FASTF) {
1387  dist1 = -dist1;
1388  }
1389 
1390  /* if the direction of the pb vector is in the
1391  * opposite direction of the ray, then make the
1392  * distance negative
1393  */
1394  dot = VDOT(pb, d);
1395  if (dot < -SMALL_FASTF) {
1396  dist2 = -dist2;
1397  }
1398  }
1399 
1400  if (colinear && dist1 < SMALL_FASTF && dist2 < SMALL_FASTF) {
1401  /* lines are collinear but 'a' and 'b' are not on the ray */
1402  return -1; /* no intersection */
1403  }
1404 
1405  if (colinear && (dist1 > SMALL_FASTF) && (dist2 > SMALL_FASTF)) {
1406  /* lines are collinear and both points 'a' and 'b' are on the ray. */
1407  /* return the distance to the closest point */
1408  if (dist2 > dist1) {
1409  *t = dist1;
1410  } else {
1411  *t = dist2;
1412  }
1413  return 0;
1414  }
1415 
1416  if (colinear && (dist1 > SMALL_FASTF) && (dist2 < SMALL_FASTF)) {
1417  /* lines are collinear and 'a' is on the ray but 'b' is not. */
1418  /* return the distance to 'a' */
1419  *t = dist1;
1420  return 0;
1421  }
1422 
1423  if (colinear && (dist1 < SMALL_FASTF) && (dist2 > SMALL_FASTF)) {
1424  /* lines are collinear and 'b' is on the ray but 'a' is not. */
1425  /* return the distance to 'b' */
1426  *t = dist2;
1427  return 0;
1428  }
1429 
1430  dist1 = 0.0; /* sanity */
1431  dist2 = 0.0; /* sanity */
1432  code = bn_isect_line3_line3(&dist1, &dist2, p, d, a, ab, tol);
1433 
1434  if (UNLIKELY(code == 0)) {
1435  bu_bomb("bn_isect_line_lseg(): we should have already detected a collinear condition\n");
1436  }
1437 
1438  if (code < 0) {
1439  return -1; /* no intersection */
1440  }
1441 
1442  if (code == 1) {
1443  if (dist1 < -(tol->dist)) {
1444  /* the ray did isect the line segment but in the
1445  * negative direction so this is not really a hit
1446  */
1447  return -1; /* no intersection */
1448  }
1449  }
1450 
1451  if (code == 1) {
1452  /* determine if isect was before a, between a & b or after b */
1453  vect_t d_unit;
1454  vect_t a_to_isect_pt, b_to_isect_pt;
1455  point_t isect_pt;
1456  fastf_t a_to_isect_pt_mag_sq, b_to_isect_pt_mag_sq;
1457 
1458  VMOVE(d_unit, d);
1459  VUNITIZE(d_unit);
1460 
1461  dist1 = fabs(dist1); /* sanity */
1462  VSCALE(isect_pt, d_unit, dist1);
1463  VADD2(isect_pt, isect_pt, p);
1464  VSUB2(a_to_isect_pt, isect_pt, a);
1465  VSUB2(b_to_isect_pt, isect_pt, b);
1466 
1467  a_to_isect_pt_mag_sq = MAGSQ(a_to_isect_pt);
1468  b_to_isect_pt_mag_sq = MAGSQ(b_to_isect_pt);
1469 
1470  *t = dist1;
1471 
1472  if (a_to_isect_pt_mag_sq < tol->dist_sq) {
1473  /* isect at point a of line segment */
1474  return 1;
1475  }
1476 
1477  if (b_to_isect_pt_mag_sq < tol->dist_sq) {
1478  /* isect at point b of line segment */
1479  return 2;
1480  }
1481 
1482  if (UNLIKELY((a_to_isect_pt_mag_sq < tol->dist_sq) && (b_to_isect_pt_mag_sq < tol->dist_sq))) {
1483  bu_bomb("bn_isect_line_lseg(): this case should already been caught. i.e. zero length line segment\n");
1484  }
1485 
1486  dot = VDOT(a_to_isect_pt, ab);
1487  if (dot < -SMALL_FASTF) {
1488  /* isect before point a of infinite line associated
1489  * with the line segment a->b
1490  */
1491  return -3;
1492  }
1493 
1494  dot = VDOT(b_to_isect_pt, ab);
1495  if (dot > SMALL_FASTF) {
1496  /* isect after point b of infinite line associated
1497  * with the line segment a->b
1498  */
1499  return -2;
1500  }
1501 
1502  return 3; /* isect on line segment a->b but
1503  * not on the end points
1504  */
1505  }
1506 
1507  bu_bomb("bn_isect_line_lseg(): logic error, should not be here\n");
1508 
1509  return 0; /* quite compiler warning */
1510 }
1511 
1512 
1513 double
1514 bn_dist_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
1515 {
1516  vect_t f;
1517  register fastf_t FdotD;
1518 
1519  if ((FdotD = MAGNITUDE(dir)) <= SMALL_FASTF) {
1520  FdotD = 0.0;
1521  goto out;
1522  }
1523  VSUB2(f, a, pt);
1524  FdotD = VDOT(f, dir) / FdotD;
1525  FdotD = MAGSQ(f) - FdotD * FdotD;
1526  if (FdotD <= SMALL_FASTF) {
1527  FdotD = 0.0;
1528  goto out;
1529  }
1530  FdotD = sqrt(FdotD);
1531 out:
1532  if (bu_debug & BU_DEBUG_MATH) {
1533  bu_log("bn_dist_line3_pt3() ret=%g\n", FdotD);
1534  }
1535  return FdotD;
1536 }
1537 
1538 
1539 double
1540 bn_distsq_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
1541 {
1542  vect_t f;
1543  register fastf_t FdotD;
1544 
1545  VSUB2(f, pt, a);
1546  FdotD = MAGNITUDE(dir);
1547  if (ZERO(FdotD)) {
1548  FdotD = 0.0;
1549  goto out;
1550  }
1551  FdotD = VDOT(f, dir) / FdotD;
1552  FdotD = VDOT(f, f) - FdotD * FdotD;
1553  if (FdotD < SMALL_FASTF) {
1554  FdotD = 0.0;
1555  }
1556 out:
1557  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1558  bu_log("bn_distsq_line3_pt3() ret=%g\n", FdotD);
1559  }
1560  return FdotD;
1561 }
1562 
1563 
1564 double
1565 bn_dist_line_origin(const fastf_t *pt, const fastf_t *dir)
1566 {
1567  register fastf_t PTdotD;
1568 
1569  if ((PTdotD = MAGNITUDE(dir)) <= SMALL_FASTF)
1570  return 0.0;
1571  PTdotD = VDOT(pt, dir) / PTdotD;
1572  if ((PTdotD = VDOT(pt, pt) - PTdotD * PTdotD) <= SMALL_FASTF)
1573  return 0.0;
1574  return sqrt(PTdotD);
1575 }
1576 
1577 
1578 double
1579 bn_dist_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
1580 {
1581  vect_t f;
1582  register fastf_t FdotD;
1583 
1584  V2SUB2(f, pt, a);
1585  if ((FdotD = sqrt(MAG2SQ(dir))) <= SMALL_FASTF)
1586  return 0.0;
1587  FdotD = V2DOT(f, dir) / FdotD;
1588  if ((FdotD = V2DOT(f, f) - FdotD * FdotD) <= SMALL_FASTF)
1589  return 0.0;
1590  return sqrt(FdotD);
1591 }
1592 
1593 
1594 double
1595 bn_distsq_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
1596 {
1597  vect_t f;
1598  register fastf_t FdotD;
1599 
1600  V2SUB2(f, pt, a);
1601  if ((FdotD = sqrt(MAG2SQ(dir))) <= SMALL_FASTF)
1602  return 0.0;
1603  FdotD = V2DOT(f, dir) / FdotD;
1604  if ((FdotD = V2DOT(f, f) - FdotD * FdotD) <= SMALL_FASTF)
1605  return 0.0;
1606  return FdotD;
1607 }
1608 
1609 
1610 double
1611 bn_area_of_triangle(register const fastf_t *a, register const fastf_t *b, register const fastf_t *c)
1612 {
1613  register double t;
1614  register double area;
1615 
1616  t = a[Y] * (b[Z] - c[Z]) -
1617  b[Y] * (a[Z] - c[Z]) +
1618  c[Y] * (a[Z] - b[Z]);
1619  area = t*t;
1620  t = a[Z] * (b[X] - c[X]) -
1621  b[Z] * (a[X] - c[X]) +
1622  c[Z] * (a[X] - b[X]);
1623  area += t*t;
1624  t = a[X] * (b[Y] - c[Y]) -
1625  b[X] * (a[Y] - c[Y]) +
1626  c[X] * (a[Y] - b[Y]);
1627  area += t*t;
1628 
1629  return 0.5 * sqrt(area);
1630 }
1631 
1632 
1633 int
1635  const fastf_t *a,
1636  const fastf_t *b,
1637  const fastf_t *p,
1638  const struct bn_tol *tol)
1639 {
1640  vect_t AtoP, BtoP, AtoB, ABunit; /* unit vector from A to B */
1641  fastf_t APprABunit; /* Mag of projection of AtoP onto ABunit */
1642  fastf_t distsq;
1643 
1644  BN_CK_TOL(tol);
1645 
1646  VSUB2(AtoP, p, a);
1647  if (MAGSQ(AtoP) < tol->dist_sq)
1648  return 1; /* P at A */
1649 
1650  VSUB2(BtoP, p, b);
1651  if (MAGSQ(BtoP) < tol->dist_sq)
1652  return 2; /* P at B */
1653 
1654  VSUB2(AtoB, b, a);
1655  VMOVE(ABunit, AtoB);
1656  distsq = MAGSQ(ABunit);
1657  if (distsq < tol->dist_sq)
1658  return -1; /* A equals B, and P isn't there */
1659  distsq = 1/sqrt(distsq);
1660  VSCALE(ABunit, ABunit, distsq);
1661 
1662  /* Similar to bn_dist_line_pt, except we never actually have to do
1663  * the sqrt that the other routine does.
1664  */
1665 
1666  /* find dist as a function of ABunit, actually the projection of
1667  * AtoP onto ABunit
1668  */
1669  APprABunit = VDOT(AtoP, ABunit);
1670 
1671  /* because of pythgorean theorem ... */
1672  distsq = MAGSQ(AtoP) - APprABunit * APprABunit;
1673  if (distsq > tol->dist_sq)
1674  return -1; /* dist pt to line too large */
1675 
1676  /* Distance from the point to the line is within tolerance. */
1677  *dist = VDOT(AtoP, AtoB) / MAGSQ(AtoB);
1678 
1679  if (*dist > 1.0 || *dist < -SMALL_FASTF) /* P outside AtoB */
1680  return -2;
1681 
1682  return 3; /* P on AtoB */
1683 }
1684 
1685 
1686 /**
1687  * @param dist is distance along line from A to P
1688  * @param a is line start point
1689  * @param b is line end point
1690  * @param p is line intersect point
1691  */
1692 int
1693 bn_isect_pt2_lseg2(fastf_t *dist, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
1694 {
1695  vect_t AtoP,
1696  BtoP,
1697  AtoB,
1698  ABunit; /* unit vector from A to B */
1699  fastf_t APprABunit; /* Mag of projection of AtoP onto ABunit */
1700  fastf_t distsq;
1701 
1702  BN_CK_TOL(tol);
1703 
1704  V2SUB2(AtoP, p, a);
1705  if (MAG2SQ(AtoP) < tol->dist_sq)
1706  return 1; /* P at A */
1707 
1708  V2SUB2(BtoP, p, b);
1709  if (MAG2SQ(BtoP) < tol->dist_sq)
1710  return 2; /* P at B */
1711 
1712  V2SUB2(AtoB, b, a);
1713  V2MOVE(ABunit, AtoB);
1714  distsq = MAG2SQ(ABunit);
1715  if (distsq < tol->dist_sq) {
1716  if (bu_debug & BU_DEBUG_MATH) {
1717  bu_log("distsq A=%g\n", distsq);
1718  }
1719  return -1; /* A equals B, and P isn't there */
1720  }
1721  distsq = 1/sqrt(distsq);
1722  V2SCALE(ABunit, ABunit, distsq);
1723 
1724  /* Similar to bn_dist_line_pt, except we never actually have to do
1725  * the sqrt that the other routine does.
1726  */
1727 
1728  /* find dist as a function of ABunit, actually the projection of
1729  * AtoP onto ABunit
1730  */
1731  APprABunit = V2DOT(AtoP, ABunit);
1732 
1733  /* because of pythgorean theorem ... */
1734  distsq = MAG2SQ(AtoP) - APprABunit * APprABunit;
1735  if (distsq > tol->dist_sq) {
1736  if (bu_debug & BU_DEBUG_MATH) {
1737  V2PRINT("ABunit", ABunit);
1738  bu_log("distsq B=%g\n", distsq);
1739  }
1740  return -1; /* dist pt to line too large */
1741  }
1742 
1743  /* Distance from the point to the line is within tolerance. */
1744  *dist = V2DOT(AtoP, AtoB) / MAG2SQ(AtoB);
1745 
1746  if (*dist > 1.0 || *dist < 0.0) /* P outside AtoB */
1747  return -2;
1748 
1749  return 3; /* P on AtoB */
1750 }
1751 
1752 
1753 /**
1754  * This is a support function for the test function
1755  * "bn_distsq_pt3_lseg3_v2".
1756  */
1757 HIDDEN int
1759 {
1760  fastf_t ai, af, bi, bf, a, b;
1761  int ret = 0;
1762 
1763  /* hack to deal with possible underlying types for fastf_t */
1764  if (sizeof(fastf_t) == sizeof(float)) {
1765  a = nextafterf((float)a_in, (float)b_in);
1766  b = nextafterf((float)b_in, (float)a_in);
1767  af = modff((float)a, (float *)&ai);
1768  bf = modff((float)b, (float *)&bi);
1769  } else if (sizeof(fastf_t) == sizeof(double)) {
1770  a = nextafter((double)a_in, (double)b_in);
1771  b = nextafter((double)b_in, (double)a_in);
1772  af = modf((double)a, (double *)&ai);
1773  bf = modf((double)b, (double *)&bi);
1774  } else {
1775  bu_bomb("are_equal(): unexpect size for type fastf_t");
1776  }
1777 
1778  if (EQUAL(ai, bi)) {
1779  if (NEAR_EQUAL(af, bf, t)) {
1780  ret = 1;
1781  }
1782  } else {
1783  if (NEAR_EQUAL(a, b, t)) {
1784  ret = 1;
1785  }
1786  }
1787 
1788  return ret;
1789 }
1790 
1791 
1792 int
1793 bn_distsq_pt3_lseg3_v2(fastf_t *dist_sq_out, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
1794 {
1795  vect_t AtoB, BtoP;
1796  vect_t AtoP = VINIT_ZERO;
1797  fastf_t AtoB_mag_sq, AtoP_mag_sq, AtoPCA_mag_sq, PtoPCA_mag_sq, BtoP_mag_sq;
1798  fastf_t dot, dt, dist_sq;
1799  int ret;
1800  int flip;
1801 
1802  dt = tol->dist_sq;
1803 
1804  flip = 0;
1805  if (bn_pt3_pt3_equal(a, b, tol)) {
1806  /* (A=B) */
1807  if (bn_pt3_pt3_equal(a, p, tol)) {
1808  /* (A=B) (A=P) (B=P) */
1809  dist_sq = 0.0;
1810  ret = 1;
1811  } else {
1812  /* (A=B) (A!=P) */
1813  dist_sq = MAGSQ(AtoP);
1814  ret = 3;
1815  }
1816  } else {
1817  /* (A!=B) */
1818  if (bn_pt3_pt3_equal(a, p, tol)) {
1819  /* (A!=B) (A=P) */
1820  dist_sq = 0.0;
1821  ret = 1;
1822  } else if (bn_pt3_pt3_equal(b, p, tol)) {
1823  /* (A!=B) (B=P) */
1824  dist_sq = 0.0;
1825  ret = 2;
1826  } else {
1827  /* (A!=B) (A!=P) (B!=P) */
1828  VSUB2(AtoB, b, a);
1829  VSUB2(AtoP, p, a);
1830  VSUB2(BtoP, p, b);
1831 
1832  dot = VDOT(AtoP, AtoB);
1833 
1834  if (ZERO(dot)) {
1835  /* dot product undefined with (AtoP dot AtoB) */
1836  /* try flipping A and B */
1837  VSUB2(AtoB, a, b);
1838  VSUB2(AtoP, p, b);
1839  VSUB2(BtoP, p, a);
1840  dot = VDOT(AtoP, AtoB);
1841  flip = 1;
1842  }
1843  if ZERO(dot) {
1844  bu_bomb("bn_distsq_pt3_lseg3_v2(): failed");
1845  }
1846 
1847  AtoB_mag_sq = MAGSQ(AtoB);
1848  AtoP_mag_sq = MAGSQ(AtoP);
1849  BtoP_mag_sq = MAGSQ(BtoP);
1850 
1851  if (dot > SMALL_FASTF) {
1852  AtoPCA_mag_sq = (dot * dot) / AtoB_mag_sq;
1853  if (are_equal(AtoPCA_mag_sq, 0.0, dt)) {
1854  /* (PCA=A) (B!=P) */
1855  /* lsegs AtoB and AtoP are perpendicular */
1856  dist_sq = AtoP_mag_sq;
1857  ret = 3;
1858  } else if (are_equal(AtoPCA_mag_sq, AtoB_mag_sq, dt)) {
1859  /* (PCA=B) (B!=P) */
1860  dist_sq = BtoP_mag_sq;
1861  ret = 4;
1862  } else if (AtoPCA_mag_sq < AtoB_mag_sq) {
1863  /* (PCA!=A) (PCA!=B) (B!=P) */
1864  /* PCA is on lseg AtoB */
1865  PtoPCA_mag_sq = fabs(AtoP_mag_sq - AtoPCA_mag_sq);
1866 
1867  if (PtoPCA_mag_sq < dt) {
1868  /* P is on lseg AtoB */
1869  dist_sq = 0.0;
1870  ret = 0;
1871  } else {
1872  dist_sq = PtoPCA_mag_sq;
1873  ret = 5;
1874  }
1875  } else {
1876  /* AtoPCA_mag > AtoB_mag */
1877  /* P is to the right of B, above/below lseg AtoB. */
1878  /* both P and PCA and not within tolerance of lseg AtoB */
1879  dist_sq = BtoP_mag_sq;
1880  ret = 4;
1881  }
1882  } else {
1883  /* dot is neg */
1884  AtoPCA_mag_sq = (dot * dot) / AtoB_mag_sq;
1885  if (AtoPCA_mag_sq < dt) {
1886  /* (PCA=A), lsegs AtoB and AtoP are perpendicular */
1887  dist_sq = AtoP_mag_sq;
1888  ret = 3;
1889  } else {
1890  /* (PCA!=A), PCA is not on lseg AtoB */
1891  /* both P and PCA and not within tolerance of lseg AtoB */
1892  dist_sq = AtoP_mag_sq;
1893  ret = 3;
1894  }
1895  }
1896  }
1897  }
1898 
1899  if (flip && ret == 3) {
1900  ret = 4;
1901  } else if (flip && ret == 4) {
1902  ret = 3;
1903  }
1904 
1905  *dist_sq_out = dist_sq;
1906  return ret;
1907 }
1908 
1909 
1910 int
1912  fastf_t *pca,
1913  const fastf_t *a,
1914  const fastf_t *b,
1915  const fastf_t *p,
1916  const struct bn_tol *tol)
1917 {
1918  vect_t PtoA; /* P-A */
1919  vect_t PtoB; /* P-B */
1920  vect_t AtoB; /* B-A */
1921  fastf_t P_A_sq; /* |P-A|**2 */
1922  fastf_t P_B_sq; /* |P-B|**2 */
1923  fastf_t B_A; /* |B-A| */
1924  fastf_t t; /* distance along ray of projection of P */
1925 
1926  BN_CK_TOL(tol);
1927 
1928  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1929  bu_log("bn_dist_pt3_lseg3() a=(%g, %g, %g) b=(%g, %g, %g)\n\tp=(%g, %g, %g), tol->dist=%g sq=%g\n",
1930  V3ARGS(a),
1931  V3ARGS(b),
1932  V3ARGS(p),
1933  tol->dist, tol->dist_sq);
1934  }
1935 
1936  /* Check proximity to endpoint A */
1937  VSUB2(PtoA, p, a);
1938  if ((P_A_sq = MAGSQ(PtoA)) < tol->dist_sq) {
1939  /* P is within the tol->dist radius circle around A */
1940  VMOVE(pca, a);
1941  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" at A\n");
1942  *dist = 0.0;
1943  return 1;
1944  }
1945 
1946  /* Check proximity to endpoint B */
1947  VSUB2(PtoB, p, b);
1948  if ((P_B_sq = MAGSQ(PtoB)) < tol->dist_sq) {
1949  /* P is within the tol->dist radius circle around B */
1950  VMOVE(pca, b);
1951  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" at B\n");
1952  *dist = 0.0;
1953  return 2;
1954  }
1955 
1956  VSUB2(AtoB, b, a);
1957  B_A = sqrt(MAGSQ(AtoB));
1958 
1959  /* compute distance (in actual units) along line to PROJECTION of
1960  * point p onto the line: point pca
1961  */
1962  t = VDOT(PtoA, AtoB) / B_A;
1963  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
1964  bu_log("bn_dist_pt3_lseg3() B_A=%g, t=%g\n",
1965  B_A, t);
1966  }
1967 
1968  if (t <= SMALL_FASTF) {
1969  /* P is "left" of A */
1970  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" left of A\n");
1971  VMOVE(pca, a);
1972  *dist = sqrt(P_A_sq);
1973  return 3;
1974  }
1975  if (t < B_A) {
1976  /* PCA falls between A and B */
1977  register fastf_t dsq;
1978  fastf_t param_dist; /* parametric dist */
1979 
1980  /* Find PCA */
1981  param_dist = t / B_A; /* Range 0..1 */
1982  VJOIN1(pca, a, param_dist, AtoB);
1983 
1984  /* Find distance from PCA to line segment (Pythagoras) */
1985  if ((dsq = P_A_sq - t * t) <= tol->dist_sq) {
1986  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" ON lseg\n");
1987  /* Distance from PCA to lseg is zero, give param instead */
1988  *dist = param_dist; /* special! */
1989  return 0;
1990  }
1991  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" closest to lseg\n");
1992  *dist = sqrt(dsq);
1993  return 5;
1994  }
1995  /* P is "right" of B */
1996  if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log(" right of B\n");
1997  VMOVE(pca, b);
1998  *dist = sqrt(P_B_sq);
1999  return 4;
2000 }
2001 
2002 
2003 int
2004 bn_dist_pt2_lseg2(fastf_t *dist_sq, fastf_t *pca, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
2005 {
2006  vect_t PtoA; /* P-A */
2007  vect_t PtoB; /* P-B */
2008  vect_t AtoB; /* B-A */
2009  fastf_t P_A_sq; /* |P-A|**2 */
2010  fastf_t P_B_sq; /* |P-B|**2 */
2011  fastf_t B_A; /* |B-A| */
2012  fastf_t t; /* distance along ray of projection of P */
2013 
2014  BN_CK_TOL(tol);
2015 
2016  if (bu_debug & BU_DEBUG_MATH) {
2017  bu_log("bn_dist_pt3_lseg3() a=(%g, %g, %g) b=(%g, %g, %g)\n\tp=(%g, %g, %g), tol->dist=%g sq=%g\n",
2018  V3ARGS(a),
2019  V3ARGS(b),
2020  V3ARGS(p),
2021  tol->dist, tol->dist_sq);
2022  }
2023 
2024 
2025  /* Check proximity to endpoint A */
2026  V2SUB2(PtoA, p, a);
2027  if ((P_A_sq = MAG2SQ(PtoA)) < tol->dist_sq) {
2028  /* P is within the tol->dist radius circle around A */
2029  V2MOVE(pca, a);
2030  if (bu_debug & BU_DEBUG_MATH) bu_log(" at A\n");
2031  *dist_sq = 0.0;
2032  return 1;
2033  }
2034 
2035  /* Check proximity to endpoint B */
2036  V2SUB2(PtoB, p, b);
2037  if ((P_B_sq = MAG2SQ(PtoB)) < tol->dist_sq) {
2038  /* P is within the tol->dist radius circle around B */
2039  V2MOVE(pca, b);
2040  if (bu_debug & BU_DEBUG_MATH) bu_log(" at B\n");
2041  *dist_sq = 0.0;
2042  return 2;
2043  }
2044 
2045  V2SUB2(AtoB, b, a);
2046  B_A = sqrt(MAG2SQ(AtoB));
2047 
2048  /* compute distance (in actual units) along line to PROJECTION of
2049  * point p onto the line: point pca
2050  */
2051  t = V2DOT(PtoA, AtoB) / B_A;
2052  if (bu_debug & BU_DEBUG_MATH) {
2053  bu_log("bn_dist_pt3_lseg3() B_A=%g, t=%g\n",
2054  B_A, t);
2055  }
2056 
2057  if (t <= 0) {
2058  /* P is "left" of A */
2059  if (bu_debug & BU_DEBUG_MATH) bu_log(" left of A\n");
2060  V2MOVE(pca, a);
2061  *dist_sq = P_A_sq;
2062  return 3;
2063  }
2064  if (t < B_A) {
2065  /* PCA falls between A and B */
2066  register fastf_t dsq;
2067  fastf_t param_dist; /* parametric dist */
2068 
2069  /* Find PCA */
2070  param_dist = t / B_A; /* Range 0..1 */
2071  V2JOIN1(pca, a, param_dist, AtoB);
2072 
2073  /* Find distance from PCA to line segment (Pythagoras) */
2074  if ((dsq = P_A_sq - t * t) <= tol->dist_sq) {
2075  if (bu_debug & BU_DEBUG_MATH) bu_log(" ON lseg\n");
2076  /* Distance from PCA to lseg is zero, give param instead */
2077  *dist_sq = param_dist; /* special! Not squared. */
2078  return 0;
2079  }
2080  if (bu_debug & BU_DEBUG_MATH) bu_log(" closest to lseg\n");
2081  *dist_sq = dsq;
2082  return 5;
2083  }
2084  /* P is "right" of B */
2085  if (bu_debug & BU_DEBUG_MATH) bu_log(" right of B\n");
2086  V2MOVE(pca, b);
2087  *dist_sq = P_B_sq;
2088  return 4;
2089 }
2090 
2091 
2092 void
2093 bn_rotate_bbox(fastf_t *omin, fastf_t *omax, const fastf_t *mat, const fastf_t *imin, const fastf_t *imax)
2094 {
2095  point_t local; /* vertex point in local coordinates */
2096  point_t model; /* vertex point in model coordinates */
2097 
2098 #define ROT_VERT(a, b, c) { \
2099  VSET(local, a[X], b[Y], c[Z]); \
2100  MAT4X3PNT(model, mat, local); \
2101  VMINMAX(omin, omax, model); \
2102  }
2103 
2104  ROT_VERT(imin, imin, imin);
2105  ROT_VERT(imin, imin, imax);
2106  ROT_VERT(imin, imax, imin);
2107  ROT_VERT(imin, imax, imax);
2108  ROT_VERT(imax, imin, imin);
2109  ROT_VERT(imax, imin, imax);
2110  ROT_VERT(imax, imax, imin);
2111  ROT_VERT(imax, imax, imax);
2112 #undef ROT_VERT
2113 }
2114 
2115 
2116 void
2117 bn_rotate_plane(fastf_t *oplane, const fastf_t *mat, const fastf_t *iplane)
2118 {
2119  point_t orig_pt;
2120  point_t new_pt;
2121 
2122  /* First, pick a point that lies on the original halfspace */
2123  VSCALE(orig_pt, iplane, iplane[3]);
2124 
2125  /* Transform the surface normal */
2126  MAT4X3VEC(oplane, mat, iplane);
2127 
2128  /* Transform the point from original to new halfspace */
2129  MAT4X3PNT(new_pt, mat, orig_pt);
2130 
2131  /*
2132  * The transformed normal is all that is required.
2133  * The new distance is found from the transformed point on the plane.
2134  */
2135  oplane[3] = VDOT(new_pt, oplane);
2136 }
2137 
2138 
2139 int
2140 bn_coplanar(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
2141 {
2142  register fastf_t dot;
2143  vect_t pt_a, pt_b;
2144  BN_CK_TOL(tol);
2145 
2146  if (!NEAR_EQUAL(MAGSQ(a), 1.0, VUNITIZE_TOL) || !NEAR_EQUAL(MAGSQ(b), 1.0, VUNITIZE_TOL)) {
2147  bu_bomb("bn_coplanar(): input vector(s) 'a' and/or 'b' is not a unit vector.\n");
2148  }
2149 
2150  VSCALE(pt_a, a, fabs(a[W]));
2151  VSCALE(pt_b, b, fabs(b[W]));
2152  dot = VDOT(a, b);
2153 
2154  if (NEAR_ZERO(dot, tol->perp)) {
2155  return 0; /* planes are perpendicular */
2156  }
2157 
2158  /* parallel is when dot is within tol->perp of either -1 or 1 */
2159  if ((dot <= -SMALL_FASTF) ? (NEAR_EQUAL(dot, -1.0, tol->perp)) : (NEAR_EQUAL(dot, 1.0, tol->perp))) {
2160  if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) {
2161  /* true when planes are coplanar */
2162  if (dot >= SMALL_FASTF) {
2163  /* true when plane normals in same direction */
2164  return 1;
2165  } else {
2166  /* true when plane normals in opposite direction */
2167  return 2;
2168  }
2169  } else {
2170  return -1;
2171  }
2172  }
2173  return 0;
2174 }
2175 
2176 
2177 double
2178 bn_angle_measure(fastf_t *vec, const fastf_t *x_dir, const fastf_t *y_dir)
2179 {
2180  fastf_t xproj, yproj;
2181  fastf_t gam;
2182  fastf_t ang;
2183 
2184  xproj = -VDOT(vec, x_dir);
2185  yproj = -VDOT(vec, y_dir);
2186  gam = atan2(yproj, xproj); /* -pi..+pi */
2187  ang = M_PI + gam; /* 0..+2pi */
2188  if (ang < -SMALL_FASTF) {
2189  do {
2190  ang += M_2PI;
2191  } while (ang < -SMALL_FASTF);
2192  } else if (ang > M_2PI) {
2193  do {
2194  ang -= M_2PI;
2195  } while (ang > M_2PI);
2196  }
2197  if (UNLIKELY(ang < -SMALL_FASTF || ang > M_2PI))
2198  bu_bomb("bn_angle_measure() angle out of range\n");
2199 
2200  return ang;
2201 }
2202 
2203 
2204 double
2205 bn_dist_pt3_along_line3(const fastf_t *p, const fastf_t *d, const fastf_t *x)
2206 {
2207  vect_t x_p;
2208 
2209  VSUB2(x_p, x, p);
2210  return VDOT(x_p, d);
2211 }
2212 
2213 
2214 double
2215 bn_dist_pt2_along_line2(const fastf_t *p, const fastf_t *d, const fastf_t *x)
2216 {
2217  vect_t x_p;
2218  double ret;
2219 
2220  V2SUB2(x_p, x, p);
2221  ret = V2DOT(x_p, d);
2222  if (bu_debug & BU_DEBUG_MATH) {
2223  bu_log("bn_dist_pt2_along_line2() p=(%g, %g), d=(%g, %g), x=(%g, %g) ret=%g\n",
2224  V2ARGS(p),
2225  V2ARGS(d),
2226  V2ARGS(x),
2227  ret);
2228  }
2229  return ret;
2230 }
2231 
2232 
2233 int
2234 bn_between(double left, double mid, double right, const struct bn_tol *tol)
2235 {
2236  BN_CK_TOL(tol);
2237 
2238  if (left < right) {
2239  if (NEAR_EQUAL(left, right, tol->dist*0.1)) {
2240  left -= tol->dist*0.1;
2241  right += tol->dist*0.1;
2242  }
2243  if (mid < left || mid > right) goto fail;
2244  return 1;
2245  }
2246  /* The 'right' value is lowest */
2247  if (NEAR_EQUAL(left, right, tol->dist*0.1)) {
2248  right -= tol->dist*0.1;
2249  left += tol->dist*0.1;
2250  }
2251  if (mid < right || mid > left) goto fail;
2252  return 1;
2253 fail:
2254  if (bu_debug & BU_DEBUG_MATH) {
2255  bu_log("bn_between(%.17e, %.17e, %.17e) ret=0 FAIL\n",
2256  left, mid, right);
2257  }
2258  return 0;
2259 }
2260 
2261 
2262 int
2264  const point_t pt,
2265  const vect_t dir,
2266  const point_t V,
2267  const point_t A,
2268  const point_t B,
2269  point_t inter) /* output variable */
2270 {
2271  vect_t VP, VA, VB, AB, AP, N;
2272  fastf_t NdotDir;
2273  plane_t pl;
2274  fastf_t dist;
2275 
2276  /* intersect with plane */
2277 
2278  VSUB2(VA, A, V);
2279  VSUB2(VB, B, V);
2280  VCROSS(pl, VA, VB);
2281  VUNITIZE(pl);
2282 
2283  NdotDir = VDOT(pl, dir);
2284  if (ZERO(NdotDir))
2285  return 0;
2286 
2287  pl[W] = VDOT(pl, V);
2288 
2289  dist = (pl[W] - VDOT(pl, pt))/NdotDir;
2290  VJOIN1(inter, pt, dist, dir);
2291 
2292  /* determine if point is within triangle */
2293  VSUB2(VP, inter, V);
2294  VCROSS(N, VA, VP);
2295  if (VDOT(N, pl) < 0.0)
2296  return 0;
2297 
2298  VCROSS(N, VP, VB);
2299  if (VDOT(N, pl) < 0.0)
2300  return 0;
2301 
2302  VSUB2(AB, B, A);
2303  VSUB2(AP, inter, A);
2304  VCROSS(N, AB, AP);
2305  if (VDOT(N, pl) < 0.0)
2306  return 0;
2307 
2308  return 1;
2309 }
2310 
2311 
2312 int
2313 bn_hlf_class(const fastf_t *half_eqn, const fastf_t *min, const fastf_t *max, const struct bn_tol *tol)
2314 {
2315  int current_classification;
2316  fastf_t d;
2317 
2318 #define CHECK_PT(x, y, z) \
2319  d = (x)*half_eqn[0] + (y)*half_eqn[1] + (z)*half_eqn[2] - half_eqn[3]; \
2320  if (d < -tol->dist) { \
2321  if (current_classification == BN_CLASSIFY_OUTSIDE) \
2322  return BN_CLASSIFY_OVERLAPPING; \
2323  else current_classification = BN_CLASSIFY_INSIDE; \
2324  } else if (d > tol->dist) { \
2325  if (current_classification == BN_CLASSIFY_INSIDE) \
2326  return BN_CLASSIFY_OVERLAPPING; \
2327  else current_classification = BN_CLASSIFY_OUTSIDE; \
2328  } else return BN_CLASSIFY_OVERLAPPING
2329 
2330  current_classification = BN_CLASSIFY_UNIMPLEMENTED;
2331  CHECK_PT(min[X], min[Y], min[Z]);
2332  CHECK_PT(min[X], min[Y], max[Z]);
2333  CHECK_PT(min[X], max[Y], min[Z]);
2334  CHECK_PT(min[X], max[Y], max[Z]);
2335  CHECK_PT(max[X], min[Y], min[Z]);
2336  CHECK_PT(max[X], min[Y], max[Z]);
2337  CHECK_PT(max[X], max[Y], min[Z]);
2338  CHECK_PT(max[X], max[Y], max[Z]);
2339  if (current_classification == BN_CLASSIFY_UNIMPLEMENTED)
2340  bu_log("bn_hlf_class: error in implementation\
2341 min = (%g, %g, %g), max = (%g, %g, %g), half_eqn = (%g, %g, %g, %g)\n",
2342  V3ARGS(min), V3ARGS(max), V3ARGS(half_eqn),
2343  half_eqn[3]);
2344  return current_classification;
2345 }
2346 
2347 
2348 int
2350 {
2351  fastf_t de, denom;
2352  vect_t diff, PmQ, tmp;
2353  vect_t d, e;
2354  fastf_t len_e, inv_len_e, len_d, inv_len_d;
2355  int ret=0;
2356 
2357  len_e = MAGNITUDE(e_in);
2358  if (ZERO(len_e))
2359  bu_bomb("bn_distsq_line3_line3() called with zero length vector\n");
2360  inv_len_e = 1.0 / len_e;
2361 
2362  len_d = MAGNITUDE(d_in);
2363  if (ZERO(len_d))
2364  bu_bomb("bn_distsq_line3_line3() called with zero length vector\n");
2365  inv_len_d = 1.0 / len_d;
2366 
2367  VSCALE(e, e_in, inv_len_e);
2368  VSCALE(d, d_in, inv_len_d);
2369  de = VDOT(d, e);
2370 
2371  if (ZERO(de)) {
2372  /* lines are perpendicular */
2373  dist[0] = VDOT(Q, d) - VDOT(P, d);
2374  dist[1] = VDOT(P, e) - VDOT(Q, e);
2375  } else {
2376  VSUB2(PmQ, P, Q);
2377  denom = 1.0 - de*de;
2378  if (ZERO(denom)) {
2379  /* lines are parallel */
2380  dist[0] = 0.0;
2381  dist[1] = VDOT(PmQ, d);
2382  ret = 1;
2383  } else {
2384  VBLEND2(tmp, 1.0, e, -de, d);
2385  dist[1] = VDOT(PmQ, tmp)/denom;
2386  dist[0] = dist[1] * de - VDOT(PmQ, d);
2387  }
2388  }
2389  VJOIN1(pt1, P, dist[0], d);
2390  VJOIN1(pt2, Q, dist[1], e);
2391  VSUB2(diff, pt1, pt2);
2392  dist[0] *= inv_len_d;
2393  dist[1] *= inv_len_e;
2394  dist[2] = MAGSQ(diff);
2395  return ret;
2396 }
2397 
2398 
2399 int
2400 bn_isect_planes(fastf_t *pt, const fastf_t (*planes)[4], const size_t pl_count)
2401 {
2402  mat_t matrix;
2403  mat_t inverse;
2404  vect_t hpq;
2405  fastf_t det;
2406  size_t i;
2407 
2408  if (bu_debug & BU_DEBUG_MATH) {
2409  bu_log("bn_isect_planes:\n");
2410  for (i=0; i<pl_count; i++) {
2411  bu_log("Plane #%zu (%f %f %f %f)\n", i, V4ARGS(planes[i]));
2412  }
2413  }
2414 
2415  MAT_ZERO(matrix);
2416  VSET(hpq, 0.0, 0.0, 0.0);
2417 
2418  for (i=0; i<pl_count; i++) {
2419  matrix[0] += planes[i][X] * planes[i][X];
2420  matrix[5] += planes[i][Y] * planes[i][Y];
2421  matrix[10] += planes[i][Z] * planes[i][Z];
2422  matrix[1] += planes[i][X] * planes[i][Y];
2423  matrix[2] += planes[i][X] * planes[i][Z];
2424  matrix[6] += planes[i][Y] * planes[i][Z];
2425  hpq[X] += planes[i][X] * planes[i][H];
2426  hpq[Y] += planes[i][Y] * planes[i][H];
2427  hpq[Z] += planes[i][Z] * planes[i][H];
2428  }
2429 
2430  matrix[4] = matrix[1];
2431  matrix[8] = matrix[2];
2432  matrix[9] = matrix[6];
2433  matrix[15] = 1.0;
2434 
2435  /* Check that we don't have a singular matrix */
2436  det = bn_mat_determinant(matrix);
2437  if (ZERO(det))
2438  return 1;
2439 
2440  bn_mat_inv(inverse, matrix);
2441 
2442  MAT4X3PNT(pt, inverse, hpq);
2443 
2444  return 0;
2445 
2446 }
2447 
2448 
2449 /**
2450  * Intersect a line segment with a rectangular parallelepiped (RPP)
2451  * that has faces parallel to the coordinate planes (a clipping RPP).
2452  * The RPP is defined by a minimum point and a maximum point. This is
2453  * a very close relative to rt_in_rpp() from librt/shoot.c
2454  *
2455  * @return 0 if ray does not hit RPP,
2456  * @return !0 if ray hits RPP.
2457  *
2458  * @param[in, out] a Start point of lseg
2459  * @param[in, out] b End point of lseg
2460  * @param[in] min min point of RPP
2461  * @param[in] max amx point of RPP
2462  *
2463  * if !0 was returned, "a" and "b" have been clipped to the RPP.
2464  */
2465 int
2467  fastf_t *b,
2468  register fastf_t *min,
2469  register fastf_t *max)
2470 {
2471  vect_t diff;
2472  register fastf_t *pt = &a[0];
2473  register fastf_t *dir = &diff[0];
2474  register int i;
2475  register double sv;
2476  register double st;
2477  register double mindist, maxdist;
2478 
2479  mindist = -MAX_FASTF;
2480  maxdist = MAX_FASTF;
2481  VSUB2(diff, b, a);
2482 
2483  for (i=0; i < 3; i++, pt++, dir++, max++, min++) {
2484  if (*dir < -SQRT_SMALL_FASTF) {
2485  sv = (*min - *pt) / *dir;
2486  if (sv < 0.0)
2487  return 0; /* MISS */
2488 
2489  st = (*max - *pt) / *dir;
2490  V_MAX(mindist, st);
2491  V_MIN(maxdist, sv);
2492 
2493  } else if (*dir > SQRT_SMALL_FASTF) {
2494  st = (*max - *pt) / *dir;
2495  if (st < 0.0)
2496  return 0; /* MISS */
2497 
2498  sv = (*min - *pt) / *dir;
2499  V_MAX(mindist, sv);
2500  V_MIN(maxdist, st);
2501 
2502  } else {
2503  /* If direction component along this axis is NEAR 0,
2504  * (i.e., this ray is aligned with this axis), merely
2505  * check against the boundaries.
2506  */
2507  if ((*min > *pt) || (*max < *pt))
2508  return 0; /* MISS */;
2509  }
2510  }
2511  if (mindist >= maxdist)
2512  return 0; /* MISS */
2513 
2514  if (mindist > 1 || maxdist < 0)
2515  return 0; /* MISS */
2516 
2517  if (mindist >= 0 && maxdist <= 1)
2518  return 1; /* HIT within box, no clipping needed */
2519 
2520  /* Don't grow one end of a contained segment */
2521  V_MAX(mindist, 0);
2522  V_MIN(maxdist, 1);
2523 
2524  /* Compute actual intercept points */
2525  VJOIN1(b, a, maxdist, diff); /* b must go first */
2526  VJOIN1(a, a, mindist, diff);
2527  return 1; /* HIT */
2528 }
2529 
2530 
2531 int
2532 bn_lseg3_lseg3_parallel(const point_t sg1pt1, const point_t sg1pt2,
2533  const point_t sg2pt1, const point_t sg2pt2,
2534  const struct bn_tol *tol)
2535 {
2536  vect_t e_dif[2] = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
2537  vect_t e_dif_a[2] = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
2538  fastf_t e_rr[2][3] = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
2539  char e_sc[2][3] = {{0, 0, 0}, {0, 0, 0}};
2540  fastf_t dist = tol->dist;
2541  fastf_t tmp;
2542  register int i, j;
2543 
2544  VSUB2(e_dif[0], sg1pt2, sg1pt1);
2545  VSUB2(e_dif[1], sg2pt2, sg2pt1);
2546 
2547  for ( i = 0 ; i < 2 ; i++ ) {
2548  for ( j = 0 ; j < 3 ; j++ ) {
2549  e_dif_a[i][j] = fabs(e_dif[i][j]);
2550  }
2551  }
2552 
2553  for ( i = 0 ; i < 2 ; i++ ) {
2554  if ((e_dif_a[i][X] < dist) && (e_dif_a[i][Y] > dist)) {
2555  e_sc[i][0] = 1;
2556  } else if ((e_dif_a[i][X] > dist) && (e_dif_a[i][Y] < dist)) {
2557  e_sc[i][0] = 2;
2558  } else if ((e_dif_a[i][X] < dist) && (e_dif_a[i][Y] < dist)) {
2559  e_sc[i][0] = 3;
2560  } else {
2561  e_rr[i][0] = e_dif[i][Y] / e_dif[i][X];
2562  e_sc[i][0] = 0;
2563  }
2564  if ((e_dif_a[i][X] < dist) && (e_dif_a[i][Z] > dist)) {
2565  e_sc[i][1] = 1;
2566  } else if ((e_dif_a[i][X] > dist) && (e_dif_a[i][Z] < dist)) {
2567  e_sc[i][1] = 2;
2568  } else if ((e_dif_a[i][X] < dist) && (e_dif_a[i][Z] < dist)) {
2569  e_sc[i][1] = 3;
2570  } else {
2571  e_rr[i][1] = e_dif[i][Z] / e_dif[i][X];
2572  e_sc[i][1] = 0;
2573  }
2574  if ((e_dif_a[i][Y] < dist) && (e_dif_a[i][Z] > dist)) {
2575  e_sc[i][2] = 1;
2576  } else if ((e_dif_a[i][Y] > dist) && (e_dif_a[i][Z] < dist)) {
2577  e_sc[i][2] = 2;
2578  } else if ((e_dif_a[i][Y] < dist) && (e_dif_a[i][Z] < dist)) {
2579  e_sc[i][2] = 3;
2580  } else {
2581  e_rr[i][2] = e_dif[i][Z] / e_dif[i][Y];
2582  e_sc[i][2] = 0;
2583  }
2584  }
2585 
2586  /* loop thru (rise/run) ratios from xy, xz and yz planes */
2587  for ( i = 0 ; i < 3 ; i++ ) {
2588  if (e_sc[0][i] != e_sc[1][i]) {
2589  return 0;
2590  }
2591  if (e_sc[0][i] == 0) {
2592  tmp = e_rr[0][i] - e_rr[1][i];
2593  if (fabs(tmp) > dist) {
2594  return 0;
2595  }
2596  }
2597  }
2598 
2599  return 1;
2600 }
2601 
2602 
2603 /** @} */
2604 /*
2605  * Local Variables:
2606  * mode: C
2607  * tab-width: 8
2608  * indent-tabs-mode: t
2609  * c-file-style: "stroustrup"
2610  * End:
2611  * ex: shiftwidth=4 tabstop=8
2612  */
double bn_dist_pt3_along_line3(const fastf_t *p, const fastf_t *d, const fastf_t *x)
Definition: plane.c:2205
int bn_isect_pt_lseg(fastf_t *dist, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
Definition: plane.c:1634
int bn_coplanar(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
Definition: plane.c:2140
void bu_log(const char *,...) _BU_ATTR_PRINTF12
Definition: log.c:176
fastf_t bn_mat_determinant(const mat_t m)
Definition: mat.c:1117
HIDDEN int are_equal(fastf_t a_in, fastf_t b_in, fastf_t t)
Definition: plane.c:1758
#define BN_CLASSIFY_UNIMPLEMENTED
Definition: plane_calc.h:1028
double dist
>= 0
Definition: tol.h:73
#define VSET(a, b, c, d)
Definition: color.c:53
int bn_isect_pt2_lseg2(fastf_t *dist, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
Definition: plane.c:1693
#define VSETALL(a, s)
Definition: color.c:54
#define N
Definition: randmt.c:39
#define M_PI
Definition: fft.h:35
double dist_sq
dist * dist
Definition: tol.h:74
#define st
int bn_isect_lseg3_lseg3(fastf_t *dist, const fastf_t *p, const fastf_t *pdir, const fastf_t *q, const fastf_t *qdir, const struct bn_tol *tol)
Definition: plane.c:1017
int bn_between(double left, double mid, double right, const struct bn_tol *tol)
Definition: plane.c:2234
#define SMALL_FASTF
Definition: defines.h:342
double bn_angle_measure(fastf_t *vec, const fastf_t *x_dir, const fastf_t *y_dir)
Definition: plane.c:2178
Header file for the BRL-CAD common definitions.
#define V2PRINT(a, b)
Definition: raytrace.h:1880
int bn_3pts_collinear(fastf_t *a, fastf_t *b, fastf_t *c, const struct bn_tol *tol)
Definition: plane.c:100
int bn_dist_line3_lseg3(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
Definition: plane.c:433
int bn_isect_line3_line3(fastf_t *pdist, fastf_t *qdist, const fastf_t *p0, const fastf_t *pdir_i, const fastf_t *q0, const fastf_t *qdir_i, const struct bn_tol *tol)
Definition: plane.c:1173
#define MAX_FASTF
Definition: defines.h:340
double bn_dist_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
Definition: plane.c:1579
int bn_mk_plane_3pts(fastf_t *plane, const fastf_t *a, const fastf_t *b, const fastf_t *c, const struct bn_tol *tol)
Definition: plane.c:191
#define HIDDEN
Definition: common.h:86
double bn_dist_pt3_pt3(const fastf_t *a, const fastf_t *b)
Definition: plane.c:48
double bn_dist_pt2_along_line2(const fastf_t *p, const fastf_t *d, const fastf_t *x)
Definition: plane.c:2215
#define BN_VECT_ARE_PARALLEL(_dot, _tol)
Definition: tol.h:115
int bn_isect_planes(fastf_t *pt, const fastf_t(*planes)[4], const size_t pl_count)
Definition: plane.c:2400
int bn_2line3_colinear(const fastf_t *p1, const fastf_t *d1, const fastf_t *p2, const fastf_t *d2, double range, const struct bn_tol *tol)
Definition: plane.c:281
Definition: color.c:49
double bn_distsq_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
Definition: plane.c:1540
void bn_rotate_bbox(fastf_t *omin, fastf_t *omax, const fastf_t *mat, const fastf_t *imin, const fastf_t *imax)
Definition: plane.c:2093
int bn_dist_pt2_lseg2(fastf_t *dist_sq, fastf_t *pca, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
Definition: plane.c:2004
double bn_dist_line_origin(const fastf_t *pt, const fastf_t *dir)
Definition: plane.c:1565
void bn_mat_inv(mat_t output, const mat_t input)
#define V3ARGS(a)
Definition: color.c:56
#define NEAR_ZERO(val, epsilon)
Definition: color.c:55
#define SQRT_SMALL_FASTF
Definition: defines.h:346
int bn_isect_line_lseg(fastf_t *t, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
Definition: plane.c:1321
int bn_dist_pt3_lseg3(fastf_t *dist, fastf_t *pca, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
Definition: plane.c:1911
#define BN_TOL_DIST
Definition: tol.h:109
double bn_dist_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
Definition: plane.c:1514
#define ROT_VERT(a, b, c)
void bn_rotate_plane(fastf_t *oplane, const fastf_t *mat, const fastf_t *iplane)
Definition: plane.c:2117
int bn_lseg3_lseg3_parallel(const point_t sg1pt1, const point_t sg1pt2, const point_t sg2pt1, const point_t sg2pt2, const struct bn_tol *tol)
Definition: plane.c:2532
goto out
Definition: nmg_mod.c:3846
int bn_isect_lseg2_lseg2(fastf_t *dist, const fastf_t *p, const fastf_t *pdir, const fastf_t *q, const fastf_t *qdir, const struct bn_tol *tol)
Definition: plane.c:935
int bn_dist_line3_line3(fastf_t *dist, const fastf_t *p1, const fastf_t *d1, const fastf_t *p2, const fastf_t *d2, const struct bn_tol *tol)
Definition: plane.c:375
Support for uniform tolerances.
Definition: tol.h:71
#define BN_CK_TOL(_p)
Definition: tol.h:82
#define BU_DEBUG_MATH
Definition: debug.h:63
int bn_distsq_pt3_lseg3_v2(fastf_t *distsq, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
Definition: plane.c:1793
int bn_isect_line3_plane(fastf_t *dist, const fastf_t *pt, const fastf_t *dir, const fastf_t *plane, const struct bn_tol *tol)
Definition: plane.c:483
int bn_dist_pt3_line3(fastf_t *dist, fastf_t *pca, const fastf_t *a, const fastf_t *dir, const fastf_t *p, const struct bn_tol *tol)
Definition: plane.c:328
double bn_area_of_triangle(register const fastf_t *a, register const fastf_t *b, register const fastf_t *c)
Definition: plane.c:1611
double perp
nearly 0
Definition: tol.h:75
#define ZERO(val)
Definition: units.c:38
int bu_debug
Definition: globals.c:87
int bn_hlf_class(const fastf_t *half_eqn, const fastf_t *min, const fastf_t *max, const struct bn_tol *tol)
Definition: plane.c:2313
HIDDEN int code(fastf_t x, fastf_t y)
Definition: clip.c:43
#define DETERMINANT_TOL
int bn_npts_distinct(const int npts, const point_t *pts, const struct bn_tol *tol)
Definition: plane.c:173
int bn_mkpoint_3planes(fastf_t *pt, const fastf_t *a, const fastf_t *b, const fastf_t *c)
Definition: plane.c:228
int bn_3pts_distinct(const fastf_t *a, const fastf_t *b, const fastf_t *c, const struct bn_tol *tol)
Definition: plane.c:155
#define A
Definition: msr.c:51
Definition: color.c:51
int bn_pt3_pt3_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
Definition: plane.c:58
#define Q
Definition: msr.c:54
int bn_isect_line2_line2(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *c, const struct bn_tol *tol)
Definition: plane.c:610
int bn_does_ray_isect_tri(const point_t pt, const vect_t dir, const point_t V, const point_t A, const point_t B, point_t inter)
Definition: plane.c:2263
int bn_isect_lseg_rpp(fastf_t *a, fastf_t *b, register fastf_t *min, register fastf_t *max)
Definition: plane.c:2466
#define CHECK_PT(x, y, z)
int bn_distsq_line3_line3(fastf_t *dist, fastf_t *P, fastf_t *d_in, fastf_t *Q, fastf_t *e_in, fastf_t *pt1, fastf_t *pt2)
Definition: plane.c:2349
void bu_bomb(const char *str) _BU_ATTR_NORETURN
Definition: bomb.c:91
double fastf_t
Definition: defines.h:300
int bn_isect_2planes(fastf_t *pt, fastf_t *dir, const fastf_t *a, const fastf_t *b, const fastf_t *rpp_min, const struct bn_tol *tol)
Definition: plane.c:523
fastf_t area
Definition: obr.c:155
double para
nearly 1
Definition: tol.h:76
double bn_distsq_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
Definition: plane.c:1595
int bn_isect_line2_lseg2(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *c, const struct bn_tol *tol)
Definition: plane.c:764
Definition: color.c:50
int bn_pt2_pt2_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
Definition: plane.c:88
#define UNLIKELY(expression)
Definition: common.h:282