diff --git a/utils/geom.c b/utils/geom.c index 80e9bf0..cc597ee 100644 --- a/utils/geom.c +++ b/utils/geom.c @@ -2149,10 +2149,11 @@ 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. + return t < 0 || ((float)t) >= 1 ? 0 : 1; } // like the previous routine, but only count intersections if s>=0 @@ -2297,27 +2298,98 @@ 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 */ +/***************************************************************/ +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 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 v1, vector3 v2, vector3 p, double *s) +vector3 normal_to_plane(vector3 p, vector3 o, vector3 v1, vector3 v2, + double *s, int *in_quadrilateral) { vector3 v3 = unit_vector3(vector3_cross(v1, v2)); - CHECK( (vector3_norm(v3)>1.0e-6), "degenerate plane in normal_to_plane" ); + CHECK( (vector3_norm(v3)>1.0e-6), "degenerate plane in project_point_into_plane" ); matrix3x3 M; 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" + 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 ); + 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,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 v3; +} + +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) ); + + 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; nvvertices.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; - - // side walls - if (height>0.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)); @@ -451,6 +469,9 @@ int main(int argc, char *argv[]) prism *prsm=the_prism.subclass.prism_data; prism2gmsh(prsm, "test-prism.pp"); prism2gnuplot(prsm, "test-prism.gp"); + f=fopen("test-point.gp","w"); + fprintf(f,"%e %e %e\n",test_point.x,test_point.y,test_point.z); + fclose(f); printf("Wrote prism description to GNUPLOT file test-prism.gp.\n"); printf("Wrote prism description to GMSH file test-prism.geo.\n");