plane.c

Go to the documentation of this file.
00001 /*                         P L A N E . C
00002  * BRL-CAD
00003  *
00004  * Copyright (c) 2004-2012 United States Government as represented by
00005  * the U.S. Army Research Laboratory.
00006  *
00007  * This library is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU Lesser General Public License
00009  * version 2.1 as published by the Free Software Foundation.
00010  *
00011  * This library is distributed in the hope that it will be useful, but
00012  * WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Lesser General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Lesser General Public
00017  * License along with this file; see the file named COPYING for more
00018  * information.
00019  */
00020 /** @addtogroup plane */
00021 /** @{ */
00022 /** @file libbn/plane.c
00023  *
00024  * @brief
00025  * Some useful routines for dealing with planes and lines.
00026  *
00027  */
00028 
00029 #include "common.h"
00030 
00031 #include <stdio.h>
00032 #include <math.h>
00033 #include <string.h>
00034 
00035 #include "bu.h"
00036 #include "vmath.h"
00037 #include "bn.h"
00038 
00039 #define UNIT_SQ_TOL 1.0e-13
00040 
00041 /**
00042  * B N _ D I S T _ P T 3 _ P T 3
00043  * @brief
00044  * Returns distance between two points.
00045  */
00046 double
00047 bn_dist_pt3_pt3(const fastf_t *a, const fastf_t *b)
00048 {
00049     vect_t diff;
00050 
00051     VSUB2(diff, a, b);
00052     return MAGNITUDE(diff);
00053 }
00054 
00055 
00056 /**
00057  * B N _ P T 3 _ P T 3 _ E Q U A L
00058  *
00059  * @return 1    if the two points are equal, within the tolerance
00060  * @return 0    if the two points are not "the same"
00061  */
00062 int
00063 bn_pt3_pt3_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
00064 {
00065     register fastf_t tmp = tol->dist_sq;
00066     register fastf_t ab, abx, aby, abz;
00067 
00068     abx = a[X]-b[X];
00069     ab = abx * abx;
00070     if (ab > tmp) {
00071         return 0;
00072     }
00073     aby = a[Y]-b[Y];
00074     ab += (aby * aby);
00075     if (ab > tmp) {
00076         return 0;
00077     }
00078     abz = a[Z]-b[Z];
00079     ab += (abz * abz);
00080     if (ab > tmp) {
00081         return 0;
00082     }
00083 
00084     return 1;
00085 }
00086 
00087 
00088 /**
00089  * B N _ P T 2 _ P T 2 _ E Q U A L
00090  *
00091  * @return 1    if the two points are equal, within the tolerance
00092  * @return 0    if the two points are not "the same"
00093  */
00094 int
00095 bn_pt2_pt2_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
00096 {
00097     vect_t diff;
00098 
00099     BN_CK_TOL(tol);
00100     VSUB2_2D(diff, b, a);
00101     if (MAGSQ_2D(diff) < tol->dist_sq) return 1;
00102     return 0;
00103 }
00104 
00105 
00106 /**
00107  * B N _ 3 P T S _ C O L L I N E A R
00108  * @brief
00109  * Check to see if three points are collinear.
00110  *
00111  * The algorithm is designed to work properly regardless of the order
00112  * in which the points are provided.
00113  *
00114  * @return 1    If 3 points are collinear
00115  * @return 0    If they are not
00116  */
00117 int
00118 bn_3pts_collinear(fastf_t *a, fastf_t *b, fastf_t *c, const struct bn_tol *tol)
00119 {
00120     fastf_t mag_ab, mag_bc, mag_ca, max_len, dist_sq;
00121     fastf_t cos_a, cos_b, cos_c;
00122     vect_t ab, bc, ca;
00123     int max_edge_no;
00124 
00125     VSUB2(ab, b, a);
00126     VSUB2(bc, c, b);
00127     VSUB2(ca, a, c);
00128     mag_ab = MAGNITUDE(ab);
00129     mag_bc = MAGNITUDE(bc);
00130     mag_ca = MAGNITUDE(ca);
00131 
00132     /* find longest edge */
00133     max_len = mag_ab;
00134     max_edge_no = 1;
00135 
00136     if (mag_bc > max_len) {
00137         max_len = mag_bc;
00138         max_edge_no = 2;
00139     }
00140 
00141     if (mag_ca > max_len) {
00142         max_len = mag_ca;
00143         max_edge_no = 3;
00144     }
00145 
00146     switch (max_edge_no) {
00147         default:
00148         case 1:
00149             cos_b = (-VDOT(ab, bc))/(mag_ab * mag_bc);
00150             dist_sq = mag_bc*mag_bc*(1.0 - cos_b*cos_b);
00151             break;
00152         case 2:
00153             cos_c = (-VDOT(bc, ca))/(mag_bc * mag_ca);
00154             dist_sq = mag_ca*mag_ca*(1.0 - cos_c*cos_c);
00155             break;
00156         case 3:
00157             cos_a = (-VDOT(ca, ab))/(mag_ca * mag_ab);
00158             dist_sq = mag_ab*mag_ab*(1.0 - cos_a*cos_a);
00159             break;
00160     }
00161 
00162     if (dist_sq <= tol->dist_sq)
00163         return 1;
00164     else
00165         return 0;
00166 }
00167 
00168 
00169 /**
00170  * B N _ 3 P T S _ D I S T I N C T
00171  *
00172  * Check to see if three points are all distinct, i.e., ensure that
00173  * there is at least sqrt(dist_tol_sq) distance between every pair of
00174  * points.
00175  *
00176  * @return 1 If all three points are distinct
00177  * @return 0 If two or more points are closer together than dist_tol_sq
00178  */
00179 int
00180 bn_3pts_distinct(const fastf_t *a, const fastf_t *b, const fastf_t *c, const struct bn_tol *tol)
00181 {
00182     vect_t B_A;
00183     vect_t C_A;
00184     vect_t C_B;
00185 
00186     BN_CK_TOL(tol);
00187     VSUB2(B_A, b, a);
00188     if (MAGSQ(B_A) <= tol->dist_sq) return 0;
00189     VSUB2(C_A, c, a);
00190     if (MAGSQ(C_A) <= tol->dist_sq) return 0;
00191     VSUB2(C_B, c, b);
00192     if (MAGSQ(C_B) <= tol->dist_sq) return 0;
00193     return 1;
00194 }
00195 
00196 
00197 /**
00198  * B N _ N P T S _ D I S T I N C T
00199  *
00200  * Check to see if the points are all distinct, i.e., ensure that
00201  * there is at least sqrt(dist_tol_sq) distance between every pair of
00202  * points.
00203  *
00204  * @return 1 If all the points are distinct
00205  * @return 0 If two or more points are closer together than dist_tol_sq
00206  */
00207 int
00208 bn_npts_distinct(const int npt, const point_t *pts, const struct bn_tol *tol)
00209 {
00210     int i, j;
00211     point_t r;
00212 
00213     BN_CK_TOL(tol);
00214 
00215     for (i=0;i<npt;i++)
00216         for (j=i+1;j<npt;j++) {
00217             VSUB2(r, pts[i], pts[j]);
00218             if (MAGSQ(r) <= tol->dist_sq)
00219                 return 0;
00220         }
00221     return 1;
00222 }
00223 
00224 
00225 /**
00226  * B N _ M K _ P L A N E _ 3 P T S
00227  *
00228  * Find the equation of a plane that contains three points.  Note that
00229  * normal vector created is expected to point out (see vmath.h), so
00230  * the vector from A to C had better be counter-clockwise (about the
00231  * point A) from the vector from A to B.  This follows the BRL-CAD
00232  * outward-pointing normal convention, and the right-hand rule for
00233  * cross products.
00234  *
00235  @verbatim
00236  *
00237  *                      C
00238  *                      *
00239  *                      |\
00240  *                      | \
00241  *         ^     N      |  \
00242  *         |      \     |   \
00243  *         |       \    |    \
00244  *         |C-A     \   |     \
00245  *         |         \  |      \
00246  *         |          \ |       \
00247  *                     \|        \
00248  *                      *---------*
00249  *                      A         B
00250  *                         ----->
00251  *                          B-A
00252  @endverbatim
00253  *
00254  * If the points are given in the order A B C (e.g.,
00255  * *counter*-clockwise), then the outward pointing surface normal:
00256  *
00257  * N = (B-A) x (C-A).
00258  *
00259  *  @return 0   OK
00260  *  @return -1  Failure.  At least two of the points were not distinct,
00261  *              or all three were colinear.
00262  *
00263  * @param[out]  plane   The plane equation is stored here.
00264  * @param[in]   a       point 1
00265  * @param[in]   b       point 2
00266  * @param[in]   c       point 3
00267  * @param[in]   tol     Tolerance values for doing calcualtion
00268  */
00269 int
00270 bn_mk_plane_3pts(fastf_t *plane,
00271                  const fastf_t *a,
00272                  const fastf_t *b,
00273                  const fastf_t *c,
00274                  const struct bn_tol *tol)
00275 {
00276     vect_t B_A;
00277     vect_t C_A;
00278     vect_t C_B;
00279     register fastf_t mag;
00280 
00281     BN_CK_TOL(tol);
00282 
00283     VSUB2(B_A, b, a);
00284     if (MAGSQ(B_A) <= tol->dist_sq) return -1;
00285     VSUB2(C_A, c, a);
00286     if (MAGSQ(C_A) <= tol->dist_sq) return -1;
00287     VSUB2(C_B, c, b);
00288     if (MAGSQ(C_B) <= tol->dist_sq) return -1;
00289 
00290     VCROSS(plane, B_A, C_A);
00291 
00292     /* Ensure unit length normal */
00293     if ((mag = MAGNITUDE(plane)) <= SMALL_FASTF)
00294         return -1;      /* FAIL */
00295     mag = 1/mag;
00296     VSCALE(plane, plane, mag);
00297 
00298     /* Find distance from the origin to the plane */
00299     /* XXX Should do with pt that has smallest magnitude (closest to origin) */
00300     plane[3] = VDOT(plane, a);
00301 
00302     return 0;           /* OK */
00303 }
00304 
00305 
00306 /**
00307  * B N _ M K P O I N T _ 3 P L A N E S
00308  *@brief
00309  * Given the description of three planes, compute the point of intersection, if
00310  * any. The direction vectors of the planes need not be of unit length.
00311  *
00312  * Find the solution to a system of three equations in three unknowns:
00313  @verbatim
00314  * Px * Ax + Py * Ay + Pz * Az = -A3;
00315  * Px * Bx + Py * By + Pz * Bz = -B3;
00316  * Px * Cx + Py * Cy + Pz * Cz = -C3;
00317  *
00318  * OR
00319  *
00320  * [ Ax  Ay  Az ]   [ Px ]   [ -A3 ]
00321  * [ Bx  By  Bz ] * [ Py ] = [ -B3 ]
00322  * [ Cx  Cy  Cz ]   [ Pz ]   [ -C3 ]
00323  *
00324  @endverbatim
00325  *
00326  * @return 0    OK
00327  * @return -1   Failure.  Intersection is a line or plane.
00328  *
00329  * @param[out] pt       The point of intersection is stored here.
00330  * @param       a       plane 1
00331  * @param       b       plane 2
00332  * @param       c       plane 3
00333  */
00334 int
00335 bn_mkpoint_3planes(fastf_t *pt, const fastf_t *a, const fastf_t *b, const fastf_t *c)
00336 {
00337     vect_t v1;
00338     fastf_t dot;
00339 
00340     /* Find a vector perpendicular to vectors b and c (parallel to planes B
00341      * and C).
00342      */
00343     VCROSS(v1, b, c);
00344 
00345     /* If vector a is perpendicular to that vector, then two of the three
00346      * planes are parallel. We test by examining their dot product, which is
00347      * also the determinant of the matrix M^T:
00348      * [ a[X]  a[Y]  a[Z] ]
00349      * [ b[X]  b[Y]  b[Z] ]
00350      * [ c[X]  c[Y]  c[Z] ]
00351      */
00352     dot = VDOT(a, v1);
00353 
00354     if (ZERO(dot)) {
00355         return -1;
00356     } else {
00357         vect_t v2, v3;
00358         fastf_t det, aH, bH, cH;
00359 
00360         VCROSS(v2, a, c);
00361         VCROSS(v3, a, b);
00362 
00363         /* Since this algorithm assumes unit-length direction vectors, we need
00364          * to calculate the scale factors associated with the unitized
00365          * equivalents of the planes.
00366          */
00367         aH = MAGNITUDE(a) * a[H];
00368         bH = MAGNITUDE(b) * b[H];
00369         cH = MAGNITUDE(c) * c[H];
00370 
00371         /* We use the fact that det(M) = 1 / det(M^T) to calculate the
00372          * determinant of matrix M:
00373          * [ a[X] b[X] c[X] ]
00374          * [ a[Y] b[Y] c[Y] ]
00375          * [ a[Z] b[Z] c[Z] ]
00376          */
00377         det = 1 / dot;
00378 
00379         pt[X] = det * (aH * v1[X] - bH * v2[X] + cH * v3[X]);
00380         pt[Y] = det * (aH * v1[Y] - bH * v2[Y] + cH * v3[Y]);
00381         pt[Z] = det * (aH * v1[Z] - bH * v2[Z] + cH * v3[Z]);
00382     }
00383     return 0;
00384 }
00385 
00386 
00387 /**
00388  * B N _ 2 L I N E 3 _ C O L I N E A R
00389  * @brief
00390  * Returns non-zero if the 3 lines are colinear to within tol->dist
00391  * over the given distance range.
00392  *
00393  * Range should be at least one model diameter for most applications.
00394  * 1e5 might be OK for a default for "vehicle sized" models.
00395  *
00396  * The direction vectors do not need to be unit length.
00397  */
00398 int
00399 bn_2line3_colinear(const fastf_t *p1,
00400                    const fastf_t *d1,
00401                    const fastf_t *p2,
00402                    const fastf_t *d2,
00403                    double range,
00404                    const struct bn_tol *tol)
00405 {
00406     fastf_t mag1;
00407     fastf_t mag2;
00408     point_t tail;
00409 
00410     BN_CK_TOL(tol);
00411 
00412     if (!p1 || !d1 || !p2 || !d2) {
00413         goto fail;
00414     }
00415 
00416     if ((mag1 = MAGNITUDE(d1)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag1 zero\n");
00417     if ((mag2 = MAGNITUDE(d2)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag2 zero\n");
00418 
00419     /* Impose a general angular tolerance to reject "obviously" non-parallel lines */
00420     /* tol->para and RT_DOT_TOL are too tight a tolerance.  0.1 is 5 degrees */
00421     if (fabs(VDOT(d1, d2)) < 0.9 * mag1 * mag2) goto fail;
00422 
00423     /* See if start points are within tolerance of other line */
00424     if (bn_distsq_line3_pt3(p1, d1, p2) > tol->dist_sq) goto fail;
00425     if (bn_distsq_line3_pt3(p2, d2, p1) > tol->dist_sq) goto fail;
00426 
00427     VJOIN1(tail, p1, range/mag1, d1);
00428     if (bn_distsq_line3_pt3(p2, d2, tail) > tol->dist_sq) goto fail;
00429 
00430     VJOIN1(tail, p2, range/mag2, d2);
00431     if (bn_distsq_line3_pt3(p1, d1, tail) > tol->dist_sq) goto fail;
00432 
00433     if (bu_debug & BU_DEBUG_MATH) {
00434         bu_log("bn_2line3colinear(range=%g) ret=1\n", range);
00435     }
00436     return 1;
00437 fail:
00438     if (bu_debug & BU_DEBUG_MATH) {
00439         bu_log("bn_2line3colinear(range=%g) ret=0\n", range);
00440     }
00441     return 0;
00442 }
00443 
00444 
00445 /**
00446  * Find the distance from a point P to a line described by the
00447  * endpoint A and direction dir, and the point of closest approach
00448  * (PCA).
00449  *
00450  @code
00451                         P
00452                        *
00453                       /.
00454                      / .
00455                     /  .
00456                    /   . (dist)
00457                   /    .
00458                  /     .
00459                 *------*-------->
00460                 A      PCA    dir
00461  @endcode
00462  * There are three distinct cases, with these return codes -
00463  *   0 => P is within tolerance of point A.  *dist = 0, pca=A.
00464  *   1 => P is within tolerance of line.  *dist = 0, pca=computed.
00465  *   2 => P is "above/below" line.  *dist=|PCA-P|, pca=computed.
00466  *
00467  * TODO: For efficiency, a version of this routine that provides the
00468  * distance squared would be faster.
00469  */
00470 int
00471 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)
00472 {
00473     vect_t AtoP;        /* P-A */
00474     vect_t unit_dir;    /* unitized dir vector */
00475     fastf_t A_P_sq;     /* |P-A|**2 */
00476     fastf_t t;          /* distance along ray of projection of P */
00477     fastf_t dsq;        /* sqaure of distance from p to line */
00478 
00479     if (UNLIKELY(bu_debug & BU_DEBUG_MATH))
00480         bu_log("bn_dist_pt3_line3(a=(%f %f %f), dir=(%f %f %f), p=(%f %f %f)\n" ,
00481                V3ARGS(a), V3ARGS(dir), V3ARGS(p));
00482 
00483     BN_CK_TOL(tol);
00484 
00485     /* Check proximity to endpoint A */
00486     VSUB2(AtoP, p, a);
00487     A_P_sq = MAGSQ(AtoP);
00488     if (A_P_sq < tol->dist_sq) {
00489         /* P is within the tol->dist radius circle around A */
00490         VMOVE(pca, a);
00491         *dist = 0.0;
00492         return 0;
00493     }
00494 
00495     VMOVE(unit_dir, dir);
00496     VUNITIZE(unit_dir);
00497 
00498     /* compute distance (in actual units) along line to PROJECTION of
00499      * point p onto the line: point pca
00500      */
00501     t = VDOT(AtoP, unit_dir);
00502 
00503     VJOIN1(pca, a, t, unit_dir);
00504     dsq = A_P_sq - t*t;
00505     if (dsq < tol->dist_sq) {
00506         /* P is within tolerance of the line */
00507         *dist = 0.0;
00508         return 1;
00509     } else {
00510         /* P is off line */
00511         *dist = sqrt(dsq);
00512         return 2;
00513     }
00514 }
00515 
00516 
00517 /**
00518  * Calculate closest approach of two lines
00519  *
00520  * returns:
00521  *      -2 -> lines are parallel and do not intersect
00522  *      -1 -> lines are parallel and collinear
00523  *       0 -> lines intersect
00524  *       1 -> lines do not intersect
00525  *
00526  * For return values less than zero, dist is not set.  For return
00527  * valuse of 0 or 1, dist[0] is the distance from p1 in the d1
00528  * direction to the point of closest approach for that line.  Similar
00529  * for the second line.  d1 and d2 must be unit direction vectors.
00530  *
00531  * XXX How is this different from bn_isect_line3_line3() ?
00532  * XXX Why are the calling sequences just slightly different?
00533  * XXX Can we pick the better one, and get rid of the other one?
00534  * XXX If not, can we document how they differ?
00535  */
00536 int
00537 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)
00538 {
00539     fastf_t d1_d2;
00540     point_t a1, a2;
00541     vect_t a1_to_a2;
00542     vect_t p2_to_p1;
00543     fastf_t min_dist;
00544     fastf_t tol_dist_sq;
00545     fastf_t tol_dist;
00546 
00547     BN_CK_TOL(tol);
00548 
00549     if (tol->dist > 0.0)
00550         tol_dist = tol->dist;
00551     else
00552         tol_dist = 0.0005;
00553 
00554     if (tol->dist_sq > 0.0)
00555         tol_dist_sq = tol->dist_sq;
00556     else
00557         tol_dist_sq = tol_dist * tol_dist;
00558 
00559     if (!NEAR_EQUAL(MAGSQ(d1), 1.0, tol_dist_sq)) {
00560         bu_log("bn_dist_line3_line3: non-unit length direction vector (%f %f %f)\n", V3ARGS(d1));
00561         bu_bomb("bn_dist_line3_line3\n");
00562     }
00563 
00564     if (!NEAR_EQUAL(MAGSQ(d2), 1.0, tol_dist_sq)) {
00565         bu_log("bn_dist_line3_line3: non-unit length direction vector (%f %f %f)\n", V3ARGS(d2));
00566         bu_bomb("bn_dist_line3_line3\n");
00567     }
00568 
00569     d1_d2 = VDOT(d1, d2);
00570 
00571     if (BN_VECT_ARE_PARALLEL(d1_d2, tol)) {
00572         if (bn_dist_line3_pt3(p1, d1, p2) > tol_dist)
00573             return -2; /* parallel, but not collinear */
00574         else
00575             return -1; /* parallel and collinear */
00576     }
00577 
00578     VSUB2(p2_to_p1, p1, p2);
00579     dist[0] = (d1_d2 * VDOT(p2_to_p1, d2) - VDOT(p2_to_p1, d1))/(1.0 - d1_d2 * d1_d2);
00580     dist[1] = dist[0] * d1_d2 + VDOT(p2_to_p1, d2);
00581 
00582     VJOIN1(a1, p1, dist[0], d1);
00583     VJOIN1(a2, p2, dist[1], d2);
00584 
00585     VSUB2(a1_to_a2, a2, a1);
00586     min_dist = MAGNITUDE(a1_to_a2);
00587     if (min_dist < tol_dist)
00588         return 0;
00589     else
00590         return 1;
00591 }
00592 
00593 
00594 /**
00595  * calculate intersection or closest approach of a line and a line
00596  * segement.
00597  *
00598  * returns:
00599  *      -2 -> line and line segment are parallel and collinear.
00600  *      -1 -> line and line segment are parallel, not collinear.
00601  *       0 -> intersection between points a and b.
00602  *       1 -> intersection outside a and b.
00603  *       2 -> closest approach is between a and b.
00604  *       3 -> closest approach is outside a and b.
00605  *
00606  * dist[0] is actual distance from p in d direction to
00607  * closest portion of segment.
00608  * dist[1] is ratio of distance from a to b (0.0 at a, and 1.0 at b),
00609  * dist[1] may be less than 0 or greater than 1.
00610  * For return values less than 0, closest approach is defined as
00611  * closest to point p.
00612  * Direction vector, d, must be unit length.
00613  *
00614  */
00615 int
00616 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)
00617 {
00618     vect_t a_to_b;
00619     vect_t a_dir;
00620     fastf_t len_ab;
00621     int outside_segment;
00622     int ret;
00623 
00624     BN_CK_TOL(tol);
00625 
00626     VSUB2(a_to_b, b, a);
00627     len_ab = MAGNITUDE(a_to_b);
00628     VSCALE(a_dir, a_to_b, (1.0/len_ab));
00629 
00630     ret = bn_dist_line3_line3(dist, p, d, a, a_dir, tol);
00631 
00632     if (ret < 0) {
00633         vect_t to_a, to_b;
00634         fastf_t dist_to_a, dist_to_b;
00635 
00636         VSUB2(to_a, a, p);
00637         VSUB2(to_b, b, p);
00638         dist_to_a = VDOT(to_a, d);
00639         dist_to_b = VDOT(to_b, d);
00640 
00641         if (dist_to_a <= dist_to_b) {
00642             dist[0] = dist_to_a;
00643             dist[1] = 0.0;
00644         } else {
00645             dist[0] = dist_to_b;
00646             dist[1] = 1.0;
00647         }
00648         return ret;
00649     }
00650 
00651     if (dist[1] >= (-tol->dist) && dist[1] <= len_ab + tol->dist) {
00652         /* intersect or closest approach between a and b */
00653         outside_segment = 0;
00654         dist[1] = dist[1]/len_ab;
00655         if (dist[1] < 0.0)
00656             dist[1] = 0.0;
00657         if (dist[1] > 1.0)
00658             dist[1] = 1.0;
00659     } else {
00660         outside_segment = 1;
00661         dist[1] = dist[1]/len_ab;
00662     }
00663 
00664     return 2*ret + outside_segment;
00665 }
00666 
00667 
00668 /**
00669  * B N _ I S E C T _ L I N E 3 _ P L A N E
00670  *
00671  * Intersect an infinite line (specified in point and direction vector
00672  * form) with a plane that has an outward pointing normal.  The
00673  * direction vector need not have unit length.  The first three
00674  * elements of the plane equation must form a unit lengh vector.
00675  *
00676  * @return -2   missed (ray is outside halfspace)
00677  * @return -1   missed (ray is inside)
00678  * @return 0    line lies on plane
00679  * @return 1    hit (ray is entering halfspace)
00680  * @return 2    hit (ray is leaving)
00681  *
00682  * @param[out]  dist    set to the parametric distance of the intercept
00683  * @param[in]   pt      origin of ray
00684  * @param[in]   dir     direction of ray
00685  * @param[in]   plane   equation of plane
00686  * @param[in]   tol     tolerance values
00687  */
00688 int
00689 bn_isect_line3_plane(fastf_t *dist,
00690                      const fastf_t *pt,
00691                      const fastf_t *dir,
00692                      const fastf_t *plane,
00693                      const struct bn_tol *tol)
00694 {
00695     register fastf_t slant_factor;
00696     register fastf_t norm_dist;
00697     register fastf_t dot;
00698     vect_t local_dir;
00699 
00700     BN_CK_TOL(tol);
00701 
00702     norm_dist = plane[3] - VDOT(plane, pt);
00703     slant_factor = VDOT(plane, dir);
00704     VMOVE(local_dir, dir);
00705     VUNITIZE(local_dir);
00706     dot = VDOT(plane, local_dir);
00707 
00708     if (slant_factor < -SMALL_FASTF && dot < -tol->perp) {
00709         *dist = norm_dist/slant_factor;
00710         return 1;                       /* HIT, entering */
00711     } else if (slant_factor > SMALL_FASTF && dot > tol->perp) {
00712         *dist = norm_dist/slant_factor;
00713         return 2;                       /* HIT, leaving */
00714     }
00715 
00716     /*
00717      * Ray is parallel to plane when dir.N == 0.
00718      */
00719     *dist = 0;          /* sanity */
00720     if (norm_dist < -tol->dist)
00721         return -2;      /* missed, outside */
00722     if (norm_dist > tol->dist)
00723         return -1;      /* missed, inside */
00724     return 0;           /* Ray lies in the plane */
00725 }
00726 
00727 
00728 /**
00729  * B N _ I S E C T _ 2 P L A N E S
00730  *@brief
00731  * Given two planes, find the line of intersection between them, if
00732  * one exists.  The line of intersection is returned in parametric
00733  * line (point & direction vector) form.
00734  *
00735  * In order that all the geometry under consideration be in "front" of
00736  * the ray, it is necessary to pass the minimum point of the model
00737  * RPP.  If this convention is unnecessary, just pass (0, 0, 0) as
00738  * rpp_min.
00739  *
00740  * @return 0    success, line of intersection stored in 'pt' and 'dir'
00741  * @return -1   planes are coplanar
00742  * @return -2   planes are parallel but not coplanar
00743  * @return -3   error, should be intersection but unable to find
00744  *
00745  * @param[out]  pt      Starting point of line of intersection
00746  * @param[out]  dir     Direction vector of line of intersection (unit length)
00747  * @param[in]   a       plane 1 (normal must be unit length)
00748  * @param[in]   b       plane 2 (normal must be unit length)
00749  * @param[in]   rpp_min minimum poit of model RPP
00750  * @param[in]   tol     tolerance values
00751  */
00752 int
00753 bn_isect_2planes(fastf_t *pt,
00754                  fastf_t *dir,
00755                  const fastf_t *a,
00756                  const fastf_t *b,
00757                  const fastf_t *rpp_min,
00758                  const struct bn_tol *tol)
00759 {
00760     vect_t abs_dir;
00761     plane_t pl;
00762     int i;
00763 
00764     VSETALL(pt, 0.0);  /* sanity */
00765     VSETALL(dir, 0.0); /* sanity */
00766 
00767     if ((i = bn_coplanar(a, b, tol)) != 0) {
00768         if (i > 0) {
00769             return -1; /* planes are coplanar */
00770         }
00771         return -2;     /* planes are parallel but not coplanar */
00772     }
00773 
00774     /* Direction vector for ray is perpendicular to both plane
00775      * normals.
00776      */
00777     VCROSS(dir, a, b);
00778 
00779     /* Select an axis-aligned plane which has its normal pointing
00780      * along the same axis as the largest magnitude component of the
00781      * direction vector.  If the largest magnitude component is
00782      * negative, reverse the direction vector, so that model is "in
00783      * front" of start point.
00784      */
00785     abs_dir[X] = fabs(dir[X]);
00786     abs_dir[Y] = fabs(dir[Y]);
00787     abs_dir[Z] = fabs(dir[Z]);
00788 
00789     if (ZERO(abs_dir[X])) {
00790         abs_dir[X] = 0.0;
00791     }
00792     if (ZERO(abs_dir[Y])) {
00793         abs_dir[Y] = 0.0;
00794     }
00795     if (ZERO(abs_dir[Z])) {
00796         abs_dir[Z] = 0.0;
00797     }
00798 
00799     if (abs_dir[X] >= abs_dir[Y]) {
00800         if (abs_dir[X] >= abs_dir[Z]) {
00801             VSET(pl, 1, 0, 0);  /* X */
00802             pl[W] = rpp_min[X];
00803             if (dir[X] < -SMALL_FASTF) {
00804                 VREVERSE(dir, dir);
00805             }
00806         } else {
00807             VSET(pl, 0, 0, 1);  /* Z */
00808             pl[W] = rpp_min[Z];
00809             if (dir[Z] < -SMALL_FASTF) {
00810                 VREVERSE(dir, dir);
00811             }
00812         }
00813     } else {
00814         if (abs_dir[Y] >= abs_dir[Z]) {
00815             VSET(pl, 0, 1, 0);  /* Y */
00816             pl[W] = rpp_min[Y];
00817             if (dir[Y] < -SMALL_FASTF) {
00818                 VREVERSE(dir, dir);
00819             }
00820         } else {
00821             VSET(pl, 0, 0, 1);  /* Z */
00822             pl[W] = rpp_min[Z];
00823             if (dir[Z] < -SMALL_FASTF) {
00824                 VREVERSE(dir, dir);
00825             }
00826         }
00827     }
00828 
00829     /* Intersection of the 3 planes defines ray start point */
00830     if (bn_mkpoint_3planes(pt, pl, a, b) < 0) {
00831         return -3;  /* error, should be intersection but unable to find */
00832     }
00833 
00834     /* success, line of intersection stored in 'pt' and 'dir' */
00835     return 0;
00836 }
00837 
00838 
00839 /**
00840  * B N _ I S E C T _ L I N E 2 _ L I N E 2
00841  *
00842  * Intersect two lines, each in given in parametric form:
00843  @verbatim
00844 
00845  X = P + t * D
00846  and
00847  X = A + u * C
00848 
00849  @endverbatim
00850  *
00851  * While the parametric form is usually used to denote a ray (ie,
00852  * positive values of the parameter only), in this case the full line
00853  * is considered.
00854  *
00855  * The direction vectors C and D need not have unit length.
00856  *
00857  * @return -1   no intersection, lines are parallel.
00858  * @return 0    lines are co-linear
00859  *@n                    dist[0] gives distance from P to A,
00860  *@n                    dist[1] gives distance from P to (A+C) [not same as below]
00861  * @return 1    intersection found (t and u returned)
00862  *@n                    dist[0] gives distance from P to isect,
00863  *@n                    dist[1] gives distance from A to isect.
00864  *
00865  * @param dist When explicit return > 0, dist[0] and dist[1] are the
00866  * line parameters of the intersection point on the 2 rays.  The
00867  * actual intersection coordinates can be found by substituting either
00868  * of these into the original ray equations.
00869  *
00870  * @param p     point of first line
00871  * @param d     direction of first line
00872  * @param a     point of second line
00873  * @param c     direction of second line
00874  * @param tol   tolerance values
00875  *
00876  * Note that for lines which are very nearly parallel, but not quite
00877  * parallel enough to have the determinant go to "zero", the
00878  * intersection can turn up in surprising places.  (e.g. when
00879  * det=1e-15 and det1=5.5e-17, t=0.5)
00880  */
00881 int
00882 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)
00883 /* dist[2] */
00884 
00885 
00886 {
00887     fastf_t hx, hy;             /* A - P */
00888     register fastf_t det;
00889     register fastf_t det1;
00890     vect_t unit_d;
00891     vect_t unit_c;
00892     vect_t unit_h;
00893     int parallel;
00894     int parallel1;
00895 
00896     BN_CK_TOL(tol);
00897     if (bu_debug & BU_DEBUG_MATH) {
00898         bu_log("bn_isect_line2_line2() p=(%g, %g), d=(%g, %g)\n\t\t\ta=(%g, %g), c=(%g, %g)\n",
00899                V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c));
00900     }
00901 
00902     /*
00903      * From the two components q and r, form a system of 2 equations
00904      * in 2 unknowns.  Solve for t and u in the system:
00905      *
00906      * Px + t * Dx = Ax + u * Cx
00907      * Py + t * Dy = Ay + u * Cy
00908      * or
00909      * t * Dx - u * Cx = Ax - Px
00910      * t * Dy - u * Cy = Ay - Py
00911      *
00912      * Let H = A - P, resulting in:
00913      *
00914      * t * Dx - u * Cx = Hx
00915      * t * Dy - u * Cy = Hy
00916      *
00917      * or
00918      *
00919      * [ Dx  -Cx ]   [ t ]   [ Hx ]
00920      * [         ] * [   ] = [    ]
00921      * [ Dy  -Cy ]   [ u ]   [ Hy ]
00922      *
00923      * This system can be solved by direct substitution, or by finding
00924      * the determinants by Cramers rule:
00925      *
00926      *               [ Dx  -Cx ]
00927      *  det(M) = det [         ] = -Dx * Cy + Cx * Dy
00928      *               [ Dy  -Cy ]
00929      *
00930      * If det(M) is zero, then the lines are parallel (perhaps
00931      * colinear).  Otherwise, exactly one solution exists.
00932      */
00933     det = c[X] * d[Y] - d[X] * c[Y];
00934 
00935     /*
00936      * det(M) is non-zero, so there is exactly one solution.  Using
00937      * Cramer's rule, det1(M) replaces the first column of M with the
00938      * constant column vector, in this case H.  Similarly, det2(M)
00939      * replaces the second column.  Computation of the determinant is
00940      * done as before.
00941      *
00942      * Now,
00943      *
00944      *                    [ Hx  -Cx ]
00945      *                det [         ]
00946      *      det1(M)       [ Hy  -Cy ]   -Hx * Cy + Cx * Hy
00947      *  t = ------- = --------------- = ------------------
00948      *       det(M) det(M)        -Dx * Cy + Cx * Dy
00949      *
00950      * and
00951      *
00952      *                    [ Dx   Hx ]
00953      *                det [         ]
00954      *      det2(M)       [ Dy   Hy ]    Dx * Hy - Hx * Dy
00955      *  u = ------- = --------------- = ------------------
00956      *       det(M) det(M)        -Dx * Cy + Cx * Dy
00957      */
00958     hx = a[X] - p[X];
00959     hy = a[Y] - p[Y];
00960     det1 = (c[X] * hy - hx * c[Y]);
00961 
00962     unit_d[0] = d[0];
00963     unit_d[1] = d[1];
00964     unit_d[2] = 0.0;
00965     VUNITIZE(unit_d);
00966     unit_c[0] = c[0];
00967     unit_c[1] = c[1];
00968     unit_c[2] = 0.0;
00969     VUNITIZE(unit_c);
00970     unit_h[0] = hx;
00971     unit_h[1] = hy;
00972     unit_h[2] = 0.0;
00973     VUNITIZE(unit_h);
00974 
00975     if (fabs(VDOT(unit_d, unit_c)) >= tol->para)
00976         parallel = 1;
00977     else
00978         parallel = 0;
00979 
00980     if (fabs(VDOT(unit_h, unit_c)) >= tol->para)
00981         parallel1 = 1;
00982     else
00983         parallel1 = 0;
00984 
00985     /* XXX This zero tolerance here should actually be
00986      * XXX determined by something like
00987      * XXX max(c[X], c[Y], d[X], d[Y]) / MAX_FASTF_DYNAMIC_RANGE
00988      * XXX In any case, nothing smaller than 1e-16
00989      */
00990 #define DETERMINANT_TOL 1.0e-14         /* XXX caution on non-IEEE machines */
00991     if (parallel || NEAR_ZERO(det, DETERMINANT_TOL)) {
00992         /* Lines are parallel */
00993         if (!parallel1 && !NEAR_ZERO(det1, DETERMINANT_TOL)) {
00994             /* Lines are NOT co-linear, just parallel */
00995             if (bu_debug & BU_DEBUG_MATH) {
00996                 bu_log("\tparallel, not co-linear.  det=%e, det1=%g\n", det, det1);
00997             }
00998             return -1;  /* parallel, no intersection */
00999         }
01000 
01001         /*
01002          * Lines are co-linear.
01003          * Determine t as distance from P to A.
01004          * Determine u as distance from P to (A+C).  [special!]
01005          * Use largest direction component, for numeric stability
01006          * (and avoiding division by zero).
01007          */
01008         if (fabs(d[X]) >= fabs(d[Y])) {
01009             dist[0] = hx/d[X];
01010             dist[1] = (hx + c[X]) / d[X];
01011         } else {
01012             dist[0] = hy/d[Y];
01013             dist[1] = (hy + c[Y]) / d[Y];
01014         }
01015         if (bu_debug & BU_DEBUG_MATH) {
01016             bu_log("\tcolinear, t = %g, u = %g\n", dist[0], dist[1]);
01017         }
01018         return 0;       /* Lines co-linear */
01019     }
01020     if (bu_debug & BU_DEBUG_MATH) {
01021         /* XXX This print is temporary */
01022         bu_log("\thx=%g, hy=%g, det=%g, det1=%g, det2=%g\n", hx, hy, det, det1, (d[X] * hy - hx * d[Y]));
01023     }
01024     det = 1/det;
01025     dist[0] = det * det1;
01026     dist[1] = det * (d[X] * hy - hx * d[Y]);
01027     if (bu_debug & BU_DEBUG_MATH) {
01028         bu_log("\tintersection, t = %g, u = %g\n", dist[0], dist[1]);
01029     }
01030 
01031     return 1;           /* Intersection found */
01032 }
01033 
01034 
01035 /**
01036  * B N _ I S E C T _ L I N E 2 _ L S E G 2
01037  *@brief
01038  * Intersect a line in parametric form:
01039  *
01040  * X = P + t * D
01041  *
01042  * with a line segment defined by two distinct points A and B=(A+C).
01043  *
01044  * XXX probably should take point B, not vector C.  Sigh.
01045  *
01046  * @return -4   A and B are not distinct points
01047  * @return -3   Lines do not intersect
01048  * @return -2   Intersection exists, but outside segemnt, < A
01049  * @return -1   Intersection exists, but outside segment, > B
01050  * @return 0    Lines are co-linear (special meaning of dist[1])
01051  * @return 1    Intersection at vertex A
01052  * @return 2    Intersection at vertex B (A+C)
01053  * @return 3    Intersection between A and B
01054  *
01055  * Implicit Returns -
01056  * @param dist  When explicit return >= 0, t is the parameter that describes
01057  *              the intersection of the line and the line segment.
01058  *              The actual intersection coordinates can be found by
01059  *              solving P + t * D.  However, note that for return codes
01060  *              1 and 2 (intersection exactly at a vertex), it is
01061  *              strongly recommended that the original values passed in
01062  *              A or B are used instead of solving P + t * D, to prevent
01063  *              numeric error from creeping into the position of
01064  *              the endpoints.
01065  *
01066  * @param p     point of first line
01067  * @param d     direction of first line
01068  * @param a     point of second line
01069  * @param c     direction of second line
01070  * @param tol   tolerance values
01071  */
01072 int
01073 bn_isect_line2_lseg2(fastf_t *dist,
01074                      const fastf_t *p,
01075                      const fastf_t *d,
01076                      const fastf_t *a,
01077                      const fastf_t *c,
01078                      const struct bn_tol *tol)
01079 {
01080     register fastf_t f;
01081     fastf_t ctol;
01082     int ret;
01083     point_t b;
01084 
01085     BN_CK_TOL(tol);
01086     if (bu_debug & BU_DEBUG_MATH) {
01087         bu_log("bn_isect_line2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\t\ta=(%g, %g), adir=(%g, %g)\n",
01088                V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c));
01089     }
01090 
01091     /* To keep the values of u between 0 and 1.  C should NOT be
01092      * scaled to have unit length.  However, it is a good idea to make
01093      * sure that C is a non-zero vector, (ie, that A and B are
01094      * distinct).
01095      */
01096     if ((ctol = MAGSQ_2D(c)) <= tol->dist_sq) {
01097         ret = -4;               /* points A and B are not distinct */
01098         goto out;
01099     }
01100 
01101     /* Detecting colinearity is difficult, and very very important.
01102      * As a first step, check to see if both points A and B lie within
01103      * tolerance of the line.  If so, then the line segment AC is ON
01104      * the line.
01105      */
01106     VADD2_2D(b, a, c);
01107     if (bn_distsq_line2_point2(p, d, a) <= tol->dist_sq  &&
01108         (ctol=bn_distsq_line2_point2(p, d, b)) <= tol->dist_sq) {
01109         if (bu_debug & BU_DEBUG_MATH) {
01110             bu_log("b=(%g, %g), b_dist_sq=%g\n", V2ARGS(b), ctol);
01111             bu_log("bn_isect_line2_lseg2() pts A and B within tol of line\n");
01112         }
01113         /* Find the parametric distance along the ray */
01114         dist[0] = bn_dist_pt2_along_line2(p, d, a);
01115         dist[1] = bn_dist_pt2_along_line2(p, d, b);
01116         ret = 0;                /* Colinear */
01117         goto out;
01118     }
01119 
01120     if ((ret = bn_isect_line2_line2(dist, p, d, a, c, tol)) < 0) {
01121         /* Lines are parallel, non-colinear */
01122         ret = -3;               /* No intersection found */
01123         goto out;
01124     }
01125     if (ret == 0) {
01126         fastf_t dtol;
01127         /* Lines are colinear */
01128         /* If P within tol of either endpoint (0, 1), make exact. */
01129         dtol = tol->dist / sqrt(MAGSQ_2D(d));
01130         if (bu_debug & BU_DEBUG_MATH) {
01131             bu_log("bn_isect_line2_lseg2() dtol=%g, dist[0]=%g, dist[1]=%g\n",
01132                    dtol, dist[0], dist[1]);
01133         }
01134         if (dist[0] > -dtol && dist[0] < dtol) dist[0] = 0;
01135         else if (dist[0] > 1-dtol && dist[0] < 1+dtol) dist[0] = 1;
01136 
01137         if (dist[1] > -dtol && dist[1] < dtol) dist[1] = 0;
01138         else if (dist[1] > 1-dtol && dist[1] < 1+dtol) dist[1] = 1;
01139         ret = 0;                /* Colinear */
01140         goto out;
01141     }
01142 
01143     /* The two lines are claimed to intersect at a point.  First,
01144      * validate that hit point represented by dist[0] is in fact on
01145      * and between A--B.  (Nearly parallel lines can result in odd
01146      * situations here).  The performance hit of doing this is vastly
01147      * preferable to returning wrong answers.  Know a faster
01148      * algorithm?
01149      */
01150     {
01151         fastf_t ab_dist = 0;
01152         point_t hit_pt;
01153         point_t hit2;
01154 
01155         VJOIN1_2D(hit_pt, p, dist[0], d);
01156         VJOIN1_2D(hit2, a, dist[1], c);
01157         /* Check both hit point value calculations */
01158         if (bn_pt2_pt2_equal(a, hit_pt, tol) ||
01159             bn_pt2_pt2_equal(a, hit2, tol)) {
01160             dist[1] = 0;
01161             ret = 1;    /* Intersect is at A */
01162         }
01163         if (bn_pt2_pt2_equal(b, hit_pt, tol) ||
01164             bn_pt2_pt2_equal(b, hit_pt, tol)) {
01165             dist[1] = 1;
01166             ret = 2;    /* Intersect is at B */
01167         }
01168 
01169         ret = bn_isect_pt2_lseg2(&ab_dist, a, b, hit_pt, tol);
01170         if (bu_debug & BU_DEBUG_MATH) {
01171             /* XXX This is temporary */
01172             V2PRINT("a", a);
01173             V2PRINT("hit", hit_pt);
01174             V2PRINT("b", b);
01175             bu_log("bn_isect_pt2_lseg2() hit2d=(%g, %g) ab_dist=%g, ret=%d\n", hit_pt[X], hit_pt[Y], ab_dist, ret);
01176             bu_log("\tother hit2d=(%g, %g)\n", hit2[X], hit2[Y]);
01177         }
01178         if (ret <= 0) {
01179             if (ab_dist < 0) {
01180                 ret = -2;       /* Intersection < A */
01181             } else {
01182                 ret = -1;       /* Intersection >B */
01183             }
01184             goto out;
01185         }
01186         if (ret == 1) {
01187             dist[1] = 0;
01188             ret = 1;    /* Intersect is at A */
01189             goto out;
01190         }
01191         if (ret == 2) {
01192             dist[1] = 1;
01193             ret = 2;    /* Intersect is at B */
01194             goto out;
01195         }
01196         /* ret == 3, hit_pt is between A and B */
01197 
01198         if (!bn_between(a[X], hit_pt[X], b[X], tol) ||
01199             !bn_between(a[Y], hit_pt[Y], b[Y], tol)) {
01200             bu_bomb("bn_isect_line2_lseg2() hit_pt not between A and B!\n");
01201         }
01202     }
01203 
01204     /* If the dist[1] parameter is outside the range (0..1), reject
01205      * the intersection, because it falls outside the line segment
01206      * A--B.
01207      *
01208      * Convert the tol->dist into allowable deviation in terms of
01209      * (0..1) range of the parameters.
01210      */
01211     ctol = tol->dist / sqrt(ctol);
01212     if (bu_debug & BU_DEBUG_MATH) {
01213         bu_log("bn_isect_line2_lseg2() ctol=%g, dist[1]=%g\n", ctol, dist[1]);
01214     }
01215     if (dist[1] < -ctol) {
01216         ret = -2;               /* Intersection < A */
01217         goto out;
01218     }
01219     if ((f=(dist[1]-1)) > ctol) {
01220         ret = -1;               /* Intersection > B */
01221         goto out;
01222     }
01223 
01224     /* Check for ctoly intersection with one of the verticies */
01225     if (dist[1] < ctol) {
01226         dist[1] = 0;
01227         ret = 1;                /* Intersection at A */
01228         goto out;
01229     }
01230     if (f >= -ctol) {
01231         dist[1] = 1;
01232         ret = 2;                /* Intersection at B */
01233         goto out;
01234     }
01235     ret = 3;                    /* Intersection between A and B */
01236 out:
01237     if (bu_debug & BU_DEBUG_MATH) {
01238         bu_log("bn_isect_line2_lseg2() dist[0]=%g, dist[1]=%g, ret=%d\n",
01239                dist[0], dist[1], ret);
01240     }
01241     return ret;
01242 }
01243 
01244 
01245 /**
01246  * B N _ I S E C T _ L S E G 2 _ L S E G 2
01247  *@brief
01248  * Intersect two 2D line segments, defined by two points and two
01249  * vectors.  The vectors are unlikely to be unit length.
01250  *
01251  * @return -2   missed (line segments are parallel)
01252  * @return -1   missed (colinear and non-overlapping)
01253  * @return 0    hit (line segments colinear and overlapping)
01254  * @return 1    hit (normal intersection)
01255  *
01256  * @param dist  The value at dist[] is set to the parametric distance of the
01257  *              intercept.
01258  *@n    dist[0] is parameter along p, range 0 to 1, to intercept.
01259  *@n    dist[1] is parameter along q, range 0 to 1, to intercept.
01260  *@n    If within distance tolerance of the endpoints, these will be
01261  *      exactly 0.0 or 1.0, to ease the job of caller.
01262  *
01263  * Special note: when return code is "0" for co-linearity, dist[1] has
01264  * an alternate interpretation: it's the parameter along p (not q)
01265  * which takes you from point p to the point (q + qdir), i.e., its
01266  * the endpoint of the q linesegment, since in this case there may be
01267  * *two* intersections, if q is contained within span p to (p + pdir).
01268  * And either may be -10 if the point is outside the span.
01269  *
01270  * @param p     point 1
01271  * @param pdir  direction1
01272  * @param q     point 2
01273  * @param qdir  direction2
01274  * @param tol   tolerance values
01275  */
01276 int
01277 bn_isect_lseg2_lseg2(fastf_t *dist,
01278                      const fastf_t *p,
01279                      const fastf_t *pdir,
01280                      const fastf_t *q,
01281                      const fastf_t *qdir,
01282                      const struct bn_tol *tol)
01283 {
01284     fastf_t ptol, qtol; /* length in parameter space == tol->dist */
01285     int status;
01286 
01287     BN_CK_TOL(tol);
01288     if (bu_debug & BU_DEBUG_MATH) {
01289         bu_log("bn_isect_lseg2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n",
01290                V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir));
01291     }
01292 
01293     status = bn_isect_line2_line2(dist, p, pdir, q, qdir, tol);
01294     if (status < 0) {
01295         /* Lines are parallel, non-colinear */
01296         return -1;      /* No intersection */
01297     }
01298     if (status == 0) {
01299         int nogood = 0;
01300         /* Lines are colinear */
01301         /* If P within tol of either endpoint (0, 1), make exact. */
01302         ptol = tol->dist / sqrt(MAGSQ_2D(pdir));
01303         if (bu_debug & BU_DEBUG_MATH) {
01304             bu_log("ptol=%g\n", ptol);
01305         }
01306         if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0;
01307         else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1;
01308 
01309         if (dist[1] > -ptol && dist[1] < ptol) dist[1] = 0;
01310         else if (dist[1] > 1-ptol && dist[1] < 1+ptol) dist[1] = 1;
01311 
01312         if (dist[1] < 0 || dist[1] > 1) nogood = 1;
01313         if (dist[0] < 0 || dist[0] > 1) nogood++;
01314         if (nogood >= 2)
01315             return -1;  /* colinear, but not overlapping */
01316         if (bu_debug & BU_DEBUG_MATH) {
01317             bu_log("  HIT colinear!\n");
01318         }
01319         return 0;               /* colinear and overlapping */
01320     }
01321     /* Lines intersect */
01322     /* If within tolerance of an endpoint (0, 1), make exact. */
01323     ptol = tol->dist / sqrt(MAGSQ_2D(pdir));
01324     if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0;
01325     else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1;
01326 
01327     qtol = tol->dist / sqrt(MAGSQ_2D(qdir));
01328     if (dist[1] > -qtol && dist[1] < qtol) dist[1] = 0;
01329     else if (dist[1] > 1-qtol && dist[1] < 1+qtol) dist[1] = 1;
01330 
01331     if (bu_debug & BU_DEBUG_MATH) {
01332         bu_log("ptol=%g, qtol=%g\n", ptol, qtol);
01333     }
01334     if (dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1) {
01335         if (bu_debug & BU_DEBUG_MATH) {
01336             bu_log("  MISS\n");
01337         }
01338         return -1;              /* missed */
01339     }
01340     if (bu_debug & BU_DEBUG_MATH) {
01341         bu_log("  HIT!\n");
01342     }
01343     return 1;                   /* hit, normal intersection */
01344 }
01345 
01346 
01347 /**
01348  * B N _ I S E C T _ L S E G 3 _ L S E G 3
01349  *@brief
01350  * Intersect two 3D line segments, defined by two points and two
01351  * vectors.  The vectors are unlikely to be unit length.
01352  *
01353  *
01354  * @return -3   missed
01355  * @return -2   missed (line segments are parallel)
01356  * @return -1   missed (colinear and non-overlapping)
01357  * @return 0    hit (line segments colinear and overlapping)
01358  * @return 1    hit (normal intersection)
01359  *
01360  * @param[out] dist
01361  *      The value at dist[] is set to the parametric distance of the
01362  *      intercept dist[0] is parameter along p, range 0 to 1, to
01363  *      intercept.  dist[1] is parameter along q, range 0 to 1, to
01364  *      intercept.  If within distance tolerance of the endpoints,
01365  *      these will be exactly 0.0 or 1.0, to ease the job of caller.
01366  *
01367  *      CLARIFICATION: This function 'bn_isect_lseg3_lseg3'
01368  *      returns distance values scaled where an intersect at the start
01369  *      point of the line segement (within tol->dist) results in 0.0
01370  *      and when the intersect is at the end point of the line
01371  *      segement (within tol->dist), the result is 1.0.  Intersects
01372  *      before the start point return a negative distance.  Intersects
01373  *      after the end point result in a return value > 1.0.
01374  *
01375  * Special note: when return code is "0" for co-linearity, dist[1] has
01376  * an alternate interpretation: it's the parameter along p (not q)
01377  * which takes you from point p to the point (q + qdir), i.e., it's
01378  * the endpoint of the q linesegment, since in this case there may be
01379  * *two* intersections, if q is contained within span p to (p + pdir).
01380  *
01381  * @param p     point 1
01382  * @param pdir  direction-1
01383  * @param q     point 2
01384  * @param qdir  direction-2
01385  * @param tol   tolerance values
01386  */
01387 int
01388 bn_isect_lseg3_lseg3(fastf_t *dist,
01389                      const fastf_t *p,
01390                      const fastf_t *pdir,
01391                      const fastf_t *q,
01392                      const fastf_t *qdir,
01393                      const struct bn_tol *tol)
01394 {
01395     fastf_t ptol, qtol; /* length in parameter space == tol->dist */
01396     fastf_t pmag, qmag;
01397     int status;
01398 
01399     BN_CK_TOL(tol);
01400     if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01401         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",
01402                V3ARGS(p), V3ARGS(pdir), V3ARGS(q), V3ARGS(qdir));
01403     }
01404 
01405     status = bn_isect_line3_line3(&dist[0], &dist[1], p, pdir, q, qdir, tol);
01406 
01407     /* It is expected that dist[0] and dist[1] returned from
01408      * 'bn_isect_line3_line3' are the actual distance to the
01409      * intersect, i.e. not scaled. Distances in the opposite
01410      * of the line direction vector result in a negative distance.
01411      */
01412 
01413     /* sanity check */
01414     if (UNLIKELY(status < -2 || status > 1)) {
01415         bu_bomb("bn_isect_lseg3_lseg3() function 'bn_isect_line3_line3' returned an invalid status\n");
01416     }
01417 
01418     if (status == -1) {
01419         /* Infinite lines do not intersect and are not parallel
01420          * therefore line segments do not intersect and are not
01421          * parallel.
01422          */
01423         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01424             bu_log("bn_isect_lseg3_lseg3(): MISS, line segments do not intersect and are not parallel\n");
01425         }
01426         return -3; /* missed */
01427     }
01428 
01429     if (status == -2) {
01430         /* infinite lines do not intersect, they are parallel */
01431         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01432             bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are parallel, i.e. do not intersect\n");
01433         }
01434         return -2; /* missed (line segments are parallel) */
01435     }
01436 
01437     pmag = MAGNITUDE(pdir);
01438     qmag = MAGNITUDE(qdir);
01439 
01440     if (UNLIKELY(pmag < SMALL_FASTF)) {
01441         bu_bomb("bn_isect_lseg3_lseg3(): |p|=0\n");
01442     }
01443 
01444     if (UNLIKELY(qmag < SMALL_FASTF)) {
01445         bu_bomb("bn_isect_lseg3_lseg3(): |q|=0\n");
01446     }
01447 
01448     ptol = tol->dist / pmag;
01449     qtol = tol->dist / qmag;
01450     dist[0] = dist[0] / pmag;
01451     if (status == 0) {  /* infinite lines are colinear */
01452         /* When line segments are colinear, dist[1] has an alternate
01453          * interpretation: it's the parameter along p (not q)
01454          * therefore dist[1] must be scaled by pmag not qmag.
01455          */
01456         dist[1] = dist[1] / pmag;
01457     } else {
01458         dist[1] = dist[1] / qmag;
01459     }
01460 
01461     if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01462         bu_log("ptol=%g, qtol=%g\n", ptol, qtol);
01463     }
01464 
01465     /* If 'p' within tol of either endpoint (0.0, 1.0), make exact. */
01466     if (dist[0] > -ptol && dist[0] < ptol) {
01467         dist[0] = 0.0;
01468     } else if (dist[0] > 1.0-ptol && dist[0] < 1.0+ptol) {
01469         dist[0] = 1.0;
01470     }
01471 
01472     if (status == 0) {  /* infinite lines are colinear */
01473         /* When line segments are colinear, dist[1] has an alternate
01474          * interpretation: it's the parameter along p (not q)
01475          * therefore dist[1] must use tolerance ptol not qtol.
01476          * If 'q' within tol of either endpoint (0.0, 1.0), make exact.
01477          */
01478         if (dist[1] > -ptol && dist[1] < ptol) {
01479             dist[1] = 0.0;
01480         } else if (dist[1] > 1.0-ptol && dist[1] < 1.0+ptol) {
01481             dist[1] = 1.0;
01482         }
01483     } else {
01484         /* If 'q' within tol of either endpoint (0.0, 1.0), make exact. */
01485         if (dist[1] > -qtol && dist[1] < qtol) {
01486             dist[1] = 0.0;
01487         } else if (dist[1] > 1.0-qtol && dist[1] < 1.0+qtol) {
01488             dist[1] = 1.0;
01489         }
01490     }
01491 
01492     if (status == 0) {  /* infinite lines are colinear */
01493         /* Lines are colinear */
01494         if ((dist[0] > 1.0+ptol && dist[1] > 1.0+ptol) || (dist[0] < -ptol && dist[1] < -ptol)) {
01495             if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01496                 bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are colinear but not overlapping!\n");
01497             }
01498             return -1;   /* line segments are colinear but not overlapping */
01499         }
01500 
01501         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01502             bu_log("bn_isect_lseg3_lseg3(): HIT, line segments are colinear and overlapping!\n");
01503         }
01504 
01505         return 0; /* line segments are colinear and overlapping */
01506     }
01507 
01508     /* At this point we know the infinite lines intersect and are not colinear */
01509 
01510 
01511     if (dist[0] < -ptol || dist[0] > 1.0+ptol || dist[1] < -qtol || dist[1] > 1.0+qtol) {
01512         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01513             bu_log("bn_isect_lseg3_lseg3(): MISS, infinite lines intersect but line segments do not!\n");
01514         }
01515         return -3;  /* missed, infinite lines intersect but line segments do not */
01516     }
01517 
01518     if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
01519         bu_log("bn_isect_lseg3_lseg3(): HIT, line segments intersect!\n");
01520     }
01521 
01522     /* sanity check */
01523     if (UNLIKELY(dist[0] < -SMALL_FASTF || dist[0] > 1.0 || dist[1] < -SMALL_FASTF || dist[1] > 1.0)) {
01524         bu_bomb("bn_isect_lseg3_lseg3(): INTERNAL ERROR, intersect distance values must be in the range 0-1\n");
01525     }
01526 
01527     return 1; /* hit, line segments intersect */
01528 }
01529 
01530 
01531 /**
01532  * B N _ I S E C T _ L I N E 3 _ L I N E 3
01533  *
01534  * Intersect two line segments, each in given in parametric form:
01535  *
01536  * X = p0 + pdist * pdir_i   (i.e. line p0->p1)
01537  * and
01538  * X = q0 + qdist * qdir_i   (i.e. line q0->q1)
01539  *
01540  * The input vectors 'pdir_i' and 'qdir_i' must NOT be unit vectors
01541  * for this function to work correctly. The magnitude of the direction
01542  * vectors indicates line segment length.
01543  *
01544  * The 'pdist' and 'qdist' values returned from this function are the
01545  * actual distance to the intersect (i.e. not scaled). Distances may
01546  * be negative, see below.
01547  *
01548  * @return -2   no intersection, lines are parallel.
01549  * @return -1   no intersection
01550  * @return 0    lines are co-linear (pdist and qdist returned) (see below)
01551  * @return 1    intersection found  (pdist and qdist returned) (see below)
01552  *
01553  * @param       p0      point 1
01554  * @param       pdir_i  direction 1
01555  * @param       q0      point 2
01556  * @param       qdir_i  direction 2
01557  * @param tol   tolerance values
01558  * @param[out]  pdist, qdist (distances to intersection) (see below)
01559  *
01560  *              When return = 1, pdist is the distance along line p0->p1 to the
01561  *              intersect with line q0->q1. If the intersect is along p0->p1 but
01562  *              in the opposite direction of vector pdir_i (i.e. occuring before
01563  *              p0 on line p0->p1) then the distance will be negative. The value
01564  *              if qdist is the same as pdist except it is the distance along line
01565  *              q0->q1 to the intersect with line p0->p1.
01566  *
01567  *              When return code = 0 for co-linearity, pdist and qdist have a
01568  *              different meaning. pdist is the distance from point p0 to point q0
01569  *              and qdist is the distance from point p0 to point q1. If point q0
01570  *              occurs before point p0 on line segment p0->p1 then pdist will be
01571  *              negative. The same occurs for the distance to point q1.
01572  */
01573 int
01574 bn_isect_line3_line3(fastf_t *pdist,        /**< see above */
01575                      fastf_t *qdist,        /**< see above */
01576                      const fastf_t *p0,     /**< line p start point */
01577                      const fastf_t *pdir_i, /**< line p direction, must not be unit vector */
01578                      const fastf_t *q0,     /**< line q start point */
01579                      const fastf_t *qdir_i, /**< line q direction, must not be unit vector */
01580                      const struct bn_tol *tol)
01581 {
01582     fastf_t b, d, e, sc, tc, sc_numerator, tc_numerator, denominator;
01583     vect_t w0, qc_to_pc, u_scaled, v_scaled, v_scaled_to_u_scaled, tmp_vec, p0_to_q1;
01584     point_t p1, q1;
01585     fastf_t pdir_mag_sq;
01586     fastf_t qdir_mag_sq;
01587 
01588     int parallel = 0;
01589     int colinear = 0;
01590     fastf_t dot, d1, d2, d3, d4;
01591     vect_t pdir, qdir;
01592 
01593     VMOVE(pdir, pdir_i);
01594     VMOVE(qdir, qdir_i);
01595 
01596     pdir_mag_sq = MAGSQ(pdir);
01597     qdir_mag_sq = MAGSQ(qdir);
01598 
01599     if (UNLIKELY((pdir_mag_sq < tol->dist_sq) || (qdir_mag_sq < tol->dist_sq))) {
01600         bu_log("  p0 = %g %g %g\n", V3ARGS(p0));
01601         bu_log("pdir = %g %g %g\n", V3ARGS(pdir));
01602         bu_log("  q0 = %g %g %g\n", V3ARGS(q0));
01603         bu_log("qdir = %g %g %g\n", V3ARGS(qdir));
01604         bu_bomb("bn_isect_line3_line3(): input vector(s) 'pdir' and/or 'qdir' is zero magnitude.\n");
01605     }
01606 
01607     *pdist = 0.0;
01608     *qdist = 0.0;
01609 
01610     /* assumes pdir & qdir are not unit vectors */
01611     VADD2(p1, p0, pdir);
01612     VADD2(q1, q0, qdir);
01613 
01614     VSUB2(p0_to_q1, q1, p0);
01615 
01616     d1 = bn_distsq_line3_pt3(q0,qdir,p0);
01617     d2 = bn_distsq_line3_pt3(q0,qdir,p1);
01618     d3 = bn_distsq_line3_pt3(p0,pdir,q0);
01619     d4 = bn_distsq_line3_pt3(p0,pdir,q1);
01620 
01621     /* if all distances are within distance tolerance of each
01622      * other then they a parallel 
01623      */
01624     if (NEAR_EQUAL(d1, d2, tol->dist_sq) &&
01625         NEAR_EQUAL(d1, d3, tol->dist_sq) &&
01626         NEAR_EQUAL(d1, d4, tol->dist_sq) &&
01627         NEAR_EQUAL(d2, d3, tol->dist_sq) &&
01628         NEAR_EQUAL(d2, d4, tol->dist_sq) &&
01629         NEAR_EQUAL(d3, d4, tol->dist_sq)) {
01630         parallel = 1;
01631     }
01632 
01633     if (NEAR_ZERO(d1, tol->dist_sq) &&
01634         NEAR_ZERO(d2, tol->dist_sq) &&
01635         NEAR_ZERO(d3, tol->dist_sq) &&
01636         NEAR_ZERO(d4, tol->dist_sq)) {
01637         colinear = 1;
01638     }
01639 
01640     VSUB2(w0, p0, q0);
01641     b = VDOT(pdir, qdir);
01642     d = VDOT(pdir, w0);
01643     e = VDOT(qdir, w0);
01644     denominator = pdir_mag_sq * qdir_mag_sq - b * b;
01645 
01646     if (UNLIKELY(!parallel && colinear)) {
01647         bu_bomb("bn_isect_line3_line3(): logic error, lines colinear but not parallel\n");
01648     }
01649 
01650     if (parallel && !colinear) {
01651         /* lines are parallel */
01652         sc = d / pdir_mag_sq;
01653         tc = 0.0;
01654         return -2; /* no intersection, lines are parallel */
01655     }
01656 
01657     if (parallel && colinear) {
01658 
01659         /* when colinear pdist has a different meaning, it is the
01660          * distance from p0 to q0 
01661          */
01662         *pdist = MAGNITUDE(w0); /* w0 is opposite direction of p0 to q0 */
01663         dot = VDOT(pdir, w0);
01664         if (dot > SMALL_FASTF) {
01665             *pdist = -(*pdist);
01666         }
01667 
01668         /* when colinear qdist has a different meaning, it is the
01669          * distance from p0 to q1 
01670          */
01671         *qdist = MAGNITUDE(p0_to_q1);
01672 
01673         /* if vectors pdir and p0_to_q1 are not the same direction
01674          * then make the distance negative
01675          */
01676         dot = VDOT(pdir, p0_to_q1);
01677         if (dot < -SMALL_FASTF) {
01678             *qdist = -(*qdist);
01679         }
01680 
01681         return 0; /* colinear intersection */
01682     }
01683 
01684     sc_numerator = (b * e - qdir_mag_sq * d);
01685     tc_numerator = (pdir_mag_sq * e - b * d);
01686     sc = sc_numerator / denominator;
01687     tc = tc_numerator / denominator;
01688 
01689     VSCALE(u_scaled, pdir, sc_numerator);
01690     VSCALE(v_scaled, qdir, tc_numerator);
01691     VSUB2(v_scaled_to_u_scaled, u_scaled, v_scaled);
01692     VSCALE(tmp_vec, v_scaled_to_u_scaled, 1.0/denominator);
01693     VADD2(qc_to_pc, w0, tmp_vec);
01694 
01695     if (MAGSQ(qc_to_pc) <= tol->dist_sq) {
01696         *pdist = sc * sqrt(pdir_mag_sq);
01697         *qdist = tc * sqrt(qdir_mag_sq);
01698         return 1; /* intersection */
01699     } else {
01700         return -1; /* no intersection */
01701     }
01702 }
01703 
01704 
01705 /**
01706  * B N _ I S E C T _ L I N E _ L S E G
01707  *@brief
01708  * Intersect a line in parametric form:
01709  *
01710  * X = P + t * D
01711  *
01712  * with a line segment defined by two distinct points A and B.
01713  *
01714  *
01715  * @return -4   A and B are not distinct points
01716  * @return -3   Intersection exists, < A (t is returned)
01717  * @return -2   Intersection exists, > B (t is returned)
01718  * @return -1   Lines do not intersect
01719  * @return 0    Lines are co-linear (t for A is returned)
01720  * @return 1    Intersection at vertex A
01721  * @return 2    Intersection at vertex B
01722  * @return 3    Intersection between A and B
01723  *
01724  * @par Implicit Returns -
01725  *
01726  * t When explicit return >= 0, t is the parameter that describes the
01727  * intersection of the line and the line segment.  The actual
01728  * intersection coordinates can be found by solving P + t * D.
01729  * However, note that for return codes 1 and 2 (intersection exactly
01730  * at a vertex), it is strongly recommended that the original values
01731  * passed in A or B are used instead of solving P + t * D, to prevent
01732  * numeric error from creeping into the position of the endpoints.
01733  *
01734  * XXX should probably be called bn_isect_line3_lseg3()
01735  * XXX should probably be changed to return dist[2]
01736  */
01737 int
01738 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)
01739 {
01740     vect_t ab, pa, pb;          /* direction vectors a->b, p->a, p->b */
01741     fastf_t ab_mag;
01742     fastf_t pa_mag_sq;
01743     fastf_t pb_mag_sq;
01744     fastf_t d_mag_sq;
01745     int code;
01746     fastf_t dist1, dist2, d1, d2;
01747     int colinear = 0;
01748     fastf_t dot;
01749 
01750     BN_CK_TOL(tol);
01751 
01752     *t = 0.0;
01753 
01754     d_mag_sq = MAGSQ(d);
01755     if (UNLIKELY(NEAR_ZERO(d_mag_sq, tol->dist_sq))) {
01756         bu_bomb("bn_isect_line_lseg(): ray direction vector zero magnitude\n");
01757     }
01758 
01759     VSUB2(ab, b, a);
01760     ab_mag = MAGNITUDE(ab);
01761     if (ab_mag < tol->dist) {
01762         /* points A and B are not distinct */
01763         return -4;
01764     }
01765 
01766     VSUB2(pa, a, p);
01767     pa_mag_sq = MAGSQ(pa);
01768     if (pa_mag_sq < tol->dist_sq) {
01769         /* Intersection at vertex A */
01770         *t = sqrt(pa_mag_sq);
01771         return 1;
01772     }
01773 
01774     VSUB2(pb, b, p);
01775     pb_mag_sq = MAGSQ(pb);
01776     if (pb_mag_sq < tol->dist_sq) {
01777         /* Intersection at vertex B */
01778         *t = sqrt(pb_mag_sq);
01779         return 2;
01780     }
01781 
01782     /* just check that the vertices of the line segement are
01783      * within distance tolerance of the ray. it may cause problems
01784      * to also require the ray start and end points to be within
01785      * distance tolerance of the infinite line associated with
01786      * the line segement.
01787      */
01788     d1 = bn_distsq_line3_pt3(p,d,a); /* distance of point a to ray */
01789     d2 = bn_distsq_line3_pt3(p,d,b); /* distance of point b to ray */
01790 
01791     colinear = 0;
01792     if (NEAR_ZERO(d1, tol->dist_sq) && NEAR_ZERO(d2, tol->dist_sq)) {
01793         colinear = 1;
01794 
01795         dist1 = sqrt(pa_mag_sq);
01796         dist2 = sqrt(pb_mag_sq);
01797 
01798         /* if the direction of the pa vector is in the
01799          * opposite direction of the ray, then make the
01800          * distance negative
01801          */
01802         dot = VDOT(pa, d);
01803         if (dot < -SMALL_FASTF) {
01804             dist1 = -dist1;
01805         }
01806 
01807         /* if the direction of the pb vector is in the
01808          * opposite direction of the ray, then make the
01809          * distance negative
01810          */
01811         dot = VDOT(pb, d);
01812         if (dot < -SMALL_FASTF) {
01813             dist2 = -dist2;
01814         }
01815     }
01816 
01817     if (colinear && dist1 < SMALL_FASTF && dist2 < SMALL_FASTF) {
01818         /* lines are colinear but 'a' and 'b' are not on the ray */
01819         return -1; /* no intersection */
01820     }
01821 
01822     if (colinear && (dist1 > SMALL_FASTF) && (dist2 > SMALL_FASTF)) {
01823         /* lines are colinear and both points 'a' and 'b' are on the ray. */
01824         /* return the distance to the closest point */
01825         if (dist2 > dist1) {
01826             *t = dist1;
01827         } else {
01828             *t = dist2;
01829         }
01830         return 0;
01831     }
01832 
01833     if (colinear && (dist1 > SMALL_FASTF) && (dist2 < SMALL_FASTF)) {
01834         /* lines are colinear and 'a' is on the ray but 'b' is not. */
01835         /* return the distance to 'a' */
01836         *t = dist1;
01837         return 0;
01838     }
01839 
01840     if (colinear && (dist1 < SMALL_FASTF) && (dist2 > SMALL_FASTF)) {
01841         /* lines are colinear and 'b' is on the ray but 'a' is not. */
01842         /* return the distance to 'b' */
01843         *t = dist2;
01844         return 0;
01845     }
01846 
01847     dist1 = 0.0; /* sanity */
01848     dist2 = 0.0; /* sanity */
01849     code = bn_isect_line3_line3(&dist1, &dist2, p, d, a, ab, tol);
01850 
01851     if (UNLIKELY(code == 0)) {
01852         bu_bomb("bn_isect_line_lseg(): we should have already detected a colinear condition\n");
01853     }
01854 
01855     if (code < 0) {
01856         return -1; /* no intersection */
01857     }
01858 
01859     if (code == 1) {
01860         if (dist1 < -(tol->dist)) {
01861             /* the ray did isect the line segment but in the 
01862              * negative direction so this is not really a hit
01863              */
01864             return -1; /* no intersection */
01865         }
01866     }
01867 
01868     if (code == 1) {
01869         /* determine if isect was before a, between a & b or after b */
01870         vect_t d_unit;
01871         vect_t  a_to_isect_pt, b_to_isect_pt;
01872         point_t isect_pt;
01873         fastf_t a_to_isect_pt_mag_sq, b_to_isect_pt_mag_sq;
01874 
01875         VMOVE(d_unit, d);
01876         VUNITIZE(d_unit);
01877 
01878         dist1 = fabs(dist1); /* sanity */
01879         VSCALE(isect_pt, d_unit, dist1);
01880         VSUB2(a_to_isect_pt, isect_pt, a);
01881         VSUB2(b_to_isect_pt, isect_pt, b);
01882 
01883         a_to_isect_pt_mag_sq = MAGSQ(a_to_isect_pt);
01884         b_to_isect_pt_mag_sq = MAGSQ(b_to_isect_pt);
01885 
01886         *t = dist1;
01887 
01888         if (a_to_isect_pt_mag_sq < tol->dist_sq) {
01889             /* isect at point a of line segement */
01890             return 1;
01891         }
01892 
01893         if (b_to_isect_pt_mag_sq < tol->dist_sq) {
01894             /* isect at point b of line segement */
01895             return 2;
01896         }
01897 
01898         if (UNLIKELY((a_to_isect_pt_mag_sq < tol->dist_sq) && (b_to_isect_pt_mag_sq < tol->dist_sq))) {
01899             bu_bomb("bn_isect_line_lseg(): this case should already been caught. i.e. zero length line segment\n");
01900         }
01901 
01902         dot = VDOT(a_to_isect_pt, ab);
01903         if (dot < -SMALL_FASTF) {
01904             /* isect before point a of infinite line associated
01905              * with the line segment a->b
01906              */
01907             return -3;
01908         }
01909 
01910         dot = VDOT(b_to_isect_pt, ab);
01911         if (dot > SMALL_FASTF) {
01912             /* isect after point b of infinite line associated
01913              * with the line segment a->b
01914              */
01915             return -2;
01916         }
01917 
01918         return 3; /* isect on line segement a->b but
01919                    * not on the end points
01920                    */
01921     }
01922 
01923     bu_bomb("bn_isect_line_lseg(): logic error, should not be here\n");
01924 
01925     return 0;  /* quite compiler warning */
01926 }
01927 
01928 
01929 /**
01930  * B N _ D I S T _ L I N E 3_ P T 3
01931  *@brief
01932  * Given a parametric line defined by PT + t * DIR and a point A,
01933  * return the closest distance between the line and the point.
01934  *
01935  *  'dir' need not have unit length.
01936  *
01937  * Find parameter for PCA along line with unitized DIR:
01938  * d = VDOT(f, dir) / MAGNITUDE(dir);
01939  * Find distance g from PCA to A using Pythagoras:
01940  * g = sqrt(MAGSQ(f) - d**2)
01941  *
01942  * Return -
01943  * Distance
01944  */
01945 double
01946 bn_dist_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
01947 {
01948     vect_t f;
01949     register fastf_t FdotD;
01950 
01951     if ((FdotD = MAGNITUDE(dir)) <= SMALL_FASTF) {
01952         FdotD = 0.0;
01953         goto out;
01954     }
01955     VSUB2(f, a, pt);
01956     FdotD = VDOT(f, dir) / FdotD;
01957     FdotD = MAGSQ(f) - FdotD * FdotD;
01958     if (FdotD <= SMALL_FASTF) {
01959         FdotD = 0.0;
01960         goto out;
01961     }
01962     FdotD = sqrt(FdotD);
01963 out:
01964     if (bu_debug & BU_DEBUG_MATH) {
01965         bu_log("bn_dist_line3_pt3() ret=%g\n", FdotD);
01966     }
01967     return FdotD;
01968 }
01969 
01970 
01971 /**
01972  * B N _ D I S T S Q _ L I N E 3 _ P T 3
01973  *
01974  * Given a parametric line defined by PT + t * DIR and a point A,
01975  * return the square of the closest distance between the line and the
01976  * point.
01977  *
01978  * 'dir' need not have unit length.
01979  *
01980  * Return -
01981  * Distance squared
01982  */
01983 double
01984 bn_distsq_line3_pt3(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
01985 {
01986     vect_t f;
01987     register fastf_t FdotD;
01988 
01989     VSUB2(f, pt, a);
01990     FdotD = MAGNITUDE(dir);
01991     if (ZERO(FdotD)) {
01992         FdotD = 0.0;
01993         goto out;
01994     }
01995     FdotD = VDOT(f, dir) / FdotD;
01996     FdotD = VDOT(f, f) - FdotD * FdotD;
01997     if (FdotD < SMALL_FASTF) {
01998         FdotD = 0.0;
01999     }
02000 out:
02001     if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
02002         bu_log("bn_distsq_line3_pt3() ret=%g\n", FdotD);
02003     }
02004     return FdotD;
02005 }
02006 
02007 
02008 /**
02009  * B N _ D I S T _ L I N E _ O R I G I N
02010  *@brief
02011  * Given a parametric line defined by PT + t * DIR, return the closest
02012  * distance between the line and the origin.
02013  *
02014  * 'dir' need not have unit length.
02015  *
02016  * @return Distance
02017  */
02018 double
02019 bn_dist_line_origin(const fastf_t *pt, const fastf_t *dir)
02020 {
02021     register fastf_t PTdotD;
02022 
02023     if ((PTdotD = MAGNITUDE(dir)) <= SMALL_FASTF)
02024         return 0.0;
02025     PTdotD = VDOT(pt, dir) / PTdotD;
02026     if ((PTdotD = VDOT(pt, pt) - PTdotD * PTdotD) <= SMALL_FASTF)
02027         return 0.0;
02028     return sqrt(PTdotD);
02029 }
02030 
02031 
02032 /**
02033  * B N _ D I S T _ L I N E 2 _ P O I N T 2
02034  *@brief
02035  * Given a parametric line defined by PT + t * DIR and a point A,
02036  * return the closest distance between the line and the point.
02037  *
02038  * 'dir' need not have unit length.
02039  *
02040  * @return Distance
02041  */
02042 double
02043 bn_dist_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
02044 {
02045     vect_t f;
02046     register fastf_t FdotD;
02047 
02048     VSUB2_2D(f, pt, a);
02049     if ((FdotD = sqrt(MAGSQ_2D(dir))) <= SMALL_FASTF)
02050         return 0.0;
02051     FdotD = VDOT_2D(f, dir) / FdotD;
02052     if ((FdotD = VDOT_2D(f, f) - FdotD * FdotD) <= SMALL_FASTF)
02053         return 0.0;
02054     return sqrt(FdotD);
02055 }
02056 
02057 
02058 /**
02059  * B N _ D I S T S Q _ L I N E 2 _ P O I N T 2
02060  *@brief
02061  * Given a parametric line defined by PT + t * DIR and a point A,
02062  * return the closest distance between the line and the point,
02063  * squared.
02064  *
02065  * 'dir' need not have unit length.
02066  *
02067  * @return
02068  * Distance squared
02069  */
02070 double
02071 bn_distsq_line2_point2(const fastf_t *pt, const fastf_t *dir, const fastf_t *a)
02072 {
02073     vect_t f;
02074     register fastf_t FdotD;
02075 
02076     VSUB2_2D(f, pt, a);
02077     if ((FdotD = sqrt(MAGSQ_2D(dir))) <= SMALL_FASTF)
02078         return 0.0;
02079     FdotD = VDOT_2D(f, dir) / FdotD;
02080     if ((FdotD = VDOT_2D(f, f) - FdotD * FdotD) <= SMALL_FASTF)
02081         return 0.0;
02082     return FdotD;
02083 }
02084 
02085 
02086 /**
02087  * B N _ A R E A _ O F _ T R I A N G L E
02088  *@brief
02089  * Returns the area of a triangle. Algorithm by Jon Leech 3/24/89.
02090  */
02091 double
02092 bn_area_of_triangle(register const fastf_t *a, register const fastf_t *b, register const fastf_t *c)
02093 {
02094     register double t;
02095     register double area;
02096 
02097     t = a[Y] * (b[Z] - c[Z]) -
02098         b[Y] * (a[Z] - c[Z]) +
02099         c[Y] * (a[Z] - b[Z]);
02100     area  = t*t;
02101     t = a[Z] * (b[X] - c[X]) -
02102         b[Z] * (a[X] - c[X]) +
02103         c[Z] * (a[X] - b[X]);
02104     area += t*t;
02105     t =         a[X] * (b[Y] - c[Y]) -
02106         b[X] * (a[Y] - c[Y]) +
02107         c[X] * (a[Y] - b[Y]);
02108     area += t*t;
02109 
02110     return 0.5 * sqrt(area);
02111 }
02112 
02113 
02114 /**
02115  * B N _ I S E C T _ P T _ L S E G
02116  *@brief
02117  * Intersect a point P with the line segment defined by two distinct
02118  * points A and B.
02119  *
02120  * @return -2   P on line AB but outside range of AB,
02121  *                      dist = distance from A to P on line.
02122  * @return -1   P not on line of AB within tolerance
02123  * @return 1    P is at A
02124  * @return 2    P is at B
02125  * @return 3    P is on AB, dist = distance from A to P on line.
02126  @verbatim
02127  B *
02128  |
02129  P'*-tol-*P
02130  |    /   _
02131  dist  /   /|
02132  |  /   /
02133  | /   / AtoP
02134  |/   /
02135  A *   /
02136 
02137  tol = distance limit from line to pt P;
02138  dist = parametric distance from A to P' (in terms of A to B)
02139  @endverbatim
02140  *
02141  * @param p     point
02142  * @param a     start of lseg
02143  * @param b     end of lseg
02144  * @param tol   tolerance values
02145  * @param[out] dist     parametric distance from A to P' (in terms of A to B)
02146  */
02147 int bn_isect_pt_lseg(fastf_t *dist,
02148                      const fastf_t *a,
02149                      const fastf_t *b,
02150                      const fastf_t *p,
02151                      const struct bn_tol *tol)
02152 /* distance along line from A to P */
02153 /* points for line and intersect */
02154 
02155 {
02156     vect_t AtoP, BtoP, AtoB,
02157         ABunit; /* unit vector from A to B */
02158     fastf_t APprABunit; /* Mag of projection of AtoP onto ABunit */
02159     fastf_t distsq;
02160 
02161     BN_CK_TOL(tol);
02162 
02163     VSUB2(AtoP, p, a);
02164     if (MAGSQ(AtoP) < tol->dist_sq)
02165         return 1;       /* P at A */
02166 
02167     VSUB2(BtoP, p, b);
02168     if (MAGSQ(BtoP) < tol->dist_sq)
02169         return 2;       /* P at B */
02170 
02171     VSUB2(AtoB, b, a);
02172     VMOVE(ABunit, AtoB);
02173     distsq = MAGSQ(ABunit);
02174     if (distsq < tol->dist_sq)
02175         return -1;      /* A equals B, and P isn't there */
02176     distsq = 1/sqrt(distsq);
02177     VSCALE(ABunit, ABunit, distsq);
02178 
02179     /* Similar to bn_dist_line_pt, except we never actually have to do
02180      * the sqrt that the other routine does.
02181      */
02182 
02183     /* find dist as a function of ABunit, actually the projection of
02184      * AtoP onto ABunit
02185      */
02186     APprABunit = VDOT(AtoP, ABunit);
02187 
02188     /* because of pythgorean theorem ... */
02189     distsq = MAGSQ(AtoP) - APprABunit * APprABunit;
02190     if (distsq > tol->dist_sq)
02191         return -1;      /* dist pt to line too large */
02192 
02193     /* Distance from the point to the line is within tolerance. */
02194     *dist = VDOT(AtoP, AtoB) / MAGSQ(AtoB);
02195 
02196     if (*dist > 1.0 || *dist < -SMALL_FASTF)    /* P outside AtoB */
02197         return -2;
02198 
02199     return 3;   /* P on AtoB */
02200 }
02201 
02202 
02203 /**
02204  * B N _ I S E C T _ P T 2 _ L S E G 2
02205  * @brief
02206  * Intersect a point P with the line segment defined by two distinct
02207  * points A and B.
02208  *
02209  * @return -2   P on line AB but outside range of AB,
02210  *                      dist = distance from A to P on line.
02211  * @return -1   P not on line of AB within tolerance
02212  * @return 1    P is at A
02213  * @return 2    P is at B
02214  * @return 3    P is on AB, dist = distance from A to P on line.
02215  @verbatim
02216  B *
02217  |
02218  P'*-tol-*P
02219  |    /  _
02220  dist  /   /|
02221  |  /   /
02222  | /   / AtoP
02223  |/   /
02224  A *   /
02225 
02226  tol = distance limit from line to pt P;
02227  dist = distance from A to P'
02228  @endverbatim
02229 */
02230 int
02231 bn_isect_pt2_lseg2(fastf_t *dist, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol)
02232 /* distance along line from A to P */
02233 /* points for line and intersect */
02234 
02235 {
02236     vect_t AtoP,
02237         BtoP,
02238         AtoB,
02239         ABunit; /* unit vector from A to B */
02240     fastf_t APprABunit; /* Mag of projection of AtoP onto ABunit */
02241     fastf_t distsq;
02242 
02243     BN_CK_TOL(tol);
02244 
02245     VSUB2_2D(AtoP, p, a);
02246     if (MAGSQ_2D(AtoP) < tol->dist_sq)
02247         return 1;       /* P at A */
02248 
02249     VSUB2_2D(BtoP, p, b);
02250     if (MAGSQ_2D(BtoP) < tol->dist_sq)
02251         return 2;       /* P at B */
02252 
02253     VSUB2_2D(AtoB, b, a);
02254     VMOVE_2D(ABunit, AtoB);
02255     distsq = MAGSQ_2D(ABunit);
02256     if (distsq < tol->dist_sq) {
02257         if (bu_debug & BU_DEBUG_MATH) {
02258             bu_log("distsq A=%g\n", distsq);
02259         }
02260         return -1;      /* A equals B, and P isn't there */
02261     }
02262     distsq = 1/sqrt(distsq);
02263     VSCALE_2D(ABunit, ABunit, distsq);
02264 
02265     /* Similar to bn_dist_line_pt, except we never actually have to do
02266      * the sqrt that the other routine does.
02267      */
02268 
02269     /* find dist as a function of ABunit, actually the projection of
02270      * AtoP onto ABunit
02271      */
02272     APprABunit = VDOT_2D(AtoP, ABunit);
02273 
02274     /* because of pythgorean theorem ... */
02275     distsq = MAGSQ_2D(AtoP) - APprABunit * APprABunit;
02276     if (distsq > tol->dist_sq) {
02277         if (bu_debug & BU_DEBUG_MATH) {
02278             V2PRINT("ABunit", ABunit);
02279             bu_log("distsq B=%g\n", distsq);
02280         }
02281         return -1;      /* dist pt to line too large */
02282     }
02283 
02284     /* Distance from the point to the line is within tolerance. */
02285     *dist = VDOT_2D(AtoP, AtoB) / MAGSQ_2D(AtoB);
02286 
02287     if (*dist > 1.0 || *dist < 0.0)     /* P outside AtoB */
02288         return -2;
02289 
02290     return 3;   /* P on AtoB */
02291 }
02292 
02293 
02294 /**
02295  * B N _ D I S T _ P T 3 _ L S E G 3
02296  *@brief
02297  * Find the distance from a point P to a line segment described by the
02298  * two endpoints A and B, and the point of closest approach (PCA).
02299  @verbatim
02300  *
02301  *                      P
02302  *                     *
02303  *                    /.
02304  *                   / .
02305  *                  /  .
02306  *                 /   . (dist)
02307  *                /    .
02308  *               /     .
02309  *              *------*--------*
02310  *              A      PCA      B
02311  @endverbatim
02312  *
02313  * @return 0    P is within tolerance of lseg AB.  *dist isn't 0: (SPECIAL!!!)
02314  *                *dist = parametric dist = |PCA-A| / |B-A|.  pca=computed.
02315  * @return 1    P is within tolerance of point A.  *dist = 0, pca=A.
02316  * @return 2    P is within tolerance of point B.  *dist = 0, pca=B.
02317  * @return 3    P is to the "left" of point A.  *dist=|P-A|, pca=A.
02318  * @return 4    P is to the "right" of point B.  *dist=|P-B|, pca=B.
02319  * @return 5    P is "above/below" lseg AB.  *dist=|PCA-P|, pca=computed.
02320  *
02321  * This routine was formerly called bn_dist_pt_lseg().
02322  *
02323  * XXX For efficiency, a version of this routine that provides the
02324  * XXX distance squared would be faster.
02325  */
02326 int
02327 bn_dist_pt3_lseg3(fastf_t *dist,
02328                   fastf_t *pca,
02329                   const fastf_t *a,
02330                   const fastf_t *b,
02331                   const fastf_t *p,
02332                   const struct bn_tol *tol)
02333 {
02334     vect_t PtoA;                /* P-A */
02335     vect_t PtoB;                /* P-B */
02336     vect_t AtoB;                /* B-A */
02337     fastf_t P_A_sq;             /* |P-A|**2 */
02338     fastf_t P_B_sq;             /* |P-B|**2 */
02339     fastf_t B_A;                /* |B-A| */
02340     fastf_t t;          /* distance along ray of projection of P */
02341 
02342     BN_CK_TOL(tol);
02343 
02344     if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
02345         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",
02346                V3ARGS(a),
02347                V3ARGS(b),
02348                V3ARGS(p),
02349                tol->dist, tol->dist_sq);
02350     }
02351 
02352     /* Check proximity to endpoint A */
02353     VSUB2(PtoA, p, a);
02354     if ((P_A_sq = MAGSQ(PtoA)) < tol->dist_sq) {
02355         /* P is within the tol->dist radius circle around A */
02356         VMOVE(pca, a);
02357         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log("  at A\n");
02358         *dist = 0.0;
02359         return 1;
02360     }
02361 
02362     /* Check proximity to endpoint B */
02363     VSUB2(PtoB, p, b);
02364     if ((P_B_sq = MAGSQ(PtoB)) < tol->dist_sq) {
02365         /* P is within the tol->dist radius circle around B */
02366         VMOVE(pca, b);
02367         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log("  at B\n");
02368         *dist = 0.0;
02369         return 2;
02370     }
02371 
02372     VSUB2(AtoB, b, a);
02373     B_A = sqrt(MAGSQ(AtoB));
02374 
02375     /* compute distance (in actual units) along line to PROJECTION of
02376      * point p onto the line: point pca
02377      */
02378     t = VDOT(PtoA, AtoB) / B_A;
02379     if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) {
02380         bu_log("bn_dist_pt3_lseg3() B_A=%g, t=%g\n",
02381                B_A, t);
02382     }
02383 
02384     if (t <= SMALL_FASTF) {
02385         /* P is "left" of A */
02386         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log("  left of A\n");
02387         VMOVE(pca, a);
02388         *dist = sqrt(P_A_sq);
02389         return 3;
02390     }
02391     if (t < B_A) {
02392         /* PCA falls between A and B */
02393         register fastf_t dsq;
02394         fastf_t param_dist;     /* parametric dist */
02395 
02396         /* Find PCA */
02397         param_dist = t / B_A;           /* Range 0..1 */
02398         VJOIN1(pca, a, param_dist, AtoB);
02399 
02400         /* Find distance from PCA to line segment (Pythagorus) */
02401         if ((dsq = P_A_sq - t * t) <= tol->dist_sq) {
02402             if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log("  ON lseg\n");
02403             /* Distance from PCA to lseg is zero, give param instead */
02404             *dist = param_dist; /* special! */
02405             return 0;
02406         }
02407         if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log("  closest to lseg\n");
02408         *dist = sqrt(dsq);
02409         return 5;
02410     }
02411     /* P is "right" of B */
02412     if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log("  right of B\n");
02413     VMOVE(pca, b);
02414     *dist = sqrt(P_B_sq);
02415     return 4;
02416 }
02417 
02418 
02419 /**
02420  * B N _ D I S T _ P T 2 _ L S E G 2
02421  *@brief
02422  * Find the distance from a point P to a line segment described by the
02423  * two endpoints A and B, and the point of closest approach (PCA).
02424  @verbatim
02425  *                      P
02426  *                     *
02427  *                    /.
02428  *                   / .
02429  *                  /  .
02430  *                 /   . (dist)
02431  *                /    .
02432  *               /     .
02433  *              *------*--------*
02434  *              A      PCA      B
02435  @endverbatim
02436  * There are six distinct cases, with these return codes -
02437  * @return 0    P is within tolerance of lseg AB.  *dist isn't 0: (SPECIAL!!!)
02438  *                *dist = parametric dist = |PCA-A| / |B-A|.  pca=computed.
02439  * @return 1    P is within tolerance of point A.  *dist = 0, pca=A.
02440  * @return 2    P is within tolerance of point B.  *dist = 0, pca=B.
02441  * @return 3    P is to the "left" of point A.  *dist=|P-A|**2, pca=A.
02442  * @return 4    P is to the "right" of point B.  *dist=|P-B|**2, pca=B.
02443  * @return 5    P is "above/below" lseg AB.  *dist=|PCA-P|**2, pca=computed.
02444  *
02445  *
02446  * Patterned after bn_dist_pt3_lseg3().
02447  */
02448 int
02449 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)
02450 {
02451     vect_t PtoA;                /* P-A */
02452     vect_t PtoB;                /* P-B */
02453     vect_t AtoB;                /* B-A */
02454     fastf_t P_A_sq;             /* |P-A|**2 */
02455     fastf_t P_B_sq;             /* |P-B|**2 */
02456     fastf_t B_A;                /* |B-A| */
02457     fastf_t t;          /* distance along ray of projection of P */
02458 
02459     BN_CK_TOL(tol);
02460 
02461     if (bu_debug & BU_DEBUG_MATH) {
02462         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",
02463                V3ARGS(a),
02464                V3ARGS(b),
02465                V3ARGS(p),
02466                tol->dist, tol->dist_sq);
02467     }
02468 
02469 
02470     /* Check proximity to endpoint A */
02471     VSUB2_2D(PtoA, p, a);
02472     if ((P_A_sq = MAGSQ_2D(PtoA)) < tol->dist_sq) {
02473         /* P is within the tol->dist radius circle around A */
02474         V2MOVE(pca, a);
02475         if (bu_debug & BU_DEBUG_MATH) bu_log("  at A\n");
02476         *dist_sq = 0.0;
02477         return 1;
02478     }
02479 
02480     /* Check proximity to endpoint B */
02481     VSUB2_2D(PtoB, p, b);
02482     if ((P_B_sq = MAGSQ_2D(PtoB)) < tol->dist_sq) {
02483         /* P is within the tol->dist radius circle around B */
02484         V2MOVE(pca, b);
02485         if (bu_debug & BU_DEBUG_MATH) bu_log("  at B\n");
02486         *dist_sq = 0.0;
02487         return 2;
02488     }
02489 
02490     VSUB2_2D(AtoB, b, a);
02491     B_A = sqrt(MAGSQ_2D(AtoB));
02492 
02493     /* compute distance (in actual units) along line to PROJECTION of
02494      * point p onto the line: point pca
02495      */
02496     t = VDOT_2D(PtoA, AtoB) / B_A;
02497     if (bu_debug & BU_DEBUG_MATH) {
02498         bu_log("bn_dist_pt3_lseg3() B_A=%g, t=%g\n",
02499                B_A, t);
02500     }
02501 
02502     if (t <= 0) {
02503         /* P is "left" of A */
02504         if (bu_debug & BU_DEBUG_MATH) bu_log("  left of A\n");
02505         V2MOVE(pca, a);
02506         *dist_sq = P_A_sq;
02507         return 3;
02508     }
02509     if (t < B_A) {
02510         /* PCA falls between A and B */
02511         register fastf_t dsq;
02512         fastf_t param_dist;     /* parametric dist */
02513 
02514         /* Find PCA */
02515         param_dist = t / B_A;           /* Range 0..1 */
02516         V2JOIN1(pca, a, param_dist, AtoB);
02517 
02518         /* Find distance from PCA to line segment (Pythagorus) */
02519         if ((dsq = P_A_sq - t * t) <= tol->dist_sq) {
02520             if (bu_debug & BU_DEBUG_MATH) bu_log("  ON lseg\n");
02521             /* Distance from PCA to lseg is zero, give param instead */
02522             *dist_sq = param_dist;      /* special! Not squared. */
02523             return 0;
02524         }
02525         if (bu_debug & BU_DEBUG_MATH) bu_log("  closest to lseg\n");
02526         *dist_sq = dsq;
02527         return 5;
02528     }
02529     /* P is "right" of B */
02530     if (bu_debug & BU_DEBUG_MATH) bu_log("  right of B\n");
02531     V2MOVE(pca, b);
02532     *dist_sq = P_B_sq;
02533     return 4;
02534 }
02535 
02536 
02537 /**
02538  * B N _ R O T A T E _ B B O X
02539  *@brief
02540  * Transform a bounding box (RPP) by the given 4x4 matrix.  There are
02541  * 8 corners to the bounding RPP.  Each one needs to be transformed
02542  * and min/max'ed.  This is not minimal, but does fully contain any
02543  * internal object, using an axis-aligned RPP.
02544  */
02545 void
02546 bn_rotate_bbox(fastf_t *omin, fastf_t *omax, const fastf_t *mat, const fastf_t *imin, const fastf_t *imax)
02547 {
02548     point_t local;              /* vertex point in local coordinates */
02549     point_t model;              /* vertex point in model coordinates */
02550 
02551 #define ROT_VERT(a, b, c) {                     \
02552         VSET(local, a[X], b[Y], c[Z]);          \
02553         MAT4X3PNT(model, mat, local);           \
02554         VMINMAX(omin, omax, model);             \
02555     }
02556 
02557     ROT_VERT(imin, imin, imin);
02558     ROT_VERT(imin, imin, imax);
02559     ROT_VERT(imin, imax, imin);
02560     ROT_VERT(imin, imax, imax);
02561     ROT_VERT(imax, imin, imin);
02562     ROT_VERT(imax, imin, imax);
02563     ROT_VERT(imax, imax, imin);
02564     ROT_VERT(imax, imax, imax);
02565 #undef ROT_VERT
02566 }
02567 
02568 
02569 /**
02570  * B N _ R O T A T E _ P L A N E
02571  *@brief
02572  * Transform a plane equation by the given 4x4 matrix.
02573  */
02574 void
02575 bn_rotate_plane(fastf_t *oplane, const fastf_t *mat, const fastf_t *iplane)
02576 {
02577     point_t orig_pt;
02578     point_t new_pt;
02579 
02580     /* First, pick a point that lies on the original halfspace */
02581     VSCALE(orig_pt, iplane, iplane[3]);
02582 
02583     /* Transform the surface normal */
02584     MAT4X3VEC(oplane, mat, iplane);
02585 
02586     /* Transform the point from original to new halfspace */
02587     MAT4X3PNT(new_pt, mat, orig_pt);
02588 
02589     /*
02590      * The transformed normal is all that is required.
02591      * The new distance is found from the transformed point on the plane.
02592      */
02593     oplane[3] = VDOT(new_pt, oplane);
02594 }
02595 
02596 
02597 /**
02598  * B N _ C O P L A N A R
02599  *@brief
02600  * Test if two planes are identical.  If so, their dot products will
02601  * be either +1 or -1, with the distance from the origin equal in
02602  * magnitude.
02603  *
02604  * @return -1   not coplanar, parallel but distinct
02605  * @return 0    not coplanar, not parallel.  Planes intersect.
02606  * @return 1    coplanar, same normal direction
02607  * @return 2    coplanar, opposite normal direction
02608  */
02609 int
02610 bn_coplanar(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol)
02611 {
02612     register fastf_t dot;
02613     vect_t pt_a, pt_b;
02614     BN_CK_TOL(tol);
02615 
02616     if (!NEAR_EQUAL(MAGSQ(a), 1.0, VUNITIZE_TOL) || !NEAR_EQUAL(MAGSQ(b), 1.0, VUNITIZE_TOL)) {
02617         bu_bomb("bn_coplanar(): input vector(s) 'a' and/or 'b' is not a unit vector.\n");
02618     }
02619 
02620     VSCALE(pt_a, a, fabs(a[W]));
02621     VSCALE(pt_b, b, fabs(b[W]));
02622     dot = VDOT(a, b);
02623 
02624     if (NEAR_ZERO(dot, tol->perp)) {
02625         return 0; /* planes are perpendicular */
02626     }
02627 
02628     /* parallel is when dot is within tol->perp of either -1 or 1 */
02629     if ((dot <= -SMALL_FASTF) ? (NEAR_EQUAL(dot, -1.0, tol->perp)) : (NEAR_EQUAL(dot, 1.0, tol->perp))) {
02630         if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) {
02631             /* true when planes are coplanar */
02632             if (dot >= SMALL_FASTF) {
02633                 /* true when plane normals in same direction */
02634                 return 1;
02635             } else {
02636                 /* true when plane normals in opposite direction */
02637                 return 2;
02638             }
02639         } else {
02640             return -1;
02641         }
02642     }
02643     return 0;
02644 }
02645 
02646 
02647 /**
02648  * B N _ A N G L E _ M E A S U R E
02649  *
02650  * Using two perpendicular vectors (x_dir and y_dir) which lie in the
02651  * same plane as 'vec', return the angle (in radians) of 'vec' from
02652  * x_dir, going CCW around the perpendicular x_dir CROSS y_dir.
02653  *
02654  * Trig note -
02655  *
02656  * theta = atan2(x, y) returns an angle in the range -pi to +pi.
02657  * Here, we need an angle in the range of 0 to 2pi.  This could be
02658  * implemented by adding 2pi to theta when theta is negative, but this
02659  * could have nasty numeric ambiguity right in the vicinity of theta =
02660  * +pi, which is a very critical angle for the applications using this
02661  * routine.
02662  *
02663  * So, an alternative formulation is to compute gamma = atan2(-x, -y),
02664  * and then theta = gamma + pi.  Now, any error will occur in the
02665  * vicinity of theta = 0, which can be handled much more readily.
02666  *
02667  * If theta is negative, or greater than two pi, wrap it around.
02668  * These conditions only occur if there are problems in atan2().
02669  *
02670  * @return vec == x_dir returns 0,
02671  * @return vec == y_dir returns pi/2,
02672  * @return vec == -x_dir returns pi,
02673  * @return vec == -y_dir returns 3*pi/2.
02674  *
02675  * In all cases, the returned value is between 0 and bn_twopi.
02676  */
02677 double
02678 bn_angle_measure(fastf_t *vec, const fastf_t *x_dir, const fastf_t *y_dir)
02679 {
02680     fastf_t xproj, yproj;
02681     fastf_t gam;
02682     fastf_t ang;
02683 
02684     xproj = -VDOT(vec, x_dir);
02685     yproj = -VDOT(vec, y_dir);
02686     gam = atan2(yproj, xproj);  /* -pi..+pi */
02687     ang = bn_pi + gam;          /* 0..+2pi */
02688     if (ang < -SMALL_FASTF) {
02689         do {
02690             ang += bn_twopi;
02691         } while (ang < -SMALL_FASTF);
02692     } else if (ang > bn_twopi) {
02693         do {
02694             ang -= bn_twopi;
02695         } while (ang > bn_twopi);
02696     }
02697     if (UNLIKELY(ang < -SMALL_FASTF || ang > bn_twopi))
02698         bu_bomb("bn_angle_measure() angle out of range\n");
02699 
02700     return ang;
02701 }
02702 
02703 
02704 /**
02705  * B N _ D I S T _ P T 3 _ A L O N G _ L I N E 3
02706  *@brief
02707  * Return the parametric distance t of a point X along a line defined
02708  * as a ray, i.e. solve X = P + t * D.  If the point X does not lie on
02709  * the line, then t is the distance of the perpendicular projection of
02710  * point X onto the line.
02711  */
02712 double
02713 bn_dist_pt3_along_line3(const fastf_t *p, const fastf_t *d, const fastf_t *x)
02714 {
02715     vect_t x_p;
02716 
02717     VSUB2(x_p, x, p);
02718     return VDOT(x_p, d);
02719 }
02720 
02721 
02722 /**
02723  * B N _ D I S T _ P T 2 _ A L O N G _ L I N E 2
02724  *@brief
02725  * Return the parametric distance t of a point X along a line defined
02726  * as a ray, i.e. solve X = P + t * D.  If the point X does not lie on
02727  * the line, then t is the distance of the perpendicular projection of
02728  * point X onto the line.
02729  */
02730 double
02731 bn_dist_pt2_along_line2(const fastf_t *p, const fastf_t *d, const fastf_t *x)
02732 {
02733     vect_t x_p;
02734     double ret;
02735 
02736     VSUB2_2D(x_p, x, p);
02737     ret = VDOT_2D(x_p, d);
02738     if (bu_debug & BU_DEBUG_MATH) {
02739         bu_log("bn_dist_pt2_along_line2() p=(%g, %g), d=(%g, %g), x=(%g, %g) ret=%g\n",
02740                V2ARGS(p),
02741                V2ARGS(d),
02742                V2ARGS(x),
02743                ret);
02744     }
02745     return ret;
02746 }
02747 
02748 
02749 /**
02750  *
02751  * @return 1    if left <= mid <= right
02752  * @return 0    if mid is not in the range.
02753  */
02754 int
02755 bn_between(double left, double mid, double right, const struct bn_tol *tol)
02756 {
02757     BN_CK_TOL(tol);
02758 
02759     if (left < right) {
02760         if (NEAR_EQUAL(left, right, tol->dist*0.1)) {
02761             left -= tol->dist*0.1;
02762             right += tol->dist*0.1;
02763         }
02764         if (mid < left || mid > right) goto fail;
02765         return 1;
02766     }
02767     /* The 'right' value is lowest */
02768     if (NEAR_EQUAL(left, right, tol->dist*0.1)) {
02769         right -= tol->dist*0.1;
02770         left += tol->dist*0.1;
02771     }
02772     if (mid < right || mid > left) goto fail;
02773     return 1;
02774 fail:
02775     if (bu_debug & BU_DEBUG_MATH) {
02776         bu_log("bn_between(%.17e, %.17e, %.17e) ret=0 FAIL\n",
02777                left, mid, right);
02778     }
02779     return 0;
02780 }
02781 
02782 
02783 /**
02784  * B N _ D O E S _ R A Y _ I S E C T _ T R I
02785  *
02786  * @return 0    No intersection
02787  * @return 1    Intersection, 'inter' has intersect point.
02788  */
02789 int
02790 bn_does_ray_isect_tri(
02791     const point_t pt,
02792     const vect_t dir,
02793     const point_t V,
02794     const point_t A,
02795     const point_t B,
02796     point_t inter)                      /* output variable */
02797 {
02798     vect_t VP, VA, VB, AB, AP, N;
02799     fastf_t NdotDir;
02800     plane_t pl;
02801     fastf_t dist;
02802 
02803     /* insersect with plane */
02804 
02805     VSUB2(VA, A, V);
02806     VSUB2(VB, B, V);
02807     VCROSS(pl, VA, VB);
02808     VUNITIZE(pl);
02809 
02810     NdotDir = VDOT(pl, dir);
02811     if (ZERO(NdotDir))
02812         return 0;
02813 
02814     pl[W] = VDOT(pl, V);
02815 
02816     dist = (pl[W] - VDOT(pl, pt))/NdotDir;
02817     VJOIN1(inter, pt, dist, dir);
02818 
02819     /* determine if point is within triangle */
02820     VSUB2(VP, inter, V);
02821     VCROSS(N, VA, VP);
02822     if (VDOT(N, pl) < 0.0)
02823         return 0;
02824 
02825     VCROSS(N, VP, VB);
02826     if (VDOT(N, pl) < 0.0)
02827         return 0;
02828 
02829     VSUB2(AB, B, A);
02830     VSUB2(AP, inter, A);
02831     VCROSS(N, AB, AP);
02832     if (VDOT(N, pl) < 0.0)
02833         return 0;
02834 
02835     return 1;
02836 }
02837 
02838 
02839 #if 0
02840 /*
02841  * B N _ I S E C T _ R A Y _ T R I
02842  *
02843  * Intersect a infinite ray with a triangle specified by 3 points.
02844  * From: "Graphics Gems" ed Andrew S. Glassner:
02845  *              "An Efficient Ray-Polygon Intersection"  P 390
02846  *
02847  *            o A
02848  *            |\
02849  *            | \
02850  *            |  \
02851  *            |   \
02852  *         _  |  o \
02853  *   alpha|   |  P  \
02854  *        |   |      \
02855  *        |_  o-------o
02856  *             V       B
02857  *
02858  *            |__|
02859  *              beta
02860  *
02861  * The intersection point P is computed by determining 2 quantities,
02862  * (alpha, beta) which indicate the parametric distance along VA and
02863  * VB respectively.  The intersection point is thus:
02864  *
02865  * P = V + (alpha * VA) + (beta * VB)
02866  *
02867  * Return:
02868  *      0       Miss
02869  *      1       Hit, incoming
02870  *      -1      Hit, outgoing
02871  *
02872  * If Hit, the following parameters are also set:
02873  *      dist    parametric distance to the triangle intercept.
02874  *      N       If non-null, surface normal of triangle
02875  *      Alpha   If non-null, parametric distance along VA
02876  *      Beta    If non-null, parametric distance along VB
02877 
02878  * The parameters Alpha and Beta may be null, otherwise they will be
02879  * set.
02880  */
02881 int
02882 bn_isect_ray_tri(dist_p, N_p, Alpha_p, Beta_p, pt, dir, V, A, B)
02883     fastf_t *dist_p;
02884     fastf_t *N_p;
02885     fastf_t *Alpha_p;
02886     fastf_t *Beta_p;
02887     const point_t pt;
02888     const vect_t dir;
02889     const point_t V;
02890     const point_t A;
02891     const point_t B;
02892 {
02893     vect_t VA;  /* V -> A vector */
02894     vect_t VB;  /* V -> B vector */
02895     vect_t pt_V;        /* Ray_origin -> V vector */
02896     vect_t N;   /* Triangle Normal */
02897     vect_t VPP; /* perpendicular to vector V -> P */
02898     fastf_t alpha;      /* parametric distance along VA */
02899     fastf_t beta;       /* parametric distance along VB */
02900     fastf_t NdotDir;
02901     fastf_t entleave;
02902     fastf_t k;
02903 
02904     /* form edge vectors of triangle */
02905     VSUB2(VB, B, V);
02906     VSUB2(VA, A, V);
02907 
02908     /* form triangle normal */
02909     VCROSS(N, VA, VB);
02910 
02911     NdotDir = VDOT(N, dir);
02912     if (fabs(NdotDir) < SQRT_SMALL_FASTF)
02913         return 0;       /* ray parallel to triangle */
02914 
02915     if (NdotDir >= 0.0) entleave = -1;  /* leaving */
02916     else entleave = 1;                  /* entering */
02917 
02918     VSUB2(pt_V, V, pt);
02919     VCROSS(VPP, pt_V, dir);
02920 
02921     /* alpha is projection of VPP onto VA (not necessaily in plane)
02922      * If alpha < 0.0 then p is "before" point V on line V->A
02923      * No-one can figure out why alpha > NdotDir is important.
02924      */
02925     alpha = VDOT(VA, VPP) * entleave;
02926     if (alpha < 0.0 || alpha > fabs(NdotDir)) return 0;
02927 
02928 
02929     /* beta is projection of VPP onto VB (not necessaily in plane) */
02930     beta = VDOT(VB, VPP) * (-1 * entleave);
02931     if (beta < 0.0 || beta > fabs(NdotDir)) return 0;
02932 
02933     k = VDOT(VPP, N) / NdotDir;
02934 
02935     if (dist_p) *dist_p = k;
02936     if (N_p) {
02937         VUNITIZE(N);
02938         VMOVE(N_p, N);
02939     }
02940     if (Alpha_p) *Alpha_p = alpha;
02941     if (Beta_p) *Beta_p = beta;
02942 
02943     return entleave;
02944 }
02945 #endif
02946 
02947 /**
02948  * B N _ H L F _ C L A S S
02949  *@brief
02950  * Classify a halfspace, specified by its plane equation, against a
02951  * bounding RPP.
02952  *
02953  * @return BN_CLASSIFY_INSIDE
02954  * @return BN_CLASSIFY_OVERLAPPING
02955  * @return BN_CLASSIFY_OUTSIDE
02956  */
02957 int
02958 bn_hlf_class(const fastf_t *half_eqn, const fastf_t *min, const fastf_t *max, const struct bn_tol *tol)
02959 {
02960     int class;  /* current classification */
02961     fastf_t d;
02962 
02963 #define CHECK_PT(x, y, z)                                               \
02964     d = (x)*half_eqn[0] + (y)*half_eqn[1] + (z)*half_eqn[2] - half_eqn[3]; \
02965     if (d < -tol->dist) {                                               \
02966         if (class == BN_CLASSIFY_OUTSIDE)                               \
02967             return BN_CLASSIFY_OVERLAPPING;                             \
02968         else class = BN_CLASSIFY_INSIDE;                                \
02969     } else if (d > tol->dist) {                                         \
02970         if (class == BN_CLASSIFY_INSIDE)                                \
02971             return BN_CLASSIFY_OVERLAPPING;                             \
02972         else class = BN_CLASSIFY_OUTSIDE;                               \
02973     } else return BN_CLASSIFY_OVERLAPPING
02974 
02975     class = BN_CLASSIFY_UNIMPLEMENTED;
02976     CHECK_PT(min[X], min[Y], min[Z]);
02977     CHECK_PT(min[X], min[Y], max[Z]);
02978     CHECK_PT(min[X], max[Y], min[Z]);
02979     CHECK_PT(min[X], max[Y], max[Z]);
02980     CHECK_PT(max[X], min[Y], min[Z]);
02981     CHECK_PT(max[X], min[Y], max[Z]);
02982     CHECK_PT(max[X], max[Y], min[Z]);
02983     CHECK_PT(max[X], max[Y], max[Z]);
02984     if (class == BN_CLASSIFY_UNIMPLEMENTED)
02985         bu_log("bn_hlf_class: error in implementation\
02986 min = (%g, %g, %g), max = (%g, %g, %g), half_eqn = (%g, %g, %g, %g)\n",
02987                V3ARGS(min), V3ARGS(max), V3ARGS(half_eqn),
02988                half_eqn[3]);
02989     return class;
02990 }
02991 
02992 
02993 /** B N _ D I S T S Q _ L I N E 3 _ L I N E 3
02994  *@brief
02995  * Calculate the square of the distance of closest approach for two
02996  * lines.
02997  *
02998  * The lines are specifed as a point and a vector each.  The vectors
02999  * need not be unit length.  P and d define one line; Q and e define
03000  * the other.
03001  *
03002  * @return 0 - normal return
03003  * @return 1 - lines are parallel, dist[0] is set to 0.0
03004  *
03005  * Output values:
03006  * dist[0] is the parametric distance along the first line P + dist[0] * d of the PCA
03007  * dist[1] is the parametric distance along the second line Q + dist[1] * e of the PCA
03008  * dist[3] is the square of the distance between the points of closest approach
03009  * pt1 is the point of closest approach on the first line
03010  * pt2 is the point of closest approach on the second line
03011  *
03012  * This algoritm is based on expressing the distance sqaured, taking
03013  * partials with respect to the two unknown parameters (dist[0] and
03014  * dist[1]), seeting the two partails equal to 0, and solving the two
03015  * simutaneous equations
03016  */
03017 int
03018 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)
03019 {
03020     fastf_t de, denom;
03021     vect_t diff, PmQ, tmp;
03022     vect_t d, e;
03023     fastf_t len_e, inv_len_e, len_d, inv_len_d;
03024     int ret=0;
03025 
03026     len_e = MAGNITUDE(e_in);
03027     if (ZERO(len_e))
03028         bu_bomb("bn_distsq_line3_line3() called with zero length vector\n");
03029     inv_len_e = 1.0 / len_e;
03030 
03031     len_d = MAGNITUDE(d_in);
03032     if (ZERO(len_d))
03033         bu_bomb("bn_distsq_line3_line3() called with zero length vector\n");
03034     inv_len_d = 1.0 / len_d;
03035 
03036     VSCALE(e, e_in, inv_len_e);
03037     VSCALE(d, d_in, inv_len_d);
03038     de = VDOT(d, e);
03039 
03040     if (ZERO(de)) {
03041         /* lines are perpendicular */
03042         dist[0] = VDOT(Q, d) - VDOT(P, d);
03043         dist[1] = VDOT(P, e) - VDOT(Q, e);
03044     } else {
03045         VSUB2(PmQ, P, Q);
03046         denom = 1.0 - de*de;
03047         if (ZERO(denom)) {
03048             /* lines are parallel */
03049             dist[0] = 0.0;
03050             dist[1] = VDOT(PmQ, d);
03051             ret = 1;
03052         } else {
03053             VBLEND2(tmp, 1.0, e, -de, d);
03054             dist[1] = VDOT(PmQ, tmp)/denom;
03055             dist[0] = dist[1] * de - VDOT(PmQ, d);
03056         }
03057     }
03058     VJOIN1(pt1, P, dist[0], d);
03059     VJOIN1(pt2, Q, dist[1], e);
03060     VSUB2(diff, pt1, pt2);
03061     dist[0] *= inv_len_d;
03062     dist[1] *= inv_len_e;
03063     dist[2] =  MAGSQ(diff);
03064     return ret;
03065 }
03066 
03067 
03068 /**
03069  * B N _ I S E C T _ P L A N E S
03070  *@brief
03071  * Calculates the point that is the minimum distance from all the
03072  * planes in the "planes" array.  If the planes intersect at a single
03073  * point, that point is the solution.
03074  *
03075  * The method used here is based on:
03076 
03077  * An expression for the distance from a point to a plane is:
03078  * VDOT(pt, plane)-plane[H].
03079  * Square that distance and sum for all planes to get the "total"
03080  * distance.
03081  * For minimum total distance, the partial derivatives of this
03082  * expression (with respect to x, y, and z) must all be zero.
03083  * This produces a set of three equations in three unknowns (x, y, z).
03084 
03085  * This routine sets up the three equations as [matrix][pt] = [hpq]
03086  * and solves by inverting "matrix" into "inverse" and
03087  * [pt] = [inverse][hpq].
03088  *
03089  * There is likely a more economical solution rather than matrix
03090  * inversion, but bn_mat_inv was handy at the time.
03091  *
03092  * Checks if these planes form a singular matrix and returns.
03093  *
03094  * @return 0 - all is well
03095  * @return 1 - planes form a singular matrix (no solution)
03096  */
03097 int
03098 bn_isect_planes(fastf_t *pt, const fastf_t (*planes)[4], const size_t pl_count)
03099 {
03100     mat_t matrix;
03101     mat_t inverse;
03102     vect_t hpq;
03103     fastf_t det;
03104     size_t i;
03105 
03106     if (bu_debug & BU_DEBUG_MATH) {
03107         bu_log("bn_isect_planes:\n");
03108         for (i=0; i<pl_count; i++) {
03109             bu_log("Plane #%zu (%f %f %f %f)\n", i, V4ARGS(planes[i]));
03110         }
03111     }
03112 
03113     MAT_ZERO(matrix);
03114     VSET(hpq, 0.0, 0.0, 0.0);
03115 
03116     for (i=0; i<pl_count; i++) {
03117         matrix[0] += planes[i][X] * planes[i][X];
03118         matrix[5] += planes[i][Y] * planes[i][Y];
03119         matrix[10] += planes[i][Z] * planes[i][Z];
03120         matrix[1] += planes[i][X] * planes[i][Y];
03121         matrix[2] += planes[i][X] * planes[i][Z];
03122         matrix[6] += planes[i][Y] * planes[i][Z];
03123         hpq[X] += planes[i][X] * planes[i][H];
03124         hpq[Y] += planes[i][Y] * planes[i][H];
03125         hpq[Z] += planes[i][Z] * planes[i][H];
03126     }
03127 
03128     matrix[4] = matrix[1];
03129     matrix[8] = matrix[2];
03130     matrix[9] = matrix[6];
03131     matrix[15] = 1.0;
03132 
03133     /* Check that we don't have a singular matrix */
03134     det = bn_mat_determinant(matrix);
03135     if (ZERO(det))
03136         return 1;
03137 
03138     bn_mat_inv(inverse, matrix);
03139 
03140     MAT4X3PNT(pt, inverse, hpq);
03141 
03142     return 0;
03143 
03144 }
03145 
03146 
03147 /**
03148  * B N _ I S E C T _ L S E G _ R P P
03149  *@brief
03150  * Intersect a line segment with a rectangular parallelpiped (RPP)
03151  * that has faces parallel to the coordinate planes (a clipping RPP).
03152  * The RPP is defined by a minimum point and a maximum point.  This is
03153  * a very close relative to rt_in_rpp() from librt/shoot.c
03154  *
03155  * @return 0   if ray does not hit RPP,
03156  * @return !0  if ray hits RPP.
03157  *
03158  * @param[in, out] a    Start point of lseg
03159  * @param[in, out] b    End point of lseg
03160  * @param[in] min       min point of RPP
03161  * @param[in] max       amx point of RPP
03162  *
03163  * if !0 was returned, "a" and "b" have been clipped to the RPP.
03164  */
03165 int
03166 bn_isect_lseg_rpp(fastf_t *a,
03167                   fastf_t *b,
03168                   register fastf_t *min,
03169                   register fastf_t *max)
03170 {
03171     auto vect_t diff;
03172     register fastf_t *pt = &a[0];
03173     register fastf_t *dir = &diff[0];
03174     register int i;
03175     register double sv;
03176     register double st;
03177     register double mindist, maxdist;
03178 
03179     mindist = -MAX_FASTF;
03180     maxdist = MAX_FASTF;
03181     VSUB2(diff, b, a);
03182 
03183     for (i=0; i < 3; i++, pt++, dir++, max++, min++) {
03184         if (*dir < -SQRT_SMALL_FASTF) {
03185             if ((sv = (*min - *pt) / *dir) < 0.0)
03186                 return 0;       /* MISS */
03187             if (maxdist > sv)
03188                 maxdist = sv;
03189             if (mindist < (st = (*max - *pt) / *dir))
03190                 mindist = st;
03191         }  else if (*dir > SQRT_SMALL_FASTF) {
03192             if ((st = (*max - *pt) / *dir) < 0.0)
03193                 return 0;       /* MISS */
03194             if (maxdist > st)
03195                 maxdist = st;
03196             if (mindist < ((sv = (*min - *pt) / *dir)))
03197                 mindist = sv;
03198         } else {
03199             /* If direction component along this axis is NEAR 0, (ie,
03200              * this ray is aligned with this axis), merely check
03201              * against the boundaries.
03202              */
03203             if ((*min > *pt) || (*max < *pt))
03204                 return 0;       /* MISS */;
03205         }
03206     }
03207     if (mindist >= maxdist)
03208         return 0;       /* MISS */
03209 
03210     if (mindist > 1 || maxdist < 0)
03211         return 0;       /* MISS */
03212 
03213     if (mindist >= 0 && maxdist <= 1)
03214         return 1;       /* HIT within box, no clipping needed */
03215 
03216     /* Don't grow one end of a contained segment */
03217     if (mindist < 0)
03218         mindist = 0;
03219     if (maxdist > 1)
03220         maxdist = 1;
03221 
03222     /* Compute actual intercept points */
03223     VJOIN1(b, a, maxdist, diff);                /* b must go first */
03224     VJOIN1(a, a, mindist, diff);
03225     return 1;           /* HIT */
03226 }
03227 
03228 
03229 /** @} */
03230 /*
03231  * Local Variables:
03232  * mode: C
03233  * tab-width: 8
03234  * indent-tabs-mode: t
03235  * c-file-style: "stroustrup"
03236  * End:
03237  * ex: shiftwidth=4 tabstop=8
03238  */
03239 
Generated on Tue Dec 11 13:14:27 2012 for LIBBN by  doxygen 1.6.3