Thread: [brlcad-commits] SF.net SVN: brlcad:[34429] brlcad/trunk/src/libbn/plane.c
Open Source Solid Modeling CAD
Brought to you by:
brlcad
From: <br...@us...> - 2009-05-06 01:07:37
|
Revision: 34429 http://brlcad.svn.sourceforge.net/brlcad/?rev=34429&view=rev Author: brlcad Date: 2009-05-06 01:07:34 +0000 (Wed, 06 May 2009) Log Message: ----------- ws, style consistency, and major comment cleanup Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2009-05-06 00:34:08 UTC (rev 34428) +++ brlcad/trunk/src/libbn/plane.c 2009-05-06 01:07:34 UTC (rev 34429) @@ -22,7 +22,7 @@ /** @file plane.c * * @brief - * Some useful routines for dealing with planes and lines. + * Some useful routines for dealing with planes and lines. * */ @@ -36,75 +36,73 @@ #include "vmath.h" #include "bn.h" -#define UNIT_SQ_TOL 1.0e-13 +#define UNIT_SQ_TOL 1.0e-13 /** - * B N _ D I S T _ P T 3 _ P T 3 + * B N _ D I S T _ P T 3 _ P T 3 * @brief - * Returns distance between two points. + * Returns distance between two points. */ double bn_dist_pt3_pt3(const fastf_t *a, const fastf_t *b) { - vect_t diff; + vect_t diff; - VSUB2( diff, a, b ); - return MAGNITUDE( diff ); + VSUB2(diff, a, b); + return MAGNITUDE(diff); } /** - * B N _ P T 3 _ P T 3 _ E Q U A L + * B N _ P T 3 _ P T 3 _ E Q U A L * - * - * @return 1 if the two points are equal, within the tolerance - * @return 0 if the two points are not "the same" + * @return 1 if the two points are equal, within the tolerance + * @return 0 if the two points are not "the same" */ int bn_pt3_pt3_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol) { - vect_t diff; + vect_t diff; BN_CK_TOL(tol); - VSUB2( diff, b, a ); - if ( MAGSQ( diff ) < tol->dist_sq ) return 1; + VSUB2(diff, b, a); + if (MAGSQ(diff) < tol->dist_sq) return 1; return 0; } /** - * B N _ P T 2 _ P T 2 _ E Q U A L + * B N _ P T 2 _ P T 2 _ E Q U A L * - * - * @return 1 if the two points are equal, within the tolerance - * @return 0 if the two points are not "the same" + * @return 1 if the two points are equal, within the tolerance + * @return 0 if the two points are not "the same" */ int bn_pt2_pt2_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol) { - vect_t diff; + vect_t diff; BN_CK_TOL(tol); - VSUB2_2D( diff, b, a ); - if ( MAGSQ_2D( diff ) < tol->dist_sq ) return 1; + VSUB2_2D(diff, b, a); + if (MAGSQ_2D(diff) < tol->dist_sq) return 1; return 0; } /** - * B N _ 3 P T S _ C O L L I N E A R + * B N _ 3 P T S _ C O L L I N E A R * @brief - * Check to see if three points are collinear. + * Check to see if three points are collinear. * - * The algorithm is designed to work properly regardless of the - * order in which the points are provided. + * The algorithm is designed to work properly regardless of the order + * in which the points are provided. * - * @return 1 If 3 points are collinear - * @return 0 If they are not + * @return 1 If 3 points are collinear + * @return 0 If they are not */ int bn_3pts_collinear(fastf_t *a, fastf_t *b, fastf_t *c, const struct bn_tol *tol) { - fastf_t mag_ab, mag_bc, mag_ca, max_len, dist_sq; + fastf_t mag_ab, mag_bc, mag_ca, max_len, dist_sq; fastf_t cos_a, cos_b, cos_c; - vect_t ab, bc, ca; + vect_t ab, bc, ca; int max_edge_no; VSUB2(ab, b, a); @@ -118,79 +116,78 @@ max_len = mag_ab; max_edge_no = 1; - if ( mag_bc > max_len ) + if (mag_bc > max_len) { max_len = mag_bc; max_edge_no = 2; } - if ( mag_ca > max_len ) + if (mag_ca > max_len) { max_len = mag_ca; max_edge_no = 3; } - switch ( max_edge_no ) + switch (max_edge_no) { default: case 1: - cos_b = (-VDOT( ab, bc ))/(mag_ab * mag_bc ); - dist_sq = mag_bc*mag_bc*( 1.0 - cos_b*cos_b); + cos_b = (-VDOT(ab, bc))/(mag_ab * mag_bc); + dist_sq = mag_bc*mag_bc*(1.0 - cos_b*cos_b); break; case 2: - cos_c = (-VDOT( bc, ca ))/(mag_bc * mag_ca ); + cos_c = (-VDOT(bc, ca))/(mag_bc * mag_ca); dist_sq = mag_ca*mag_ca*(1.0 - cos_c*cos_c); break; case 3: - cos_a = (-VDOT( ca, ab ))/(mag_ca * mag_ab ); + cos_a = (-VDOT(ca, ab))/(mag_ca * mag_ab); dist_sq = mag_ab*mag_ab*(1.0 - cos_a*cos_a); break; } - if ( dist_sq <= tol->dist_sq ) - return( 1 ); + if (dist_sq <= tol->dist_sq) + return(1); else - return( 0 ); + return(0); } /** - * B N _ 3 P T S _ D I S T I N C T + * B N _ 3 P T S _ D I S T I N C T * - * Check to see if three points are all distinct, i.e., - * ensure that there is at least sqrt(dist_tol_sq) distance - * between every pair of points. + * Check to see if three points are all distinct, i.e., ensure that + * there is at least sqrt(dist_tol_sq) distance between every pair of + * points. * - * - * @return 1 If all three points are distinct - * @return 0 If two or more points are closer together than dist_tol_sq + * @return 1 If all three points are distinct + * @return 0 If two or more points are closer together than dist_tol_sq */ int bn_3pts_distinct(const fastf_t *a, const fastf_t *b, const fastf_t *c, const struct bn_tol *tol) { - vect_t B_A; - vect_t C_A; - vect_t C_B; + vect_t B_A; + vect_t C_A; + vect_t C_B; BN_CK_TOL(tol); - VSUB2( B_A, b, a ); - if ( MAGSQ( B_A ) <= tol->dist_sq ) return(0); - VSUB2( C_A, c, a ); - if ( MAGSQ( C_A ) <= tol->dist_sq ) return(0); - VSUB2( C_B, c, b ); - if ( MAGSQ( C_B ) <= tol->dist_sq ) return(0); + VSUB2(B_A, b, a); + if (MAGSQ(B_A) <= tol->dist_sq) return(0); + VSUB2(C_A, c, a); + if (MAGSQ(C_A) <= tol->dist_sq) return(0); + VSUB2(C_B, c, b); + if (MAGSQ(C_B) <= tol->dist_sq) return(0); return(1); } /** - * B N _ M K _ P L A N E _ 3 P T S + * B N _ M K _ P L A N E _ 3 P T S * - * Find the equation of a plane that contains three points. - * Note that normal vector created is expected to point out (see vmath.h), - * so the vector from A to C had better be counter-clockwise - * (about the point A) from the vector from A to B. - * This follows the BRL-CAD outward-pointing normal convention, and the - * right-hand rule for cross products. + * Find the equation of a plane that contains three points. Note that + * normal vector created is expected to point out (see vmath.h), so + * the vector from A to C had better be counter-clockwise (about the + * point A) from the vector from A to B. This follows the BRL-CAD + * outward-pointing normal convention, and the right-hand rule for + * cross products. * @verbatim * @@ -211,9 +208,10 @@ * B-A @endverbatim * - * If the points are given in the order A B C (eg, *counter*-clockwise), - * then the outward pointing surface normal N = (B-A) x (C-A). + * If the points are given in the order A B C (e.g., + * *counter*-clockwise), then the outward pointing surface normal: * + * N = (B-A) x (C-A). * * @return 0 OK * @return -1 Failure. At least two of the points were not distinct, @@ -232,39 +230,39 @@ const fastf_t *c, const struct bn_tol *tol) { - vect_t B_A; - vect_t C_A; - vect_t C_B; + vect_t B_A; + vect_t C_A; + vect_t C_B; register fastf_t mag; BN_CK_TOL(tol); - VSUB2( B_A, b, a ); - if ( MAGSQ( B_A ) <= tol->dist_sq ) return(-1); - VSUB2( C_A, c, a ); - if ( MAGSQ( C_A ) <= tol->dist_sq ) return(-1); - VSUB2( C_B, c, b ); - if ( MAGSQ( C_B ) <= tol->dist_sq ) return(-1); + VSUB2(B_A, b, a); + if (MAGSQ(B_A) <= tol->dist_sq) return(-1); + VSUB2(C_A, c, a); + if (MAGSQ(C_A) <= tol->dist_sq) return(-1); + VSUB2(C_B, c, b); + if (MAGSQ(C_B) <= tol->dist_sq) return(-1); - VCROSS( plane, B_A, C_A ); + VCROSS(plane, B_A, C_A); /* Ensure unit length normal */ - if ( (mag = MAGNITUDE(plane)) <= SMALL_FASTF ) + if ((mag = MAGNITUDE(plane)) <= SMALL_FASTF) return(-1); /* FAIL */ mag = 1/mag; - VSCALE( plane, plane, mag ); + VSCALE(plane, plane, mag); /* Find distance from the origin to the plane */ /* XXX Should do with pt that has smallest magnitude (closest to origin) */ - plane[3] = VDOT( plane, a ); + plane[3] = VDOT(plane, a); #if 0 /* Check to be sure that angle between A-Origin and N vect < 89 degrees */ /* XXX Could complain for pts on axis-aligned plane, far from origin */ mag = MAGSQ(a); - if ( mag > tol->dist_sq ) { + if (mag > tol->dist_sq) { /* cos(89 degrees) = 0.017452406, reciprocal is 57.29 */ - if ( plane[3]/sqrt(mag) < 0.017452406 ) { + if (plane[3]/sqrt(mag) < 0.017452406) { bu_log("bn_mk_plane_3pts() WARNING: plane[3] value is suspect\n"); } } @@ -273,18 +271,18 @@ } /** - * B N _ M K P O I N T _ 3 P L A N E S + * B N _ M K P O I N T _ 3 P L A N E S *@brief - * Given the description of three planes, compute the point of intersection, - * if any. + * Given the description of three planes, compute the point of + * intersection, if any. * - * Find the solution to a system of three equations in three unknowns: + * Find the solution to a system of three equations in three unknowns: @verbatim * Px * Ax + Py * Ay + Pz * Az = -A3; * Px * Bx + Py * By + Pz * Bz = -B3; * Px * Cx + Py * Cy + Pz * Cz = -C3; * - * or + * or * * [ Ax Ay Az ] [ Px ] [ -A3 ] * [ Bx By Bz ] * [ Py ] = [ -B3 ] @@ -304,22 +302,22 @@ int bn_mkpoint_3planes(fastf_t *pt, const fastf_t *a, const fastf_t *b, const fastf_t *c) { - vect_t v1, v2, v3; + vect_t v1, v2, v3; register fastf_t det; /* Find a vector perpendicular to both planes B and C */ - VCROSS( v1, b, c ); + VCROSS(v1, b, c); - /* If that vector is perpendicular to A, - * then A is parallel to either B or C, and no intersection exists. - * This dot&cross product is the determinant of the matrix M. - * (I suspect there is some deep significance to this!) + /* If that vector is perpendicular to A, then A is parallel to + * either B or C, and no intersection exists. This dot&cross + * product is the determinant of the matrix M. (I suspect there + * is some deep significance to this!) */ - det = VDOT( a, v1 ); - if ( NEAR_ZERO( det, SMALL_FASTF ) ) return(-1); + det = VDOT(a, v1); + if (NEAR_ZERO(det, SMALL_FASTF)) return(-1); - VCROSS( v2, a, c ); - VCROSS( v3, a, b ); + VCROSS(v2, a, c); + VCROSS(v3, a, b); det = 1/det; pt[X] = det*(a[3]*v1[X] - b[3]*v2[X] + c[3]*v3[X]); @@ -329,15 +327,15 @@ } /** - * B N _ 2 L I N E 3 _ C O L I N E A R + * B N _ 2 L I N E 3 _ C O L I N E A R * @brief - * Returns non-zero if the 3 lines are colinear to within tol->dist - * over the given distance range. + * Returns non-zero if the 3 lines are colinear to within tol->dist + * over the given distance range. * - * Range should be at least one model diameter for most applications. - * 1e5 might be OK for a default for "vehicle sized" models. + * Range should be at least one model diameter for most applications. + * 1e5 might be OK for a default for "vehicle sized" models. * - * The direction vectors do not need to be unit length. + * The direction vectors do not need to be unit length. */ int bn_2line3_colinear(const fastf_t *p1, @@ -347,9 +345,9 @@ double range, const struct bn_tol *tol) { - fastf_t mag1; - fastf_t mag2; - point_t tail; + fastf_t mag1; + fastf_t mag2; + point_t tail; BN_CK_TOL(tol); @@ -357,42 +355,41 @@ goto fail; } - if ( (mag1 = MAGNITUDE(d1)) < SMALL_FASTF ) bu_bomb("bn_2line3_colinear() mag1 zero\n"); - if ( (mag2 = MAGNITUDE(d2)) < SMALL_FASTF ) bu_bomb("bn_2line3_colinear() mag2 zero\n"); + if ((mag1 = MAGNITUDE(d1)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag1 zero\n"); + if ((mag2 = MAGNITUDE(d2)) < SMALL_FASTF) bu_bomb("bn_2line3_colinear() mag2 zero\n"); /* Impose a general angular tolerance to reject "obviously" non-parallel lines */ /* tol->para and RT_DOT_TOL are too tight a tolerance. 0.1 is 5 degrees */ - if ( fabs(VDOT(d1, d2)) < 0.9 * mag1 * mag2 ) goto fail; + if (fabs(VDOT(d1, d2)) < 0.9 * mag1 * mag2) goto fail; /* See if start points are within tolerance of other line */ - if ( bn_distsq_line3_pt3( p1, d1, p2 ) > tol->dist_sq ) goto fail; - if ( bn_distsq_line3_pt3( p2, d2, p1 ) > tol->dist_sq ) goto fail; + if (bn_distsq_line3_pt3(p1, d1, p2) > tol->dist_sq) goto fail; + if (bn_distsq_line3_pt3(p2, d2, p1) > tol->dist_sq) goto fail; - VJOIN1( tail, p1, range/mag1, d1 ); - if ( bn_distsq_line3_pt3( p2, d2, tail ) > tol->dist_sq ) goto fail; + VJOIN1(tail, p1, range/mag1, d1); + if (bn_distsq_line3_pt3(p2, d2, tail) > tol->dist_sq) goto fail; - VJOIN1( tail, p2, range/mag2, d2 ); - if ( bn_distsq_line3_pt3( p1, d1, tail ) > tol->dist_sq ) goto fail; + VJOIN1(tail, p2, range/mag2, d2); + if (bn_distsq_line3_pt3(p1, d1, tail) > tol->dist_sq) goto fail; - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("bn_2line3colinear(range=%g) ret=1\n", range); } return 1; fail: - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("bn_2line3colinear(range=%g) ret=0\n", range); } return 0; } /** - * B N _ I S E C T _ L I N E 3 _ P L A N E + * B N _ I S E C T _ L I N E 3 _ P L A N E * - * Intersect an infinite line (specified in point and direction vector form) - * with a plane that has an outward pointing normal. - * The direction vector need not have unit length. - * The first three elements of the plane equation must form - * a unit lengh vector. + * Intersect an infinite line (specified in point and direction vector + * form) with a plane that has an outward pointing normal. The + * direction vector need not have unit length. The first three + * elements of the plane equation must form a unit lengh vector. * * * @return -2 missed (ray is outside halfspace) @@ -414,49 +411,49 @@ const fastf_t *plane, const struct bn_tol *tol) { - register fastf_t slant_factor; - register fastf_t norm_dist; - register fastf_t dot; - vect_t local_dir; + register fastf_t slant_factor; + register fastf_t norm_dist; + register fastf_t dot; + vect_t local_dir; BN_CK_TOL(tol); - norm_dist = plane[3] - VDOT( plane, pt ); - slant_factor = VDOT( plane, dir ); - VMOVE( local_dir, dir ) - VUNITIZE( local_dir ) - dot = VDOT( plane, local_dir ); + norm_dist = plane[3] - VDOT(plane, pt); + slant_factor = VDOT(plane, dir); + VMOVE(local_dir, dir) + VUNITIZE(local_dir) + dot = VDOT(plane, local_dir); - if ( slant_factor < -SMALL_FASTF && dot < -tol->perp ) { + if (slant_factor < -SMALL_FASTF && dot < -tol->perp) { *dist = norm_dist/slant_factor; return 1; /* HIT, entering */ - } else if ( slant_factor > SMALL_FASTF && dot > tol->perp ) { + } else if (slant_factor > SMALL_FASTF && dot > tol->perp) { *dist = norm_dist/slant_factor; return 2; /* HIT, leaving */ } /* - * Ray is parallel to plane when dir.N == 0. + * Ray is parallel to plane when dir.N == 0. */ *dist = 0; /* sanity */ - if ( norm_dist < -tol->dist ) + if (norm_dist < -tol->dist) return -2; /* missed, outside */ - if ( norm_dist > tol->dist ) + if (norm_dist > tol->dist) return -1; /* missed, inside */ return 0; /* Ray lies in the plane */ } /** - * B N _ I S E C T _ 2 P L A N E S + * B N _ I S E C T _ 2 P L A N E S *@brief - * Given two planes, find the line of intersection between them, - * if one exists. - * The line of intersection is returned in parametric line - * (point & direction vector) form. + * Given two planes, find the line of intersection between them, if + * one exists. The line of intersection is returned in parametric + * line (point & direction vector) form. * - * In order that all the geometry under consideration be in "front" - * of the ray, it is necessary to pass the minimum point of the model - * RPP. If this convention is unnecessary, just pass (0, 0, 0) as rpp_min. + * In order that all the geometry under consideration be in "front" of + * the ray, it is necessary to pass the minimum point of the model + * RPP. If this convention is unnecessary, just pass (0, 0, 0) as + * rpp_min. * * * @return 0 OK, line of intersection stored in `pt' and `dir'. @@ -480,72 +477,73 @@ const fastf_t *rpp_min, const struct bn_tol *tol) { - vect_t abs_dir; - plane_t pl; - int i; + vect_t abs_dir; + plane_t pl; + int i; - if ( (i = bn_coplanar( a, b, tol )) != 0 ) { - if ( i > 0 ) + if ((i = bn_coplanar(a, b, tol)) != 0) { + if (i > 0) return(-1); /* FAIL -- coplanar */ return(-2); /* FAIL -- parallel & distinct */ } - /* Direction vector for ray is perpendicular to both plane normals */ - VCROSS( dir, a, b ); - VUNITIZE( dir ); /* safety */ + /* Direction vector for ray is perpendicular to both plane + * normals. + */ + VCROSS(dir, a, b); + VUNITIZE(dir); /* safety */ - /* - * Select an axis-aligned plane which has it's normal pointing - * along the same axis as the largest magnitude component of - * the direction vector. - * If the largest magnitude component is negative, reverse the - * direction vector, so that model is "in front" of start point. + /* Select an axis-aligned plane which has it's normal pointing + * along the same axis as the largest magnitude component of the + * direction vector. If the largest magnitude component is + * negative, reverse the direction vector, so that model is "in + * front" of start point. */ abs_dir[X] = (dir[X] >= 0) ? dir[X] : (-dir[X]); abs_dir[Y] = (dir[Y] >= 0) ? dir[Y] : (-dir[Y]); abs_dir[Z] = (dir[Z] >= 0) ? dir[Z] : (-dir[Z]); - if ( abs_dir[X] >= abs_dir[Y] ) { - if ( abs_dir[X] >= abs_dir[Z] ) { - VSET( pl, 1, 0, 0 ); /* X */ + if (abs_dir[X] >= abs_dir[Y]) { + if (abs_dir[X] >= abs_dir[Z]) { + VSET(pl, 1, 0, 0); /* X */ pl[W] = rpp_min[X]; - if ( dir[X] < 0 ) { - VREVERSE( dir, dir ); + if (dir[X] < 0) { + VREVERSE(dir, dir); } } else { - VSET( pl, 0, 0, 1 ); /* Z */ + VSET(pl, 0, 0, 1); /* Z */ pl[W] = rpp_min[Z]; - if ( dir[Z] < 0 ) { - VREVERSE( dir, dir ); + if (dir[Z] < 0) { + VREVERSE(dir, dir); } } } else { - if ( abs_dir[Y] >= abs_dir[Z] ) { - VSET( pl, 0, 1, 0 ); /* Y */ + if (abs_dir[Y] >= abs_dir[Z]) { + VSET(pl, 0, 1, 0); /* Y */ pl[W] = rpp_min[Y]; - if ( dir[Y] < 0 ) { - VREVERSE( dir, dir ); + if (dir[Y] < 0) { + VREVERSE(dir, dir); } } else { - VSET( pl, 0, 0, 1 ); /* Z */ + VSET(pl, 0, 0, 1); /* Z */ pl[W] = rpp_min[Z]; - if ( dir[Z] < 0 ) { - VREVERSE( dir, dir ); + if (dir[Z] < 0) { + VREVERSE(dir, dir); } } } /* Intersection of the 3 planes defines ray start point */ - if ( bn_mkpoint_3planes( pt, pl, a, b ) < 0 ) + if (bn_mkpoint_3planes(pt, pl, a, b) < 0) return(-3); /* FAIL -- no intersection */ return(0); /* OK */ } /** - * B N _ I S E C T _ L I N E 2 _ L I N E 2 + * B N _ I S E C T _ L I N E 2 _ L I N E 2 * - * Intersect two lines, each in given in parametric form: + * Intersect two lines, each in given in parametric form: @verbatim X = P + t * D @@ -553,13 +551,14 @@ X = A + u * C @endverbatim - * While the parametric form is usually used to denote a ray - * (ie, positive values of the parameter only), in this case - * the full line is considered. * - * The direction vectors C and D need not have unit length. + * While the parametric form is usually used to denote a ray (ie, + * positive values of the parameter only), in this case the full line + * is considered. * + * The direction vectors C and D need not have unit length. * + * * @return -1 no intersection, lines are parallel. * @return 0 lines are co-linear *@n dist[0] gives distance from P to A, @@ -568,10 +567,10 @@ *@n dist[0] gives distance from P to isect, *@n dist[1] gives distance from A to isect. * - * @param dist When explicit return > 0, dist[0] and dist[1] are the - * line parameters of the intersection point on the 2 rays. - * The actual intersection coordinates can be found by - * substituting either of these into the original ray equations. + * @param dist When explicit return > 0, dist[0] and dist[1] are the + * line parameters of the intersection point on the 2 rays. The + * actual intersection coordinates can be found by substituting either + * of these into the original ray equations. * * @param p point of first line * @param d direction of first line @@ -579,10 +578,10 @@ * @param c direction of second line * @param tol tolerance values * - * Note that for lines which are very nearly parallel, but not - * quite parallel enough to have the determinant go to "zero", - * the intersection can turn up in surprising places. - * (e.g. when det=1e-15 and det1=5.5e-17, t=0.5) + * Note that for lines which are very nearly parallel, but not quite + * parallel enough to have the determinant go to "zero", the + * intersection can turn up in surprising places. (e.g. when + * det=1e-15 and det1=5.5e-17, t=0.5) */ int bn_isect_line2_line2(fastf_t *dist, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *c, const struct bn_tol *tol) @@ -590,63 +589,62 @@ { - fastf_t hx, hy; /* A - P */ - register fastf_t det; - register fastf_t det1; - vect_t unit_d; - vect_t unit_c; - vect_t unit_h; - int parallel; - int parallel1; + fastf_t hx, hy; /* A - P */ + register fastf_t det; + register fastf_t det1; + vect_t unit_d; + vect_t unit_c; + vect_t unit_h; + int parallel; + int parallel1; BN_CK_TOL(tol); - if ( bu_debug & BU_DEBUG_MATH ) { - bu_log("bn_isect_line2_line2() p=(%g,%g), d=(%g,%g)\n\t\t\ta=(%g,%g), c=(%g,%g)\n", - V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c) ); + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_line2_line2() p=(%g, %g), d=(%g, %g)\n\t\t\ta=(%g, %g), c=(%g, %g)\n", + V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c)); } /* - * From the two components q and r, form a system - * of 2 equations in 2 unknowns. - * Solve for t and u in the system: + * From the two components q and r, form a system of 2 equations + * in 2 unknowns. Solve for t and u in the system: * - * Px + t * Dx = Ax + u * Cx - * Py + t * Dy = Ay + u * Cy - * or - * t * Dx - u * Cx = Ax - Px - * t * Dy - u * Cy = Ay - Py + * Px + t * Dx = Ax + u * Cx + * Py + t * Dy = Ay + u * Cy + * or + * t * Dx - u * Cx = Ax - Px + * t * Dy - u * Cy = Ay - Py * - * Let H = A - P, resulting in: + * Let H = A - P, resulting in: * - * t * Dx - u * Cx = Hx - * t * Dy - u * Cy = Hy + * t * Dx - u * Cx = Hx + * t * Dy - u * Cy = Hy * - * or + * or * * [ Dx -Cx ] [ t ] [ Hx ] * [ ] * [ ] = [ ] * [ Dy -Cy ] [ u ] [ Hy ] * - * This system can be solved by direct substitution, or by - * finding the determinants by Cramers rule: + * This system can be solved by direct substitution, or by finding + * the determinants by Cramers rule: * * [ Dx -Cx ] * det(M) = det [ ] = -Dx * Cy + Cx * Dy * [ Dy -Cy ] * - * If det(M) is zero, then the lines are parallel (perhaps colinear). - * Otherwise, exactly one solution exists. + * If det(M) is zero, then the lines are parallel (perhaps + * colinear). Otherwise, exactly one solution exists. */ det = c[X] * d[Y] - d[X] * c[Y]; /* - * det(M) is non-zero, so there is exactly one solution. - * Using Cramer's rule, det1(M) replaces the first column - * of M with the constant column vector, in this case H. - * Similarly, det2(M) replaces the second column. - * Computation of the determinant is done as before. + * det(M) is non-zero, so there is exactly one solution. Using + * Cramer's rule, det1(M) replaces the first column of M with the + * constant column vector, in this case H. Similarly, det2(M) + * replaces the second column. Computation of the determinant is + * done as before. * - * Now, + * Now, * * [ Hx -Cx ] * det [ ] @@ -654,7 +652,7 @@ * t = ------- = --------------- = ------------------ * det(M) det(M) -Dx * Cy + Cx * Dy * - * and + * and * * [ Dx Hx ] * det [ ] @@ -669,22 +667,22 @@ unit_d[0] = d[0]; unit_d[1] = d[1]; unit_d[2] = 0.0; - VUNITIZE( unit_d ); + VUNITIZE(unit_d); unit_c[0] = c[0]; unit_c[1] = c[1]; unit_c[2] = 0.0; - VUNITIZE( unit_c ); + VUNITIZE(unit_c); unit_h[0] = hx; unit_h[1] = hy; unit_h[2] = 0.0; - VUNITIZE( unit_h ); + VUNITIZE(unit_h); - if ( fabs( VDOT( unit_d, unit_c ) ) >= tol->para ) + if (fabs(VDOT(unit_d, unit_c)) >= tol->para) parallel = 1; else parallel = 0; - if ( fabs( VDOT( unit_h, unit_c ) ) >= tol->para ) + if (fabs(VDOT(unit_h, unit_c)) >= tol->para) parallel1 = 1; else parallel1 = 0; @@ -694,67 +692,69 @@ * XXX max(c[X], c[Y], d[X], d[Y]) / MAX_FASTF_DYNAMIC_RANGE * XXX In any case, nothing smaller than 1e-16 */ -#define DETERMINANT_TOL 1.0e-14 /* XXX caution on non-IEEE machines */ - if ( parallel || NEAR_ZERO( det, DETERMINANT_TOL ) ) { +#define DETERMINANT_TOL 1.0e-14 /* XXX caution on non-IEEE machines */ + if (parallel || NEAR_ZERO(det, DETERMINANT_TOL)) { /* Lines are parallel */ - if ( !parallel1 && !NEAR_ZERO( det1, DETERMINANT_TOL ) ) { + if (!parallel1 && !NEAR_ZERO(det1, DETERMINANT_TOL)) { /* Lines are NOT co-linear, just parallel */ - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("\tparallel, not co-linear. det=%e, det1=%g\n", det, det1); } return -1; /* parallel, no intersection */ } /* - * Lines are co-linear. - * Determine t as distance from P to A. - * Determine u as distance from P to (A+C). [special!] - * Use largest direction component, for numeric stability - * (and avoiding division by zero). + * Lines are co-linear. + * Determine t as distance from P to A. + * Determine u as distance from P to (A+C). [special!] + * Use largest direction component, for numeric stability + * (and avoiding division by zero). */ - if ( fabs(d[X]) >= fabs(d[Y]) ) { + if (fabs(d[X]) >= fabs(d[Y])) { dist[0] = hx/d[X]; dist[1] = (hx + c[X]) / d[X]; } else { dist[0] = hy/d[Y]; dist[1] = (hy + c[Y]) / d[Y]; } - if ( bu_debug & BU_DEBUG_MATH ) { - bu_log("\tcolinear, t = %g, u = %g\n", dist[0], dist[1] ); + if (bu_debug & BU_DEBUG_MATH) { + bu_log("\tcolinear, t = %g, u = %g\n", dist[0], dist[1]); } return 0; /* Lines co-linear */ } - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { /* XXX This print is temporary */ - bu_log("\thx=%g, hy=%g, det=%g, det1=%g, det2=%g\n", hx, hy, det, det1, (d[X] * hy - hx * d[Y]) ); + bu_log("\thx=%g, hy=%g, det=%g, det1=%g, det2=%g\n", hx, hy, det, det1, (d[X] * hy - hx * d[Y])); } det = 1/det; dist[0] = det * det1; dist[1] = det * (d[X] * hy - hx * d[Y]); - if ( bu_debug & BU_DEBUG_MATH ) { - bu_log("\tintersection, t = %g, u = %g\n", dist[0], dist[1] ); + if (bu_debug & BU_DEBUG_MATH) { + bu_log("\tintersection, t = %g, u = %g\n", dist[0], dist[1]); } #if 0 /* XXX This isn't any good. - * 1) Sometimes, dist[0] is very large. Only caller can tell whether - * that is useful to him or not. - * 2) Sometimes, the difference between the two hit points is - * not much more than tol->dist. Either hit point is perfectly - * good; the caller just needs to be careful and not use *both*. + * + * 1) Sometimes, dist[0] is very large. Only caller can tell + * whether that is useful to him or not. + * + * 2) Sometimes, the difference between the two hit points is not + * much more than tol->dist. Either hit point is perfectly good; + * the caller just needs to be careful and not use *both*. */ { - point_t hit1, hit2; - vect_t diff; - fastf_t dist_sq; + point_t hit1, hit2; + vect_t diff; + fastf_t dist_sq; - VJOIN1_2D( hit1, p, dist[0], d ); - VJOIN1_2D( hit2, a, dist[1], c ); - VSUB2_2D( diff, hit1, hit2 ); - dist_sq = MAGSQ_2D( diff ); - if ( dist_sq >= tol->dist_sq ) { - if ( bu_debug & BU_DEBUG_MATH || dist_sq < 100*tol->dist_sq ) { - bu_log("bn_isect_line2_line2(): dist=%g >%g, inconsistent solution, hit1=(%g,%g), hit2=(%g,%g)\n", + VJOIN1_2D(hit1, p, dist[0], d); + VJOIN1_2D(hit2, a, dist[1], c); + VSUB2_2D(diff, hit1, hit2); + dist_sq = MAGSQ_2D(diff); + if (dist_sq >= tol->dist_sq) { + if (bu_debug & BU_DEBUG_MATH || dist_sq < 100*tol->dist_sq) { + bu_log("bn_isect_line2_line2(): dist=%g >%g, inconsistent solution, hit1=(%g, %g), hit2=(%g, %g)\n", sqrt(dist_sq), tol->dist, hit1[X], hit1[Y], hit2[X], hit2[Y]); } @@ -767,17 +767,16 @@ } /** - * B N _ I S E C T _ L I N E 2 _ L S E G 2 + * B N _ I S E C T _ L I N E 2 _ L S E G 2 *@brief - * Intersect a line in parametric form: + * Intersect a line in parametric form: * - * X = P + t * D + * X = P + t * D * - * with a line segment defined by two distinct points A and B=(A+C). + * with a line segment defined by two distinct points A and B=(A+C). * - * XXX probably should take point B, not vector C. Sigh. + * XXX probably should take point B, not vector C. Sigh. * - * * @return -4 A and B are not distinct points * @return -3 Lines do not intersect * @return -2 Intersection exists, but outside segemnt, < A @@ -787,7 +786,7 @@ * @return 2 Intersection at vertex B (A+C) * @return 3 Intersection between A and B * - * Implicit Returns - + * Implicit Returns - * @param dist When explicit return >= 0, t is the parameter that describes * the intersection of the line and the line segment. * The actual intersection coordinates can be found by @@ -813,167 +812,163 @@ const struct bn_tol *tol) { register fastf_t f; - fastf_t ctol; - int ret; - point_t b; + fastf_t ctol; + int ret; + point_t b; BN_CK_TOL(tol); - if ( bu_debug & BU_DEBUG_MATH ) { - bu_log("bn_isect_line2_lseg2() p=(%g,%g), pdir=(%g,%g)\n\t\t\ta=(%g,%g), adir=(%g,%g)\n", - V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c) ); + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_line2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\t\ta=(%g, %g), adir=(%g, %g)\n", + V2ARGS(p), V2ARGS(d), V2ARGS(a), V2ARGS(c)); } - /* - * To keep the values of u between 0 and 1, - * C should NOT be scaled to have unit length. - * However, it is a good idea to make sure that - * C is a non-zero vector, (ie, that A and B are distinct). + /* To keep the values of u between 0 and 1. C should NOT be + * scaled to have unit length. However, it is a good idea to make + * sure that C is a non-zero vector, (ie, that A and B are + * distinct). */ - if ( (ctol = MAGSQ_2D(c)) <= tol->dist_sq ) { + if ((ctol = MAGSQ_2D(c)) <= tol->dist_sq) { ret = -4; /* points A and B are not distinct */ goto out; } - /* - * Detecting colinearity is difficult, and very very important. - * As a first step, check to see if both points A and B lie - * within tolerance of the line. If so, then the line segment AC - * is ON the line. + /* Detecting colinearity is difficult, and very very important. + * As a first step, check to see if both points A and B lie within + * tolerance of the line. If so, then the line segment AC is ON + * the line. */ - VADD2_2D( b, a, c ); - if ( bn_distsq_line2_point2( p, d, a ) <= tol->dist_sq && - (ctol=bn_distsq_line2_point2( p, d, b )) <= tol->dist_sq ) { - if ( bu_debug & BU_DEBUG_MATH ) { + VADD2_2D(b, a, c); + if (bn_distsq_line2_point2(p, d, a) <= tol->dist_sq && + (ctol=bn_distsq_line2_point2(p, d, b)) <= tol->dist_sq) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("b=(%g, %g), b_dist_sq=%g\n", V2ARGS(b), ctol); bu_log("bn_isect_line2_lseg2() pts A and B within tol of line\n"); } /* Find the parametric distance along the ray */ - dist[0] = bn_dist_pt2_along_line2( p, d, a ); - dist[1] = bn_dist_pt2_along_line2( p, d, b ); + dist[0] = bn_dist_pt2_along_line2(p, d, a); + dist[1] = bn_dist_pt2_along_line2(p, d, b); ret = 0; /* Colinear */ goto out; } - if ( (ret = bn_isect_line2_line2( dist, p, d, a, c, tol )) < 0 ) { + if ((ret = bn_isect_line2_line2(dist, p, d, a, c, tol)) < 0) { /* Lines are parallel, non-colinear */ ret = -3; /* No intersection found */ goto out; } - if ( ret == 0 ) { - fastf_t dtol; - /* Lines are colinear */ - /* If P within tol of either endpoint (0, 1), make exact. */ + if (ret == 0) { + fastf_t dtol; + /* Lines are colinear */ + /* If P within tol of either endpoint (0, 1), make exact. */ dtol = tol->dist / sqrt(MAGSQ_2D(d)); - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("bn_isect_line2_lseg2() dtol=%g, dist[0]=%g, dist[1]=%g\n", dtol, dist[0], dist[1]); } - if ( dist[0] > -dtol && dist[0] < dtol ) dist[0] = 0; - else if ( dist[0] > 1-dtol && dist[0] < 1+dtol ) dist[0] = 1; + if (dist[0] > -dtol && dist[0] < dtol) dist[0] = 0; + else if (dist[0] > 1-dtol && dist[0] < 1+dtol) dist[0] = 1; - if ( dist[1] > -dtol && dist[1] < dtol ) dist[1] = 0; - else if ( dist[1] > 1-dtol && dist[1] < 1+dtol ) dist[1] = 1; + if (dist[1] > -dtol && dist[1] < dtol) dist[1] = 0; + else if (dist[1] > 1-dtol && dist[1] < 1+dtol) dist[1] = 1; ret = 0; /* Colinear */ goto out; } - /* - * The two lines are claimed to intersect at a point. - * First, validate that hit point represented by dist[0] - * is in fact on and between A--B. - * (Nearly parallel lines can result in odd situations here). - * The performance hit of doing this is vastly preferable - * to returning wrong answers. Know a faster algorithm? + /* The two lines are claimed to intersect at a point. First, + * validate that hit point represented by dist[0] is in fact on + * and between A--B. (Nearly parallel lines can result in odd + * situations here). The performance hit of doing this is vastly + * preferable to returning wrong answers. Know a faster + * algorithm? */ { - fastf_t ab_dist = 0; - point_t hit_pt; - point_t hit2; + fastf_t ab_dist = 0; + point_t hit_pt; + point_t hit2; - VJOIN1_2D( hit_pt, p, dist[0], d ); - VJOIN1_2D( hit2, a, dist[1], c ); + VJOIN1_2D(hit_pt, p, dist[0], d); + VJOIN1_2D(hit2, a, dist[1], c); /* Check both hit point value calculations */ - if ( bn_pt2_pt2_equal( a, hit_pt, tol ) || - bn_pt2_pt2_equal( a, hit2, tol ) ) { + if (bn_pt2_pt2_equal(a, hit_pt, tol) || + bn_pt2_pt2_equal(a, hit2, tol)) { dist[1] = 0; ret = 1; /* Intersect is at A */ } - if ( bn_pt2_pt2_equal( b, hit_pt, tol ) || - bn_pt2_pt2_equal( b, hit_pt, tol ) ) { + if (bn_pt2_pt2_equal(b, hit_pt, tol) || + bn_pt2_pt2_equal(b, hit_pt, tol)) { dist[1] = 1; ret = 2; /* Intersect is at B */ } - ret = bn_isect_pt2_lseg2( &ab_dist, a, b, hit_pt, tol ); - if ( bu_debug & BU_DEBUG_MATH ) { + ret = bn_isect_pt2_lseg2(&ab_dist, a, b, hit_pt, tol); + if (bu_debug & BU_DEBUG_MATH) { /* XXX This is temporary */ V2PRINT("a", a); V2PRINT("hit", hit_pt); V2PRINT("b", b); - bu_log("bn_isect_pt2_lseg2() hit2d=(%g,%g) ab_dist=%g, ret=%d\n", hit_pt[X], hit_pt[Y], ab_dist, ret); - bu_log("\tother hit2d=(%g,%g)\n", hit2[X], hit2[Y] ); + bu_log("bn_isect_pt2_lseg2() hit2d=(%g, %g) ab_dist=%g, ret=%d\n", hit_pt[X], hit_pt[Y], ab_dist, ret); + bu_log("\tother hit2d=(%g, %g)\n", hit2[X], hit2[Y]); } - if ( ret <= 0 ) { - if ( ab_dist < 0 ) { + if (ret <= 0) { + if (ab_dist < 0) { ret = -2; /* Intersection < A */ } else { ret = -1; /* Intersection >B */ } goto out; } - if ( ret == 1 ) { + if (ret == 1) { dist[1] = 0; ret = 1; /* Intersect is at A */ goto out; } - if ( ret == 2 ) { + if (ret == 2) { dist[1] = 1; ret = 2; /* Intersect is at B */ goto out; } /* ret == 3, hit_pt is between A and B */ - if ( !bn_between( a[X], hit_pt[X], b[X], tol ) || - !bn_between( a[Y], hit_pt[Y], b[Y], tol ) ) { + if (!bn_between(a[X], hit_pt[X], b[X], tol) || + !bn_between(a[Y], hit_pt[Y], b[Y], tol)) { bu_bomb("bn_isect_line2_lseg2() hit_pt not between A and B!\n"); } } - /* - * If the dist[1] parameter is outside the range (0..1), - * reject the intersection, because it falls outside - * the line segment A--B. + /* If the dist[1] parameter is outside the range (0..1), reject + * the intersection, because it falls outside the line segment + * A--B. * - * Convert the tol->dist into allowable deviation in terms of - * (0..1) range of the parameters. + * Convert the tol->dist into allowable deviation in terms of + * (0..1) range of the parameters. */ ctol = tol->dist / sqrt(ctol); - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("bn_isect_line2_lseg2() ctol=%g, dist[1]=%g\n", ctol, dist[1]); } - if ( dist[1] < -ctol ) { + if (dist[1] < -ctol) { ret = -2; /* Intersection < A */ goto out; } - if ( (f=(dist[1]-1)) > ctol ) { + if ((f=(dist[1]-1)) > ctol) { ret = -1; /* Intersection > B */ goto out; } /* Check for ctoly intersection with one of the verticies */ - if ( dist[1] < ctol ) { + if (dist[1] < ctol) { dist[1] = 0; ret = 1; /* Intersection at A */ goto out; } - if ( f >= -ctol ) { + if (f >= -ctol) { dist[1] = 1; ret = 2; /* Intersection at B */ goto out; } ret = 3; /* Intersection between A and B */ out: - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("bn_isect_line2_lseg2() dist[0]=%g, dist[1]=%g, ret=%d\n", dist[0], dist[1], ret); } @@ -981,10 +976,10 @@ } /** - * B N _ I S E C T _ L S E G 2 _ L S E G 2 + * B N _ I S E C T _ L S E G 2 _ L S E G 2 *@brief - * Intersect two 2D line segments, defined by two points and two vectors. - * The vectors are unlikely to be unit length. + * Intersect two 2D line segments, defined by two points and two + * vectors. The vectors are unlikely to be unit length. * * * @return -2 missed (line segments are parallel) @@ -1000,12 +995,12 @@ *@n If within distance tolerance of the endpoints, these will be * exactly 0.0 or 1.0, to ease the job of caller. * - * Special note: when return code is "0" for co-linearity, dist[1] has - * an alternate interpretation: it's the parameter along p (not q) - * which takes you from point p to the point (q + qdir), i.e., it's - * the endpoint of the q linesegment, since in this case there may be - * *two* intersections, if q is contained within span p to (p + pdir). - * And either may be -10 if the point is outside the span. + * Special note: when return code is "0" for co-linearity, dist[1] has + * an alternate interpretation: it's the parameter along p (not q) + * which takes you from point p to the point (q + qdir), i.e., it's + * the endpoint of the q linesegment, since in this case there may be + * *two* intersections, if q is contained within span p to (p + pdir). + * And either may be -10 if the point is outside the span. * * * @param p point 1 @@ -1023,73 +1018,73 @@ const fastf_t *qdir, const struct bn_tol *tol) { - fastf_t ptol, qtol; /* length in parameter space == tol->dist */ - int status; + fastf_t ptol, qtol; /* length in parameter space == tol->dist */ + int status; BN_CK_TOL(tol); - if ( bu_debug & BU_DEBUG_MATH ) { - bu_log("bn_isect_lseg2_lseg2() p=(%g,%g), pdir=(%g,%g)\n\t\tq=(%g,%g), qdir=(%g,%g)\n", - V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir) ); + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_lseg2_lseg2() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n", + V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir)); } - status = bn_isect_line2_line2( dist, p, pdir, q, qdir, tol ); - if ( status < 0 ) { + status = bn_isect_line2_line2(dist, p, pdir, q, qdir, tol); + if (status < 0) { /* Lines are parallel, non-colinear */ return -1; /* No intersection */ } - if ( status == 0 ) { - int nogood = 0; + if (status == 0) { + int nogood = 0; /* Lines are colinear */ - /* If P within tol of either endpoint (0, 1), make exact. */ - ptol = tol->dist / sqrt( MAGSQ_2D(pdir) ); - if ( bu_debug & BU_DEBUG_MATH ) { + /* If P within tol of either endpoint (0, 1), make exact. */ + ptol = tol->dist / sqrt(MAGSQ_2D(pdir)); + if (bu_debug & BU_DEBUG_MATH) { bu_log("ptol=%g\n", ptol); } - if ( dist[0] > -ptol && dist[0] < ptol ) dist[0] = 0; - else if ( dist[0] > 1-ptol && dist[0] < 1+ptol ) dist[0] = 1; + if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0; + else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1; - if ( dist[1] > -ptol && dist[1] < ptol ) dist[1] = 0; - else if ( dist[1] > 1-ptol && dist[1] < 1+ptol ) dist[1] = 1; + if (dist[1] > -ptol && dist[1] < ptol) dist[1] = 0; + else if (dist[1] > 1-ptol && dist[1] < 1+ptol) dist[1] = 1; - if ( dist[1] < 0 || dist[1] > 1 ) nogood = 1; - if ( dist[0] < 0 || dist[0] > 1 ) nogood++; - if ( nogood >= 2 ) + if (dist[1] < 0 || dist[1] > 1) nogood = 1; + if (dist[0] < 0 || dist[0] > 1) nogood++; + if (nogood >= 2) return -1; /* colinear, but not overlapping */ - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log(" HIT colinear!\n"); } return 0; /* colinear and overlapping */ } /* Lines intersect */ - /* If within tolerance of an endpoint (0, 1), make exact. */ - ptol = tol->dist / sqrt( MAGSQ_2D(pdir) ); - if ( dist[0] > -ptol && dist[0] < ptol ) dist[0] = 0; - else if ( dist[0] > 1-ptol && dist[0] < 1+ptol ) dist[0] = 1; + /* If within tolerance of an endpoint (0, 1), make exact. */ + ptol = tol->dist / sqrt(MAGSQ_2D(pdir)); + if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0; + else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1; - qtol = tol->dist / sqrt( MAGSQ_2D(qdir) ); - if ( dist[1] > -qtol && dist[1] < qtol ) dist[1] = 0; - else if ( dist[1] > 1-qtol && dist[1] < 1+qtol ) dist[1] = 1; + qtol = tol->dist / sqrt(MAGSQ_2D(qdir)); + if (dist[1] > -qtol && dist[1] < qtol) dist[1] = 0; + else if (dist[1] > 1-qtol && dist[1] < 1+qtol) dist[1] = 1; - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("ptol=%g, qtol=%g\n", ptol, qtol); } - if ( dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1 ) { - if ( bu_debug & BU_DEBUG_MATH ) { + if (dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1) { + if (bu_debug & BU_DEBUG_MATH) { bu_log(" MISS\n"); } return -1; /* missed */ } - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log(" HIT!\n"); } return 1; /* hit, normal intersection */ } /** - * B N _ I S E C T _ L S E G 3 _ L S E G 3 + * B N _ I S E C T _ L S E G 3 _ L S E G 3 *@brief - * Intersect two 3D line segments, defined by two points and two vectors. - * The vectors are unlikely to be unit length. + * Intersect two 3D line segments, defined by two points and two + * vectors. The vectors are unlikely to be unit length. * * * @return -2 missed (line segments are parallel) @@ -1098,18 +1093,18 @@ * @return 1 hit (normal intersection) * * @param[out] dist - * The value at dist[] is set to the parametric distance of the intercept - * dist[0] is parameter along p, range 0 to 1, to intercept. - * dist[1] is parameter along q, range 0 to 1, to intercept. - * If within distance tolerance of the endpoints, these will be - * exactly 0.0 or 1.0, to ease the job of caller. + * The value at dist[] is set to the parametric distance of the + * intercept dist[0] is parameter along p, range 0 to 1, to + * intercept. dist[1] is parameter along q, range 0 to 1, to + * intercept. If within distance tolerance of the endpoints, + * these will be exactly 0.0 or 1.0, to ease the job of caller. * - * Special note: when return code is "0" for co-linearity, dist[1] has - * an alternate interpretation: it's the parameter along p (not q) - * which takes you from point p to the point (q + qdir), i.e., it's - * the endpoint of the q linesegment, since in this case there may be - * *two* intersections, if q is contained within span p to (p + pdir). - * And either may be -10 if the point is outside the span. + * Special note: when return code is "0" for co-linearity, dist[1] has + * an alternate interpretation: it's the parameter along p (not q) + * which takes you from point p to the point (q + qdir), i.e., it's + * the endpoint of the q linesegment, since in this case there may be + * *two* intersections, if q is contained within span p to (p + pdir). + * And either may be -10 if the point is outside the span. * * @param p point 1 * @param pdir direction-1 @@ -1125,89 +1120,89 @@ const fastf_t *qdir, const struct bn_tol *tol) { - fastf_t ptol, qtol; /* length in parameter space == tol->dist */ - fastf_t pmag, qmag; - int status; + fastf_t ptol, qtol; /* length in parameter space == tol->dist */ + fastf_t pmag, qmag; + int status; BN_CK_TOL(tol); - if ( bu_debug & BU_DEBUG_MATH ) { - bu_log("bn_isect_lseg3_lseg3() p=(%g,%g), pdir=(%g,%g)\n\t\tq=(%g,%g), qdir=(%g,%g)\n", - V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir) ); + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_lseg3_lseg3() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n", + V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir)); } - status = bn_isect_line3_line3( &dist[0], &dist[1], p, pdir, q, qdir, tol ); - if ( status < 0 ) { + status = bn_isect_line3_line3(&dist[0], &dist[1], p, pdir, q, qdir, tol); + if (status < 0) { /* Lines are parallel, non-colinear */ return -1; /* No intersection */ } pmag = MAGNITUDE(pdir); - if ( pmag < SMALL_FASTF ) + if (pmag < SMALL_FASTF) bu_bomb("bn_isect_lseg3_lseg3: |p|=0\n"); - if ( status == 0 ) { - int nogood = 0; + if (status == 0) { + int nogood = 0; /* Lines are colinear */ - /* If P within tol of either endpoint (0, 1), make exact. */ + /* If P within tol of either endpoint (0, 1), make exact. */ ptol = tol->dist / pmag; - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("ptol=%g\n", ptol); } - if ( dist[0] > -ptol && dist[0] < ptol ) dist[0] = 0; - else if ( dist[0] > 1-ptol && dist[0] < 1+ptol ) dist[0] = 1; + if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0; + else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1; - if ( dist[1] > -ptol && dist[1] < ptol ) dist[1] = 0; - else if ( dist[1] > 1-ptol && dist[1] < 1+ptol ) dist[1] = 1; + if (dist[1] > -ptol && dist[1] < ptol) dist[1] = 0; + else if (dist[1] > 1-ptol && dist[1] < 1+ptol) dist[1] = 1; - if ( dist[1] < 0 || dist[1] > 1 ) nogood = 1; - if ( dist[0] < 0 || dist[0] > 1 ) nogood++; - if ( nogood >= 2 ) + if (dist[1] < 0 || dist[1] > 1) nogood = 1; + if (dist[0] < 0 || dist[0] > 1) nogood++; + if (nogood >= 2) return -1; /* colinear, but not overlapping */ - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log(" HIT colinear!\n"); } return 0; /* colinear and overlapping */ } /* Lines intersect */ - /* If within tolerance of an endpoint (0, 1), make exact. */ + /* If within tolerance of an endpoint (0, 1), make exact. */ ptol = tol->dist / pmag; - if ( dist[0] > -ptol && dist[0] < ptol ) dist[0] = 0; - else if ( dist[0] > 1-ptol && dist[0] < 1+ptol ) dist[0] = 1; + if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0; + else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1; qmag = MAGNITUDE(qdir); - if ( qmag < SMALL_FASTF ) + if (qmag < SMALL_FASTF) bu_bomb("bn_isect_lseg3_lseg3: |q|=0\n"); qtol = tol->dist / qmag; - if ( dist[1] > -qtol && dist[1] < qtol ) dist[1] = 0; - else if ( dist[1] > 1-qtol && dist[1] < 1+qtol ) dist[1] = 1; + if (dist[1] > -qtol && dist[1] < qtol) dist[1] = 0; + else if (dist[1] > 1-qtol && dist[1] < 1+qtol) dist[1] = 1; - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log("ptol=%g, qtol=%g\n", ptol, qtol); } - if ( dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1 ) { - if ( bu_debug & BU_DEBUG_MATH ) { + if (dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1) { + if (bu_debug & BU_DEBUG_MATH) { bu_log(" MISS\n"); } return -1; /* missed */ } - if ( bu_debug & BU_DEBUG_MATH ) { + if (bu_debug & BU_DEBUG_MATH) { bu_log(" HIT!\n"); } return 1; /* hit, normal intersection */ } /** - * B N _ I S E C T _ L I N E 3 _ L I N E 3 + * B N _ I S E C T _ L I N E 3 _ L I N E 3 * - * Intersect two lines, each in given in parametric form: + * Intersect two lines, each in given in parametric form: * - * X = P + t * D - * and - * X = A + u * C + * X = P + t * D + * and + * X = A + u * C * - * While the parametric form is usually used to denote a ray - * (ie, positive values of the parameter only), in this case - * the full line is considered. + * While the parametric form is usually used to denote a ray (ie, + * positive values of the parameter only), in this case the full line + * is considered. * - * The direction vectors C and D need not have unit length. + * The direction vectors C and D need not have unit length. * * * @return -2 no intersection, lines are parallel. @@ -1233,8 +1228,6 @@ * substituting either of these into the original ray equations. * * XXX It would be sensible to change the t, u pair to dist[2]. - * - * */ int bn_isect_line3_line3(fastf_t *t, @@ -1245,71 +1238,69 @@ const fastf_t *c, const struct bn_tol *tol) { - vect_t n; - vect_t abs_n; - vect_t h; - register fastf_t det; - register fastf_t det1; - register short int q, r, s; + vect_t n; + vect_t abs_n; + vect_t h; + register fastf_t det; + register fastf_t det1; + register short int q, r, s; BN_CK_TOL(tol); - /* - * Any intersection will occur in the plane with surface - * normal D cross C, which may not have unit length. - * The plane containing the two lines will be a constant - * distance from a plane with the same normal that contains - * the origin. Therfore, the projection of any point on the - * plane along N has the same length. - * Verify that this holds for P and A. - * If N dot P != N dot A, there is no intersection, because - * P and A must lie on parallel planes that are different - * distances from the origin. + /* Any intersection will occur in the plane with surface normal D + * cross C, which may not have unit length. The plane containing + * the two lines will be a constant distance from a plane with the + * same normal that contains the origin. Therfore, the projection + * of any point on the plane along N has the same length. Verify + * that this holds for P and A. If N dot P != N dot A, there is + * no intersection, because P and A must lie on parallel planes + * that are different distances from the origin. */ - VCROSS( n, d, c ); - det = VDOT( n, p ) - VDOT( n, a ); - if ( !NEAR_ZERO( det, tol->dist ) ) { + VCROSS(n, d, c); + det = VDOT(n, p) - VDOT(n, a); + if (!NEAR_ZERO(det, tol->dist)) { return(-1); /* No intersection */ } - if ( NEAR_ZERO( MAGSQ( n ), SMALL_FASTF ) ) + if (NEAR_ZERO(MAGSQ(n), SMALL_FASTF)) { vect_t a_to_p; - /* lines are parallel, must find another way to get normal vector */ - VSUB2( a_to_p, p, a ); - VCROSS( n, a_to_p, d ); + /* lines are parallel, must find another way to get normal + * vector. + */ + VSUB2(a_to_p, p, a); + VCROSS(n, a_to_p, d); - if ( NEAR_ZERO( MAGSQ( n ), SMALL_FASTF ) ) - bn_vec_ortho( n, d ); + if (NEAR_ZERO(MAGSQ(n), SMALL_FASTF)) + bn_vec_ortho(n, d); } - /* - * Solve for t and u in the system: + /* Solve for t and u in the system: * - * Px + t * Dx = Ax + u * Cx - * Py + t * Dy = Ay + u * Cy - * Pz + t * Dz = Az + u * Cz + * Px + t * Dx = Ax + u * Cx + * Py + t * Dy = Ay + u * Cy + * Pz + t * Dz = Az + u * Cz * - * This system is over-determined, having 3 equations in 2 unknowns. - * However, the intersection problem is really only a 2-dimensional - * problem, being located in the surface of a plane. - * Therefore, the "least important" of these equations can - * be initially ignored, leaving a system of 2 equations in - * 2 unknowns. + * This system is over-determined, having 3 equations in 2 + * unknowns. However, the intersection problem is really only a + * 2-dimensional problem, being located in the surface of a plane. + * Therefore, the "least important" of these equations can be + * initially ignored, leaving a system of 2 equations in 2 + * unknowns. * - * Find the component of N with the largest magnitude. - * This component will have the least effect on the parameters - * in the system, being most nearly perpendicular to the plane. - * Denote the two remaining components by the - * subscripts q and r, rather than x, y, z. - * Subscript s is the smallest component, used for checking later. + * Find the component of N with the largest magnitude. This + * component will have the least effect on the parameters in the + * system, being most nearly perpendicular to the plane. Denote + * the two remaining components by the subscripts q and r, rather + * than x, y, z. Subscript s is the smallest component, used for + * checking later. */ abs_n[X] = (n[X] >= 0) ? n[X] : (-n[X]); abs_n[Y] = (n[Y] >= 0) ? n[Y] : (-n[Y]); abs_n[Z] = (n[Z] >= 0) ? n[Z] : (-n[Z]); - if ( abs_n[X] >= abs_n[Y] ) { - if ( abs_n[X] >= abs_n[Z] ) { + if (abs_n[X] >= abs_n[Y]) { + if (abs_n[X] >= abs_n[Z]) { /* X is largest in magnitude */ q = Y; r = Z; @@ -1321,7 +1312,7 @@ s = Z; } } else { - if ( abs_n[Y] >= abs_n[Z] ) { + if (abs_n[Y] >= abs_n[Z]) { /* Y is largest in magnitude */ q = X; r = Z; @@ -1337,47 +1328,49 @@ #if 0 /* XXX Use bn_isect_line2_line2() here */ /* move the 2d vectors around */ - bn_isect_line2_line2( &dist, p, d, a, c, tol ); + bn_isect_line2_line2(&dist, p, d, a, c, tol); #endif /* - * From the two components q and r, form a system - * of 2 equations in 2 unknowns: + * From the two components q and r, form a system of 2 equations + * in 2 unknowns: * - * Pq + t * Dq = Aq + u * Cq - * Pr + t * Dr = Ar + u * Cr - * or - * t * Dq - u * Cq = Aq - Pq - * t * Dr - u * Cr = Ar - Pr + * Pq + t * Dq = Aq + u * Cq + * Pr + t * Dr = Ar + u * Cr + * or + * t * Dq - u * Cq = Aq - Pq + * t * Dr - u * Cr = Ar - Pr * - * Let H = A - P, resulting in: + * Let H = A - P, resulting in: * - * t * Dq - u * Cq = Hq - * t * Dr - u * Cr = Hr + * t * Dq - u * Cq = Hq + * t * Dr - u * Cr = Hr * - * or + * or * * [ Dq -Cq ] [ t ] [ Hq ] * [ ] * [ ] = [ ] * [ Dr -Cr ] [ u ] [ Hr ] * - * This system can be solved by direct substitution, or by - * finding the determinants by Cramers rule: + * This system can be solved by direct substitution, or by finding + * the determinants by Cramers rule: * * [ Dq -Cq ] * det(M) = det [ ] = -Dq * Cr + Cq * Dr * [ Dr -Cr ] * - * If det(M) is zero, then the lines are parallel (perhaps colinear). - * Otherwise, exactly one solution exists. + * If det(M) is zero, then the lines are parallel (perhaps + * colinear). Otherwise, exactly one solution exists. */ - VSUB2( h, a, p ); + VSUB2(h, a, p); det = c[q] * d[r] - d[q] * c[r]; det1 = (c[q] * h[r] - h[q] * c[r]); /* see below */ - /* XXX This should be no smaller than 1e-16. See bn_isect_line2_line2 for details */ - if ( NEAR_ZERO( det, DETERMINANT_TOL ) ) { + /* XXX This should be no smaller than 1e-16. See + * bn_isect_line2_line2 for details. + */ + if (NEAR_ZERO(det, DETERMINANT_TOL)) { /* Lines are parallel */ - if ( !NEAR_ZERO( det1, DETERMINANT_TOL ) ) { + if (!NEAR_ZERO(det1, DETERMINANT_TOL)) { /* Lines are NOT co-linear, just parallel */ return -2; /* parallel, no intersection */ } @@ -1386,7 +1379,7 @@ /* Compute t for u=0 as a convenience to caller */ *u = 0; /* Use largest direction component */ - if ( fabs(d[q]) >= fabs(d[r]) ) { + if (fabs(d[q]) >= fabs(d[r])) { *t = h[q]/d[q]; } else { *t = h[r]/d[r]; @@ -1394,14 +1387,13 @@ return(0); /* Lines co-linear */ } - /* - * det(M) is non-zero, so there is exactly one solution. - * Using Cramer's rule, det1(M) replaces the first column - * of M with the constant column vector, in this case H. - * Similarly, det2(M) replaces the second column. - * Computation of the determinant is done as before. + /* det(M) is non-zero, so there is exactly one solution. Using + * Cramer's rule, det1(M) replaces the first column of M with the + * constant column vector, in this case H. Similarly, det2(M) + * replaces the second column. Computation of the determinant is + * done as before. * - * Now, + * Now, * * [ Hq -Cq ] * det [ ] @@ -1409,7 +1401,7 @@ * t = ------- = --------------- = ------------------ * det(M) det(M) -Dq * Cr + Cq * Dr * - * and + * and * * [ Dq Hq ] * det [ ] @@ -1421,31 +1413,32 @@ *t = det * det1; *u = det * (d[q] * h[r] - h[q] * d[r]); - /* - * Check that these values of t and u satisfy the 3rd equation - * as well! - * XXX It isn't clear that "det" is exactly a model-space distance. + /* Check that these values of t and u satisfy the 3rd equation as + * well! + * + * XXX It isn't clear that "det" is exactly a model-space + * distance. */ det = *t * d[s] - *u * c[s] - h[s]; - if ( !NEAR_ZERO( det, tol->dist ) ) { + if (!NEAR_ZERO(det, tol->dist)) { /* XXX This tolerance needs to be much less loose than - * XXX SQRT_SMALL_FASTF. What about DETERMINANT_TOL? + * SQRT_SMALL_FASTF. What about DETERMINANT_TOL? */ /* Inconsistent solution, lines miss each other */ return(-1); } - /* To prevent errors, check the answer. - * Not returning bogus results to our caller is worth the extra time. + /* To prevent errors, check the answer. Not returning bogus + * results to our caller is worth the extra time. */ { - point_t hit1, hit2; + point_t hit1, hit2; - VJOIN1( hit1, p, *t, d ); - VJOIN1( hit2, a, *u, c ); - if ( !bn_pt3_pt3_equal( hit1, hit2, tol ) ) { -/* bu_log("bn_isect_line3_l... [truncated message content] |
From: <eri...@us...> - 2010-08-27 19:17:21
|
Revision: 40354 http://brlcad.svn.sourceforge.net/brlcad/?rev=40354&view=rev Author: erikgreenwald Date: 2010-08-27 19:16:47 +0000 (Fri, 27 Aug 2010) Log Message: ----------- rogue backslash in a macro define removed (bad ws.sh) Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-08-27 18:47:27 UTC (rev 40353) +++ brlcad/trunk/src/libbn/plane.c 2010-08-27 19:16:47 UTC (rev 40354) @@ -2134,8 +2134,8 @@ VSET(local, a[X], b[Y], c[Z]); \ MAT4X3PNT(model, mat, local); \ VMINMAX(omin, omax, model) \ - \ - ROT_VERT(imin, imin, imin); + + ROT_VERT(imin, imin, imin); ROT_VERT(imin, imin, imax); ROT_VERT(imin, imax, imin); ROT_VERT(imin, imax, imax); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <br...@us...> - 2010-08-27 20:06:54
|
Revision: 40355 http://brlcad.svn.sourceforge.net/brlcad/?rev=40355&view=rev Author: brlcad Date: 2010-08-27 20:06:47 +0000 (Fri, 27 Aug 2010) Log Message: ----------- bad ROT_VERT altogether. missing semicolon after VMINMAX. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-08-27 19:16:47 UTC (rev 40354) +++ brlcad/trunk/src/libbn/plane.c 2010-08-27 20:06:47 UTC (rev 40355) @@ -2130,10 +2130,11 @@ point_t local; /* vertex point in local coordinates */ point_t model; /* vertex point in model coordinates */ -#define ROT_VERT(a, b, c) \ - VSET(local, a[X], b[Y], c[Z]); \ - MAT4X3PNT(model, mat, local); \ - VMINMAX(omin, omax, model) \ +#define ROT_VERT(a, b, c) { \ + VSET(local, a[X], b[Y], c[Z]); \ + MAT4X3PNT(model, mat, local); \ + VMINMAX(omin, omax, model); \ + } ROT_VERT(imin, imin, imin); ROT_VERT(imin, imin, imax); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2010-10-14 22:24:39
|
Revision: 40995 http://brlcad.svn.sourceforge.net/brlcad/?rev=40995&view=rev Author: r_weiss Date: 2010-10-14 22:24:32 +0000 (Thu, 14 Oct 2010) Log Message: ----------- Updated function bn_coplanar. Improved the algorithm, used distance tolerance for testing for coplanar, used a very tight tolerance (<= SMALL_FASTF) for testing for parallel. Since these are planes, if they are not as close to parallel as we can measure then they will intersect. Added bu_bomb for invalid input. The input test will slow things down some but, for now, it is better than cascade failure. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-10-14 21:07:56 UTC (rev 40994) +++ brlcad/trunk/src/libbn/plane.c 2010-10-14 22:24:32 UTC (rev 40995) @@ -2191,33 +2191,30 @@ { register fastf_t f; register fastf_t dot; - + vect_t pt_a, pt_b; + BN_CK_TOL(tol); - /* Check to see if the planes are parallel */ dot = VDOT(a, b); - if (dot >= 0) { - /* Normals head in generally the same directions */ - if (dot < tol->para) - return 0; /* Planes intersect */ + VSCALE(pt_a, a, a[3]); + VSCALE(pt_b, b, b[3]); - /* Planes have "exactly" the same normal vector */ - f = a[3] - b[3]; - if (NEAR_ZERO(f, tol->dist)) { - return 1; /* Coplanar, same direction */ - } - return -1; /* Parallel but distinct */ + if (NEAR_ZERO(MAGSQ(pt_a), SQRT_SMALL_FASTF) || NEAR_ZERO(MAGSQ(pt_b), SQRT_SMALL_FASTF)) { + bu_bomb("bn_coplanar(): zero magnitude input vector\n"); } - /* Normals head in generally opposite directions */ - if (-dot < tol->para) - return 0; /* Planes intersect */ - /* Planes have "exactly" opposite normal vectors */ - f = a[3] + b[3]; - if (NEAR_ZERO(f, tol->dist)) { - return 2; /* Coplanar, opposite directions */ + if ((dot < 0) ? (-dot >= (1 - SMALL_FASTF)) : (dot >= (1 - SMALL_FASTF))) { /* test for parallel */ + if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) { /* test for coplanar */ + if ( dot > 0 ) { /* test normals in same direction */ + return 1; + } else { + return 2; + } + } else { + return -1; + } } - return -1; /* Parallel but distinct */ + return 0; } /** This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2010-10-15 21:13:29
|
Revision: 41007 http://brlcad.svn.sourceforge.net/brlcad/?rev=41007&view=rev Author: r_weiss Date: 2010-10-15 21:13:22 +0000 (Fri, 15 Oct 2010) Log Message: ----------- Fixed some bugs in function bn_coplanar. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-10-15 21:05:47 UTC (rev 41006) +++ brlcad/trunk/src/libbn/plane.c 2010-10-15 21:13:22 UTC (rev 41007) @@ -2192,20 +2192,24 @@ register fastf_t f; register fastf_t dot; vect_t pt_a, pt_b; - BN_CK_TOL(tol); + if (!NEAR_ZERO(MAGSQ(a) - 1.0, VUNITIZE_TOL) || !NEAR_ZERO(MAGSQ(b) - 1.0, VUNITIZE_TOL)) { + bu_bomb("bn_coplanar(): input vector(s) 'a' and/or 'b' is not a unit vector.\n"); + } + dot = VDOT(a, b); VSCALE(pt_a, a, a[3]); VSCALE(pt_b, b, b[3]); - if (NEAR_ZERO(MAGSQ(pt_a), SQRT_SMALL_FASTF) || NEAR_ZERO(MAGSQ(pt_b), SQRT_SMALL_FASTF)) { - bu_bomb("bn_coplanar(): zero magnitude input vector\n"); + if (NEAR_ZERO(dot, SMALL_FASTF)) { + return 0; /* planes are perpendicular */ } - if ((dot < 0) ? (-dot >= (1 - SMALL_FASTF)) : (dot >= (1 - SMALL_FASTF))) { /* test for parallel */ + /* parallel is when dot is within SMALL_FASTF of either -1 or 1 */ + if ((dot <= -SMALL_FASTF) ? (NEAR_ZERO(dot + 1.0, SMALL_FASTF)) : (NEAR_ZERO(dot - 1.0, SMALL_FASTF))) { if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) { /* test for coplanar */ - if ( dot > 0 ) { /* test normals in same direction */ + if ( dot >= SMALL_FASTF) { /* test normals in same direction */ return 1; } else { return 2; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2010-10-15 22:13:59
|
Revision: 41012 http://brlcad.svn.sourceforge.net/brlcad/?rev=41012&view=rev Author: r_weiss Date: 2010-10-15 22:13:52 +0000 (Fri, 15 Oct 2010) Log Message: ----------- In function bn_coplanar, removed unused variable. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-10-15 22:03:32 UTC (rev 41011) +++ brlcad/trunk/src/libbn/plane.c 2010-10-15 22:13:52 UTC (rev 41012) @@ -2189,7 +2189,6 @@ int bn_coplanar(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol) { - register fastf_t f; register fastf_t dot; vect_t pt_a, pt_b; BN_CK_TOL(tol); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2010-10-16 02:49:58
|
Revision: 41013 http://brlcad.svn.sourceforge.net/brlcad/?rev=41013&view=rev Author: r_weiss Date: 2010-10-16 02:49:48 +0000 (Sat, 16 Oct 2010) Log Message: ----------- In function bn_isect_line3_line3, made many changes to tolerances and added checks for zero magnitude vectors. Made a logic change to improve detecting colinear lines. This function needs more work but I believe it is working better. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-10-15 22:13:52 UTC (rev 41012) +++ brlcad/trunk/src/libbn/plane.c 2010-10-16 02:49:48 UTC (rev 41013) @@ -1222,9 +1222,16 @@ register fastf_t det; register fastf_t det1; register short int q, r, s; + int parallel = 0; + int colinear = 0; + BN_CK_TOL(tol); + if (NEAR_ZERO(MAGSQ(c), VUNITIZE_TOL) || NEAR_ZERO(MAGSQ(d), VUNITIZE_TOL)) { + bu_bomb("bn_isect_line3_line3(): input vector(s) 'c' and/or 'd' is zero magnitude.\n"); + } + /* Any intersection will occur in the plane with surface normal D * cross C, which may not have unit length. The plane containing * the two lines will be a constant distance from a plane with the @@ -1234,26 +1241,33 @@ * no intersection, because P and A must lie on parallel planes * that are different distances from the origin. */ + VCROSS(n, d, c); det = VDOT(n, p) - VDOT(n, a); - if (!NEAR_ZERO(det, tol->dist)) { - return -1; /* No intersection */ + if (!NEAR_ZERO(det, SMALL_FASTF)) { + return -1; /* no intersection, lines not in same plane */ } - if (NEAR_ZERO(MAGSQ(n), SMALL_FASTF)) - { + if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { + /* lines are parallel, must find another way to get normal vector */ vect_t a_to_p; - /* lines are parallel, must find another way to get normal - * vector. - */ + parallel = 1; VSUB2(a_to_p, p, a); VCROSS(n, a_to_p, d); - if (NEAR_ZERO(MAGSQ(n), SMALL_FASTF)) + if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { + /* lines are parallel and colinear */ + + colinear = 1; bn_vec_ortho(n, d); + } } + if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { + bu_bomb("bn_isect_line3_line3(): ortho vector zero magnitude\n"); + } + /* Solve for t and u in the system: * * Px + t * Dx = Ax + u * Cx @@ -1274,9 +1288,19 @@ * than x, y, z. Subscript s is the smallest component, used for * checking later. */ + if (NEAR_ZERO(n[X], SMALL_FASTF)) { + n[X] = 0.0; + } + if (NEAR_ZERO(n[Y], SMALL_FASTF)) { + n[Y] = 0.0; + } + if (NEAR_ZERO(n[Z], SMALL_FASTF)) { + n[Z] = 0.0; + } abs_n[X] = (n[X] >= 0) ? n[X] : (-n[X]); abs_n[Y] = (n[Y] >= 0) ? n[Y] : (-n[Y]); abs_n[Z] = (n[Z] >= 0) ? n[Z] : (-n[Z]); + if (abs_n[X] >= abs_n[Y]) { if (abs_n[X] >= abs_n[Z]) { /* X is largest in magnitude */ @@ -1342,9 +1366,8 @@ */ if (NEAR_ZERO(det, DETERMINANT_TOL)) { /* Lines are parallel */ - if (!NEAR_ZERO(det1, DETERMINANT_TOL)) { - /* Lines are NOT co-linear, just parallel */ - return -2; /* parallel, no intersection */ + if (!colinear || !NEAR_ZERO(det1, DETERMINANT_TOL)) { + return -2; /* parallel, not colinear, no intersection */ } /* Lines are co-linear */ @@ -1392,7 +1415,7 @@ * distance. */ det = *t * d[s] - *u * c[s] - h[s]; - if (!NEAR_ZERO(det, tol->dist)) { + if (!NEAR_ZERO(det, SMALL_FASTF)) { /* XXX This tolerance needs to be much less loose than * SQRT_SMALL_FASTF. What about DETERMINANT_TOL? */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2010-11-25 02:07:35
|
Revision: 41463 http://brlcad.svn.sourceforge.net/brlcad/?rev=41463&view=rev Author: r_weiss Date: 2010-11-25 02:07:28 +0000 (Thu, 25 Nov 2010) Log Message: ----------- Made changes to functions bn_isect_line3_line3 and bn_coplanar. Updated most of the tolerances in these functions using values determined by capturing values during test runs and determining where the values converge to 0, 1, -1 etc. These tolerances are not perfect and I believe these values should be computed since they can vary. These tolerance changes appear to improve the results of the mged 'ev' command and 'facetize' command and fewer error messages are reported. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-11-24 20:37:44 UTC (rev 41462) +++ brlcad/trunk/src/libbn/plane.c 2010-11-25 02:07:28 UTC (rev 41463) @@ -1235,7 +1235,6 @@ int parallel = 0; int colinear = 0; - BN_CK_TOL(tol); if (NEAR_ZERO(MAGSQ(c), VUNITIZE_TOL) || NEAR_ZERO(MAGSQ(d), VUNITIZE_TOL)) { @@ -1254,11 +1253,12 @@ VCROSS(n, d, c); det = VDOT(n, p) - VDOT(n, a); - if (!NEAR_ZERO(det, SMALL_FASTF)) { + + if (!NEAR_ZERO(det, 1.0e-10)) { return -1; /* no intersection, lines not in same plane */ } - if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { + if (NEAR_ZERO(MAGSQ(n), 1.0e-19)) { /* lines are parallel, must find another way to get normal vector */ vect_t a_to_p; @@ -1266,7 +1266,7 @@ VSUB2(a_to_p, p, a); VCROSS(n, a_to_p, d); - if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { + if (NEAR_ZERO(MAGSQ(n), 1.0e-19)) { /* lines are parallel and colinear */ colinear = 1; @@ -1274,7 +1274,7 @@ } } - if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { + if (NEAR_ZERO(MAGSQ(n), 1.0e-19)) { bu_bomb("bn_isect_line3_line3(): ortho vector zero magnitude\n"); } @@ -1370,11 +1370,9 @@ */ VSUB2(h, a, p); det = c[q] * d[r] - d[q] * c[r]; - det1 = (c[q] * h[r] - h[q] * c[r]); /* see below */ - /* XXX This should be no smaller than 1e-16. See - * bn_isect_line2_line2 for details. - */ - if (NEAR_ZERO(det, DETERMINANT_TOL)) { + det1 = (c[q] * h[r] - h[q] * c[r]); + + if (NEAR_ZERO(det, 1.0e-10)) { /* Lines are parallel */ if (!colinear || !NEAR_ZERO(det1, DETERMINANT_TOL)) { return -2; /* parallel, not colinear, no intersection */ @@ -1420,15 +1418,9 @@ /* Check that these values of t and u satisfy the 3rd equation as * well! - * - * XXX It isn't clear that "det" is exactly a model-space - * distance. */ det = *t * d[s] - *u * c[s] - h[s]; - if (!NEAR_ZERO(det, SMALL_FASTF)) { - /* XXX This tolerance needs to be much less loose than - * SQRT_SMALL_FASTF. What about DETERMINANT_TOL? - */ + if (!NEAR_ZERO(det, 1.0e-23)) { /* Inconsistent solution, lines miss each other */ return -1; } @@ -2244,15 +2236,16 @@ } dot = VDOT(a, b); + VSCALE(pt_a, a, a[3]); VSCALE(pt_b, b, b[3]); - if (NEAR_ZERO(dot, SMALL_FASTF)) { + if (NEAR_ZERO(dot, 1.0e-8)) { return 0; /* planes are perpendicular */ } - /* parallel is when dot is within SMALL_FASTF of either -1 or 1 */ - if ((dot <= -SMALL_FASTF) ? (NEAR_ZERO(dot + 1.0, SMALL_FASTF)) : (NEAR_ZERO(dot - 1.0, SMALL_FASTF))) { + /* parallel is when dot is within 4.5e-16 of either -1 or 1 */ + if ((dot <= -SMALL_FASTF) ? (NEAR_ZERO(dot + 1.0, 4.5e-16)) : (NEAR_ZERO(dot - 1.0, 4.5e-16))) { if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) { /* test for coplanar */ if (dot >= SMALL_FASTF) { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2010-12-03 20:54:37
|
Revision: 41498 http://brlcad.svn.sourceforge.net/brlcad/?rev=41498&view=rev Author: r_weiss Date: 2010-12-03 20:54:30 +0000 (Fri, 03 Dec 2010) Log Message: ----------- Updated functions bn_coplanar and bn_isect_line3_line3. Removed the magic number tolerances. Also made additional tolerance changes which should improve facetize and 'ev'. Currently in process of testing. Initial results appear good. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2010-12-03 18:22:32 UTC (rev 41497) +++ brlcad/trunk/src/libbn/plane.c 2010-12-03 20:54:30 UTC (rev 41498) @@ -1235,6 +1235,7 @@ int parallel = 0; int colinear = 0; + BN_CK_TOL(tol); if (NEAR_ZERO(MAGSQ(c), VUNITIZE_TOL) || NEAR_ZERO(MAGSQ(d), VUNITIZE_TOL)) { @@ -1253,12 +1254,11 @@ VCROSS(n, d, c); det = VDOT(n, p) - VDOT(n, a); - - if (!NEAR_ZERO(det, 1.0e-10)) { + if (!NEAR_ZERO(det, tol->perp)) { return -1; /* no intersection, lines not in same plane */ } - if (NEAR_ZERO(MAGSQ(n), 1.0e-19)) { + if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { /* lines are parallel, must find another way to get normal vector */ vect_t a_to_p; @@ -1266,7 +1266,7 @@ VSUB2(a_to_p, p, a); VCROSS(n, a_to_p, d); - if (NEAR_ZERO(MAGSQ(n), 1.0e-19)) { + if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { /* lines are parallel and colinear */ colinear = 1; @@ -1274,7 +1274,7 @@ } } - if (NEAR_ZERO(MAGSQ(n), 1.0e-19)) { + if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { bu_bomb("bn_isect_line3_line3(): ortho vector zero magnitude\n"); } @@ -1370,11 +1370,13 @@ */ VSUB2(h, a, p); det = c[q] * d[r] - d[q] * c[r]; - det1 = (c[q] * h[r] - h[q] * c[r]); - - if (NEAR_ZERO(det, 1.0e-10)) { + det1 = (c[q] * h[r] - h[q] * c[r]); /* see below */ + /* XXX This should be no smaller than 1e-16. See + * bn_isect_line2_line2 for details. + */ + if (NEAR_ZERO(det, VUNITIZE_TOL)) { /* Lines are parallel */ - if (!colinear || !NEAR_ZERO(det1, DETERMINANT_TOL)) { + if (!colinear || !NEAR_ZERO(det1, VUNITIZE_TOL)) { return -2; /* parallel, not colinear, no intersection */ } @@ -1418,9 +1420,15 @@ /* Check that these values of t and u satisfy the 3rd equation as * well! + * + * XXX It isn't clear that "det" is exactly a model-space + * distance. */ det = *t * d[s] - *u * c[s] - h[s]; - if (!NEAR_ZERO(det, 1.0e-23)) { + if (!NEAR_ZERO(det, VUNITIZE_TOL)) { + /* XXX This tolerance needs to be much less loose than + * SQRT_SMALL_FASTF. What about DETERMINANT_TOL? + */ /* Inconsistent solution, lines miss each other */ return -1; } @@ -2236,16 +2244,15 @@ } dot = VDOT(a, b); - VSCALE(pt_a, a, a[3]); VSCALE(pt_b, b, b[3]); - if (NEAR_ZERO(dot, 1.0e-8)) { + if (NEAR_ZERO(dot, tol->perp)) { return 0; /* planes are perpendicular */ } - /* parallel is when dot is within 4.5e-16 of either -1 or 1 */ - if ((dot <= -SMALL_FASTF) ? (NEAR_ZERO(dot + 1.0, 4.5e-16)) : (NEAR_ZERO(dot - 1.0, 4.5e-16))) { + /* parallel is when dot is within tol->perp of either -1 or 1 */ + if ((dot <= -SMALL_FASTF) ? (NEAR_ZERO(dot + 1.0, tol->perp)) : (NEAR_ZERO(dot - 1.0, tol->perp))) { if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) { /* test for coplanar */ if (dot >= SMALL_FASTF) { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-04-08 15:00:35
|
Revision: 44278 http://brlcad.svn.sourceforge.net/brlcad/?rev=44278&view=rev Author: r_weiss Date: 2011-04-08 15:00:28 +0000 (Fri, 08 Apr 2011) Log Message: ----------- Added new prototype versions of the functions 'bn_isect_lseg3_lseg3' and 'bn_isect_line3_line3' to the file 'plane.c'. The names of these new function are 'bn_isect_lseg3_lseg3_new' and 'bn_isect_line3_line3_new'. These functions support the new prototype function 'nmg_triangulate_fu' (nmg triangulate faceuse). At some point the new and original version of these functions will be consolidated. Preprocessor commands were added so these new function are disabled by default. This code is a work in progress. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-04-08 03:37:51 UTC (rev 44277) +++ brlcad/trunk/src/libbn/plane.c 2011-04-08 15:00:28 UTC (rev 44278) @@ -1068,7 +1068,165 @@ } +#ifdef TRI_PROTOTYPE /** + * B N _ I S E C T _ L S E G 3 _ L S E G 3 _ N E W + *@brief + * Intersect two 3D line segments, defined by two points and two + * vectors. The vectors are unlikely to be unit length. + * + * + * @return -3 missed + * @return -2 missed (line segments are parallel) + * @return -1 missed (colinear and non-overlapping) + * @return 0 hit (line segments colinear and overlapping) + * @return 1 hit (normal intersection) + * + * @param[out] dist + * The value at dist[] is set to the parametric distance of the + * intercept dist[0] is parameter along p, range 0 to 1, to + * intercept. dist[1] is parameter along q, range 0 to 1, to + * intercept. If within distance tolerance of the endpoints, + * these will be exactly 0.0 or 1.0, to ease the job of caller. + * + * Special note: when return code is "0" for co-linearity, dist[1] has + * an alternate interpretation: it's the parameter along p (not q) + * which takes you from point p to the point (q + qdir), i.e., it's + * the endpoint of the q linesegment, since in this case there may be + * *two* intersections, if q is contained within span p to (p + pdir). + * And either may be -10 if the point is outside the span. + * + * @param p point 1 + * @param pdir direction-1 + * @param q point 2 + * @param qdir direction-2 + * @param tol tolerance values + */ +int +bn_isect_lseg3_lseg3_new(fastf_t *dist, + const fastf_t *p, + const fastf_t *pdir, + const fastf_t *q, + const fastf_t *qdir, + const struct bn_tol *tol) +{ + fastf_t ptol, qtol; /* length in parameter space == tol->dist */ + fastf_t pmag, qmag; + int status; + + BN_CK_TOL(tol); + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_lseg3_lseg3_new() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n", + V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir)); + } + + status = bn_isect_line3_line3_new(&dist[0], &dist[1], p, pdir, q, qdir, tol); + + if (status < -2 || status > 1) { + bu_bomb("bn_isect_lseg3_lseg3_new() function bn_isect_line3_line3 returned an invalid status\n"); + } + + if (status == -1) { + /* infinite lines do not intersect and are not parallel */ + return -3; /* missed */ + } + + if (status == -2) { + /* infinite lines do not intersect, they are parallel */ + return -2; /* missed (line segments are parallel) */ + } + + pmag = MAGNITUDE(pdir); + qmag = MAGNITUDE(qdir); + + if (pmag < SMALL_FASTF) + bu_bomb("bn_isect_lseg3_lseg3_new: |p|=0\n"); + + if (qmag < SMALL_FASTF) + bu_bomb("bn_isect_lseg3_lseg3_new: |q|=0\n"); + + ptol = tol->dist / pmag; + qtol = tol->dist / qmag; + dist[0] = dist[0] / pmag; + dist[1] = dist[1] / qmag; + + if (dist[0] > -ptol && dist[0] < ptol) { + dist[0] = 0.0; + } else if (dist[0] > 1.0-ptol && dist[0] < 1.0+ptol) { + dist[0] = 1.0; + } + + if (dist[1] > -qtol && dist[1] < qtol) { + dist[1] = 0.0; + } else if (dist[1] > 1.0-qtol && dist[1] < 1.0+qtol) { + dist[1] = 1.0; + } + + if (status == 0) { /* infinite lines are colinear */ +#if 0 + int nogood = 0; +#endif + /* Lines are colinear */ + /* If P within tol of either endpoint (0, 1), make exact. */ + if (bu_debug & BU_DEBUG_MATH) { + bu_log("ptol=%g\n", ptol); + } + +#if 0 + if (dist[0] <= -ptol || dist[0] >= 1+ptol) nogood = 1; + if (dist[1] <= -qtol || dist[1] >= 1+qtol) nogood++; + if (nogood >= 2) { + return -1; /* line segments are colinear but not overlapping */ + } +#endif + + if ((dist[0] > 1+ptol && dist[1] > 1+ptol) || (dist[0] < -ptol && dist[1] < -ptol)) { + return -1; /* line segments are colinear but not overlapping */ + } + + + if (bu_debug & BU_DEBUG_MATH) { + bu_log(" HIT colinear!\n"); + } + +/* + * Special note: when return code is "0" for co-linearity, dist[1] has + * an alternate interpretation: it's the parameter along p (not q) + * which takes you from point p to the point (q + qdir), i.e., it's + * the endpoint of the q linesegment, since in this case there may be + * *two* intersections, if q is contained within span p to (p + pdir). + * And either may be -10 if the point is outside the span. + */ + return 0; /* line segments are colinear and overlapping */ + } + + /* infinite lines intersect and are not colinear */ + + if (dist[0] <= -ptol || dist[0] >= 1+ptol || dist[1] <= -qtol || dist[1] >= 1+qtol) { + return -3; /* missed, infinite lines intersect but line segments do not */ + } + + if (bu_debug & BU_DEBUG_MATH) { + bu_log("ptol=%g, qtol=%g\n", ptol, qtol); + } + if (bu_debug & BU_DEBUG_MATH) { + bu_log(" HIT!\n"); + } +#if 0 + bu_log("p = %g %g %g pdir = %g %g %g dist[0] = %g ptol = %g pmag = %g q = %g %g %g qdir = %g %g %g dist[1] = %g qtol = %g qmag = %g\n", + V3ARGS(p), V3ARGS(pdir), dist[0], ptol, pmag, + V3ARGS(q), V3ARGS(qdir), dist[1], qtol, qmag); +#endif + /* sanity check */ + if (dist[0] < 0.0 || dist[0] > 1.0 || dist[1] < 0.0 || dist[1] > 1.0) { + bu_bomb("bn_isect_lseg3_lseg3_new() internal error, intersect distance values must be in the range 0-1\n"); + } + return 1; /* hit, line segments intersect */ +} +#endif + + +/** * B N _ I S E C T _ L S E G 3 _ L S E G 3 *@brief * Intersect two 3D line segments, defined by two points and two @@ -1178,7 +1336,282 @@ } +#ifdef TRI_PROTOTYPE /** + * B N _ I S E C T _ L I N E 3 _ L I N E 3 _ N E W + * + * Intersect two lines, each in given in parametric form: + * + * X = p + s * u + * and + * X = q + t * v + * + * While the parametric form is usually used to denote a ray (ie, + * positive values of the parameter only), in this case the full line + * is considered. + * + * The direction vectors u and v need not have unit length. + * + * @return -2 no intersection, lines are parallel. + * @return -1 no intersection + * @return 0 lines are co-linear (s returned for t=0 to give distance to q0) + * @return 1 intersection found (s and t returned) + * + * @param[out] s, t line parameter of interseciton + * When explicit return >= 0, s and t are the + * line parameters of the intersection point on the 2 rays. + * The actual intersection coordinates can be found by + * substituting either of these into the original ray equations. + * + * @param p0 point 1 + * @param u direction 1 + * @param q0 point 2 + * @param v direction 2 + * @param tol tolerance values + * + * s, t When explicit return >= 0, s and t are the + * line parameters of the intersection point on the 2 rays. + * The actual intersection coordinates can be found by + * substituting either of these into the original ray equations. + * + * XXX It would be sensible to change the s, t pair to dist[2]. + */ +int +bn_isect_line3_line3_new(fastf_t *s, + fastf_t *t, + const fastf_t *p0, + const fastf_t *u, + const fastf_t *q0, + const fastf_t *v, + const struct bn_tol *tol) +{ + fastf_t a, b, c, d, e, sc, tc, sc_numerator, tc_numerator, denominator; + vect_t w0, qc_to_pc, u_scaled, v_scaled, v_scaled_to_u_scaled, tmp_vec, p0_to_q1; + point_t q_intersect, p0_to_q_intersect, p1, q1; + + int parallel1 = 0; + int parallel2 = 0; + int colinear = 0; + vect_t u_unit, v_unit; + fastf_t p_dot, d1,d2,d3,d4; + + VMOVE(u_unit,u); + VMOVE(v_unit,v); + VUNITIZE(u_unit); + VUNITIZE(v_unit); + + p_dot = VDOT(u_unit,v_unit); + parallel1 = BN_VECT_ARE_PARALLEL(p_dot,tol); + + a = MAGSQ(u); + c = MAGSQ(v); + + if (NEAR_ZERO(a, SMALL_FASTF) || NEAR_ZERO(c, SMALL_FASTF)) { + bu_log("p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("u = %g %g %g\n", V3ARGS(u)); + bu_log("q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("v = %g %g %g\n", V3ARGS(v)); + bu_bomb("bn_isect_line3_line3_new(): input vector(s) 'u' and/or 'v' is zero magnitude.\n"); + } + + *s = 0.0; + *t = 0.0; + + VADD2(p1, p0, u); + VADD2(q1, q0, v); + VSUB2(p0_to_q1, q1, p0); + +#if 0 + bu_log("p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("p1 = %g %g %g\n", V3ARGS(p1)); + bu_log("u = %g %g %g\n", V3ARGS(u)); + bu_log("q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("q1 = %g %g %g\n", V3ARGS(q1)); + bu_log("v = %g %g %g\n", V3ARGS(v)); +#endif + + d1 = bn_dist_line3_pt3(q0,v,p0); + d2 = bn_dist_line3_pt3(q0,v,p1); + d3 = bn_dist_line3_pt3(p0,u,q0); + d4 = bn_dist_line3_pt3(p0,u,q1); + + /* if all distances are within distance tolerance of each + * other then they a parallel + */ + if (NEAR_ZERO(d1-d2, tol->dist) && + NEAR_ZERO(d1-d3, tol->dist) && + NEAR_ZERO(d1-d4, tol->dist) && + NEAR_ZERO(d2-d3, tol->dist) && + NEAR_ZERO(d2-d4, tol->dist) && + NEAR_ZERO(d3-d4, tol->dist)) { +#if 0 + bu_log("all values within distance tolerance of each other\n"); +#endif + parallel2 = 1; + } + + if (NEAR_ZERO(d1, tol->dist) && + NEAR_ZERO(d2, tol->dist) && + NEAR_ZERO(d3, tol->dist) && + NEAR_ZERO(d4, tol->dist)) { +#if 0 + bu_log("all values within distance tolerance of each other\n"); +#endif + colinear = 1; + } + +#if 0 + if (parallel1 != parallel2) { + bu_log("p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("p1 = %g %g %g\n", V3ARGS(p1)); + bu_log("u = %g %g %g\n", V3ARGS(u)); + bu_log("q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("q1 = %g %g %g\n", V3ARGS(q1)); + bu_log("v = %g %g %g\n", V3ARGS(v)); + bu_log("p_dot = %.15f\n", p_dot); + bu_log("dist p0 to line q0-v = %g\n", d1); + bu_log("dist p1 to line q0-v = %g\n", d2); + bu_log("dist q0 to line p0-u = %g\n", d3); + bu_log("dist q1 to line p0-u = %g\n", d4); + bu_log("parallel1 = %d parallel2 = %d\n", parallel1, parallel2); + bu_bomb("parallel1 != parallel2\n"); + } +#endif +#if 0 + bu_log("dist p0 to line q0-v = %g\n", d1); + bu_log("dist p1 to line q0-v = %g\n", d2); + bu_log("dist q0 to line p0-u = %g\n", d3); + bu_log("dist q1 to line p0-u = %g\n", d4); +#endif + + VSUB2(w0, p0, q0); + b = VDOT(u, v); + d = VDOT(u, w0); + e = VDOT(v, w0); + denominator = a * c - b * b; + +#if 0 + if (NEAR_ZERO(denominator, VUNITIZE_TOL)) { +#endif + +#if 0 + if ((denominator > SMALL_FASTF && denominator < VUNITIZE_TOL) || + (denominator < -SMALL_FASTF && denominator > -VUNITIZE_TOL)) { + bu_log("denominator btwn SMALL_FASTF VUNITIZE_TOL == %g\n", denominator); + bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); + bu_log("a * c = %.15f b * b = %.15f\n", a * c, b * b); + } +#endif + +#if 0 + if (NEAR_ZERO(denominator, SMALL_FASTF)) { +#endif + if (parallel2) { +#if 0 + if (!parallel2) { + bu_log("p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("p1 = %g %g %g\n", V3ARGS(p1)); + bu_log("u = %g %g %g\n", V3ARGS(u)); + bu_log("q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("q1 = %g %g %g\n", V3ARGS(q1)); + bu_log("v = %g %g %g\n", V3ARGS(v)); + bu_log("new true, old false; denominator = %g p_dot = %g\n", denominator, p_dot); + bu_bomb("stop\n"); + } +#endif + /* lines are parallel */ +#if 0 + tc = d/b; /* or tc = e/c */ +#endif + sc = d/a; + tc = 0.0; + VSCALE(q_intersect, v, tc); + VADD2(q_intersect, q_intersect, q0); + VSUB2(p0_to_q_intersect, q_intersect, p0); + +#if 0 + if (MAGSQ(p0_to_q_intersect) <= (tol->dist_sq)) { +#endif + if (colinear) { + if (!colinear) { + bu_log("dist p0 to line q0-v = %g\n", d1); + bu_log("dist p1 to line q0-v = %g\n", d2); + bu_log("dist q0 to line p0-u = %g\n", d3); + bu_log("dist q1 to line p0-u = %g\n", d4); + bu_bomb("colinear new true, old false\n"); + } + *s = sc * sqrt(a); + *t = 0.0; + + *t = MAGNITUDE(p0_to_q1) / sqrt(a); + + p_dot = VDOT(u, p0_to_q1); + if (p_dot < -SMALL_FASTF) { + *t = *t * -1.0; + } + +#if 0 + bu_log("colinear, distance = %.15f s = %.15f denom = %.15f\n", MAGNITUDE(p0_to_q_intersect), *s, denominator); + bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); + bu_log("p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("u = %g %g %g\n", V3ARGS(u)); + bu_log("q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("v = %g %g %g\n", V3ARGS(v)); +#endif + return 0; /* lines are co-linear (s returned for t=0 to give distance to q0) */ + } else { +#if 0 + *s = tc * sqrt(a); + *t = 0.0; + bu_log("parallel not-colinear, distance = %.15f dist p0-2-q0 = %.15f denom = %.15f\n", MAGNITUDE(p0_to_q_intersect), *s, denominator); + bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); + bu_log("p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("u = %g %g %g\n", V3ARGS(u)); + bu_log("q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("v = %g %g %g\n", V3ARGS(v)); +#endif + return -2; /* no intersection, lines are parallel */ + } + } + + if (parallel2) { + bu_log("new false, old true; denominator = %g p_dot = %g\n", denominator, p_dot); + } + + sc_numerator = (b * e - c * d); + tc_numerator = (a * e - b * d); + sc = sc_numerator / denominator; + tc = tc_numerator / denominator; + + VSCALE(u_scaled, u, sc_numerator); + VSCALE(v_scaled, v, tc_numerator); + VSUB2(v_scaled_to_u_scaled, u_scaled, v_scaled); + VSCALE(tmp_vec, v_scaled_to_u_scaled, 1.0/denominator); + VADD2(qc_to_pc, w0, tmp_vec); + + if (MAGSQ(qc_to_pc) <= tol->dist_sq) { + *s = sc * sqrt(a); + *t = tc * sqrt(c); +#if 0 + bu_log("intersect, min dist btwn lines = %.15f s = %.15f t = %.15f denom = %.15f\n", MAGNITUDE(qc_to_pc), *s, *t, denominator); + bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); + bu_log("p0_to_q1 mag = %.15f\n", MAGNITUDE(p0_to_q1)); + bu_log("p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("p1 = %g %g %g\n", V3ARGS(p1)); + bu_log("u = %g %g %g\n", V3ARGS(u)); + bu_log("q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("q1 = %g %g %g\n", V3ARGS(q1)); + bu_log("v = %g %g %g\n", V3ARGS(v)); +#endif + return 1; /* intersection found (s and t returned) */ + } else { + return -1; /* no intersection */ + } +} +#endif + + +/** * B N _ I S E C T _ L I N E 3 _ L I N E 3 * * Intersect two lines, each in given in parametric form: @@ -2861,3 +3294,9 @@ * End: * ex: shiftwidth=4 tabstop=8 */ + + + + + + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-04-13 18:07:03
|
Revision: 44357 http://brlcad.svn.sourceforge.net/brlcad/?rev=44357&view=rev Author: r_weiss Date: 2011-04-13 18:06:57 +0000 (Wed, 13 Apr 2011) Log Message: ----------- Corrected a bug in function 'bn_isect_lseg3_lseg3_new' in file 'plane.c' within the libbn library. This is a prototype version of the function 'bn_isect_lseg3_lseg3'. Preprocessor commands were added so this prototype version is disabled by default. The bug was when line segments are colinear, dist[1] returned from this function has an alternate meaning, it is the distance along the 'p' line segment (not 'q') therefore it must be scaled by the magnitude of the 'p' line segment (i.e. pmag), not the magnitude of the 'q' line segment. Added/updated/moved bu_log messages for debugging, added/updated comments. This prototype function supports the new prototype function 'nmg_triangulate_fu' (nmg triangulate faceuse). At some point the new and original version of 'bn_isect_lseg3_lseg3' will be consolidated. This code is a work in progress. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-04-13 16:13:44 UTC (rev 44356) +++ brlcad/trunk/src/libbn/plane.c 2011-04-13 18:06:57 UTC (rev 44357) @@ -1089,12 +1089,19 @@ * intercept. If within distance tolerance of the endpoints, * these will be exactly 0.0 or 1.0, to ease the job of caller. * + * CLARIFICATION: This function 'bn_isect_lseg3_lseg3_new' + * returns distance values scaled where an intersect at the start + * point of the line segement (within tol->dist) results in 0.0 + * and when the intersect is at the end point of the line + * segement (within tol->dist), the result is 1.0. Intersects + * before the start point return a negative distance. Intersects + * after the end point result in a return value > 1.0. + * * Special note: when return code is "0" for co-linearity, dist[1] has * an alternate interpretation: it's the parameter along p (not q) * which takes you from point p to the point (q + qdir), i.e., it's * the endpoint of the q linesegment, since in this case there may be * *two* intersections, if q is contained within span p to (p + pdir). - * And either may be -10 if the point is outside the span. * * @param p point 1 * @param pdir direction-1 @@ -1116,23 +1123,39 @@ BN_CK_TOL(tol); if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n", - V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir)); + bu_log("bn_isect_lseg3_lseg3_new() p=(%g, %g, %g), pdir=(%g, %g, %g)\n\t\tq=(%g, %g, %g), qdir=(%g, %g, %g)\n", + V3ARGS(p), V3ARGS(pdir), V3ARGS(q), V3ARGS(qdir)); } status = bn_isect_line3_line3_new(&dist[0], &dist[1], p, pdir, q, qdir, tol); + /* It is expected that dist[0] and dist[1] returned from + * 'bn_isect_line3_line3_new' are the actual distance to the + * intersect, i.e. not scaled. Distances in the opposite + * of the line direction vector result in a negative distance. + */ + + /* sanity check */ if (status < -2 || status > 1) { - bu_bomb("bn_isect_lseg3_lseg3_new() function bn_isect_line3_line3 returned an invalid status\n"); + bu_bomb("bn_isect_lseg3_lseg3_new() function 'bn_isect_line3_line3_new' returned an invalid status\n"); } if (status == -1) { - /* infinite lines do not intersect and are not parallel */ + /* Infinite lines do not intersect and are not parallel + * therefore line segments do not intersect and are not + * parallel. + */ + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_lseg3_lseg3_new(): MISS, line segments do not intersect and are not parallel\n"); + } return -3; /* missed */ } if (status == -2) { /* infinite lines do not intersect, they are parallel */ + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_lseg3_lseg3_new(): MISS, line segments are parallel, i.e. do not intersect\n"); + } return -2; /* missed (line segments are parallel) */ } @@ -1140,22 +1163,36 @@ qmag = MAGNITUDE(qdir); if (pmag < SMALL_FASTF) - bu_bomb("bn_isect_lseg3_lseg3_new: |p|=0\n"); + bu_bomb("bn_isect_lseg3_lseg3_new(): |p|=0\n"); if (qmag < SMALL_FASTF) - bu_bomb("bn_isect_lseg3_lseg3_new: |q|=0\n"); + bu_bomb("bn_isect_lseg3_lseg3_new(): |q|=0\n"); ptol = tol->dist / pmag; qtol = tol->dist / qmag; dist[0] = dist[0] / pmag; - dist[1] = dist[1] / qmag; + if (status == 0) { /* infinite lines are colinear */ + /* When line segments are colinear, dist[1] has an alternate + * interpretation: it's the parameter along p (not q) + * therefore dist[1] must be scaled by pmag not qmag. + */ + dist[1] = dist[1] / pmag; + } else { + dist[1] = dist[1] / qmag; + } + if (bu_debug & BU_DEBUG_MATH) { + bu_log("ptol=%g, qtol=%g\n", ptol, qtol); + } + + /* If 'p' within tol of either endpoint (0.0, 1.0), make exact. */ if (dist[0] > -ptol && dist[0] < ptol) { dist[0] = 0.0; } else if (dist[0] > 1.0-ptol && dist[0] < 1.0+ptol) { dist[0] = 1.0; } + /* If 'q' within tol of either endpoint (0.0, 1.0), make exact. */ if (dist[1] > -qtol && dist[1] < qtol) { dist[1] = 0.0; } else if (dist[1] > 1.0-qtol && dist[1] < 1.0+qtol) { @@ -1163,64 +1200,40 @@ } if (status == 0) { /* infinite lines are colinear */ -#if 0 - int nogood = 0; -#endif /* Lines are colinear */ - /* If P within tol of either endpoint (0, 1), make exact. */ - if (bu_debug & BU_DEBUG_MATH) { - bu_log("ptol=%g\n", ptol); - } - -#if 0 - if (dist[0] <= -ptol || dist[0] >= 1+ptol) nogood = 1; - if (dist[1] <= -qtol || dist[1] >= 1+qtol) nogood++; - if (nogood >= 2) { - return -1; /* line segments are colinear but not overlapping */ - } -#endif - - if ((dist[0] > 1+ptol && dist[1] > 1+ptol) || (dist[0] < -ptol && dist[1] < -ptol)) { + if ((dist[0] > 1.0+ptol && dist[1] > 1.0+ptol) || (dist[0] < -ptol && dist[1] < -ptol)) { + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_lseg3_lseg3_new(): MISS, line segments are colinear but not overlapping!\n"); + } return -1; /* line segments are colinear but not overlapping */ } - if (bu_debug & BU_DEBUG_MATH) { - bu_log(" HIT colinear!\n"); + bu_log("bn_isect_lseg3_lseg3_new(): HIT, line segments are colinear and overlapping!\n"); } -/* - * Special note: when return code is "0" for co-linearity, dist[1] has - * an alternate interpretation: it's the parameter along p (not q) - * which takes you from point p to the point (q + qdir), i.e., it's - * the endpoint of the q linesegment, since in this case there may be - * *two* intersections, if q is contained within span p to (p + pdir). - * And either may be -10 if the point is outside the span. - */ return 0; /* line segments are colinear and overlapping */ } - /* infinite lines intersect and are not colinear */ + /* At this point we know the infinite lines intersect and are not colinear */ - if (dist[0] <= -ptol || dist[0] >= 1+ptol || dist[1] <= -qtol || dist[1] >= 1+qtol) { + + if (dist[0] <= -ptol || dist[0] >= 1.0+ptol || dist[1] <= -qtol || dist[1] >= 1.0+qtol) { + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_lseg3_lseg3_new(): MISS, infinite lines intersect but line segments do not!\n"); + } return -3; /* missed, infinite lines intersect but line segments do not */ } if (bu_debug & BU_DEBUG_MATH) { - bu_log("ptol=%g, qtol=%g\n", ptol, qtol); + bu_log("bn_isect_lseg3_lseg3_new(): HIT, line segments intersect!\n"); } - if (bu_debug & BU_DEBUG_MATH) { - bu_log(" HIT!\n"); - } -#if 0 - bu_log("p = %g %g %g pdir = %g %g %g dist[0] = %g ptol = %g pmag = %g q = %g %g %g qdir = %g %g %g dist[1] = %g qtol = %g qmag = %g\n", - V3ARGS(p), V3ARGS(pdir), dist[0], ptol, pmag, - V3ARGS(q), V3ARGS(qdir), dist[1], qtol, qmag); -#endif + /* sanity check */ if (dist[0] < 0.0 || dist[0] > 1.0 || dist[1] < 0.0 || dist[1] > 1.0) { - bu_bomb("bn_isect_lseg3_lseg3_new() internal error, intersect distance values must be in the range 0-1\n"); + bu_bomb("bn_isect_lseg3_lseg3_new(): INTERNAL ERROR, intersect distance values must be in the range 0-1\n"); } + return 1; /* hit, line segments intersect */ } #endif This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-07-18 21:23:34
|
Revision: 45531 http://brlcad.svn.sourceforge.net/brlcad/?rev=45531&view=rev Author: r_weiss Date: 2011-07-18 21:23:26 +0000 (Mon, 18 Jul 2011) Log Message: ----------- Created a new prototype version of function bn_isect_line_lseg in the libbn library within file plane.c. This new version is disabled by default and exists to support the prototype version of nmg_triangulate_fu. The changes to this function were necessary to process the output from the prototype function bn_isect_line3_line3_new. This is a work in progress. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-07-18 20:55:33 UTC (rev 45530) +++ brlcad/trunk/src/libbn/plane.c 2011-07-18 21:23:26 UTC (rev 45531) @@ -1933,6 +1933,129 @@ int bn_isect_line_lseg(fastf_t *t, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol) { +#ifdef TRI_PROTOTYPE + vect_t ab, pa, pb; /* direction vectors a->b, p->a, p->b */ + auto fastf_t u; /* As in, A + u * C = X */ + register int ret; + + fastf_t ab_mag; + fastf_t pa_mag_sq; + fastf_t pb_mag_sq; + fastf_t d_mag_sq; + + BN_CK_TOL(tol); + + d_mag_sq = MAGSQ(d); + if (ZERO(d_mag_sq)) { + bu_bomb("bn_isect_line_lseg(): ray direction vector zero magnitude\n"); + } + + VSUB2(ab, b, a); + ab_mag = MAGNITUDE(ab); + if (ab_mag < tol->dist) { + /* points A and B are not distinct */ + return -4; + } + + VSUB2(pa, a, p); + pa_mag_sq = MAGSQ(pa); + if (pa_mag_sq < tol->dist_sq) { + /* Intersection at vertex A */ + *t = sqrt(pa_mag_sq); + return 1; + } + + VSUB2(pb, b, p); + pb_mag_sq = MAGSQ(pb); + if (pb_mag_sq < tol->dist_sq) { + /* Intersection at vertex B */ + *t = sqrt(pb_mag_sq); + return 2; + } + + /* Detecting colinearity is difficult, and very very important. + * As a first step, check to see if both points A and B lie within + * tolerance of the line. If so, then the line segment AC is ON + * the line. + */ + if (bn_distsq_line3_pt3(p, d, a) <= tol->dist_sq && + bn_distsq_line3_pt3(p, d, b) <= tol->dist_sq) { + if (bu_debug & BU_DEBUG_MATH) { + bu_log("bn_isect_line3_lseg3() pts A and B within tol of line\n"); + } + /* Find the parametric distance along the ray */ + *t = bn_dist_pt3_along_line3(p, d, a); + + if (*t < -tol->dist) { + /* intersection of ray and line segment but in the + * negative direction of the ray + */ + return -1; + } else { + if (ZERO(*t)) { + *t = 0.0; + } + /* co-linear (t was computed for point A, u=0) */ + return 0; + } + } + + if ((ret = bn_isect_line3_line3_new(t, &u, p, d, a, ab, tol)) < 0) { + /* No intersection found */ + return -1; + } + + if (ret == 0) { + /* co-linear (t was computed for point A, u=0) */ + return 0; + } + + if (ZERO(*t)) { + *t = 0.0; + } + + if (ZERO(u)) { + u = 0.0; + } + + if (*t < -tol->dist) { + /* intersection of ray and line segment but in the + * negative direction of the ray + */ + return -1; + } + + if (NEAR_ZERO(u, tol->dist)) { + /* Intersection at vertex A */ + /* use actual distance instead of hit point */ + *t = sqrt(pa_mag_sq); + return 1; + } + if (u < -tol->dist) { + /* Intersection exists, < A (t is returned) */ + return -3; + } + + /* the computation (u - ab_mag) might cause some problems + * because 'u' can be negative but 'ab_mag' can not + */ + if (NEAR_ZERO(u - ab_mag, tol->dist)) { + /* Intersection at vertex B */ + /* use actual distance instead of hit point */ + *t = sqrt(pb_mag_sq); + return 2; + } + + if (u > ab_mag + tol->dist) { + /* Intersection exists, > B (t is returned) */ + return -2; + } + + /* Intersection between A and B */ + return 3; + +#else + vect_t c; /* Direction vector from A to B */ auto fastf_t u; /* As in, A + u * C = X */ register fastf_t f; @@ -1995,6 +2118,8 @@ return 2; /* Intersection at B */ return 3; /* Intersection between A and B */ + +#endif } @@ -3308,8 +3433,3 @@ * ex: shiftwidth=4 tabstop=8 */ - - - - - This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-07-20 23:58:35
|
Revision: 45558 http://brlcad.svn.sourceforge.net/brlcad/?rev=45558&view=rev Author: r_weiss Date: 2011-07-20 23:58:29 +0000 (Wed, 20 Jul 2011) Log Message: ----------- Updated the protoype function bn_isect_line3_line3_new within file plane.c which supports the prototype function nmg_triangulate_fu. These changed are disabled by default. Performed code cleanup and improved some of the logic. More cleanup is needed. This is a work in progress. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-07-20 23:30:03 UTC (rev 45557) +++ brlcad/trunk/src/libbn/plane.c 2011-07-20 23:58:29 UTC (rev 45558) @@ -1387,67 +1387,82 @@ * The actual intersection coordinates can be found by * substituting either of these into the original ray equations. * + * The 'pdist' and 'qdist' values returned from this function + * are the actual distance to the intersect, i.e. not scaled. + * Distances in the opposite of the line direction vector result + * in a negative distance. + * + * The input vectors 'pdir' and 'qdir' must NOT be unit vectors + * for this function to work correctly. + * * XXX It would be sensible to change the s, t pair to dist[2]. */ int -bn_isect_line3_line3_new(fastf_t *s, - fastf_t *t, - const fastf_t *p0, - const fastf_t *u, - const fastf_t *q0, - const fastf_t *v, +bn_isect_line3_line3_new(fastf_t *pdist, /* distance from p0 to line q intersect, can be negative (s) */ + fastf_t *qdist, /* distance from q0 to line p intersect, can be negative (t) */ + const fastf_t *p0, /* line p start point */ + const fastf_t *pdir_i, /* line p direction, must not be a unit vector (u) */ + const fastf_t *q0, /* line q start point */ + const fastf_t *qdir_i, /* line q direction, must not be a unit vector (v) */ const struct bn_tol *tol) { - fastf_t a, b, c, d, e, sc, tc, sc_numerator, tc_numerator, denominator; + fastf_t b, d, e, sc, tc, sc_numerator, tc_numerator, denominator; vect_t w0, qc_to_pc, u_scaled, v_scaled, v_scaled_to_u_scaled, tmp_vec, p0_to_q1; - point_t q_intersect, p0_to_q_intersect, p1, q1; + point_t p1, q1; + fastf_t pdir_mag_sq; + fastf_t qdir_mag_sq; - int parallel1 = 0; - int parallel2 = 0; + int parallel = 0; int colinear = 0; - vect_t u_unit, v_unit; - fastf_t p_dot, d1,d2,d3,d4; + int error_occured = 0; + fastf_t dot, d1, d2, d3, d4; + vect_t pdir, qdir; - VMOVE(u_unit,u); - VMOVE(v_unit,v); - VUNITIZE(u_unit); - VUNITIZE(v_unit); + VMOVE(pdir, pdir_i); + VMOVE(qdir, qdir_i); - p_dot = VDOT(u_unit,v_unit); - parallel1 = BN_VECT_ARE_PARALLEL(p_dot,tol); + pdir_mag_sq = MAGSQ(pdir); + qdir_mag_sq = MAGSQ(qdir); - a = MAGSQ(u); - c = MAGSQ(v); + if (NEAR_ZERO(pdir_mag_sq, SMALL_FASTF) || NEAR_ZERO(qdir_mag_sq, SMALL_FASTF)) { + bu_log(" p0 = %g %g %g\n", V3ARGS(p0)); + bu_log("pdir = %g %g %g\n", V3ARGS(pdir)); + bu_log(" q0 = %g %g %g\n", V3ARGS(q0)); + bu_log("qdir = %g %g %g\n", V3ARGS(qdir)); + bu_bomb("bn_isect_line3_line3_new(): input vector(s) 'pdir' and/or 'qdir' is zero magnitude.\n"); + } - if (NEAR_ZERO(a, SMALL_FASTF) || NEAR_ZERO(c, SMALL_FASTF)) { - bu_log("p0 = %g %g %g\n", V3ARGS(p0)); - bu_log("u = %g %g %g\n", V3ARGS(u)); - bu_log("q0 = %g %g %g\n", V3ARGS(q0)); - bu_log("v = %g %g %g\n", V3ARGS(v)); - bu_bomb("bn_isect_line3_line3_new(): input vector(s) 'u' and/or 'v' is zero magnitude.\n"); + if (NEAR_ZERO(pdir_mag_sq - 1.0, SMALL_FASTF)) { + VSCALE(pdir, pdir, 304800); /* 304800mm = 1000ft */ + pdir_mag_sq = MAGSQ(pdir); + error_occured++; } - *s = 0.0; - *t = 0.0; + if (NEAR_ZERO(qdir_mag_sq - 1.0, SMALL_FASTF)) { + VSCALE(qdir, qdir, 304800); /* 304800mm = 1000ft */ + qdir_mag_sq = MAGSQ(qdir); + error_occured++; + } - VADD2(p1, p0, u); - VADD2(q1, q0, v); - VSUB2(p0_to_q1, q1, p0); + if (error_occured == 2) { + bu_log("pdir = %g %g %g qdir = %g %g %g\n", V3ARGS(pdir_i), V3ARGS(qdir_i)); + bu_log("bn_isect_line3_line3_new(): input vector(s) 'pdir' and 'qdir' are unit vectors.\n"); + } -#if 0 - bu_log("p0 = %g %g %g\n", V3ARGS(p0)); - bu_log("p1 = %g %g %g\n", V3ARGS(p1)); - bu_log("u = %g %g %g\n", V3ARGS(u)); - bu_log("q0 = %g %g %g\n", V3ARGS(q0)); - bu_log("q1 = %g %g %g\n", V3ARGS(q1)); - bu_log("v = %g %g %g\n", V3ARGS(v)); -#endif + *pdist = 0.0; + *qdist = 0.0; - d1 = bn_dist_line3_pt3(q0,v,p0); - d2 = bn_dist_line3_pt3(q0,v,p1); - d3 = bn_dist_line3_pt3(p0,u,q0); - d4 = bn_dist_line3_pt3(p0,u,q1); + /* assumes pdir & qdir are not unit vectors */ + VADD2(p1, p0, pdir); + VADD2(q1, q0, qdir); + VSUB2(p0_to_q1, q1, p0); + + d1 = bn_dist_line3_pt3(q0,qdir,p0); + d2 = bn_dist_line3_pt3(q0,qdir,p1); + d3 = bn_dist_line3_pt3(p0,pdir,q0); + d4 = bn_dist_line3_pt3(p0,pdir,q1); + /* if all distances are within distance tolerance of each * other then they a parallel */ @@ -1457,165 +1472,74 @@ NEAR_EQUAL(d2, d3, tol->dist) && NEAR_EQUAL(d2, d4, tol->dist) && NEAR_EQUAL(d3, d4, tol->dist)) { -#if 0 - bu_log("all values within distance tolerance of each other\n"); -#endif - parallel2 = 1; + parallel = 1; } if (NEAR_ZERO(d1, tol->dist) && NEAR_ZERO(d2, tol->dist) && NEAR_ZERO(d3, tol->dist) && NEAR_ZERO(d4, tol->dist)) { -#if 0 - bu_log("all values within distance tolerance of each other\n"); -#endif colinear = 1; } -#if 0 - if (parallel1 != parallel2) { - bu_log("p0 = %g %g %g\n", V3ARGS(p0)); - bu_log("p1 = %g %g %g\n", V3ARGS(p1)); - bu_log("u = %g %g %g\n", V3ARGS(u)); - bu_log("q0 = %g %g %g\n", V3ARGS(q0)); - bu_log("q1 = %g %g %g\n", V3ARGS(q1)); - bu_log("v = %g %g %g\n", V3ARGS(v)); - bu_log("p_dot = %.15f\n", p_dot); - bu_log("dist p0 to line q0-v = %g\n", d1); - bu_log("dist p1 to line q0-v = %g\n", d2); - bu_log("dist q0 to line p0-u = %g\n", d3); - bu_log("dist q1 to line p0-u = %g\n", d4); - bu_log("parallel1 = %d parallel2 = %d\n", parallel1, parallel2); - bu_bomb("parallel1 != parallel2\n"); - } -#endif -#if 0 - bu_log("dist p0 to line q0-v = %g\n", d1); - bu_log("dist p1 to line q0-v = %g\n", d2); - bu_log("dist q0 to line p0-u = %g\n", d3); - bu_log("dist q1 to line p0-u = %g\n", d4); -#endif - VSUB2(w0, p0, q0); - b = VDOT(u, v); - d = VDOT(u, w0); - e = VDOT(v, w0); - denominator = a * c - b * b; + b = VDOT(pdir, qdir); + d = VDOT(pdir, w0); + e = VDOT(qdir, w0); + denominator = pdir_mag_sq * qdir_mag_sq - b * b; -#if 0 - if (NEAR_ZERO(denominator, VUNITIZE_TOL)) { -#endif - -#if 0 - if ((denominator > SMALL_FASTF && denominator < VUNITIZE_TOL) || - (denominator < -SMALL_FASTF && denominator > -VUNITIZE_TOL)) { - bu_log("denominator btwn SMALL_FASTF VUNITIZE_TOL == %g\n", denominator); - bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); - bu_log("a * c = %.15f b * b = %.15f\n", a * c, b * b); + if (!parallel && colinear) { + bu_bomb("bn_isect_line3_line3_new(): logic error, lines colinear but not parallel\n"); } -#endif -#if 0 - if (NEAR_ZERO(denominator, SMALL_FASTF)) { -#endif - if (parallel2) { -#if 0 - if (!parallel2) { - bu_log("p0 = %g %g %g\n", V3ARGS(p0)); - bu_log("p1 = %g %g %g\n", V3ARGS(p1)); - bu_log("u = %g %g %g\n", V3ARGS(u)); - bu_log("q0 = %g %g %g\n", V3ARGS(q0)); - bu_log("q1 = %g %g %g\n", V3ARGS(q1)); - bu_log("v = %g %g %g\n", V3ARGS(v)); - bu_log("new true, old false; denominator = %g p_dot = %g\n", denominator, p_dot); - bu_bomb("stop\n"); - } -#endif + if (parallel && !colinear) { /* lines are parallel */ -#if 0 - tc = d/b; /* or tc = e/c */ -#endif - sc = d/a; + sc = d / pdir_mag_sq; tc = 0.0; - VSCALE(q_intersect, v, tc); - VADD2(q_intersect, q_intersect, q0); - VSUB2(p0_to_q_intersect, q_intersect, p0); + return -2; /* no intersection, lines are parallel */ + } -#if 0 - if (MAGSQ(p0_to_q_intersect) <= (tol->dist_sq)) { -#endif - if (colinear) { - if (!colinear) { - bu_log("dist p0 to line q0-v = %g\n", d1); - bu_log("dist p1 to line q0-v = %g\n", d2); - bu_log("dist q0 to line p0-u = %g\n", d3); - bu_log("dist q1 to line p0-u = %g\n", d4); - bu_bomb("colinear new true, old false\n"); - } - *s = sc * sqrt(a); - *t = 0.0; + if (parallel && colinear) { - *t = MAGNITUDE(p0_to_q1) / sqrt(a); + /* when colinear pdist has a different meaning, it is the + * distance from p0 to q0 + */ + *pdist = MAGNITUDE(w0); /* w0 is opposite direction of p0 to q0 */ + dot = VDOT(pdir, w0); + if (dot > SMALL_FASTF) { + *pdist = -(*pdist); + } - p_dot = VDOT(u, p0_to_q1); - if (p_dot < -SMALL_FASTF) { - *t = *t * -1.0; - } + /* when colinear qdist has a different meaning, it is the + * distance from p0 to q1 + */ + *qdist = MAGNITUDE(p0_to_q1); -#if 0 - bu_log("colinear, distance = %.15f s = %.15f denom = %.15f\n", MAGNITUDE(p0_to_q_intersect), *s, denominator); - bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); - bu_log("p0 = %g %g %g\n", V3ARGS(p0)); - bu_log("u = %g %g %g\n", V3ARGS(u)); - bu_log("q0 = %g %g %g\n", V3ARGS(q0)); - bu_log("v = %g %g %g\n", V3ARGS(v)); -#endif - return 0; /* lines are co-linear (s returned for t=0 to give distance to q0) */ - } else { -#if 0 - *s = tc * sqrt(a); - *t = 0.0; - bu_log("parallel not-colinear, distance = %.15f dist p0-2-q0 = %.15f denom = %.15f\n", MAGNITUDE(p0_to_q_intersect), *s, denominator); - bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); - bu_log("p0 = %g %g %g\n", V3ARGS(p0)); - bu_log("u = %g %g %g\n", V3ARGS(u)); - bu_log("q0 = %g %g %g\n", V3ARGS(q0)); - bu_log("v = %g %g %g\n", V3ARGS(v)); -#endif - return -2; /* no intersection, lines are parallel */ + /* if vectors pdir and p0_to_q1 are not the same direction + * then make the distance negative + */ + dot = VDOT(pdir, p0_to_q1); + if (dot < -SMALL_FASTF) { + *qdist = -(*qdist); } - } - if (parallel2) { - bu_log("new false, old true; denominator = %g p_dot = %g\n", denominator, p_dot); + return 0; } - sc_numerator = (b * e - c * d); - tc_numerator = (a * e - b * d); + sc_numerator = (b * e - qdir_mag_sq * d); + tc_numerator = (pdir_mag_sq * e - b * d); sc = sc_numerator / denominator; tc = tc_numerator / denominator; - VSCALE(u_scaled, u, sc_numerator); - VSCALE(v_scaled, v, tc_numerator); + VSCALE(u_scaled, pdir, sc_numerator); + VSCALE(v_scaled, qdir, tc_numerator); VSUB2(v_scaled_to_u_scaled, u_scaled, v_scaled); VSCALE(tmp_vec, v_scaled_to_u_scaled, 1.0/denominator); VADD2(qc_to_pc, w0, tmp_vec); if (MAGSQ(qc_to_pc) <= tol->dist_sq) { - *s = sc * sqrt(a); - *t = tc * sqrt(c); -#if 0 - bu_log("intersect, min dist btwn lines = %.15f s = %.15f t = %.15f denom = %.15f\n", MAGNITUDE(qc_to_pc), *s, *t, denominator); - bu_log("a = %.15f c = %.15f b = %.15f\n", a, c, b); - bu_log("p0_to_q1 mag = %.15f\n", MAGNITUDE(p0_to_q1)); - bu_log("p0 = %g %g %g\n", V3ARGS(p0)); - bu_log("p1 = %g %g %g\n", V3ARGS(p1)); - bu_log("u = %g %g %g\n", V3ARGS(u)); - bu_log("q0 = %g %g %g\n", V3ARGS(q0)); - bu_log("q1 = %g %g %g\n", V3ARGS(q1)); - bu_log("v = %g %g %g\n", V3ARGS(v)); -#endif + *pdist = sc * sqrt(pdir_mag_sq); + *qdist = tc * sqrt(qdir_mag_sq); return 1; /* intersection found (s and t returned) */ } else { return -1; /* no intersection */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-07-21 23:31:13
|
Revision: 45569 http://brlcad.svn.sourceforge.net/brlcad/?rev=45569&view=rev Author: r_weiss Date: 2011-07-21 23:31:06 +0000 (Thu, 21 Jul 2011) Log Message: ----------- Updated the prototype version of function bn_isect_line_lseg and bn_isect_line3_line3_new within the libbn library within file plane.c. These changes are disabled by default. These functions support the prototype function nmg_triangulate_fu. Made changes to code logic and performed code cleanup. This is a work in progress. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-07-21 21:24:56 UTC (rev 45568) +++ brlcad/trunk/src/libbn/plane.c 2011-07-21 23:31:06 UTC (rev 45569) @@ -1414,7 +1414,6 @@ int parallel = 0; int colinear = 0; - int error_occured = 0; fastf_t dot, d1, d2, d3, d4; vect_t pdir, qdir; @@ -1424,7 +1423,7 @@ pdir_mag_sq = MAGSQ(pdir); qdir_mag_sq = MAGSQ(qdir); - if (NEAR_ZERO(pdir_mag_sq, SMALL_FASTF) || NEAR_ZERO(qdir_mag_sq, SMALL_FASTF)) { + if ((pdir_mag_sq < tol->dist_sq) || (qdir_mag_sq < tol->dist_sq)) { bu_log(" p0 = %g %g %g\n", V3ARGS(p0)); bu_log("pdir = %g %g %g\n", V3ARGS(pdir)); bu_log(" q0 = %g %g %g\n", V3ARGS(q0)); @@ -1432,23 +1431,6 @@ bu_bomb("bn_isect_line3_line3_new(): input vector(s) 'pdir' and/or 'qdir' is zero magnitude.\n"); } - if (NEAR_ZERO(pdir_mag_sq - 1.0, SMALL_FASTF)) { - VSCALE(pdir, pdir, 304800); /* 304800mm = 1000ft */ - pdir_mag_sq = MAGSQ(pdir); - error_occured++; - } - - if (NEAR_ZERO(qdir_mag_sq - 1.0, SMALL_FASTF)) { - VSCALE(qdir, qdir, 304800); /* 304800mm = 1000ft */ - qdir_mag_sq = MAGSQ(qdir); - error_occured++; - } - - if (error_occured == 2) { - bu_log("pdir = %g %g %g qdir = %g %g %g\n", V3ARGS(pdir_i), V3ARGS(qdir_i)); - bu_log("bn_isect_line3_line3_new(): input vector(s) 'pdir' and 'qdir' are unit vectors.\n"); - } - *pdist = 0.0; *qdist = 0.0; @@ -1458,27 +1440,27 @@ VSUB2(p0_to_q1, q1, p0); - d1 = bn_dist_line3_pt3(q0,qdir,p0); - d2 = bn_dist_line3_pt3(q0,qdir,p1); - d3 = bn_dist_line3_pt3(p0,pdir,q0); - d4 = bn_dist_line3_pt3(p0,pdir,q1); + d1 = bn_distsq_line3_pt3(q0,qdir,p0); + d2 = bn_distsq_line3_pt3(q0,qdir,p1); + d3 = bn_distsq_line3_pt3(p0,pdir,q0); + d4 = bn_distsq_line3_pt3(p0,pdir,q1); /* if all distances are within distance tolerance of each * other then they a parallel */ - if (NEAR_EQUAL(d1, d2, tol->dist) && - NEAR_EQUAL(d1, d3, tol->dist) && - NEAR_EQUAL(d1, d4, tol->dist) && - NEAR_EQUAL(d2, d3, tol->dist) && - NEAR_EQUAL(d2, d4, tol->dist) && - NEAR_EQUAL(d3, d4, tol->dist)) { + if (NEAR_EQUAL(d1, d2, tol->dist_sq) && + NEAR_EQUAL(d1, d3, tol->dist_sq) && + NEAR_EQUAL(d1, d4, tol->dist_sq) && + NEAR_EQUAL(d2, d3, tol->dist_sq) && + NEAR_EQUAL(d2, d4, tol->dist_sq) && + NEAR_EQUAL(d3, d4, tol->dist_sq)) { parallel = 1; } - if (NEAR_ZERO(d1, tol->dist) && - NEAR_ZERO(d2, tol->dist) && - NEAR_ZERO(d3, tol->dist) && - NEAR_ZERO(d4, tol->dist)) { + if (NEAR_ZERO(d1, tol->dist_sq) && + NEAR_ZERO(d2, tol->dist_sq) && + NEAR_ZERO(d3, tol->dist_sq) && + NEAR_ZERO(d4, tol->dist_sq)) { colinear = 1; } @@ -1859,18 +1841,21 @@ { #ifdef TRI_PROTOTYPE vect_t ab, pa, pb; /* direction vectors a->b, p->a, p->b */ - auto fastf_t u; /* As in, A + u * C = X */ - register int ret; - fastf_t ab_mag; fastf_t pa_mag_sq; fastf_t pb_mag_sq; fastf_t d_mag_sq; + int code; + fastf_t dist1, dist2, d1, d2; + int colinear = 0; + fastf_t dot; BN_CK_TOL(tol); + *t = 0.0; + d_mag_sq = MAGSQ(d); - if (ZERO(d_mag_sq)) { + if (NEAR_ZERO(d_mag_sq, tol->dist_sq)) { bu_bomb("bn_isect_line_lseg(): ray direction vector zero magnitude\n"); } @@ -1897,87 +1882,164 @@ return 2; } - /* Detecting colinearity is difficult, and very very important. - * As a first step, check to see if both points A and B lie within - * tolerance of the line. If so, then the line segment AC is ON - * the line. + /* just check that the vertices of the line segement are + * within distance tolerance of the ray. it may cause problems + * to also require the ray start and end points to be within + * distance tolerance of the infinite line associated with + * the line segement. */ - if (bn_distsq_line3_pt3(p, d, a) <= tol->dist_sq && - bn_distsq_line3_pt3(p, d, b) <= tol->dist_sq) { - if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_line3_lseg3() pts A and B within tol of line\n"); - } - /* Find the parametric distance along the ray */ - *t = bn_dist_pt3_along_line3(p, d, a); + d1 = bn_distsq_line3_pt3(p,d,a); /* distance of point a to ray */ + d2 = bn_distsq_line3_pt3(p,d,b); /* distance of point b to ray */ - if (*t < -tol->dist) { - /* intersection of ray and line segment but in the - * negative direction of the ray - */ - return -1; - } else { - if (ZERO(*t)) { - *t = 0.0; - } - /* co-linear (t was computed for point A, u=0) */ - return 0; + colinear = 0; + if (NEAR_ZERO(d1, tol->dist_sq) && NEAR_ZERO(d2, tol->dist_sq)) { + colinear = 1; + + dist1 = sqrt(pa_mag_sq); + dist2 = sqrt(pb_mag_sq); + + /* if the direction of the pa vector is in the + * opposite direction of the ray, then make the + * distance negative + */ + dot = VDOT(pa, d); + if (dot < -SMALL_FASTF) { + dist1 = -dist1; } + + /* if the direction of the pb vector is in the + * opposite direction of the ray, then make the + * distance negative + */ + dot = VDOT(pb, d); + if (dot < -SMALL_FASTF) { + dist2 = -dist2; + } } - if ((ret = bn_isect_line3_line3_new(t, &u, p, d, a, ab, tol)) < 0) { - /* No intersection found */ - return -1; + if (colinear && dist1 < SMALL_FASTF && dist2 < SMALL_FASTF) { + /* lines are colinear but 'a' and 'b' are not on the ray */ + return -1; /* no intersection */ } - if (ret == 0) { - /* co-linear (t was computed for point A, u=0) */ - return 0; + if (colinear && (dist1 > SMALL_FASTF) && (dist2 > SMALL_FASTF)) { + /* lines are colinear and both points 'a' and 'b' are on the ray. */ + /* return the distance to the closest point */ + if (dist2 > dist1) { + *t = dist1; + } else { + *t = dist2; + } + return 0; } - if (ZERO(*t)) { - *t = 0.0; + if (colinear && (dist1 > SMALL_FASTF) && (dist2 < SMALL_FASTF)) { + /* lines are colinear and 'a' is on the ray but 'b' is not. */ + /* return the distance to 'a' */ + *t = dist1; + return 0; } - if (ZERO(u)) { - u = 0.0; + if (colinear && (dist1 < SMALL_FASTF) && (dist2 > SMALL_FASTF)) { + /* lines are colinear and 'b' is on the ray but 'a' is not. */ + /* return the distance to 'b' */ + *t = dist2; + return 0; } - if (*t < -tol->dist) { - /* intersection of ray and line segment but in the - * negative direction of the ray - */ - return -1; + dist1 = 0.0; /* sanity */ + dist2 = 0.0; /* sanity */ + code = bn_isect_line3_line3_new(&dist1, &dist2, p, d, a, ab, tol); + + if (code == 0) { + bu_bomb("bn_isect_line_lseg(): we should have already detected a colinear condition\n"); } - if (NEAR_ZERO(u, tol->dist)) { - /* Intersection at vertex A */ - /* use actual distance instead of hit point */ - *t = sqrt(pa_mag_sq); - return 1; + if (code < 0) { + return -1; /* no intersection */ } - if (u < -tol->dist) { - /* Intersection exists, < A (t is returned) */ - return -3; - } - /* the computation (u - ab_mag) might cause some problems - * because 'u' can be negative but 'ab_mag' can not - */ - if (NEAR_ZERO(u - ab_mag, tol->dist)) { - /* Intersection at vertex B */ - /* use actual distance instead of hit point */ - *t = sqrt(pb_mag_sq); - return 2; + if (code == 1) { + if (dist1 < -(tol->dist)) { + /* the ray did isect the line segment but in the + * negative direction so this is not really a hit + */ + return -1; /* no intersection */ + } } - if (u > ab_mag + tol->dist) { - /* Intersection exists, > B (t is returned) */ - return -2; + if (code == 1) { + /* determine if isect was before a, between a & b or after b */ + vect_t d_unit; + vect_t a_to_isect_pt, b_to_isect_pt; + point_t isect_pt; + fastf_t a_to_isect_pt_mag_sq, b_to_isect_pt_mag_sq; + + VMOVE(d_unit, d); + VUNITIZE(d_unit); + +#if 0 + if (NEAR_ZERO(dist1, tol->dist)) { + bu_log("bn_isect_line_lseg(): dist1 = %.15f\n", dist1); + bu_bomb("bn_isect_line_lseg(): dist1 is zero\n"); + } +#endif + + dist1 = fabs(dist1); /* sanity */ + VSCALE(isect_pt, d_unit, dist1); + VSUB2(a_to_isect_pt, isect_pt, a); + VSUB2(b_to_isect_pt, isect_pt, b); + + a_to_isect_pt_mag_sq = MAGSQ(a_to_isect_pt); + b_to_isect_pt_mag_sq = MAGSQ(b_to_isect_pt); + + *t = dist1; + + if (a_to_isect_pt_mag_sq < tol->dist_sq) { + /* isect at point a of line segement */ + return 1; + } + + if (b_to_isect_pt_mag_sq < tol->dist_sq) { + /* isect at point b of line segement */ + return 2; + } + + if ((a_to_isect_pt_mag_sq < tol->dist_sq) && (b_to_isect_pt_mag_sq < tol->dist_sq)) { + bu_bomb("bn_isect_line_lseg(): this case should already been caught. i.e. zero length line segment\n"); + } + + dot = VDOT(a_to_isect_pt, ab); + if (dot < -SMALL_FASTF) { + /* isect before point a of infinite line associated + * with the line segment a->b + */ + return -3; + } + + dot = VDOT(b_to_isect_pt, ab); + if (dot > SMALL_FASTF) { + /* isect after point b of infinite line associated + * with the line segment a->b + */ + return -2; + } + +#if 0 + bu_log("p = %f %f %f a = %f %f %f b = %f %f %f isect = %f %f %f dist1 = %f d = %f %f %f\n", + V3ARGS(p), V3ARGS(a), V3ARGS(b), V3ARGS(isect_pt), dist1, V3ARGS(d)); + bu_log("bn_isect_line_lseg(): isect on line segement\n"); +#endif + + return 3; /* isect on line segement a->b but + * not on the end points + */ } - /* Intersection between A and B */ - return 3; + bu_bomb("bn_isect_line_lseg(): logic error, should not be here\n"); + return 0; /* quite compiler warning */ + #else vect_t c; /* Direction vector from A to B */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <br...@us...> - 2011-07-28 04:27:25
|
Revision: 45674 http://brlcad.svn.sourceforge.net/brlcad/?rev=45674&view=rev Author: brlcad Date: 2011-07-28 04:27:18 +0000 (Thu, 28 Jul 2011) Log Message: ----------- parallel is set but unused, kill it Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-07-28 04:22:15 UTC (rev 45673) +++ brlcad/trunk/src/libbn/plane.c 2011-07-28 04:27:18 UTC (rev 45674) @@ -1584,7 +1584,6 @@ register fastf_t det; register fastf_t det1; register short int q, r, s; - int parallel = 0; int colinear = 0; @@ -1614,7 +1613,6 @@ /* lines are parallel, must find another way to get normal vector */ vect_t a_to_p; - parallel = 1; VSUB2(a_to_p, p, a); VCROSS(n, a_to_p, d); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-08-17 19:16:45
|
Revision: 46219 http://brlcad.svn.sourceforge.net/brlcad/?rev=46219&view=rev Author: r_weiss Date: 2011-08-17 19:16:39 +0000 (Wed, 17 Aug 2011) Log Message: ----------- Within file 'plane.c' updated functions 'bn_coplanar', 'bn_isect_2planes' and 'bn_isect_lseg3_lseg3_new'. Corrected a bug in 'bn_coplanar' when computing the distance between planes. Within function 'bn_isect_2planes' changed some floating point compares from zero to SMALL_FASTF and improved the comments. Within 'bn_isect_lseg3_lseg3_new' fixed a bug when line segments are colinear and the intersection in on the end point(s), the clamping of the exact end point value was not being performed. The changes to function 'bn_isect_lseg3_lseg3_new' are disabled by default and is a work in progress. The function 'bn_isect_lseg3_lseg3_new' is a prototype function which supports the prototype version of function 'nmg_triangulate_fu'. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-08-17 18:57:57 UTC (rev 46218) +++ brlcad/trunk/src/libbn/plane.c 2011-08-17 19:16:39 UTC (rev 46219) @@ -476,15 +476,15 @@ * RPP. If this convention is unnecessary, just pass (0, 0, 0) as * rpp_min. * - * @return 0 OK, line of intersection stored in `pt' and `dir'. - * @return -1 FAIL, planes are identical (co-planar) - * @return -2 FAIL, planes are parallel and distinct - * @return -3 FAIL, unable to find line of intersection + * @return 0 success, line of intersection stored in 'pt' and 'dir' + * @return -1 planes are coplanar + * @return -2 planes are parallel but not coplanar + * @return -3 error, should be intersection but unable to find * * @param[out] pt Starting point of line of intersection * @param[out] dir Direction vector of line of intersection (unit length) - * @param[in] a plane 1 - * @param[in] b plane 2 + * @param[in] a plane 1 (normal must be unit length) + * @param[in] b plane 2 (normal must be unit length) * @param[in] rpp_min minimum poit of model RPP * @param[in] tol tolerance values */ @@ -500,17 +500,20 @@ plane_t pl; int i; + VSETALL(pt, 0.0); /* sanity */ + VSETALL(dir, 0.0); /* sanity */ + if ((i = bn_coplanar(a, b, tol)) != 0) { - if (i > 0) - return -1; /* FAIL -- coplanar */ - return -2; /* FAIL -- parallel & distinct */ + if (i > 0) { + return -1; /* planes are coplanar */ + } + return -2; /* planes are parallel but not coplanar */ } /* Direction vector for ray is perpendicular to both plane * normals. */ VCROSS(dir, a, b); - VUNITIZE(dir); /* safety */ /* Select an axis-aligned plane which has its normal pointing * along the same axis as the largest magnitude component of the @@ -518,21 +521,31 @@ * negative, reverse the direction vector, so that model is "in * front" of start point. */ - abs_dir[X] = (dir[X] >= 0) ? dir[X] : (-dir[X]); - abs_dir[Y] = (dir[Y] >= 0) ? dir[Y] : (-dir[Y]); - abs_dir[Z] = (dir[Z] >= 0) ? dir[Z] : (-dir[Z]); + abs_dir[X] = fabs(dir[X]); + abs_dir[Y] = fabs(dir[Y]); + abs_dir[Z] = fabs(dir[Z]); + if (ZERO(abs_dir[X])) { + abs_dir[X] = 0.0; + } + if (ZERO(abs_dir[Y])) { + abs_dir[Y] = 0.0; + } + if (ZERO(abs_dir[Z])) { + abs_dir[Z] = 0.0; + } + if (abs_dir[X] >= abs_dir[Y]) { if (abs_dir[X] >= abs_dir[Z]) { VSET(pl, 1, 0, 0); /* X */ pl[W] = rpp_min[X]; - if (dir[X] < 0) { + if (dir[X] < -SMALL_FASTF) { VREVERSE(dir, dir); } } else { VSET(pl, 0, 0, 1); /* Z */ pl[W] = rpp_min[Z]; - if (dir[Z] < 0) { + if (dir[Z] < -SMALL_FASTF) { VREVERSE(dir, dir); } } @@ -540,23 +553,25 @@ if (abs_dir[Y] >= abs_dir[Z]) { VSET(pl, 0, 1, 0); /* Y */ pl[W] = rpp_min[Y]; - if (dir[Y] < 0) { + if (dir[Y] < -SMALL_FASTF) { VREVERSE(dir, dir); } } else { VSET(pl, 0, 0, 1); /* Z */ pl[W] = rpp_min[Z]; - if (dir[Z] < 0) { + if (dir[Z] < -SMALL_FASTF) { VREVERSE(dir, dir); } } } /* Intersection of the 3 planes defines ray start point */ - if (bn_mkpoint_3planes(pt, pl, a, b) < 0) - return -3; /* FAIL -- no intersection */ + if (bn_mkpoint_3planes(pt, pl, a, b) < 0) { + return -3; /* error, should be intersection but unable to find */ + } - return 0; /* OK */ + /* success, line of intersection stored in 'pt' and 'dir' */ + return 0; } @@ -1192,11 +1207,24 @@ dist[0] = 1.0; } - /* If 'q' within tol of either endpoint (0.0, 1.0), make exact. */ - if (dist[1] > -qtol && dist[1] < qtol) { - dist[1] = 0.0; - } else if (dist[1] > 1.0-qtol && dist[1] < 1.0+qtol) { - dist[1] = 1.0; + if (status == 0) { /* infinite lines are colinear */ + /* When line segments are colinear, dist[1] has an alternate + * interpretation: it's the parameter along p (not q) + * therefore dist[1] must use tolerance ptol not qtol. + * If 'q' within tol of either endpoint (0.0, 1.0), make exact. + */ + if (dist[1] > -ptol && dist[1] < ptol) { + dist[1] = 0.0; + } else if (dist[1] > 1.0-ptol && dist[1] < 1.0+ptol) { + dist[1] = 1.0; + } + } else { + /* If 'q' within tol of either endpoint (0.0, 1.0), make exact. */ + if (dist[1] > -qtol && dist[1] < qtol) { + dist[1] = 0.0; + } else if (dist[1] > 1.0-qtol && dist[1] < 1.0+qtol) { + dist[1] = 1.0; + } } if (status == 0) { /* infinite lines are colinear */ @@ -2798,9 +2826,9 @@ bu_bomb("bn_coplanar(): input vector(s) 'a' and/or 'b' is not a unit vector.\n"); } + VSCALE(pt_a, a, fabs(a[W])); + VSCALE(pt_b, b, fabs(b[W])); dot = VDOT(a, b); - VSCALE(pt_a, a, a[3]); - VSCALE(pt_b, b, b[3]); if (NEAR_ZERO(dot, tol->perp)) { return 0; /* planes are perpendicular */ @@ -2809,11 +2837,12 @@ /* parallel is when dot is within tol->perp of either -1 or 1 */ if ((dot <= -SMALL_FASTF) ? (NEAR_EQUAL(dot, -1.0, tol->perp)) : (NEAR_EQUAL(dot, 1.0, tol->perp))) { if (bn_pt3_pt3_equal(pt_a, pt_b, tol)) { - /* test for coplanar */ + /* true when planes are coplanar */ if (dot >= SMALL_FASTF) { - /* test normals in same direction */ + /* true when plane normals in same direction */ return 1; } else { + /* true when plane normals in opposite direction */ return 2; } } else { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-09-09 20:44:31
|
Revision: 46646 http://brlcad.svn.sourceforge.net/brlcad/?rev=46646&view=rev Author: r_weiss Date: 2011-09-09 20:44:24 +0000 (Fri, 09 Sep 2011) Log Message: ----------- Updated file 'plane.c' to enable the prototype versions of functions bn_isect_lseg3_lseg3, bn_isect_line3_line3, and bn_isect_line_lseg. The original functions are removed. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-09-09 20:42:16 UTC (rev 46645) +++ brlcad/trunk/src/libbn/plane.c 2011-09-09 20:44:24 UTC (rev 46646) @@ -1083,9 +1083,8 @@ } -#ifdef TRI_PROTOTYPE /** - * B N _ I S E C T _ L S E G 3 _ L S E G 3 _ N E W + * B N _ I S E C T _ L S E G 3 _ L S E G 3 *@brief * Intersect two 3D line segments, defined by two points and two * vectors. The vectors are unlikely to be unit length. @@ -1104,7 +1103,7 @@ * intercept. If within distance tolerance of the endpoints, * these will be exactly 0.0 or 1.0, to ease the job of caller. * - * CLARIFICATION: This function 'bn_isect_lseg3_lseg3_new' + * CLARIFICATION: This function 'bn_isect_lseg3_lseg3' * returns distance values scaled where an intersect at the start * point of the line segement (within tol->dist) results in 0.0 * and when the intersect is at the end point of the line @@ -1125,12 +1124,12 @@ * @param tol tolerance values */ int -bn_isect_lseg3_lseg3_new(fastf_t *dist, - const fastf_t *p, - const fastf_t *pdir, - const fastf_t *q, - const fastf_t *qdir, - const struct bn_tol *tol) +bn_isect_lseg3_lseg3(fastf_t *dist, + const fastf_t *p, + const fastf_t *pdir, + const fastf_t *q, + const fastf_t *qdir, + const struct bn_tol *tol) { fastf_t ptol, qtol; /* length in parameter space == tol->dist */ fastf_t pmag, qmag; @@ -1138,21 +1137,21 @@ BN_CK_TOL(tol); if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new() p=(%g, %g, %g), pdir=(%g, %g, %g)\n\t\tq=(%g, %g, %g), qdir=(%g, %g, %g)\n", + 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", V3ARGS(p), V3ARGS(pdir), V3ARGS(q), V3ARGS(qdir)); } - status = bn_isect_line3_line3_new(&dist[0], &dist[1], p, pdir, q, qdir, tol); + status = bn_isect_line3_line3(&dist[0], &dist[1], p, pdir, q, qdir, tol); /* It is expected that dist[0] and dist[1] returned from - * 'bn_isect_line3_line3_new' are the actual distance to the + * 'bn_isect_line3_line3' are the actual distance to the * intersect, i.e. not scaled. Distances in the opposite * of the line direction vector result in a negative distance. */ /* sanity check */ if (status < -2 || status > 1) { - bu_bomb("bn_isect_lseg3_lseg3_new() function 'bn_isect_line3_line3_new' returned an invalid status\n"); + bu_bomb("bn_isect_lseg3_lseg3() function 'bn_isect_line3_line3' returned an invalid status\n"); } if (status == -1) { @@ -1161,7 +1160,7 @@ * parallel. */ if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new(): MISS, line segments do not intersect and are not parallel\n"); + bu_log("bn_isect_lseg3_lseg3(): MISS, line segments do not intersect and are not parallel\n"); } return -3; /* missed */ } @@ -1169,7 +1168,7 @@ if (status == -2) { /* infinite lines do not intersect, they are parallel */ if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new(): MISS, line segments are parallel, i.e. do not intersect\n"); + bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are parallel, i.e. do not intersect\n"); } return -2; /* missed (line segments are parallel) */ } @@ -1178,10 +1177,10 @@ qmag = MAGNITUDE(qdir); if (pmag < SMALL_FASTF) - bu_bomb("bn_isect_lseg3_lseg3_new(): |p|=0\n"); + bu_bomb("bn_isect_lseg3_lseg3(): |p|=0\n"); if (qmag < SMALL_FASTF) - bu_bomb("bn_isect_lseg3_lseg3_new(): |q|=0\n"); + bu_bomb("bn_isect_lseg3_lseg3(): |q|=0\n"); ptol = tol->dist / pmag; qtol = tol->dist / qmag; @@ -1231,13 +1230,13 @@ /* Lines are colinear */ if ((dist[0] > 1.0+ptol && dist[1] > 1.0+ptol) || (dist[0] < -ptol && dist[1] < -ptol)) { if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new(): MISS, line segments are colinear but not overlapping!\n"); + bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are colinear but not overlapping!\n"); } return -1; /* line segments are colinear but not overlapping */ } if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new(): HIT, line segments are colinear and overlapping!\n"); + bu_log("bn_isect_lseg3_lseg3(): HIT, line segments are colinear and overlapping!\n"); } return 0; /* line segments are colinear and overlapping */ @@ -1248,191 +1247,74 @@ if (dist[0] <= -ptol || dist[0] >= 1.0+ptol || dist[1] <= -qtol || dist[1] >= 1.0+qtol) { if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new(): MISS, infinite lines intersect but line segments do not!\n"); + bu_log("bn_isect_lseg3_lseg3(): MISS, infinite lines intersect but line segments do not!\n"); } return -3; /* missed, infinite lines intersect but line segments do not */ } if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3_new(): HIT, line segments intersect!\n"); + bu_log("bn_isect_lseg3_lseg3(): HIT, line segments intersect!\n"); } /* sanity check */ if (dist[0] < 0.0 || dist[0] > 1.0 || dist[1] < 0.0 || dist[1] > 1.0) { - bu_bomb("bn_isect_lseg3_lseg3_new(): INTERNAL ERROR, intersect distance values must be in the range 0-1\n"); + bu_bomb("bn_isect_lseg3_lseg3(): INTERNAL ERROR, intersect distance values must be in the range 0-1\n"); } return 1; /* hit, line segments intersect */ } -#endif /** - * B N _ I S E C T _ L S E G 3 _ L S E G 3 - *@brief - * Intersect two 3D line segments, defined by two points and two - * vectors. The vectors are unlikely to be unit length. + * B N _ I S E C T _ L I N E 3 _ L I N E 3 * + * Intersect two line segments, each in given in parametric form: * - * @return -2 missed (line segments are parallel) - * @return -1 missed (colinear and non-overlapping) - * @return 0 hit (line segments colinear and overlapping) - * @return 1 hit (normal intersection) - * - * @param[out] dist - * The value at dist[] is set to the parametric distance of the - * intercept dist[0] is parameter along p, range 0 to 1, to - * intercept. dist[1] is parameter along q, range 0 to 1, to - * intercept. If within distance tolerance of the endpoints, - * these will be exactly 0.0 or 1.0, to ease the job of caller. - * - * Special note: when return code is "0" for co-linearity, dist[1] has - * an alternate interpretation: it's the parameter along p (not q) - * which takes you from point p to the point (q + qdir), i.e., it's - * the endpoint of the q linesegment, since in this case there may be - * *two* intersections, if q is contained within span p to (p + pdir). - * And either may be -10 if the point is outside the span. - * - * @param p point 1 - * @param pdir direction-1 - * @param q point 2 - * @param qdir direction-2 - * @param tol tolerance values - */ -int -bn_isect_lseg3_lseg3(fastf_t *dist, - const fastf_t *p, - const fastf_t *pdir, - const fastf_t *q, - const fastf_t *qdir, - const struct bn_tol *tol) -{ - fastf_t ptol, qtol; /* length in parameter space == tol->dist */ - fastf_t pmag, qmag; - int status; - - BN_CK_TOL(tol); - if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_lseg3_lseg3() p=(%g, %g), pdir=(%g, %g)\n\t\tq=(%g, %g), qdir=(%g, %g)\n", - V2ARGS(p), V2ARGS(pdir), V2ARGS(q), V2ARGS(qdir)); - } - - status = bn_isect_line3_line3(&dist[0], &dist[1], p, pdir, q, qdir, tol); - if (status < 0) { - /* Lines are parallel, non-colinear */ - return -1; /* No intersection */ - } - pmag = MAGNITUDE(pdir); - if (pmag < SMALL_FASTF) - bu_bomb("bn_isect_lseg3_lseg3: |p|=0\n"); - if (status == 0) { - int nogood = 0; - /* Lines are colinear */ - /* If P within tol of either endpoint (0, 1), make exact. */ - ptol = tol->dist / pmag; - if (bu_debug & BU_DEBUG_MATH) { - bu_log("ptol=%g\n", ptol); - } - if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0; - else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1; - - if (dist[1] > -ptol && dist[1] < ptol) dist[1] = 0; - else if (dist[1] > 1-ptol && dist[1] < 1+ptol) dist[1] = 1; - - if (dist[1] < 0 || dist[1] > 1) nogood = 1; - if (dist[0] < 0 || dist[0] > 1) nogood++; - if (nogood >= 2) - return -1; /* colinear, but not overlapping */ - if (bu_debug & BU_DEBUG_MATH) { - bu_log(" HIT colinear!\n"); - } - return 0; /* colinear and overlapping */ - } - /* Lines intersect */ - /* If within tolerance of an endpoint (0, 1), make exact. */ - ptol = tol->dist / pmag; - if (dist[0] > -ptol && dist[0] < ptol) dist[0] = 0; - else if (dist[0] > 1-ptol && dist[0] < 1+ptol) dist[0] = 1; - - qmag = MAGNITUDE(qdir); - if (qmag < SMALL_FASTF) - bu_bomb("bn_isect_lseg3_lseg3: |q|=0\n"); - qtol = tol->dist / qmag; - if (dist[1] > -qtol && dist[1] < qtol) dist[1] = 0; - else if (dist[1] > 1-qtol && dist[1] < 1+qtol) dist[1] = 1; - - if (bu_debug & BU_DEBUG_MATH) { - bu_log("ptol=%g, qtol=%g\n", ptol, qtol); - } - if (dist[0] < 0 || dist[0] > 1 || dist[1] < 0 || dist[1] > 1) { - if (bu_debug & BU_DEBUG_MATH) { - bu_log(" MISS\n"); - } - return -1; /* missed */ - } - if (bu_debug & BU_DEBUG_MATH) { - bu_log(" HIT!\n"); - } - return 1; /* hit, normal intersection */ -} - - -#ifdef TRI_PROTOTYPE -/** - * B N _ I S E C T _ L I N E 3 _ L I N E 3 _ N E W - * - * Intersect two lines, each in given in parametric form: - * - * X = p + s * u + * X = p0 + pdist * pdir_i (i.e. line p0->p1) * and - * X = q + t * v + * X = q0 + qdist * qdir_i (i.e. line q0->q1) * - * While the parametric form is usually used to denote a ray (ie, - * positive values of the parameter only), in this case the full line - * is considered. + * The input vectors 'pdir_i' and 'qdir_i' must NOT be unit vectors + * for this function to work correctly. The magnitude of the direction + * vectors indicates line segment length. * - * The direction vectors u and v need not have unit length. + * The 'pdist' and 'qdist' values returned from this function are the + * actual distance to the intersect (i.e. not scaled). Distances may + * be negative, see below. * * @return -2 no intersection, lines are parallel. * @return -1 no intersection - * @return 0 lines are co-linear (s returned for t=0 to give distance to q0) - * @return 1 intersection found (s and t returned) + * @return 0 lines are co-linear (pdist and qdist returned) (see below) + * @return 1 intersection found (pdist and qdist returned) (see below) * - * @param[out] s, t line parameter of interseciton - * When explicit return >= 0, s and t are the - * line parameters of the intersection point on the 2 rays. - * The actual intersection coordinates can be found by - * substituting either of these into the original ray equations. - * * @param p0 point 1 - * @param u direction 1 + * @param pdir_i direction 1 * @param q0 point 2 - * @param v direction 2 + * @param qdir_i direction 2 * @param tol tolerance values + * @param[out] pdist, qdist (distances to intersection) (see below) * - * s, t When explicit return >= 0, s and t are the - * line parameters of the intersection point on the 2 rays. - * The actual intersection coordinates can be found by - * substituting either of these into the original ray equations. + * When return = 1, pdist is the distance along line p0->p1 to the + * intersect with line q0->q1. If the intersect is along p0->p1 but + * in the opposite direction of vector pdir_i (i.e. occuring before + * p0 on line p0->p1) then the distance will be negative. The value + * if qdist is the same as pdist except it is the distance along line + * q0->q1 to the intersect with line p0->p1. * - * The 'pdist' and 'qdist' values returned from this function - * are the actual distance to the intersect, i.e. not scaled. - * Distances in the opposite of the line direction vector result - * in a negative distance. - * - * The input vectors 'pdir' and 'qdir' must NOT be unit vectors - * for this function to work correctly. - * - * XXX It would be sensible to change the s, t pair to dist[2]. + * When return code = 0 for co-linearity, pdist and qdist have a + * different meaning. pdist is the distance from point p0 to point q0 + * and qdist is the distance from point p0 to point q1. If point q0 + * occurs before point p0 on line segment p0->p1 then pdist will be + * negative. The same occurs for the distance to point q1. */ int -bn_isect_line3_line3_new(fastf_t *pdist, /* distance from p0 to line q intersect, can be negative (s) */ - fastf_t *qdist, /* distance from q0 to line p intersect, can be negative (t) */ - const fastf_t *p0, /* line p start point */ - const fastf_t *pdir_i, /* line p direction, must not be a unit vector (u) */ - const fastf_t *q0, /* line q start point */ - const fastf_t *qdir_i, /* line q direction, must not be a unit vector (v) */ - const struct bn_tol *tol) +bn_isect_line3_line3(fastf_t *pdist, /* see above */ + fastf_t *qdist, /* see above */ + const fastf_t *p0, /* line p start point */ + const fastf_t *pdir_i, /* line p direction, must not be unit vector */ + const fastf_t *q0, /* line q start point */ + const fastf_t *qdir_i, /* line q direction, must not be unit vector */ + const struct bn_tol *tol) { fastf_t b, d, e, sc, tc, sc_numerator, tc_numerator, denominator; vect_t w0, qc_to_pc, u_scaled, v_scaled, v_scaled_to_u_scaled, tmp_vec, p0_to_q1; @@ -1456,7 +1338,7 @@ bu_log("pdir = %g %g %g\n", V3ARGS(pdir)); bu_log(" q0 = %g %g %g\n", V3ARGS(q0)); bu_log("qdir = %g %g %g\n", V3ARGS(qdir)); - bu_bomb("bn_isect_line3_line3_new(): input vector(s) 'pdir' and/or 'qdir' is zero magnitude.\n"); + bu_bomb("bn_isect_line3_line3(): input vector(s) 'pdir' and/or 'qdir' is zero magnitude.\n"); } *pdist = 0.0; @@ -1499,7 +1381,7 @@ denominator = pdir_mag_sq * qdir_mag_sq - b * b; if (!parallel && colinear) { - bu_bomb("bn_isect_line3_line3_new(): logic error, lines colinear but not parallel\n"); + bu_bomb("bn_isect_line3_line3(): logic error, lines colinear but not parallel\n"); } if (parallel && !colinear) { @@ -1533,7 +1415,7 @@ *qdist = -(*qdist); } - return 0; + return 0; /* colinear intersection */ } sc_numerator = (b * e - qdir_mag_sq * d); @@ -1550,287 +1432,14 @@ if (MAGSQ(qc_to_pc) <= tol->dist_sq) { *pdist = sc * sqrt(pdir_mag_sq); *qdist = tc * sqrt(qdir_mag_sq); - return 1; /* intersection found (s and t returned) */ + return 1; /* intersection */ } else { return -1; /* no intersection */ } } -#endif /** - * B N _ I S E C T _ L I N E 3 _ L I N E 3 - * - * Intersect two lines, each in given in parametric form: - * - * X = P + t * D - * and - * X = A + u * C - * - * While the parametric form is usually used to denote a ray (ie, - * positive values of the parameter only), in this case the full line - * is considered. - * - * The direction vectors C and D need not have unit length. - * - * @return -2 no intersection, lines are parallel. - * @return -1 no intersection - * @return 0 lines are co-linear (t returned for u=0 to give distance to A) - * @return 1 intersection found (t and u returned) - * - * @param[out] t, u line parameter of interseciton - * When explicit return >= 0, t and u are the - * line parameters of the intersection point on the 2 rays. - * The actual intersection coordinates can be found by - * substituting either of these into the original ray equations. - - * @param p point 1 - * @param d direction 1 - * @param a point 2 - * @param c direction 2 - * @param tol tolerance values - * - * t, u When explicit return >= 0, t and u are the - * line parameters of the intersection point on the 2 rays. - * The actual intersection coordinates can be found by - * substituting either of these into the original ray equations. - * - * XXX It would be sensible to change the t, u pair to dist[2]. - */ -int -bn_isect_line3_line3(fastf_t *t, - fastf_t *u, - const fastf_t *p, - const fastf_t *d, - const fastf_t *a, - const fastf_t *c, - const struct bn_tol *tol) -{ - vect_t n; - vect_t abs_n; - vect_t h; - register fastf_t det; - register fastf_t det1; - register short int q, r, s; - int colinear = 0; - - - BN_CK_TOL(tol); - - if (NEAR_ZERO(MAGSQ(c), VUNITIZE_TOL) || NEAR_ZERO(MAGSQ(d), VUNITIZE_TOL)) { - bu_bomb("bn_isect_line3_line3(): input vector(s) 'c' and/or 'd' is zero magnitude.\n"); - } - - /* Any intersection will occur in the plane with surface normal D - * cross C, which may not have unit length. The plane containing - * the two lines will be a constant distance from a plane with the - * same normal that contains the origin. Therfore, the projection - * of any point on the plane along N has the same length. Verify - * that this holds for P and A. If N dot P != N dot A, there is - * no intersection, because P and A must lie on parallel planes - * that are different distances from the origin. - */ - - VCROSS(n, d, c); - det = VDOT(n, p) - VDOT(n, a); - if (!NEAR_ZERO(det, tol->perp)) { - return -1; /* no intersection, lines not in same plane */ - } - - if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { - /* lines are parallel, must find another way to get normal vector */ - vect_t a_to_p; - - VSUB2(a_to_p, p, a); - VCROSS(n, a_to_p, d); - - if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { - /* lines are parallel and colinear */ - - colinear = 1; - bn_vec_ortho(n, d); - } - } - - if (NEAR_ZERO(MAGSQ(n), VUNITIZE_TOL)) { - bu_bomb("bn_isect_line3_line3(): ortho vector zero magnitude\n"); - } - - /* Solve for t and u in the system: - * - * Px + t * Dx = Ax + u * Cx - * Py + t * Dy = Ay + u * Cy - * Pz + t * Dz = Az + u * Cz - * - * This system is over-determined, having 3 equations in 2 - * unknowns. However, the intersection problem is really only a - * 2-dimensional problem, being located in the surface of a plane. - * Therefore, the "least important" of these equations can be - * initially ignored, leaving a system of 2 equations in 2 - * unknowns. - * - * Find the component of N with the largest magnitude. This - * component will have the least effect on the parameters in the - * system, being most nearly perpendicular to the plane. Denote - * the two remaining components by the subscripts q and r, rather - * than x, y, z. Subscript s is the smallest component, used for - * checking later. - */ - if (ZERO(n[X])) { - n[X] = 0.0; - } - if (ZERO(n[Y])) { - n[Y] = 0.0; - } - if (ZERO(n[Z])) { - n[Z] = 0.0; - } - abs_n[X] = (n[X] >= 0) ? n[X] : (-n[X]); - abs_n[Y] = (n[Y] >= 0) ? n[Y] : (-n[Y]); - abs_n[Z] = (n[Z] >= 0) ? n[Z] : (-n[Z]); - - if (abs_n[X] >= abs_n[Y]) { - if (abs_n[X] >= abs_n[Z]) { - /* X is largest in magnitude */ - q = Y; - r = Z; - s = X; - } else { - /* Z is largest in magnitude */ - q = X; - r = Y; - s = Z; - } - } else { - if (abs_n[Y] >= abs_n[Z]) { - /* Y is largest in magnitude */ - q = X; - r = Z; - s = Y; - } else { - /* Z is largest in magnitude */ - q = X; - r = Y; - s = Z; - } - } - - /* - * From the two components q and r, form a system of 2 equations - * in 2 unknowns: - * - * Pq + t * Dq = Aq + u * Cq - * Pr + t * Dr = Ar + u * Cr - * or - * t * Dq - u * Cq = Aq - Pq - * t * Dr - u * Cr = Ar - Pr - * - * Let H = A - P, resulting in: - * - * t * Dq - u * Cq = Hq - * t * Dr - u * Cr = Hr - * - * or - * - * [ Dq -Cq ] [ t ] [ Hq ] - * [ ] * [ ] = [ ] - * [ Dr -Cr ] [ u ] [ Hr ] - * - * This system can be solved by direct substitution, or by finding - * the determinants by Cramers rule: - * - * [ Dq -Cq ] - * det(M) = det [ ] = -Dq * Cr + Cq * Dr - * [ Dr -Cr ] - * - * If det(M) is zero, then the lines are parallel (perhaps - * colinear). Otherwise, exactly one solution exists. - */ - VSUB2(h, a, p); - det = c[q] * d[r] - d[q] * c[r]; - det1 = (c[q] * h[r] - h[q] * c[r]); /* see below */ - /* XXX This should be no smaller than 1e-16. See - * bn_isect_line2_line2 for details. - */ - if (NEAR_ZERO(det, VUNITIZE_TOL)) { - /* Lines are parallel */ - if (!colinear || !NEAR_ZERO(det1, VUNITIZE_TOL)) { - return -2; /* parallel, not colinear, no intersection */ - } - - /* Lines are co-linear */ - /* Compute t for u=0 as a convenience to caller */ - *u = 0; - /* Use largest direction component */ - if (fabs(d[q]) >= fabs(d[r])) { - *t = h[q]/d[q]; - } else { - *t = h[r]/d[r]; - } - return 0; /* Lines co-linear */ - } - - /* det(M) is non-zero, so there is exactly one solution. Using - * Cramer's rule, det1(M) replaces the first column of M with the - * constant column vector, in this case H. Similarly, det2(M) - * replaces the second column. Computation of the determinant is - * done as before. - * - * Now, - * - * [ Hq -Cq ] - * det [ ] - * det1(M) [ Hr -Cr ] -Hq * Cr + Cq * Hr - * t = ------- = --------------- = ------------------ - * det(M) det(M) -Dq * Cr + Cq * Dr - * - * and - * - * [ Dq Hq ] - * det [ ] - * det2(M) [ Dr Hr ] Dq * Hr - Hq * Dr - * u = ------- = --------------- = ------------------ - * det(M) det(M) -Dq * Cr + Cq * Dr - */ - det = 1/det; - *t = det * det1; - *u = det * (d[q] * h[r] - h[q] * d[r]); - - /* Check that these values of t and u satisfy the 3rd equation as - * well! - * - * XXX It isn't clear that "det" is exactly a model-space - * distance. - */ - det = *t * d[s] - *u * c[s] - h[s]; - if (!NEAR_ZERO(det, VUNITIZE_TOL)) { - /* XXX This tolerance needs to be much less loose than - * SQRT_SMALL_FASTF. What about DETERMINANT_TOL? - */ - /* Inconsistent solution, lines miss each other */ - return -1; - } - - /* To prevent errors, check the answer. Not returning bogus - * results to our caller is worth the extra time. - */ - { - point_t hit1, hit2; - - VJOIN1(hit1, p, *t, d); - VJOIN1(hit2, a, *u, c); - if (!bn_pt3_pt3_equal(hit1, hit2, tol)) { -/* bu_log("bn_isect_line3_line3(): BOGUS RESULT, hit1=(%g, %g, %g), hit2=(%g, %g, %g)\n", - hit1[X], hit1[Y], hit1[Z], hit2[X], hit2[Y], hit2[Z]); */ - return -1; - } - } - - return 1; /* Intersection found */ -} - - -/** * B N _ I S E C T _ L I N E _ L S E G *@brief * Intersect a line in parametric form: @@ -1865,7 +1474,6 @@ int bn_isect_line_lseg(fastf_t *t, const fastf_t *p, const fastf_t *d, const fastf_t *a, const fastf_t *b, const struct bn_tol *tol) { -#ifdef TRI_PROTOTYPE vect_t ab, pa, pb; /* direction vectors a->b, p->a, p->b */ fastf_t ab_mag; fastf_t pa_mag_sq; @@ -1975,7 +1583,7 @@ dist1 = 0.0; /* sanity */ dist2 = 0.0; /* sanity */ - code = bn_isect_line3_line3_new(&dist1, &dist2, p, d, a, ab, tol); + code = bn_isect_line3_line3(&dist1, &dist2, p, d, a, ab, tol); if (code == 0) { bu_bomb("bn_isect_line_lseg(): we should have already detected a colinear condition\n"); @@ -2004,13 +1612,6 @@ VMOVE(d_unit, d); VUNITIZE(d_unit); -#if 0 - if (NEAR_ZERO(dist1, tol->dist)) { - bu_log("bn_isect_line_lseg(): dist1 = %.15f\n", dist1); - bu_bomb("bn_isect_line_lseg(): dist1 is zero\n"); - } -#endif - dist1 = fabs(dist1); /* sanity */ VSCALE(isect_pt, d_unit, dist1); VSUB2(a_to_isect_pt, isect_pt, a); @@ -2051,12 +1652,6 @@ return -2; } -#if 0 - bu_log("p = %f %f %f a = %f %f %f b = %f %f %f isect = %f %f %f dist1 = %f d = %f %f %f\n", - V3ARGS(p), V3ARGS(a), V3ARGS(b), V3ARGS(isect_pt), dist1, V3ARGS(d)); - bu_log("bn_isect_line_lseg(): isect on line segement\n"); -#endif - return 3; /* isect on line segement a->b but * not on the end points */ @@ -2065,73 +1660,6 @@ bu_bomb("bn_isect_line_lseg(): logic error, should not be here\n"); return 0; /* quite compiler warning */ - -#else - - vect_t c; /* Direction vector from A to B */ - auto fastf_t u; /* As in, A + u * C = X */ - register fastf_t f; - register int ret; - fastf_t fuzz; - - BN_CK_TOL(tol); - - VSUB2(c, b, a); - /* To keep the values of u between 0 and 1, C should NOT be scaled - * to have unit length. However, it is a good idea to make sure - * that C is a non-zero vector, (ie, that A and B are distinct). - */ - if ((fuzz = MAGSQ(c)) < tol->dist_sq) { - return -4; /* points A and B are not distinct */ - } - - /* Detecting colinearity is difficult, and very very important. - * As a first step, check to see if both points A and B lie within - * tolerance of the line. If so, then the line segment AC is ON - * the line. - */ - if (bn_distsq_line3_pt3(p, d, a) <= tol->dist_sq && - bn_distsq_line3_pt3(p, d, b) <= tol->dist_sq) { - if (bu_debug & BU_DEBUG_MATH) { - bu_log("bn_isect_line3_lseg3() pts A and B within tol of line\n"); - } - /* Find the parametric distance along the ray */ - *t = bn_dist_pt3_along_line3(p, d, a); - /*** dist[1] = bn_dist_pt3_along_line3(p, d, b); ***/ - return 0; /* Colinear */ - } - - if ((ret = bn_isect_line3_line3(t, &u, p, d, a, c, tol)) < 0) { - /* No intersection found */ - return -1; - } - if (ret == 0) { - /* co-linear (t was computed for point A, u=0) */ - return 0; - } - - /* The two lines intersect at a point. If the u parameter is - * outside the range (0..1), reject the intersection, because it - * falls outside the line segment A--B. - * - * Convert the tol->dist into allowable deviation in terms of - * (0..1) range of the parameters. - */ - fuzz = tol->dist / sqrt(fuzz); - if (u < -fuzz) - return -3; /* Intersection < A */ - if ((f=(u-1)) > fuzz) - return -2; /* Intersection > B */ - - /* Check for fuzzy intersection with one of the verticies */ - if (u < fuzz) - return 1; /* Intersection at A */ - if (f >= -fuzz) - return 2; /* Intersection at B */ - - return 3; /* Intersection between A and B */ - -#endif } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <sta...@us...> - 2011-11-04 18:15:52
|
Revision: 47431 http://brlcad.svn.sourceforge.net/brlcad/?rev=47431&view=rev Author: starseeker Date: 2011-11-04 18:15:45 +0000 (Fri, 04 Nov 2011) Log Message: ----------- we're in libbn, so used the same debug triggers as other bn functions... Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-11-04 18:12:29 UTC (rev 47430) +++ brlcad/trunk/src/libbn/plane.c 2011-11-04 18:15:45 UTC (rev 47431) @@ -438,7 +438,7 @@ fastf_t t; /* distance along ray of projection of P */ fastf_t dsq; /* sqaure of distance from p to line */ - if (rt_g.NMG_debug & DEBUG_BASIC) + if (bu_debug & BU_DEBUG_MATH) bu_log("bn_dist_pt3_line3(a=(%f %f %f), dir=(%f %f %f), p=(%f %f %f)\n" , V3ARGS(a), V3ARGS(dir), V3ARGS(p)); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2011-12-12 20:12:33
|
Revision: 47912 http://brlcad.svn.sourceforge.net/brlcad/?rev=47912&view=rev Author: r_weiss Date: 2011-12-12 20:12:26 +0000 (Mon, 12 Dec 2011) Log Message: ----------- Update to the libbn function 'bn_pt3_pt3_equal' in file 'plane.c'. This change was to improve performance. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2011-12-12 20:01:34 UTC (rev 47911) +++ brlcad/trunk/src/libbn/plane.c 2011-12-12 20:12:26 UTC (rev 47912) @@ -62,12 +62,26 @@ int bn_pt3_pt3_equal(const fastf_t *a, const fastf_t *b, const struct bn_tol *tol) { - vect_t diff; + register fastf_t tmp = tol->dist_sq; + register fastf_t ab, abx, aby, abz; - BN_CK_TOL(tol); - VSUB2(diff, b, a); - if (MAGSQ(diff) < tol->dist_sq) return 1; - return 0; + abx = a[X]-b[X]; + ab = abx * abx; + if (ab > tmp) { + return 0; + } + aby = a[Y]-b[Y]; + ab += (aby * aby); + if (ab > tmp) { + return 0; + } + abz = a[Z]-b[Z]; + ab += (abz * abz); + if (ab > tmp) { + return 0; + } + + return 1; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <n_...@us...> - 2012-01-06 22:11:48
|
Revision: 48199 http://brlcad.svn.sourceforge.net/brlcad/?rev=48199&view=rev Author: n_reed Date: 2012-01-06 22:11:41 +0000 (Fri, 06 Jan 2012) Log Message: ----------- generalize bn_mkpoint_3planes for non-unitized direction vectors Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2012-01-06 03:43:58 UTC (rev 48198) +++ brlcad/trunk/src/libbn/plane.c 2012-01-06 22:11:41 UTC (rev 48199) @@ -306,8 +306,8 @@ /** * B N _ M K P O I N T _ 3 P L A N E S *@brief - * Given the description of three planes, compute the point of - * intersection, if any. + * Given the description of three planes, compute the point of intersection, if + * any. The direction vectors of the planes need not be of unit length. * * Find the solution to a system of three equations in three unknowns: @verbatim @@ -323,7 +323,6 @@ * @endverbatim * - * * @return 0 OK * @return -1 Failure. Intersection is a line or plane. * @@ -335,27 +334,52 @@ int bn_mkpoint_3planes(fastf_t *pt, const fastf_t *a, const fastf_t *b, const fastf_t *c) { - vect_t v1, v2, v3; - register fastf_t det; + vect_t v1; + fastf_t dot; - /* Find a vector perpendicular to both planes B and C */ + /* Find a vector perpendicular to vectors b and c (parallel to planes B + * and C). + */ VCROSS(v1, b, c); - /* If that vector is perpendicular to A, then A is parallel to - * either B or C, and no intersection exists. This dot&cross - * product is the determinant of the matrix M. (I suspect there - * is some deep significance to this!) + /* If vector a is perpendicular to that vector, then two of the three + * planes are parallel. We test by examining their dot product, which is + * also the determinant of the matrix M^T: + * [ a[X] a[Y] a[Z] ] + * [ b[X] b[Y] b[Z] ] + * [ c[X] c[Y] c[Z] ] */ - det = VDOT(a, v1); - if (ZERO(det)) return -1; + dot = VDOT(a, v1); - VCROSS(v2, a, c); - VCROSS(v3, a, b); + if (ZERO(dot)) { + return -1; + } else { + vect_t v2, v3; + fastf_t det, aH, bH, cH; - det = 1/det; - pt[X] = det*(a[3]*v1[X] - b[3]*v2[X] + c[3]*v3[X]); - pt[Y] = det*(a[3]*v1[Y] - b[3]*v2[Y] + c[3]*v3[Y]); - pt[Z] = det*(a[3]*v1[Z] - b[3]*v2[Z] + c[3]*v3[Z]); + VCROSS(v2, a, c); + VCROSS(v3, a, b); + + /* Since this algorithm assumes unit-length direction vectors, we need + * to calculate the scale factors associated with the unitized + * equivalents of the planes. + */ + aH = MAGNITUDE(a) * a[H]; + bH = MAGNITUDE(b) * b[H]; + cH = MAGNITUDE(c) * c[H]; + + /* We use the fact that det(M) = 1 / det(M^T) to calculate the + * determinant of matrix M: + * [ a[X] b[X] c[X] ] + * [ a[Y] b[Y] c[Y] ] + * [ a[Z] b[Z] c[Z] ] + */ + det = 1 / dot; + + pt[X] = det * (aH * v1[X] - bH * v2[X] + cH * v3[X]); + pt[Y] = det * (aH * v1[Y] - bH * v2[Y] + cH * v3[Y]); + pt[Z] = det * (aH * v1[Z] - bH * v2[Z] + cH * v3[Z]); + } return 0; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2012-08-13 21:45:59
|
Revision: 51983 http://brlcad.svn.sourceforge.net/brlcad/?rev=51983&view=rev Author: r_weiss Date: 2012-08-13 21:45:51 +0000 (Mon, 13 Aug 2012) Log Message: ----------- Updated function "bn_isect_line3_line3" in file "plane.c" to use function "bn_lseg3_lseg3_parallel" and to better handle special cases. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2012-08-13 21:42:21 UTC (rev 51982) +++ brlcad/trunk/src/libbn/plane.c 2012-08-13 21:45:51 UTC (rev 51983) @@ -1613,30 +1613,18 @@ VSUB2(p0_to_q1, q1, p0); - d1 = bn_distsq_line3_pt3(q0,qdir,p0); - d2 = bn_distsq_line3_pt3(q0,qdir,p1); - d3 = bn_distsq_line3_pt3(p0,pdir,q0); - d4 = bn_distsq_line3_pt3(p0,pdir,q1); - - /* if all distances are within distance tolerance of each - * other then they a parallel - */ - if (NEAR_EQUAL(d1, d2, tol->dist_sq) && - NEAR_EQUAL(d1, d3, tol->dist_sq) && - NEAR_EQUAL(d1, d4, tol->dist_sq) && - NEAR_EQUAL(d2, d3, tol->dist_sq) && - NEAR_EQUAL(d2, d4, tol->dist_sq) && - NEAR_EQUAL(d3, d4, tol->dist_sq)) { + if (bn_lseg3_lseg3_parallel(p0, p1, q0, q1, tol)) { parallel = 1; + d1 = bn_distsq_line3_pt3(q0,qdir,p0); + d2 = bn_distsq_line3_pt3(q0,qdir,p1); + d3 = bn_distsq_line3_pt3(p0,pdir,q0); + d4 = bn_distsq_line3_pt3(p0,pdir,q1); + if (NEAR_ZERO(d1, tol->dist_sq) && NEAR_ZERO(d2, tol->dist_sq) && + NEAR_ZERO(d3, tol->dist_sq) && NEAR_ZERO(d4, tol->dist_sq)) { + colinear = 1; + } } - if (NEAR_ZERO(d1, tol->dist_sq) && - NEAR_ZERO(d2, tol->dist_sq) && - NEAR_ZERO(d3, tol->dist_sq) && - NEAR_ZERO(d4, tol->dist_sq)) { - colinear = 1; - } - VSUB2(w0, p0, q0); b = VDOT(pdir, qdir); d = VDOT(pdir, w0); @@ -1683,21 +1671,52 @@ sc_numerator = (b * e - qdir_mag_sq * d); tc_numerator = (pdir_mag_sq * e - b * d); - sc = sc_numerator / denominator; - tc = tc_numerator / denominator; + if (NEAR_ZERO(denominator, tol->dist) && !NEAR_ZERO(sc_numerator, tol->dist)) { + denominator = 0.0; + sc = MAX_FASTF; + } else if (!NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(sc_numerator, tol->dist)) { + sc_numerator = 0.0; + sc = 0.0; + } else if (NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(sc_numerator, tol->dist)) { + sc_numerator = 0.0; + denominator = 0.0; + sc = 1.0; + } else { + sc = sc_numerator / denominator; + } + if (NEAR_ZERO(denominator, tol->dist) && !NEAR_ZERO(tc_numerator, tol->dist)) { + denominator = 0.0; + tc = MAX_FASTF; + } else if (!NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(tc_numerator, tol->dist)) { + tc_numerator = 0.0; + tc = 0.0; + } else if (NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(tc_numerator, tol->dist)) { + tc_numerator = 0.0; + denominator = 0.0; + tc = 1.0; + } else { + tc = tc_numerator / denominator; + } + VSCALE(u_scaled, pdir, sc_numerator); VSCALE(v_scaled, qdir, tc_numerator); VSUB2(v_scaled_to_u_scaled, u_scaled, v_scaled); - VSCALE(tmp_vec, v_scaled_to_u_scaled, 1.0/denominator); + + if (ZERO(denominator)) { + VSCALE(tmp_vec, v_scaled_to_u_scaled, MAX_FASTF); + } else { + VSCALE(tmp_vec, v_scaled_to_u_scaled, 1.0/denominator); + } + VADD2(qc_to_pc, w0, tmp_vec); if (MAGSQ(qc_to_pc) <= tol->dist_sq) { - *pdist = sc * sqrt(pdir_mag_sq); - *qdist = tc * sqrt(qdir_mag_sq); - return 1; /* intersection */ + *pdist = sc * sqrt(pdir_mag_sq); + *qdist = tc * sqrt(qdir_mag_sq); + return 1; /* intersection */ } else { - return -1; /* no intersection */ + return -1; /* no intersection */ } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <car...@us...> - 2012-08-22 20:21:24
|
Revision: 52153 http://brlcad.svn.sourceforge.net/brlcad/?rev=52153&view=rev Author: carlmoore Date: 2012-08-22 20:21:13 +0000 (Wed, 22 Aug 2012) Log Message: ----------- fix spellings, add apostrophe to 'Cramers', but do not fix 'colinear' Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2012-08-22 20:07:02 UTC (rev 52152) +++ brlcad/trunk/src/libbn/plane.c 2012-08-22 20:21:13 UTC (rev 52153) @@ -264,7 +264,7 @@ * @param[in] a point 1 * @param[in] b point 2 * @param[in] c point 3 - * @param[in] tol Tolerance values for doing calcualtion + * @param[in] tol Tolerance values for doing calculation */ int bn_mk_plane_3pts(fastf_t *plane, @@ -474,7 +474,7 @@ vect_t unit_dir; /* unitized dir vector */ fastf_t A_P_sq; /* |P-A|**2 */ fastf_t t; /* distance along ray of projection of P */ - fastf_t dsq; /* sqaure of distance from p to line */ + fastf_t dsq; /* square of distance from p to line */ if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) bu_log("bn_dist_pt3_line3(a=(%f %f %f), dir=(%f %f %f), p=(%f %f %f)\n" , @@ -593,7 +593,7 @@ /** * calculate intersection or closest approach of a line and a line - * segement. + * segment. * * returns: * -2 -> line and line segment are parallel and collinear. @@ -671,7 +671,7 @@ * Intersect an infinite line (specified in point and direction vector * form) with a plane that has an outward pointing normal. The * direction vector need not have unit length. The first three - * elements of the plane equation must form a unit lengh vector. + * elements of the plane equation must form a unit length vector. * * @return -2 missed (ray is outside halfspace) * @return -1 missed (ray is inside) @@ -921,7 +921,7 @@ * [ Dy -Cy ] [ u ] [ Hy ] * * This system can be solved by direct substitution, or by finding - * the determinants by Cramers rule: + * the determinants by Cramer's rule: * * [ Dx -Cx ] * det(M) = det [ ] = -Dx * Cy + Cx * Dy @@ -1045,7 +1045,7 @@ * * @return -4 A and B are not distinct points * @return -3 Lines do not intersect - * @return -2 Intersection exists, but outside segemnt, < A + * @return -2 Intersection exists, but outside segment, < A * @return -1 Intersection exists, but outside segment, > B * @return 0 Lines are co-linear (special meaning of dist[1]) * @return 1 Intersection at vertex A @@ -1221,7 +1221,7 @@ goto out; } - /* Check for ctoly intersection with one of the verticies */ + /* Check for ctoly intersection with one of the vertices */ if (dist[1] < ctol) { dist[1] = 0; ret = 1; /* Intersection at A */ @@ -1366,9 +1366,9 @@ * * CLARIFICATION: This function 'bn_isect_lseg3_lseg3' * returns distance values scaled where an intersect at the start - * point of the line segement (within tol->dist) results in 0.0 + * point of the line segment (within tol->dist) results in 0.0 * and when the intersect is at the end point of the line - * segement (within tol->dist), the result is 1.0. Intersects + * segment (within tol->dist), the result is 1.0. Intersects * before the start point return a negative distance. Intersects * after the end point result in a return value > 1.0. * @@ -1559,7 +1559,7 @@ * * When return = 1, pdist is the distance along line p0->p1 to the * intersect with line q0->q1. If the intersect is along p0->p1 but - * in the opposite direction of vector pdir_i (i.e. occuring before + * in the opposite direction of vector pdir_i (i.e. occurring before * p0 on line p0->p1) then the distance will be negative. The value * if qdist is the same as pdist except it is the distance along line * q0->q1 to the intersect with line p0->p1. @@ -1798,11 +1798,11 @@ return 2; } - /* just check that the vertices of the line segement are + /* just check that the vertices of the line segment are * within distance tolerance of the ray. it may cause problems * to also require the ray start and end points to be within * distance tolerance of the infinite line associated with - * the line segement. + * the line segment. */ d1 = bn_distsq_line3_pt3(p,d,a); /* distance of point a to ray */ d2 = bn_distsq_line3_pt3(p,d,b); /* distance of point b to ray */ @@ -1905,12 +1905,12 @@ *t = dist1; if (a_to_isect_pt_mag_sq < tol->dist_sq) { - /* isect at point a of line segement */ + /* isect at point a of line segment */ return 1; } if (b_to_isect_pt_mag_sq < tol->dist_sq) { - /* isect at point b of line segement */ + /* isect at point b of line segment */ return 2; } @@ -1934,7 +1934,7 @@ return -2; } - return 3; /* isect on line segement a->b but + return 3; /* isect on line segment a->b but * not on the end points */ } @@ -2819,7 +2819,7 @@ plane_t pl; fastf_t dist; - /* insersect with plane */ + /* intersect with plane */ VSUB2(VA, A, V); VSUB2(VB, B, V); @@ -2937,7 +2937,7 @@ VSUB2(pt_V, V, pt); VCROSS(VPP, pt_V, dir); - /* alpha is projection of VPP onto VA (not necessaily in plane) + /* alpha is projection of VPP onto VA (not necessarily in plane) * If alpha < 0.0 then p is "before" point V on line V->A * No-one can figure out why alpha > NdotDir is important. */ @@ -2945,7 +2945,7 @@ if (alpha < 0.0 || alpha > fabs(NdotDir)) return 0; - /* beta is projection of VPP onto VB (not necessaily in plane) */ + /* beta is projection of VPP onto VB (not necessarily in plane) */ beta = VDOT(VB, VPP) * (-1 * entleave); if (beta < 0.0 || beta > fabs(NdotDir)) return 0; @@ -3014,7 +3014,7 @@ * Calculate the square of the distance of closest approach for two * lines. * - * The lines are specifed as a point and a vector each. The vectors + * The lines are specified as a point and a vector each. The vectors * need not be unit length. P and d define one line; Q and e define * the other. * @@ -3028,10 +3028,10 @@ * pt1 is the point of closest approach on the first line * pt2 is the point of closest approach on the second line * - * This algoritm is based on expressing the distance sqaured, taking + * This algorithm is based on expressing the distance squared, taking * partials with respect to the two unknown parameters (dist[0] and - * dist[1]), seeting the two partails equal to 0, and solving the two - * simutaneous equations + * dist[1]), setting the two partails equal to 0, and solving the two + * simultaneous equations */ int bn_distsq_line3_line3(fastf_t *dist, fastf_t *P, fastf_t *d_in, fastf_t *Q, fastf_t *e_in, fastf_t *pt1, fastf_t *pt2) @@ -3166,7 +3166,7 @@ /** * B N _ I S E C T _ L S E G _ R P P *@brief - * Intersect a line segment with a rectangular parallelpiped (RPP) + * Intersect a line segment with a rectangular parallelepiped (RPP) * that has faces parallel to the coordinate planes (a clipping RPP). * The RPP is defined by a minimum point and a maximum point. This is * a very close relative to rt_in_rpp() from librt/shoot.c This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2012-08-28 19:06:37
|
Revision: 52267 http://brlcad.svn.sourceforge.net/brlcad/?rev=52267&view=rev Author: r_weiss Date: 2012-08-28 19:06:31 +0000 (Tue, 28 Aug 2012) Log Message: ----------- Fixed a tolerance bug in function "bn_isect_line3_line3". Did code cleanup in this function and "bn_isect_lseg3_lseg3". These updates were in file "plane.c". Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2012-08-28 19:04:26 UTC (rev 52266) +++ brlcad/trunk/src/libbn/plane.c 2012-08-28 19:06:31 UTC (rev 52267) @@ -1395,6 +1395,7 @@ fastf_t ptol, qtol; /* length in parameter space == tol->dist */ fastf_t pmag, qmag; int status; + int ret; BN_CK_TOL(tol); if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) { @@ -1423,7 +1424,8 @@ if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) { bu_log("bn_isect_lseg3_lseg3(): MISS, line segments do not intersect and are not parallel\n"); } - return -3; /* missed */ + ret = -3; /* missed */ + goto out; } if (status == -2) { @@ -1431,7 +1433,8 @@ if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) { bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are parallel, i.e. do not intersect\n"); } - return -2; /* missed (line segments are parallel) */ + ret = -2; /* missed (line segments are parallel) */ + goto out; } pmag = MAGNITUDE(pdir); @@ -1463,9 +1466,9 @@ } /* If 'p' within tol of either endpoint (0.0, 1.0), make exact. */ - if (dist[0] > -ptol && dist[0] < ptol) { + if (NEAR_ZERO(dist[0], ptol)) { dist[0] = 0.0; - } else if (dist[0] > 1.0-ptol && dist[0] < 1.0+ptol) { + } else if (NEAR_EQUAL(dist[0], 1.0, ptol)) { dist[0] = 1.0; } @@ -1475,16 +1478,16 @@ * therefore dist[1] must use tolerance ptol not qtol. * If 'q' within tol of either endpoint (0.0, 1.0), make exact. */ - if (dist[1] > -ptol && dist[1] < ptol) { + if (NEAR_ZERO(dist[1], ptol)) { dist[1] = 0.0; - } else if (dist[1] > 1.0-ptol && dist[1] < 1.0+ptol) { + } else if (NEAR_EQUAL(dist[1], 1.0, ptol)) { dist[1] = 1.0; } } else { /* If 'q' within tol of either endpoint (0.0, 1.0), make exact. */ - if (dist[1] > -qtol && dist[1] < qtol) { + if (NEAR_ZERO(dist[1], qtol)) { dist[1] = 0.0; - } else if (dist[1] > 1.0-qtol && dist[1] < 1.0+qtol) { + } else if (NEAR_EQUAL(dist[1], 1.0, qtol)) { dist[1] = 1.0; } } @@ -1495,14 +1498,16 @@ if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) { bu_log("bn_isect_lseg3_lseg3(): MISS, line segments are colinear but not overlapping!\n"); } - return -1; /* line segments are colinear but not overlapping */ + ret = -1; /* line segments are colinear but not overlapping */ + goto out; } if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) { bu_log("bn_isect_lseg3_lseg3(): HIT, line segments are colinear and overlapping!\n"); } - return 0; /* line segments are colinear and overlapping */ + ret = 0; /* line segments are colinear and overlapping */ + goto out; } /* At this point we know the infinite lines intersect and are not colinear */ @@ -1512,7 +1517,8 @@ if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) { bu_log("bn_isect_lseg3_lseg3(): MISS, infinite lines intersect but line segments do not!\n"); } - return -3; /* missed, infinite lines intersect but line segments do not */ + ret = -3; /* missed, infinite lines intersect but line segments do not */ + goto out; } if (UNLIKELY(bu_debug & BU_DEBUG_MATH)) { @@ -1524,7 +1530,11 @@ bu_bomb("bn_isect_lseg3_lseg3(): INTERNAL ERROR, intersect distance values must be in the range 0-1\n"); } - return 1; /* hit, line segments intersect */ + ret = 1; /* hit, line segments intersect */ + +out: + + return ret; } @@ -1631,6 +1641,7 @@ e = VDOT(qdir, w0); denominator = pdir_mag_sq * qdir_mag_sq - b * b; + if (UNLIKELY(!parallel && colinear)) { bu_bomb("bn_isect_line3_line3(): logic error, lines colinear but not parallel\n"); } @@ -1672,26 +1683,26 @@ sc_numerator = (b * e - qdir_mag_sq * d); tc_numerator = (pdir_mag_sq * e - b * d); - if (NEAR_ZERO(denominator, tol->dist) && !NEAR_ZERO(sc_numerator, tol->dist)) { + if (ZERO(denominator) && !ZERO(sc_numerator)) { denominator = 0.0; sc = MAX_FASTF; - } else if (!NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(sc_numerator, tol->dist)) { + } else if (!ZERO(denominator) && ZERO(sc_numerator)) { sc_numerator = 0.0; sc = 0.0; - } else if (NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(sc_numerator, tol->dist)) { + } else if (ZERO(denominator) && ZERO(sc_numerator)) { sc_numerator = 0.0; denominator = 0.0; sc = 1.0; } else { sc = sc_numerator / denominator; } - if (NEAR_ZERO(denominator, tol->dist) && !NEAR_ZERO(tc_numerator, tol->dist)) { + if (ZERO(denominator) && !ZERO(tc_numerator)) { denominator = 0.0; tc = MAX_FASTF; - } else if (!NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(tc_numerator, tol->dist)) { + } else if (!ZERO(denominator) && ZERO(tc_numerator)) { tc_numerator = 0.0; tc = 0.0; - } else if (NEAR_ZERO(denominator, tol->dist) && NEAR_ZERO(tc_numerator, tol->dist)) { + } else if (ZERO(denominator) && ZERO(tc_numerator)) { tc_numerator = 0.0; denominator = 0.0; tc = 1.0; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <car...@us...> - 2012-11-07 18:15:41
|
Revision: 53499 http://brlcad.svn.sourceforge.net/brlcad/?rev=53499&view=rev Author: carlmoore Date: 2012-11-07 18:15:34 +0000 (Wed, 07 Nov 2012) Log Message: ----------- change ie to i.e., and fix 1 spelling Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2012-11-07 18:01:50 UTC (rev 53498) +++ brlcad/trunk/src/libbn/plane.c 2012-11-07 18:15:34 UTC (rev 53499) @@ -848,7 +848,7 @@ @endverbatim * - * While the parametric form is usually used to denote a ray (ie, + * While the parametric form is usually used to denote a ray (i.e., * positive values of the parameter only), in this case the full line * is considered. * @@ -1090,7 +1090,7 @@ /* To keep the values of u between 0 and 1. C should NOT be * scaled to have unit length. However, it is a good idea to make - * sure that C is a non-zero vector, (ie, that A and B are + * sure that C is a non-zero vector, (i.e., that A and B are * distinct). */ if ((ctol = MAGSQ_2D(c)) <= tol->dist_sq) { @@ -3041,7 +3041,7 @@ * * This algorithm is based on expressing the distance squared, taking * partials with respect to the two unknown parameters (dist[0] and - * dist[1]), setting the two partails equal to 0, and solving the two + * dist[1]), setting the two partials equal to 0, and solving the two * simultaneous equations */ int @@ -3226,7 +3226,7 @@ if (mindist < ((sv = (*min - *pt) / *dir))) mindist = sv; } else { - /* If direction component along this axis is NEAR 0, (ie, + /* If direction component along this axis is NEAR 0, (i.e., * this ray is aligned with this axis), merely check * against the boundaries. */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <r_...@us...> - 2012-11-19 23:18:13
|
Revision: 53775 http://brlcad.svn.sourceforge.net/brlcad/?rev=53775&view=rev Author: r_weiss Date: 2012-11-19 23:18:06 +0000 (Mon, 19 Nov 2012) Log Message: ----------- Added two new functions to libbn in file "plane.c". Added "bn_distsq_pt3_lseg3_v2" which a test version of "bn_distsq_pt3_lseg3" and "bn_are_equal" which is a support function for this new test function. At some point this test function and the original will be consolidated. Modified Paths: -------------- brlcad/trunk/src/libbn/plane.c Modified: brlcad/trunk/src/libbn/plane.c =================================================================== --- brlcad/trunk/src/libbn/plane.c 2012-11-19 22:05:36 UTC (rev 53774) +++ brlcad/trunk/src/libbn/plane.c 2012-11-19 23:18:06 UTC (rev 53775) @@ -2322,6 +2322,205 @@ /** + * B N _ A R E _ E Q U A L + * + * This is a support function for the test function "bn_distsq_pt3_lseg3_v2". + * + */ +int +bn_are_equal(fastf_t a, fastf_t b, fastf_t t) +{ + fastf_t ai, af, bi, bf; + int ret = 0; + + af = modf(a, &ai); + bf = modf(b, &bi); + + if ((long)ai == (long)bi && t < 1.0) { + if (NEAR_EQUAL(af, bf, t)) { + ret = 1; + } + } else { + if (NEAR_EQUAL(a, b, t)) { + ret = 1; + } + } + return ret; +} + + +/** + * B N _ D I S T S Q _ P T 3 _ L S E G 3 _ v 2 + * + * Find the square of the distance from a point P to a line segment described + * by the two endpoints A and B. + * + * P + * * + * /. + * / . + * / . + * / . (dist) + * / . + * / . + * *------*--------* + * A PCA B + * + * There are six distinct cases, with these return codes - + * 0 P is within tolerance of lseg AB. *dist = 0. + * 1 P is within tolerance of point A. *dist = 0. + * 2 P is within tolerance of point B. *dist = 0. + * 3 PCA is within tolerance of A. *dist = |P-A|**2. + * 4 PCA is within tolerance of B. *dist = |P-B|**2. + * 5 P is "above/below" lseg AB. *dist=|PCA-P|**2. + * + * This function is a test version of "bn_distsq_pt3_lseg3". + * + */ +int +bn_distsq_pt3_lseg3_v2(fastf_t *distsq, const fastf_t *a, const fastf_t *b, const fastf_t *p, const struct bn_tol *tol) +{ + vect_t AtoB, AtoP, BtoP; + fastf_t AtoB_mag, AtoP_mag, AtoPCA_mag, PtoPCA_mag, BtoP_mag; + fastf_t dot, dt, dist; + int ret; + + dt = tol->dist; + VSUB2(AtoB, b, a); + AtoB_mag = MAGNITUDE(AtoB); + VSUB2(AtoP, p, a); + AtoP_mag = MAGNITUDE(AtoP); + if (AtoB_mag < dt) { + /* (A=B) */ + if (AtoP_mag < dt) { + /* ambiguous case: (A=B) (A=P) (B=P) */ + /* return could be 0 thru 4 */ + /* P is within tolerance of point A */ + dist = 0.0; + ret = 1; + } else { + /* ambiguous case: (A=B) (A!=P) */ + /* return could be 3 thru 5 */ + /* P is "above/below" lseg AB */ + dist = AtoP_mag; + ret = 5; + } + } else { + /* (A!=B) */ + if (AtoP_mag < dt) { + /* ambiguous case: (A!=B) (A=P) */ + /* return could be 0,1,3 */ + dist = 0.0; + ret = 1; /* P is within tolerance of point A */ + } else { + /* (A!=B) (A!=P) */ + if (bn_lseg3_lseg3_parallel(a, b, a, p, tol)) { + /* AtoB and AtoP are collinear */ + dot = VDOT(AtoB, AtoP); + if (dot > SMALL_FASTF) { + /* AtoB and AtoP pointing in the same direction */ + if (bn_are_equal(AtoB_mag, AtoP_mag, dt) || bn_pt3_pt3_equal(b, p, tol)) { + /* AtoB and AtoP pointing in the same direction */ + /* (B=P) */ + /* ambiguous case: (A!=B) (A!=P) (B=P) */ + /* return could be 0, 2, 4 */ + dist = 0.0; + ret = 2; + } else if (AtoP_mag > AtoB_mag) { + /* AtoB and AtoP pointing in the same direction */ + /* (A!=B) (A!=P) (B!=P), lsegs AtoB and AtoP are collinear. + * P is to the right of B, not above/below lseg AtoB. + */ + /* both P and PCA and not within tolerance of lseg AtoB */ + dist = AtoP_mag - AtoB_mag; + ret = 4; + } else { + /* AtoB and AtoP pointing in the same direction */ + /* (A!=B) (A!=P) (B!=P), lsegs AtoB and AtoP are collinear */ + /* AtoP_mag < AtoB_mag */ + /* P is on lseg AtoB */ + dist = 0.0; + ret = 0; + } + } else { + /* AtoB and AtoP are collinear */ + /* AtoB and AtoP pointing in opposite directions */ + /* P is to the left of A, not above/below lseg AtoB. */ + /* both P and PCA and not within tolerance of lseg AtoB */ + dist = AtoP_mag; + ret = 3; + } + } else { + /* (A!=B) (A!=P) */ + /* AtoB and AtoP are not collinear */ + dot = VDOT(AtoP, AtoB); + if ZERO(dot) { + /* (A=PCA), lsegs AtoB and AtoP are perpendicular */ + dist = AtoP_mag; + ret = 3; + } else if (dot > SMALL_FASTF) { + AtoPCA_mag = dot / AtoB_mag; + if (NEAR_ZERO(AtoPCA_mag, dt)) { + /* (A=PCA), lsegs AtoB and AtoP are perpendicular */ + dist = AtoP_mag; + ret = 3; + } else if (bn_are_equal(AtoPCA_mag, AtoB_mag, dt) || EQUAL(AtoPCA_mag, AtoB_mag)) { + /* (B=PCA) */ + VSUB2(BtoP, p, b); + BtoP_mag = MAGNITUDE(BtoP); + if (BtoP_mag < dt) { + /* ambiguous case: (B=PCA) (B=P) */ + /* return could be 2 or 4 */ + dist = 0.0; + ret = 2; + } else { + /* (B=PCA) (B!=P) */ + dist = BtoP_mag; + ret = 4; + } + } else if (AtoPCA_mag < AtoB_mag) { + /* PCA is on lseg AtoB */ + PtoPCA_mag = sqrt((AtoP_mag * AtoP_mag) + (AtoPCA_mag * AtoPCA_mag)); + if (PtoPCA_mag < dt) { + /* P is within tolerance of lseg AtoB */ + dist = 0.0; + ret = 0; + } else { + dist = PtoPCA_mag; + ret = 5; + } + } else { + /* AtoPCA_mag > AtoB_mag */ + /* P is to the right of B, above/below lseg AtoB. */ + /* both P and PCA and not within tolerance of lseg AtoB */ + VSUB2(BtoP, p, b); + BtoP_mag = MAGNITUDE(BtoP); + dist = BtoP_mag; + ret = 4; + } + } else { + /* dot is neg */ + AtoPCA_mag = dot / AtoB_mag; + if (NEAR_ZERO(AtoPCA_mag, dt)) { + /* (PCA=A), lsegs AtoB and AtoP are perpendicular */ + dist = AtoP_mag; + ret = 3; + } else { + /* (PCA!=A), PCA is not on lseg AtoB */ + /* both P and PCA and not within tolerance of lseg AtoB */ + dist = AtoP_mag; + ret = 3; + } + } + } + } + } + *distsq = dist * dist; + return ret; +} + + +/** * B N _ D I S T _ P T 3 _ L S E G 3 *@brief * Find the distance from a point P to a line segment described by the This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |