Fix "false intersection" case and attempt to fix "missing intersection"
This commit is contained in:
Roberto Fernandez Bautista
parent
318435aedb
commit
2573ac19a5
|
|
@ -67,7 +67,7 @@ static T within_range(T x, T minval, T maxval, T wrap)
|
||||||
return rv;
|
return rv;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool sliceContainsPoint( SHAPE_ARC a, VECTOR2D p )
|
static bool sliceContainsPoint( const SHAPE_ARC& a, const VECTOR2I& p )
|
||||||
{
|
{
|
||||||
double phi = 180.0 / M_PI * atan2( p.y - a.GetCenter().y, p.x - a.GetCenter().x );
|
double phi = 180.0 / M_PI * atan2( p.y - a.GetCenter().y, p.x - a.GetCenter().x );
|
||||||
double ca = a.GetCentralAngle();
|
double ca = a.GetCentralAngle();
|
||||||
|
|
@ -87,96 +87,39 @@ static bool sliceContainsPoint( SHAPE_ARC a, VECTOR2D p )
|
||||||
return within_range( phi, sa, ea, 360.0 );
|
return within_range( phi, sa, ea, 360.0 );
|
||||||
}
|
}
|
||||||
|
|
||||||
static int arclineIntersect( const SHAPE_ARC& a, const SEG& s, VECTOR2D* pts )
|
static int arclineIntersect( const SHAPE_ARC& a, const SEG& s, std::vector<VECTOR2I>* ips )
|
||||||
{
|
{
|
||||||
VECTOR2I c = a.GetCenter();
|
CIRCLE circ( a.GetCenter(), a.GetRadius() );
|
||||||
double r = a.GetRadius();
|
|
||||||
VECTOR2I nearest = s.LineProject( c );
|
|
||||||
|
|
||||||
double nd = (c - nearest).EuclideanNorm();
|
std::vector<VECTOR2I> intersections = circ.IntersectLine( s );
|
||||||
|
|
||||||
/*overlay->SetStrokeColor( RED );
|
size_t originalSize = ips->size();
|
||||||
overlay->AnnotatedPoint( s.A, 200000 );
|
|
||||||
overlay->SetStrokeColor( GREEN );
|
|
||||||
overlay->AnnotatedPoint( s.B, 200000 );
|
|
||||||
overlay->SetStrokeColor( YELLOW );
|
|
||||||
overlay->AnnotatedPoint( c, 200000 );
|
|
||||||
|
|
||||||
|
for( const VECTOR2I& intersection : intersections )
|
||||||
overlay->SetStrokeColor( WHITE );
|
|
||||||
overlay->Arc( a );
|
|
||||||
overlay->SetStrokeColor( DARKGRAY );
|
|
||||||
overlay->Circle( a.GetCenter(), a.GetRadius() );*/
|
|
||||||
|
|
||||||
if( nd > r )
|
|
||||||
return 0;
|
|
||||||
|
|
||||||
double ll = r * r - nd * nd;
|
|
||||||
double l = 0;
|
|
||||||
if( ll > 0 )
|
|
||||||
l = sqrt(ll);
|
|
||||||
|
|
||||||
int n = 0;
|
|
||||||
|
|
||||||
VECTOR2I perp = ( s.B - s.A ).Resize( l );
|
|
||||||
|
|
||||||
VECTOR2I p0( nearest + perp );
|
|
||||||
VECTOR2I p1( nearest - perp );
|
|
||||||
|
|
||||||
if( sliceContainsPoint( a, p0 ) ) { pts[n] = p0; n++; }
|
|
||||||
if( sliceContainsPoint( a, p1 ) ) { pts[n] = p1; n++; }
|
|
||||||
|
|
||||||
//for(int i = 0; i < n ; i++) overlay->AnnotatedPoint( pts[i], 40000 );
|
|
||||||
|
|
||||||
return n;
|
|
||||||
}
|
|
||||||
|
|
||||||
int intersectArc2Arc( const SHAPE_ARC& a1, const SHAPE_ARC& a2, VECTOR2D* ips )
|
|
||||||
{
|
|
||||||
VECTOR2I dc( a2.GetCenter() - a1.GetCenter() );
|
|
||||||
double dcl = dc.EuclideanNorm();
|
|
||||||
double r1 = a1.GetRadius();
|
|
||||||
double r2 = a2.GetRadius();
|
|
||||||
|
|
||||||
if( dc.x == 0 && dc.y == 0 )
|
|
||||||
{
|
{
|
||||||
if(r1 == r2)
|
if( sliceContainsPoint( a, intersection ) )
|
||||||
{
|
ips->push_back( intersection );
|
||||||
ips[0] = a1.GetP0();
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
double d = ( dcl * dcl - r2*r2 + r1*r1 ) / (2 * dcl);
|
return ips->size() - originalSize;
|
||||||
|
}
|
||||||
|
|
||||||
//overlay->SetStrokeColor( DARKGRAY );
|
int intersectArc2Arc( const SHAPE_ARC& a1, const SHAPE_ARC& a2, std::vector<VECTOR2I>* ips )
|
||||||
//overlay->Circle( a1.GetCenter(), a1.GetRadius() );
|
{
|
||||||
//overlay->SetStrokeColor( DARKGRAY );
|
CIRCLE circ1( a1.GetCenter(), a1.GetRadius() );
|
||||||
//overlay->Circle( a2.GetCenter(), a2.GetRadius() );
|
CIRCLE circ2( a2.GetCenter(), a2.GetRadius() );
|
||||||
|
|
||||||
double r1sq = r1 * r1;
|
std::vector<VECTOR2I> intersections = circ1.Intersect( circ2 );
|
||||||
double dsq = d * d;
|
|
||||||
|
|
||||||
if( r1sq < dsq )
|
size_t originalSize = ips->size();
|
||||||
return 0;
|
|
||||||
|
|
||||||
double q = sqrt ( r1sq - dsq );
|
for( const VECTOR2I& intersection : intersections )
|
||||||
|
{
|
||||||
|
if( sliceContainsPoint( a1, intersection ) && sliceContainsPoint( a2, intersection ) )
|
||||||
|
ips->push_back( intersection );
|
||||||
|
}
|
||||||
|
|
||||||
// printf("d %.1f q %.1f dcl %.1f\n", d, q, dcl );
|
return ips->size() - originalSize;
|
||||||
|
|
||||||
VECTOR2D pp = a1.GetCenter() + dc.Resize( d );
|
|
||||||
|
|
||||||
VECTOR2D ip0 = pp + dc.Perpendicular().Resize(q);
|
|
||||||
VECTOR2D ip1 = pp - dc.Perpendicular().Resize(q);
|
|
||||||
|
|
||||||
int n_ips = 0;
|
|
||||||
|
|
||||||
if( sliceContainsPoint( a1, ip0 ) )
|
|
||||||
ips[n_ips++] = ip0;
|
|
||||||
if( sliceContainsPoint( a1, ip1 ) )
|
|
||||||
ips[n_ips++] = ip1;
|
|
||||||
|
|
||||||
return n_ips;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -184,73 +127,51 @@ bool collideArc2Arc( const SHAPE_ARC& a1, const SHAPE_ARC& a2, int clearance, SE
|
||||||
{
|
{
|
||||||
SEG mediatrix( a1.GetCenter(), a2.GetCenter() );
|
SEG mediatrix( a1.GetCenter(), a2.GetCenter() );
|
||||||
|
|
||||||
int nA = 0, nB = 0;
|
std::vector<VECTOR2I> ips;
|
||||||
VECTOR2D ptsA[8];
|
|
||||||
VECTOR2D ptsB[8];
|
|
||||||
|
|
||||||
|
if( intersectArc2Arc( a1, a2, &ips ) > 0 )
|
||||||
VECTOR2D ips[4];
|
|
||||||
|
|
||||||
if( intersectArc2Arc( a1, a2, ips ) > 0 )
|
|
||||||
{
|
{
|
||||||
minDistSeg.A = minDistSeg.B = ips[0];
|
minDistSeg.A = minDistSeg.B = ips[0];
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::vector<VECTOR2I> ptsA;
|
||||||
|
std::vector<VECTOR2I> ptsB;
|
||||||
|
|
||||||
bool cocentered = ( mediatrix.A == mediatrix.B );
|
bool cocentered = ( mediatrix.A == mediatrix.B );
|
||||||
|
|
||||||
// arcs don't have the same center point
|
// arcs don't have the same center point
|
||||||
if( !cocentered )
|
if( !cocentered )
|
||||||
{
|
{
|
||||||
nA = arclineIntersect( a1, mediatrix, ptsA );
|
arclineIntersect( a1, mediatrix, &ptsA );
|
||||||
nB = arclineIntersect( a2, mediatrix, ptsB );
|
arclineIntersect( a2, mediatrix, &ptsB );
|
||||||
}
|
}
|
||||||
|
|
||||||
/*overlay->SetStrokeColor( LIGHTRED );
|
arclineIntersect( a1, SEG( a2.GetP0(), a2.GetCenter() ), &ptsA );
|
||||||
overlay->Line( SEG( a2.GetP0(), a2.GetCenter() ) );
|
arclineIntersect( a1, SEG( a2.GetP1(), a2.GetCenter() ), &ptsA );
|
||||||
overlay->Line( SEG( a2.GetP1(), a2.GetCenter() ) );*/
|
|
||||||
|
|
||||||
nA += arclineIntersect( a1, SEG( a2.GetP0(), a2.GetCenter() ), ptsA + nA );
|
arclineIntersect( a2, SEG( a1.GetP0(), a1.GetCenter() ), &ptsB );
|
||||||
nA += arclineIntersect( a1, SEG( a2.GetP1(), a2.GetCenter() ), ptsA + nA );
|
arclineIntersect( a2, SEG( a1.GetP1(), a1.GetCenter() ), &ptsB );
|
||||||
|
|
||||||
|
ptsA.push_back( a1.GetP0() );
|
||||||
|
ptsA.push_back( a1.GetP1() );
|
||||||
|
ptsB.push_back( a2.GetP0() );
|
||||||
|
ptsB.push_back( a2.GetP1() );
|
||||||
|
|
||||||
/*overlay->SetStrokeColor( LIGHTBLUE );
|
double minDist = std::numeric_limits<double>::max();
|
||||||
overlay->Line( SEG( a1.GetP0(), a1.GetCenter() ) );
|
|
||||||
overlay->Line( SEG( a1.GetP1(), a1.GetCenter() ) );*/
|
|
||||||
|
|
||||||
nB += arclineIntersect( a2, SEG( a1.GetP0(), a1.GetCenter() ), ptsB + nB );
|
|
||||||
nB += arclineIntersect( a2, SEG( a1.GetP1(), a1.GetCenter() ), ptsB + nB );
|
|
||||||
|
|
||||||
ptsA[nA++] = a1.GetP0();
|
|
||||||
ptsA[nA++] = a1.GetP1();
|
|
||||||
ptsB[nB++] = a2.GetP0();
|
|
||||||
ptsB[nB++] = a2.GetP1();
|
|
||||||
|
|
||||||
// printf("nA %d nB %d\n", nA, nB );
|
|
||||||
|
|
||||||
double minDist;
|
|
||||||
bool minDistFound = false;
|
bool minDistFound = false;
|
||||||
|
|
||||||
for( int i = 0; i < nA; i++ )
|
for( const VECTOR2I& ptA: ptsA )
|
||||||
for( int j = 0; j < nB; j++ )
|
for( const VECTOR2I& ptB : ptsB )
|
||||||
{
|
{
|
||||||
double dist =
|
double dist = ( ptA - ptB ).EuclideanNorm() - a1.GetWidth() / 2.0 - a2.GetWidth() / 2.0;
|
||||||
( ptsA[i] - ptsB[j] ).EuclideanNorm() - a1.GetWidth() / 2 - a2.GetWidth() / 2;
|
|
||||||
|
|
||||||
/* overlay->SetStrokeColor( RED );
|
|
||||||
overlay->AnnotatedPoint( ptsA[i], 100000 );
|
|
||||||
overlay->SetStrokeColor( GREEN );
|
|
||||||
overlay->AnnotatedPoint( ptsB[j], 100000 );*/
|
|
||||||
|
|
||||||
|
|
||||||
//printf("dist %.1f\n", dist );
|
|
||||||
|
|
||||||
if( dist < clearance )
|
if( dist < clearance )
|
||||||
{
|
{
|
||||||
if( !minDistFound || dist < minDist )
|
if( !minDistFound || dist < minDist )
|
||||||
{
|
{
|
||||||
minDist = dist;
|
minDist = dist;
|
||||||
minDistSeg = SEG( ptsA[i], ptsB[j] );
|
minDistSeg = SEG( ptA, ptB );
|
||||||
}
|
}
|
||||||
|
|
||||||
minDistFound = true;
|
minDistFound = true;
|
||||||
|
|
@ -337,9 +258,9 @@ int playground_main_func( int argc, char* argv[] )
|
||||||
for(int i = 0; i < arcs.size(); i+= 2)
|
for(int i = 0; i < arcs.size(); i+= 2)
|
||||||
{
|
{
|
||||||
SEG closestDist;
|
SEG closestDist;
|
||||||
VECTOR2D ips[2];
|
std::vector<VECTOR2I> ips;
|
||||||
bool collides = collideArc2Arc( arcs[i], arcs[i+1], 0, closestDist );
|
bool collides = collideArc2Arc( arcs[i], arcs[i+1], 0, closestDist );
|
||||||
int ni = intersectArc2Arc( arcs[i], arcs[i+1], ips );
|
int ni = intersectArc2Arc( arcs[i], arcs[i+1], &ips );
|
||||||
|
|
||||||
overlay->SetLineWidth( 10000.0 );
|
overlay->SetLineWidth( 10000.0 );
|
||||||
overlay->SetStrokeColor( GREEN );
|
overlay->SetStrokeColor( GREEN );
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue