// math for graphics utility routines and RC, from FreePCB #include <vector> #include <math.h> #include <float.h> #include <limits.h> #include <fctsys.h> #include <PolyLine.h> #include <math_for_graphics.h> static bool InRange( double x, double xi, double xf ); double Distance( double x1, double y1, double x2, double y2 ) { return hypot( x1 - x2, y1 - y2 ); } /** * Function TestLineHit * test for hit on line segment i.e. a point within a given distance from segment * @param x, y = cursor coords * @param xi,yi,xf,yf = the end-points of the line segment * @param dist = maximum distance for hit * return true if dist < distance between the point and the segment */ bool TestLineHit( int xi, int yi, int xf, int yf, int x, int y, double dist ) { double dd; // test for vertical or horizontal segment if( xf==xi ) { // vertical segment dd = fabs( (double) (x - xi) ); if( dd<dist && ( (yf>yi && y<yf && y>yi) || (yf<yi && y>yf && y<yi) ) ) return true; } else if( yf==yi ) { // horizontal segment dd = fabs( (double) (y - yi) ); if( dd<dist && ( (xf>xi && x<xf && x>xi) || (xf<xi && x>xf && x<xi) ) ) return true; } else { // oblique segment // find a,b such that (xi,yi) and (xf,yf) lie on y = a + bx double b = (double) (yf - yi) / (xf - xi); double a = (double) yi - b * xi; // find c,d such that (x,y) lies on y = c + dx where d=(-1/b) double d = -1.0 / b; double c = (double) y - d * x; // find nearest point to (x,y) on line segment (xi,yi) to (xf,yf) double xp = (a - c) / (d - b); double yp = a + b * xp; // find distance dd = sqrt( (x - xp) * (x - xp) + (y - yp) * (y - yp) ); if( fabs( b )>0.7 ) { // line segment more vertical than horizontal if( dd<dist && ( (yf>yi && yp<yf && yp>yi) || (yf<yi && yp>yf && yp<yi) ) ) return 1; } else { // line segment more horizontal than vertical if( dd<dist && ( (xf>xi && xp<xf && xp>xi) || (xf<xi && xp>xf && xp<xi) ) ) return true; } } return false; // no hit } /* Function FindSegmentIntersections * find intersections between line segment (xi,yi) to (xf,yf) * and line segment (xi2,yi2) to (xf2,yf2) * returns true if intersection found */ bool FindSegmentIntersections( int xi, int yi, int xf, int yf, int xi2, int yi2, int xf2, int yf2 ) { if( max( xi, xf ) < min( xi2, xf2 ) || min( xi, xf ) > max( xi2, xf2 ) || max( yi, yf ) < min( yi2, yf2 ) || min( yi, yf ) > max( yi2, yf2 ) ) return false; return TestForIntersectionOfStraightLineSegments( xi, yi, xf, yf, xi2, yi2, xf2, yf2 ); } /* Function FindLineSegmentIntersection * find intersection between line y = a + bx and line segment (xi,yi) to (xf,yf) * if b > DBL_MAX/10, assume vertical line at x = a * return false if no intersection or true if intersect * return coords of intersections in *x1, *y1, *x2, *y2 * if no intersection, returns min distance in dist */ bool FindLineSegmentIntersection( double a, double b, int xi, int yi, int xf, int yf, double* x1, double* y1, double* x2, double* y2, double* dist ) { double xx = 0, yy = 0; // Init made to avoid C compil "uninitialized" warning bool bVert = false; if( b > DBL_MAX / 10.0 ) bVert = true; if( xf != xi ) // non-vertical segment, get intersection { // horizontal or oblique straight segment // put into form y = c + dx; double d = (double) (yf - yi) / (double) (xf - xi); double c = yf - d * xf; if( bVert ) { // if vertical line, easy if( InRange( a, xi, xf ) ) { *x1 = a; *y1 = c + d * a; return 1; } else { if( dist ) *dist = min( abs( a - xi ), abs( a - xf ) ); return false; } } if( fabs( b - d ) < 1E-12 ) { // parallel lines if( dist ) { *dist = GetPointToLineDistance( a, b, xi, xf ); } return false; // lines parallel } // calculate intersection xx = (c - a) / (b - d); yy = a + b * (xx); // see if intersection is within the line segment if( yf == yi ) { // horizontal line if( (xx>=xi && xx>xf) || (xx<=xi && xx<xf) ) return false; } else { // oblique line if( (xx>=xi && xx>xf) || (xx<=xi && xx<xf) || (yy>yi && yy>yf) || (yy<yi && yy<yf) ) return false; } } else { // vertical line segment if( bVert ) return false; xx = xi; yy = a + b * xx; if( (yy>=yi && yy>yf) || (yy<=yi && yy<yf) ) return 0; } *x1 = xx; *y1 = yy; return true; } /* * Function TestForIntersectionOfStraightLineSegments * Test for intersection of line segments * If lines are parallel, returns false * If true, returns also intersection coords in x, y * if false, returns min. distance in dist (may be 0.0 if parallel) */ bool TestForIntersectionOfStraightLineSegments( int x1i, int y1i, int x1f, int y1f, int x2i, int y2i, int x2f, int y2f, int* x, int* y, double* d ) { double a, b, dist; // first, test for intersection if( x1i == x1f && x2i == x2f ) { // both segments are vertical, can't intersect } else if( y1i == y1f && y2i == y2f ) { // both segments are horizontal, can't intersect } else if( x1i == x1f && y2i == y2f ) { // first seg. vertical, second horizontal, see if they cross if( InRange( x1i, x2i, x2f ) && InRange( y2i, y1i, y1f ) ) { if( x ) *x = x1i; if( y ) *y = y2i; if( d ) *d = 0.0; return true; } } else if( y1i == y1f && x2i == x2f ) { // first seg. horizontal, second vertical, see if they cross if( InRange( y1i, y2i, y2f ) && InRange( x2i, x1i, x1f ) ) { if( x ) *x = x2i; if( y ) *y = y1i; if( d ) *d = 0.0; return true; } } else if( x1i == x1f ) { // first segment vertical, second oblique // get a and b for second line segment, so that y = a + bx; b = double( y2f - y2i ) / (x2f - x2i); a = (double) y2i - b * x2i; double x1, y1, x2, y2; int test = FindLineSegmentIntersection( a, b, x1i, y1i, x1f, y1f, &x1, &y1, &x2, &y2 ); if( test ) { if( InRange( y1, y1i, y1f ) && InRange( x1, x2i, x2f ) && InRange( y1, y2i, y2f ) ) { if( x ) *x = (int) x1; if( y ) *y = (int) y1; if( d ) *d = 0.0; return true; } } } else if( y1i == y1f ) { // first segment horizontal, second oblique // get a and b for second line segment, so that y = a + bx; b = double( y2f - y2i ) / (x2f - x2i); a = (double) y2i - b * x2i; double x1, y1, x2, y2; int test = FindLineSegmentIntersection( a, b, x1i, y1i, x1f, y1f, &x1, &y1, &x2, &y2 ); if( test ) { if( InRange( x1, x1i, x1f ) && InRange( x1, x2i, x2f ) && InRange( y1, y2i, y2f ) ) { if( x ) *x = (int) x1; if( y ) *y = (int) y1; if( d ) *d = 0.0; return true; } } } else if( x2i == x2f ) { // second segment vertical, first oblique // get a and b for first line segment, so that y = a + bx; b = double( y1f - y1i ) / (x1f - x1i); a = (double) y1i - b * x1i; double x1, y1, x2, y2; int test = FindLineSegmentIntersection( a, b, x2i, y2i, x2f, y2f, &x1, &y1, &x2, &y2 ); if( test ) { if( InRange( x1, x1i, x1f ) && InRange( y1, y1i, y1f ) && InRange( y1, y2i, y2f ) ) { if( x ) *x = (int) x1; if( y ) *y = (int) y1; if( d ) *d = 0.0; return true; } } } else if( y2i == y2f ) { // second segment horizontal, first oblique // get a and b for second line segment, so that y = a + bx; b = double( y1f - y1i ) / (x1f - x1i); a = (double) y1i - b * x1i; double x1, y1, x2, y2; int test = FindLineSegmentIntersection( a, b, x2i, y2i, x2f, y2f, &x1, &y1, &x2, &y2 ); if( test ) { if( InRange( x1, x1i, x1f ) && InRange( y1, y1i, y1f ) ) { if( x ) *x = (int) x1; if( y ) *y = (int) y1; if( d ) *d = 0.0; return true; } } } else { // both segments oblique if( long( y1f - y1i ) * (x2f - x2i) != long( y2f - y2i ) * (x1f - x1i) ) { // not parallel, get a and b for first line segment, so that y = a + bx; b = double( y1f - y1i ) / (x1f - x1i); a = (double) y1i - b * x1i; double x1, y1, x2, y2; int test = FindLineSegmentIntersection( a, b, x2i, y2i, x2f, y2f, &x1, &y1, &x2, &y2 ); // both segments oblique if( test ) { if( InRange( x1, x1i, x1f ) && InRange( y1, y1i, y1f ) ) { if( x ) *x = (int) x1; if( y ) *y = (int) y1; if( d ) *d = 0.0; return true; } } } } // don't intersect, get shortest distance between each endpoint and the other line segment dist = GetPointToLineSegmentDistance( x1i, y1i, x2i, y2i, x2f, y2f ); double xx = x1i; double yy = y1i; double dd = GetPointToLineSegmentDistance( x1f, y1f, x2i, y2i, x2f, y2f ); if( dd < dist ) { dist = dd; xx = x1f; yy = y1f; } dd = GetPointToLineSegmentDistance( x2i, y2i, x1i, y1i, x1f, y1f ); if( dd < dist ) { dist = dd; xx = x2i; yy = y2i; } dd = GetPointToLineSegmentDistance( x2f, y2f, x1i, y1i, x1f, y1f ); if( dd < dist ) { dist = dd; xx = x2f; yy = y2f; } if( x ) *x = (int) xx; if( y ) *y = (int) yy; if( d ) *d = dist; return false; } /* Function GetClearanceBetweenSegments * Get clearance between 2 segments * Returns coordinates of the closest point between these 2 segments in x, y * If clearance > max_cl, just returns max_cl+1 and doesn't return x,y */ int GetClearanceBetweenSegments( int x1i, int y1i, int x1f, int y1f, int w1, int x2i, int y2i, int x2f, int y2f, int w2, int max_cl, int* x, int* y ) { // check clearance between bounding rectangles int min_dist = max_cl + ( (w1 + w2) / 2 ); if( min( x1i, x1f ) - max( x2i, x2f ) > min_dist ) return max_cl+1; if( min( x2i, x2f ) - max( x1i, x1f ) > min_dist ) return max_cl+1; if( min( y1i, y1f ) - max( y2i, y2f ) > min_dist ) return max_cl+1; if( min( y2i, y2f ) - max( y1i, y1f ) > min_dist ) return max_cl+1; int xx, yy; double dist; TestForIntersectionOfStraightLineSegments( x1i, y1i, x1f, y1f, x2i, y2i, x2f, y2f, &xx, &yy, &dist ); int d = (int) dist - ( (w1 + w2) / 2 ); if( d < 0 ) d = 0; if( x ) *x = xx; if( y ) *y = yy; return d; } /* Function GetPointToLineDistance * Get min. distance from (x,y) to line y = a + bx * if b > DBL_MAX/10, assume vertical line at x = a * returns closest point on line in xpp, ypp */ double GetPointToLineDistance( double a, double b, int x, int y, double* xpp, double* ypp ) { if( b > DBL_MAX / 10 ) { // vertical line if( xpp && ypp ) { *xpp = a; *ypp = y; } return abs( a - x ); } // find c,d such that (x,y) lies on y = c + dx where d=(-1/b) double d = -1.0 / b; double c = (double) y - d * x; // find nearest point to (x,y) on line through (xi,yi) to (xf,yf) double xp = (a - c) / (d - b); double yp = a + b * xp; if( xpp && ypp ) { *xpp = xp; *ypp = yp; } // find distance return Distance( x, y, xp, yp ); } /** * Function GetPointToLineSegmentDistance * Get distance between line segment and point * @param x,y = point * @param xi,yi Start point of the line segament * @param xf,yf End point of the line segment * @return the distance */ double GetPointToLineSegmentDistance( int x, int y, int xi, int yi, int xf, int yf ) { // test for vertical or horizontal segment if( xf==xi ) { // vertical line segment if( InRange( y, yi, yf ) ) return abs( x - xi ); else return min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) ); } else if( yf==yi ) { // horizontal line segment if( InRange( x, xi, xf ) ) return abs( y - yi ); else return min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) ); } else { // oblique segment // find a,b such that (xi,yi) and (xf,yf) lie on y = a + bx double b = (double) (yf - yi) / (xf - xi); double a = (double) yi - b * xi; // find c,d such that (x,y) lies on y = c + dx where d=(-1/b) double d = -1.0 / b; double c = (double) y - d * x; // find nearest point to (x,y) on line through (xi,yi) to (xf,yf) double xp = (a - c) / (d - b); double yp = a + b * xp; // find distance if( InRange( xp, xi, xf ) && InRange( yp, yi, yf ) ) return Distance( x, y, xp, yp ); else return min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) ); } } // test for value within range bool InRange( double x, double xi, double xf ) { if( xf > xi ) { if( x >= xi && x <= xf ) return true; } else { if( x >= xf && x <= xi ) return true; } return false; }