From 693913ffdbb28014bef1b9fa82fcceb04ae1e52b Mon Sep 17 00:00:00 2001 From: Homer Reid Date: Tue, 10 Jul 2018 18:28:31 -0400 Subject: [PATCH 1/4] update point_in_prism to avoid double-counting of polygon edge intersections when plumb lines pass through vertices --- utils/geom.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/utils/geom.c b/utils/geom.c index 80e9bf0..a2619f8 100644 --- a/utils/geom.c +++ b/utils/geom.c @@ -2149,10 +2149,14 @@ boolean intersect_line_with_segment(double px, double py, double dx, double dy, double t = (M00*RHSy-M10*RHSx)/DetM; if (s) *s = (M11*RHSx-M01*RHSy)/DetM; - if (t<0.0 || t>1.0) // intersection of lines does not lie between vertices - return 0; - - return 1; + // the plumb line intersects the segment if 0<=t<=1, with t==0,1 + // corresponding to the endpoints; for our purposes we count + // the intersection if the plumb line runs through the t==0 vertex, but + // NOT the t==1 vertex, to avoid double-counting for complete polygons. +#define DELTAT 1.0e-6 +#define TMIN (0.0) +#define TMAX (1.0-DELTAT) + return ( ( tTMAX ) ? 0 : 1 ); } // like the previous routine, but only count intersections if s>=0 From 893208c4b5eecb1ef2f41202de67ef8201f7169d Mon Sep 17 00:00:00 2001 From: Homer Reid Date: Thu, 12 Jul 2018 08:04:47 -0400 Subject: [PATCH 2/4] revised normal_to_object implementation for prisms to account for in-plane distance from point to prism face when determining closest face --- utils/geom.c | 138 +++++++++++++++++++++++++++++++-------------- utils/test-prism.c | 20 ++++++- 2 files changed, 116 insertions(+), 42 deletions(-) diff --git a/utils/geom.c b/utils/geom.c index a2619f8..8e8f168 100644 --- a/utils/geom.c +++ b/utils/geom.c @@ -2301,27 +2301,85 @@ double intersect_line_segment_with_prism(prism *prsm, vector3 p, vector3 d, doub } /***************************************************************/ -/* compute the minimum distance from p to the plane */ -/* containing o (origin) and spanned by basis vectors v1,v2 */ -/* algorithm: solve the 3x3 system p-s*v3 = o + t*v1 + u*v2 */ -/* where v3 = v1 x v2 and s,t,u are unknowns. */ -/* return value is unit normal vector to plane and *s is such */ -/* that p-(*s)*v3 lies in the plane */ +/* compute the minimum distance from a 3D point p to the */ +/* line segment with endpoints v1,v2. */ +/* algorithm: let pLine = v1 + d*(v2-v1) be the point on the */ +/* line closest to p; d is defined by minimizing |p-pLine|^2. */ +/* --> |p-v1|^2 + d^2 |v2-v1|^2 - 2*d*dot(p-v1,v2-v1) = min */ +/* --> 2d |v2-v1|^2 - 2*dot(p-v1,v2-v1) = 0 */ +/* --> d = dot(p-v1,v2-v1) / |v2-v1|^2 */ /***************************************************************/ -vector3 normal_to_plane(vector3 o, vector3 v1, vector3 v2, vector3 p, double *s) +double min_distance_to_line_segment(vector3 p, vector3 v1, vector3 v2) +{ vector3 v2mv1 = vector3_minus(v2,v1); + vector3 pmv1 = vector3_minus(p,v1); + double d = vector3_dot(v2mv1,pmv1) / vector3_dot(v2mv1,v2mv1); + if (d<0.0) d=0.0; // if pProj lies outside the line segment, + if (d>1.0) d=1.0; // displace it to whichever vertex is closer + vector3 pLine = vector3_plus(v1, vector3_scale(d,v2mv1)); + return vector3_norm(vector3_minus(p,pLine)); +} + +/***************************************************************/ +/* compute the minimum distance from a 3D point p to the */ +/* planar quadrilateral with vertices {o, o+l1, o+l2, o+l1+l2} */ +/* (if quadrilateral==1) */ +/* or the planar triangle with vertices {o, o+l1, o+l2} */ +/* (if quadrilateral==0). */ +/* algorithm: */ +/* (A) solve a 3x3 system to compute the projection of p into */ +/* the plane of the triangle/quadrilateral (call it pPlane)*/ +/* pPlane = p-s*l3 = o + t*l1 + u*l2 */ +/* where l3 is the normal to the surface and s,t,u are */ +/* unknowns. */ +/* (B) if pPlane lies within the triangle/quadrilateral, the */ +/* minimum distance is simply s. */ +/* (C) otherwise compute the minimum distance d from pProj */ +/* to any edge of the triangle/quadrilateral and set */ +/* minimum_distance = sqrt(s^2 + d^2). */ +/* return value is normal to plane. */ +/***************************************************************/ +vector3 normal_to_plane(vector3 o, vector3 l1, vector3 l2, vector3 p, + int quadrilateral, double *min_distance) { - vector3 v3 = unit_vector3(vector3_cross(v1, v2)); - CHECK( (vector3_norm(v3)>1.0e-6), "degenerate plane in normal_to_plane" ); + vector3 l3 = unit_vector3(vector3_cross(l1, l2)); + CHECK( (vector3_norm(l3)>1.0e-6), "degenerate plane in normal_to_plane" ); matrix3x3 M; - M.c0 = v1; - M.c1 = v2; - M.c2 = v3; + M.c0 = l1; + M.c1 = l2; + M.c2 = l3; vector3 RHS = vector3_minus(p,o); vector3 tus = matrix3x3_vector3_mult(matrix3x3_inverse(M),RHS); // "t, u, s" - *s = tus.z; - return v3; + double t=tus.x, u=tus.y, s = tus.z; + int inside = ( ( 0.0<=t && t<=1.0 && 0.0<=u && u<=1.0 ) ? 1 : 0 ); + if (!quadrilateral && (t+u)>1.0) inside=0; // need (t+u)<1 for triangle interior + double d=0.0; + if(inside==0) + { vector3 pPlane = vector3_minus(p, vector3_scale(s,l3) ); + vector3 v01 = vector3_plus(o,l1); + vector3 v10 = vector3_plus(o,l2); + d=min_distance_to_line_segment(pPlane, o, v01); + d=fmin(d,min_distance_to_line_segment(pPlane, o, v10)); + if (quadrilateral) + { vector3 v11 = vector3_plus(v01,l2); + d=fmin(d,min_distance_to_line_segment(pPlane, v01, v11)); + d=fmin(d,min_distance_to_line_segment(pPlane, v11, v10)); + } + else + d=fmin(d,min_distance_to_line_segment(pPlane, v01, v10)); + } + *min_distance=sqrt(s*s+d*d); + return l3; } +vector3 normal_to_quadrilateral(vector3 o, vector3 v1, vector3 v2, vector3 p, + double *min_distance) +{ return normal_to_plane(o,v1,v2,p,1,min_distance); } + +vector3 normal_to_triangle(vector3 o, vector3 v1, vector3 v2, vector3 p, + double *min_distance) +{ return normal_to_plane(o,v1,v2,p,0,min_distance); } + + /***************************************************************/ /* find the face of the prism for which the normal distance */ /* from x to the plane of that face is the shortest, then */ @@ -2335,43 +2393,41 @@ vector3 normal_to_prism(prism *prsm, vector3 xc) int num_vertices = prsm->vertices.num_items; vector3 xp = prism_coordinate_c2p(prsm, xc); - vector3 axis = {0,0,0}; axis.z=height; + vector3 zhat = {0,0,1.0}, axis=vector3_scale(height, zhat); + if (height==0.0) + return prism_vector_p2c(prsm, zhat); vector3 retval; double min_distance=HUGE_VAL; + int nv; + for(nv=0; nv0.0) - { int nv; - for(nv=0; nv=argc) usage("too few arguments to --line"); + vector3 v1,v2; + sscanf(argv[narg+1],"%le",&(v1.x)); + sscanf(argv[narg+2],"%le",&(v1.y)); + sscanf(argv[narg+3],"%le",&(v1.z)); + sscanf(argv[narg+4],"%le",&(v2.x)); + sscanf(argv[narg+5],"%le",&(v2.y)); + sscanf(argv[narg+6],"%le",&(v2.z)); + printf("Min distance=%e\n",min_distance_to_line_segment(test_point,v1,v2)); + narg+=6; + } else if (!strcmp(argv[narg],"--dir")) { if (narg+5>=argc) usage("too few arguments to --lineseg"); sscanf(argv[narg+1],"%le",&(test_dir.x)); From 7c2368f94c46cd31d8b5ecf8b5a006162e137955 Mon Sep 17 00:00:00 2001 From: Homer Reid Date: Thu, 12 Jul 2018 12:10:57 -0400 Subject: [PATCH 3/4] re-updated normal_to_prism to behave correctly when the projection of the point into the plane does not lie within the prism face in question --- utils/geom.c | 134 ++++++++++++++++++++++++--------------------- utils/test-prism.c | 3 + 2 files changed, 75 insertions(+), 62 deletions(-) diff --git a/utils/geom.c b/utils/geom.c index 8e8f168..1d5f411 100644 --- a/utils/geom.c +++ b/utils/geom.c @@ -2320,64 +2320,77 @@ double min_distance_to_line_segment(vector3 p, vector3 v1, vector3 v2) } /***************************************************************/ -/* compute the minimum distance from a 3D point p to the */ -/* planar quadrilateral with vertices {o, o+l1, o+l2, o+l1+l2} */ -/* (if quadrilateral==1) */ -/* or the planar triangle with vertices {o, o+l1, o+l2} */ -/* (if quadrilateral==0). */ -/* algorithm: */ -/* (A) solve a 3x3 system to compute the projection of p into */ -/* the plane of the triangle/quadrilateral (call it pPlane)*/ -/* pPlane = p-s*l3 = o + t*l1 + u*l2 */ -/* where l3 is the normal to the surface and s,t,u are */ -/* unknowns. */ -/* (B) if pPlane lies within the triangle/quadrilateral, the */ -/* minimum distance is simply s. */ -/* (C) otherwise compute the minimum distance d from pProj */ -/* to any edge of the triangle/quadrilateral and set */ -/* minimum_distance = sqrt(s^2 + d^2). */ -/* return value is normal to plane. */ +/* compute the projection of a 3D point p into the plane */ +/* containing the three points {o, o+v1, o+v2}. */ +/* algorithm: solve a 3x3 system to compute the projection of */ +/* p into the plane (call it pPlane) */ +/* pPlane = p-s*v3 = o + t*v1 + u*v2 */ +/* where v3 is the normal to the plane and s,t,u */ +/* are unknowns. */ +/* on return, s is the normal distance. the return value is */ +/* the normal vector to the plane. */ +/* if in_quadrilateral is non-null it is set to 1 or 0 */ +/* according as pPlane does or does not lie in the */ +/* quadrilateral with vertices (o, o+v1, o+v2, o+v1+v2). */ /***************************************************************/ -vector3 normal_to_plane(vector3 o, vector3 l1, vector3 l2, vector3 p, - int quadrilateral, double *min_distance) +vector3 normal_to_plane(vector3 p, vector3 o, vector3 v1, vector3 v2, + double *s, int *in_quadrilateral) { - vector3 l3 = unit_vector3(vector3_cross(l1, l2)); - CHECK( (vector3_norm(l3)>1.0e-6), "degenerate plane in normal_to_plane" ); + vector3 v3 = unit_vector3(vector3_cross(v1, v2)); + CHECK( (vector3_norm(v3)>1.0e-6), "degenerate plane in project_point_into_plane" ); matrix3x3 M; - M.c0 = l1; - M.c1 = l2; - M.c2 = l3; + M.c0 = v1; + M.c1 = v2; + M.c2 = v3; vector3 RHS = vector3_minus(p,o); vector3 tus = matrix3x3_vector3_mult(matrix3x3_inverse(M),RHS); // "t, u, s" - double t=tus.x, u=tus.y, s = tus.z; - int inside = ( ( 0.0<=t && t<=1.0 && 0.0<=u && u<=1.0 ) ? 1 : 0 ); - if (!quadrilateral && (t+u)>1.0) inside=0; // need (t+u)<1 for triangle interior - double d=0.0; + double t=tus.x, u=tus.y; + *s = tus.z; + if (in_quadrilateral) + *in_quadrilateral = ( ( 0.0<=t && t<=1.0 && 0.0<=u && u<=1.0 ) ? 1 : 0 ); + return v3; +} + +vector3 normal_to_quadrilateral(vector3 p, vector3 o, vector3 v1, vector3 v2, + double *min_distance) +{ + double s, d=0.0; + int inside; + vector3 v3=normal_to_plane(p, o, v1, v2, &s, &inside); if(inside==0) - { vector3 pPlane = vector3_minus(p, vector3_scale(s,l3) ); - vector3 v01 = vector3_plus(o,l1); - vector3 v10 = vector3_plus(o,l2); - d=min_distance_to_line_segment(pPlane, o, v01); - d=fmin(d,min_distance_to_line_segment(pPlane, o, v10)); - if (quadrilateral) - { vector3 v11 = vector3_plus(v01,l2); - d=fmin(d,min_distance_to_line_segment(pPlane, v01, v11)); - d=fmin(d,min_distance_to_line_segment(pPlane, v11, v10)); - } - else - d=fmin(d,min_distance_to_line_segment(pPlane, v01, v10)); + { vector3 pPlane = vector3_minus(p, vector3_scale(s,v3) ); + vector3 p01 = vector3_plus(o,v1); + vector3 p10 = vector3_plus(o,v2); + vector3 p11 = vector3_plus(p01,v2); + d= min_distance_to_line_segment(pPlane, o, p01); + d=fmin(d,min_distance_to_line_segment(pPlane, o, p10)); + d=fmin(d,min_distance_to_line_segment(pPlane, p01, p11)); + d=fmin(d,min_distance_to_line_segment(pPlane, p11, p10)); } *min_distance=sqrt(s*s+d*d); - return l3; + return v3; } -vector3 normal_to_quadrilateral(vector3 o, vector3 v1, vector3 v2, vector3 p, - double *min_distance) -{ return normal_to_plane(o,v1,v2,p,1,min_distance); } +vector3 normal_to_prism_roof_or_ceiling(vector3 p, vector3 *vertices, int num_vertices, + double height, double *min_distance) +{ + vector3 o = {0.0,0.0,0.0}; o.z=height; + double s; + vector3 v3 = normal_to_plane(p, o, vertices[0], vertices[1], &s, 0); + vector3 pPlane = vector3_minus(p, vector3_scale(s,v3) ); -vector3 normal_to_triangle(vector3 o, vector3 v1, vector3 v2, vector3 p, - double *min_distance) -{ return normal_to_plane(o,v1,v2,p,0,min_distance); } + double d = 0.0; + if (point_in_polygon(pPlane.x,pPlane.y,vertices,num_vertices)==0) + { int nv; + d=min_distance_to_line_segment(pPlane,vertices[0],vertices[1] ); + for(nv=0; nv Date: Thu, 12 Jul 2018 14:34:50 -0400 Subject: [PATCH 4/4] updates --- utils/geom.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/utils/geom.c b/utils/geom.c index 1d5f411..cc597ee 100644 --- a/utils/geom.c +++ b/utils/geom.c @@ -2153,10 +2153,7 @@ boolean intersect_line_with_segment(double px, double py, double dx, double dy, // corresponding to the endpoints; for our purposes we count // the intersection if the plumb line runs through the t==0 vertex, but // NOT the t==1 vertex, to avoid double-counting for complete polygons. -#define DELTAT 1.0e-6 -#define TMIN (0.0) -#define TMAX (1.0-DELTAT) - return ( ( tTMAX ) ? 0 : 1 ); + return t < 0 || ((float)t) >= 1 ? 0 : 1; } // like the previous routine, but only count intersections if s>=0 @@ -2344,7 +2341,7 @@ vector3 normal_to_plane(vector3 p, vector3 o, vector3 v1, vector3 v2, M.c2 = v3; vector3 RHS = vector3_minus(p,o); vector3 tus = matrix3x3_vector3_mult(matrix3x3_inverse(M),RHS); // "t, u, s" - double t=tus.x, u=tus.y; + float t=tus.x, u=tus.y; *s = tus.z; if (in_quadrilateral) *in_quadrilateral = ( ( 0.0<=t && t<=1.0 && 0.0<=u && u<=1.0 ) ? 1 : 0 );