kicad/thirdparty/other_math/math_for_graphics.cpp

521 lines
14 KiB
C++
Raw Normal View History

// math for graphics utility routines and RC, from FreePCB
2007-12-29 19:27:58 +00:00
#include <vector>
#include <cmath>
2007-12-29 19:27:58 +00:00
#include <float.h>
#include <limits.h>
2015-12-07 13:51:46 +00:00
#include <cstdlib> // for abs function on ints
#include <algorithm>
#include <math_for_graphics.h>
2007-12-29 19:27:58 +00:00
#include <math/util.h>
2012-08-01 07:07:56 +00:00
static bool InRange( double x, double xi, double xf );
2012-08-01 07:07:56 +00:00
/* 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 )
2007-12-29 19:27:58 +00:00
{
if( std::max( xi, xf ) < std::min( xi2, xf2 )
|| std::min( xi, xf ) > std::max( xi2, xf2 )
|| std::max( yi, yf ) < std::min( yi2, yf2 )
|| std::min( yi, yf ) > std::max( yi2, yf2 ) )
2012-08-01 07:07:56 +00:00
return false;
2012-08-01 07:07:56 +00:00
return TestForIntersectionOfStraightLineSegments( xi, yi, xf, yf,
xi2, yi2, xf2, yf2 );
2007-12-29 19:27:58 +00:00
}
2012-08-01 07:07:56 +00:00
/* 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 )
2007-12-29 19:27:58 +00:00
{
double xx = 0, yy = 0; // Init made to avoid C compil "uninitialized" warning
bool bVert = false;
if( b > DBL_MAX / 10.0 )
bVert = true;
2007-12-29 19:27:58 +00:00
2012-08-01 07:07:56 +00:00
if( xf != xi ) // non-vertical segment, get intersection
{
2012-08-01 07:07:56 +00:00
// horizontal or oblique straight segment
// put into form y = c + dx;
double d = (double) (yf - yi) / (double) (xf - xi);
double c = yf - d * xf;
2012-08-01 07:07:56 +00:00
if( bVert )
{
2012-08-01 07:07:56 +00:00
// if vertical line, easy
if( InRange( a, xi, xf ) )
{
2012-08-01 07:07:56 +00:00
*x1 = a;
*y1 = c + d * a;
return 1;
}
else
{
2012-08-01 07:07:56 +00:00
if( dist )
2015-06-26 23:21:09 +00:00
*dist = std::min( std::abs( a - xi ), std::abs( a - xf ) );
2012-08-01 07:07:56 +00:00
return false;
}
2012-08-01 07:07:56 +00:00
}
2015-06-26 23:21:09 +00:00
if( std::abs( b - d ) < 1E-12 )
2012-08-01 07:07:56 +00:00
{
// parallel lines
if( dist )
{
2012-08-01 07:07:56 +00:00
*dist = GetPointToLineDistance( a, b, xi, xf );
}
2012-08-01 07:07:56 +00:00
return false; // lines parallel
}
2012-08-01 07:07:56 +00:00
// calculate intersection
xx = (c - a) / (b - d);
yy = a + b * (xx);
2012-08-01 07:07:56 +00:00
// 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
2012-08-01 07:07:56 +00:00
{
// 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 )
2012-08-01 07:07:56 +00:00
return false;
xx = xi;
yy = a + b * xx;
if( (yy>=yi && yy>yf) || (yy<=yi && yy<yf) )
return 0;
}
*x1 = xx;
*y1 = yy;
2012-08-01 07:07:56 +00:00
return true;
2007-12-29 19:27:58 +00:00
}
/*
* 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 )
2007-12-29 19:27:58 +00:00
{
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;
2012-08-01 07:07:56 +00:00
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 = KiROUND( x1 );
if( y )
2013-05-04 11:57:09 +00:00
*y = KiROUND( 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;
2012-08-01 07:07:56 +00:00
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 )
2013-05-04 11:57:09 +00:00
*x = KiROUND( x1 );
if( y )
2013-05-04 11:57:09 +00:00
*y = KiROUND( 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;
2012-08-01 07:07:56 +00:00
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 )
2013-05-04 11:57:09 +00:00
*x = KiROUND( x1 );
if( y )
2013-05-04 11:57:09 +00:00
*y = KiROUND( 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;
2012-08-01 07:07:56 +00:00
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 )
2013-05-04 11:57:09 +00:00
*x = KiROUND( x1 );
if( y )
2013-05-04 11:57:09 +00:00
*y = KiROUND( 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;
2012-08-01 07:07:56 +00:00
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 )
2013-05-04 11:57:09 +00:00
*x = KiROUND( x1 );
if( y )
2013-05-04 11:57:09 +00:00
*y = KiROUND( 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 )
2013-05-04 11:57:09 +00:00
*x = KiROUND( xx );
if( y )
2013-05-04 11:57:09 +00:00
*y = KiROUND( yy );
if( d )
*d = dist;
return false;
2007-12-29 19:27:58 +00:00
}
2012-08-01 07:07:56 +00:00
/* 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
*/
2012-08-01 07:07:56 +00:00
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 )
2007-12-29 19:27:58 +00:00
{
// check clearance between bounding rectangles
int min_dist = max_cl + ( (w1 + w2) / 2 );
if( std::min( x1i, x1f ) - std::max( x2i, x2f ) > min_dist )
return max_cl+1;
if( std::min( x2i, x2f ) - std::max( x1i, x1f ) > min_dist )
return max_cl+1;
if( std::min( y1i, y1f ) - std::max( y2i, y2f ) > min_dist )
return max_cl+1;
if( std::min( y2i, y2f ) - std::max( y1i, y1f ) > min_dist )
return max_cl+1;
2007-12-29 19:27:58 +00:00
2012-08-01 07:07:56 +00:00
int xx, yy;
double dist;
TestForIntersectionOfStraightLineSegments( x1i, y1i, x1f, y1f,
x2i, y2i, x2f, y2f, &xx, &yy, &dist );
int d = KiROUND( dist ) - ((w1 + w2) / 2);
2012-08-01 07:07:56 +00:00
if( d < 0 )
d = 0;
if( x )
2012-08-01 07:07:56 +00:00
*x = xx;
if( y )
2012-08-01 07:07:56 +00:00
*y = yy;
2012-08-01 07:07:56 +00:00
return d;
2007-12-29 19:27:58 +00:00
}
2012-08-01 07:07:56 +00:00
/* 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 )
2007-12-29 19:27:58 +00:00
{
if( b > DBL_MAX / 10 )
{
// vertical line
if( xpp && ypp )
{
*xpp = a;
*ypp = y;
}
2015-06-26 23:21:09 +00:00
return std::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 );
2007-12-29 19:27:58 +00:00
}
/**
* Function GetPointToLineSegmentDistance
2007-12-29 19:27:58 +00:00
* 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
2007-12-29 19:27:58 +00:00
* @return the distance
*/
2012-08-01 07:07:56 +00:00
double GetPointToLineSegmentDistance( int x, int y, int xi, int yi, int xf, int yf )
2007-12-29 19:27:58 +00:00
{
// test for vertical or horizontal segment
if( xf==xi )
{
// vertical line segment
if( InRange( y, yi, yf ) )
2015-12-07 13:51:46 +00:00
return std::abs( x - xi );
else
return std::min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) );
}
else if( yf==yi )
{
// horizontal line segment
if( InRange( x, xi, xf ) )
2015-12-07 13:51:46 +00:00
return std::abs( y - yi );
else
return std::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 std::min( Distance( x, y, xi, yi ), Distance( x, y, xf, yf ) );
}
2007-12-29 19:27:58 +00:00
}
2007-12-29 19:27:58 +00:00
// 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;
2007-12-29 19:27:58 +00:00
}