arc collision playground: some cleanup
This commit is contained in:
Tomasz Wlostowski
Roberto Fernandez Bautista
parent
7e94f85870
commit
9922e85ecf
|
|
@ -33,15 +33,11 @@
|
|||
|
||||
#include <geometry/shape_arc.h>
|
||||
|
||||
namespace PNS {
|
||||
extern SHAPE_LINE_CHAIN g_pnew, g_hnew;
|
||||
};
|
||||
|
||||
std::shared_ptr<PNS_LOG_VIEWER_OVERLAY> overlay;
|
||||
|
||||
|
||||
template<class T>
|
||||
T within_range(T x, T minval, T maxval, T wrap)
|
||||
static T within_range(T x, T minval, T maxval, T wrap)
|
||||
{
|
||||
int rv;
|
||||
|
||||
|
|
@ -71,11 +67,9 @@ T within_range(T x, T minval, T maxval, T wrap)
|
|||
return rv;
|
||||
}
|
||||
|
||||
bool sliceContainsPoint( SHAPE_ARC a, VECTOR2D p )
|
||||
static bool sliceContainsPoint( SHAPE_ARC a, VECTOR2D p )
|
||||
{
|
||||
#if 1
|
||||
double phi = 180.0 / M_PI * atan2( p.y - a.GetCenter().y, p.x - a.GetCenter().x );
|
||||
|
||||
double ca = a.GetCentralAngle();
|
||||
double sa = a.GetStartAngle();
|
||||
double ea;
|
||||
|
|
@ -91,26 +85,9 @@ bool sliceContainsPoint( SHAPE_ARC a, VECTOR2D p )
|
|||
|
||||
|
||||
return within_range( phi, sa, ea, 360.0 );
|
||||
#endif
|
||||
|
||||
VECTOR2D vec( p - a.GetCenter() );
|
||||
|
||||
bool ccw = a.GetCentralAngle() > 0.0;
|
||||
double rotatedVecAngle = NormalizeAngleDegreesPos( NormalizeAngleDegreesPos( RAD2DEG( vec.Angle() ) )
|
||||
- a.GetStartAngle() );
|
||||
double rotatedEndAngle = NormalizeAngleDegreesPos( a.GetEndAngle() - a.GetStartAngle() );
|
||||
|
||||
if( ( ccw && rotatedVecAngle > rotatedEndAngle )
|
||||
|| ( !ccw && rotatedVecAngle < rotatedEndAngle ) )
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
int arclineIntersect( SHAPE_ARC a, SEG s, VECTOR2D* pts )
|
||||
static int arclineIntersect( const SHAPE_ARC& a, const SEG& s, VECTOR2D* pts )
|
||||
{
|
||||
VECTOR2I c = a.GetCenter();
|
||||
double r = a.GetRadius();
|
||||
|
|
@ -154,95 +131,12 @@ int arclineIntersect( SHAPE_ARC a, SEG s, VECTOR2D* pts )
|
|||
return n;
|
||||
}
|
||||
|
||||
int arclineIntersect2( SHAPE_ARC a, SEG s, VECTOR2D* pts )
|
||||
{
|
||||
double sa, sb, sc;
|
||||
|
||||
sa = s.A.y - s.B.y;
|
||||
sb = s.B.x - s.A.x;
|
||||
sc = -sa * s.A.x - sb * s.A.y;
|
||||
|
||||
VECTOR2D ac = a.GetCenter();
|
||||
double rr = a.GetRadius();
|
||||
int n = 0;
|
||||
|
||||
overlay->SetStrokeColor( RED );
|
||||
overlay->AnnotatedPoint( s.A, 200000 );
|
||||
overlay->SetStrokeColor( GREEN );
|
||||
overlay->AnnotatedPoint( s.B, 200000 );
|
||||
overlay->SetStrokeColor( WHITE );
|
||||
overlay->Arc( a );
|
||||
|
||||
if( sb != 0.0 )
|
||||
{
|
||||
double aa = sa * sa / ( sb * sb ) + 1.0;
|
||||
double bb = (2.0 * sa * ac.y / sb
|
||||
+ 2.0 * sa * sc / (sb * sb)
|
||||
- 2.0 * ac.x );
|
||||
|
||||
double cc = ac.x * ac.x + ac.y * ac.y - rr * rr + 2.0*sc*ac.y/sb + (sc*sc) / ( sb*sb );
|
||||
|
||||
double delta = bb*bb - 4.0 * aa * cc;
|
||||
|
||||
if( delta < 0.0 )
|
||||
return 0;
|
||||
else
|
||||
{
|
||||
double x1 = -bb - sqrt(delta) / (2.0 * aa );
|
||||
double x2 = -bb + sqrt(delta) / (2.0 * aa );
|
||||
|
||||
double y1 = (-sc - sa * x1) / sb;
|
||||
double y2 = (-sc - sa * x2) / sb;
|
||||
|
||||
overlay->AnnotatedPoint( VECTOR2D(x1, y1), 2000000 );
|
||||
overlay->AnnotatedPoint( VECTOR2D(x2, y2), 2000000 );
|
||||
|
||||
|
||||
VECTOR2D p0( x1, y1 );
|
||||
VECTOR2D p1( x1, y1 );
|
||||
if( sliceContainsPoint( a, p0 ) ) { pts[n] = p0; n++; }
|
||||
if( sliceContainsPoint( a, p1 ) ) { pts[n] = p1; n++; }
|
||||
|
||||
printf("CASE1\n");
|
||||
|
||||
return n;
|
||||
}
|
||||
} else { // line is vertical
|
||||
|
||||
double aa = 1.0;
|
||||
double bb = -2.0 * ac.y;
|
||||
double cc = ac.x * ac.x + ac.y * ac.y - rr*rr + 2.0*sc*ac.x/sa + (sc * sc) / (sa * sa);
|
||||
|
||||
double delta = bb*bb - 4.0 * aa * cc;
|
||||
|
||||
if( delta < 0.0 )
|
||||
return 0;
|
||||
else
|
||||
{
|
||||
double y1 = -bb - sqrt(delta) / (2.0 * aa );
|
||||
double y2 = -bb + sqrt(delta) / (2.0 * aa );
|
||||
|
||||
VECTOR2D p0( s.A.x, y1 );
|
||||
VECTOR2D p1( s.A.y, y2 );
|
||||
/*if( sliceContainsPoint( a, p0 ) ) */ { pts[n] = p0; n++; }
|
||||
/*if( sliceContainsPoint( a, p1 ) ) */ { pts[n] = p1; n++; }
|
||||
|
||||
printf("CASE2\n");
|
||||
|
||||
return n;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int intersectArc2Arc( SHAPE_ARC a1, SHAPE_ARC a2, VECTOR2D* ips )
|
||||
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 )
|
||||
{
|
||||
|
|
@ -260,10 +154,6 @@ int intersectArc2Arc( SHAPE_ARC a1, SHAPE_ARC a2, VECTOR2D* ips )
|
|||
//overlay->SetStrokeColor( DARKGRAY );
|
||||
//overlay->Circle( a2.GetCenter(), a2.GetRadius() );
|
||||
|
||||
|
||||
// if( d<0.0 )
|
||||
// return 0;
|
||||
|
||||
double r1sq = r1 * r1;
|
||||
double dsq = d * d;
|
||||
|
||||
|
|
@ -286,73 +176,68 @@ int intersectArc2Arc( SHAPE_ARC a1, SHAPE_ARC a2, VECTOR2D* ips )
|
|||
if( sliceContainsPoint( a1, ip1 ) )
|
||||
ips[n_ips++] = ip1;
|
||||
|
||||
|
||||
|
||||
|
||||
return n_ips;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool collideArc2Arc( SHAPE_ARC a1, SHAPE_ARC a2, int clearance, SEG& minDistSeg )
|
||||
bool collideArc2Arc( const SHAPE_ARC& a1, const SHAPE_ARC& a2, int clearance, SEG& minDistSeg )
|
||||
{
|
||||
SEG mediatrix (a1.GetCenter(), a2.GetCenter() );
|
||||
SEG mediatrix( a1.GetCenter(), a2.GetCenter() );
|
||||
|
||||
int nA = 0, nB = 0;
|
||||
int nA = 0, nB = 0;
|
||||
VECTOR2D ptsA[8];
|
||||
VECTOR2D ptsB[8];
|
||||
|
||||
bool cocentered = (mediatrix.A == mediatrix.B);
|
||||
|
||||
VECTOR2D ips[4];
|
||||
|
||||
|
||||
if( intersectArc2Arc(a1, a2, ips) > 0 )
|
||||
if( intersectArc2Arc( a1, a2, ips ) > 0 )
|
||||
{
|
||||
minDistSeg.A = minDistSeg.B = ips[0];
|
||||
return true;
|
||||
}
|
||||
|
||||
// arcs don't have the same center point
|
||||
if( ! cocentered )
|
||||
{
|
||||
nA = arclineIntersect( a1, mediatrix, ptsA );
|
||||
nB = arclineIntersect( a2, mediatrix, ptsB );
|
||||
}
|
||||
bool cocentered = ( mediatrix.A == mediatrix.B );
|
||||
|
||||
overlay->SetStrokeColor( LIGHTRED );
|
||||
// arcs don't have the same center point
|
||||
if( !cocentered )
|
||||
{
|
||||
nA = arclineIntersect( a1, mediatrix, ptsA );
|
||||
nB = arclineIntersect( a2, mediatrix, ptsB );
|
||||
}
|
||||
|
||||
/*overlay->SetStrokeColor( LIGHTRED );
|
||||
overlay->Line( SEG( a2.GetP0(), a2.GetCenter() ) );
|
||||
overlay->Line( SEG( a2.GetP1(), a2.GetCenter() ) );
|
||||
overlay->Line( SEG( a2.GetP1(), a2.GetCenter() ) );*/
|
||||
|
||||
nA += arclineIntersect( a1, SEG( a2.GetP0(), a2.GetCenter() ), ptsA + nA );
|
||||
nA += arclineIntersect( a1, SEG( a2.GetP1(), a2.GetCenter() ), ptsA + nA );
|
||||
nA += arclineIntersect( a1, SEG( a2.GetP0(), a2.GetCenter() ), ptsA + nA );
|
||||
nA += arclineIntersect( a1, SEG( a2.GetP1(), a2.GetCenter() ), ptsA + nA );
|
||||
|
||||
|
||||
overlay->SetStrokeColor( LIGHTBLUE );
|
||||
/*overlay->SetStrokeColor( LIGHTBLUE );
|
||||
overlay->Line( SEG( a1.GetP0(), a1.GetCenter() ) );
|
||||
overlay->Line( SEG( a1.GetP1(), 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 );
|
||||
|
||||
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 );
|
||||
// printf("nA %d nB %d\n", nA, nB );
|
||||
|
||||
double minDist;
|
||||
bool minDistFound = false;
|
||||
bool minDistFound = false;
|
||||
|
||||
for( int i = 0; i < nA; i++ )
|
||||
for( int j = 0; j < nB; j++ )
|
||||
{
|
||||
double dist = (ptsA[i] - ptsB[j]).EuclideanNorm() - a1.GetWidth() / 2 - a2.GetWidth() / 2;
|
||||
double dist =
|
||||
( ptsA[i] - ptsB[j] ).EuclideanNorm() - a1.GetWidth() / 2 - a2.GetWidth() / 2;
|
||||
|
||||
/* overlay->SetStrokeColor( RED );
|
||||
/* overlay->SetStrokeColor( RED );
|
||||
overlay->AnnotatedPoint( ptsA[i], 100000 );
|
||||
overlay->SetStrokeColor( GREEN );
|
||||
overlay->AnnotatedPoint( ptsB[j], 100000 );*/
|
||||
|
|
@ -362,13 +247,7 @@ bool collideArc2Arc( SHAPE_ARC a1, SHAPE_ARC a2, int clearance, SEG& minDistSeg
|
|||
|
||||
if( dist < clearance )
|
||||
{
|
||||
|
||||
if( !minDistFound )
|
||||
{
|
||||
minDist = dist;
|
||||
minDistSeg = SEG( ptsA[i], ptsB[j] );
|
||||
}
|
||||
else if( dist < minDist )
|
||||
if( !minDistFound || dist < minDist )
|
||||
{
|
||||
minDist = dist;
|
||||
minDistSeg = SEG( ptsA[i], ptsB[j] );
|
||||
|
|
@ -389,74 +268,62 @@ int playground_main_func( int argc, char* argv[] )
|
|||
Pgm().App().SetTopWindow( frame ); // wxApp gets a face.
|
||||
frame->Show();
|
||||
|
||||
struct ARC_DATA
|
||||
{
|
||||
double sx, sy, ex, ey, ca, w;
|
||||
};
|
||||
|
||||
const ARC_DATA test_data [] =
|
||||
{
|
||||
{73.843527, 74.355869, 71.713528, 72.965869, -76.36664803, 0.2},
|
||||
{71.236473, 74.704131, 73.366472, 76.094131, -76.36664803, 0.2},
|
||||
{82.542335, 74.825975, 80.413528, 73.435869, -76.4, 0.2},
|
||||
{76.491192, 73.839894, 78.619999, 75.23, -76.4, 0.2},
|
||||
{94.665667, 73.772941, 96.446472, 74.934131, -267.9, 0.2},
|
||||
{94.750009, 73.74012, 93.6551, 73.025482, -255.5, 0.2},
|
||||
{89.318807, 74.810106, 87.19, 73.42, -76.4, 0.2},
|
||||
{87.045667, 74.632941, 88.826472, 75.794131, -267.9, 0.2},
|
||||
{79.279991, 80.67988, 80.3749, 81.394518, -255.5, 0.2},
|
||||
{79.279991, 80.688898, 80.3749, 81.403536, -255.5, 0.2},
|
||||
{72.87525, 80.173054, 73.970159, 80.887692, -255.5, 0.2},
|
||||
{73.063537, 82.295989, 71.968628, 81.581351, -255.5, 0.2},
|
||||
{88.495265, 81.766089, 90.090174, 82.867869, -255.5, 0.2},
|
||||
{86.995265, 81.387966, 89.090174, 82.876887, -255.5, 0.2},
|
||||
{96.149734, 81.792126, 94.99, 83.37, -347.2, 0.2},
|
||||
{94.857156, 81.240589, 95.91, 83.9, -288.5, 0.2}
|
||||
};
|
||||
|
||||
|
||||
overlay = frame->GetOverlay();
|
||||
|
||||
|
||||
overlay->SetIsFill(false);
|
||||
overlay->SetLineWidth(10000);
|
||||
|
||||
//auto hull = SHAPE_LINE_CHAIN( { VECTOR2I( 111435489, 74692234), VECTOR2I( 111812234, 74315489), VECTOR2I( 112507766, 74315489), VECTOR2I( 112884511, 74692234), VECTOR2I( 112884511, 75387766), VECTOR2I( 112507766, 75764511), VECTOR2I( 111812234, 75764511), VECTOR2I( 111435489, 75387766)}, true );;
|
||||
//auto path = SHAPE_LINE_CHAIN( { VECTOR2I( 112609520, 74417243), VECTOR2I( 112609520, 73774520), VECTOR2I( 112670046, 73774520), VECTOR2I( 112999520, 73445046), VECTOR2I( 112999520, 72054954), VECTOR2I( 112670046, 71725480), VECTOR2I( 111654954, 71725480), VECTOR2I( 111325480, 72054954), VECTOR2I( 111325480, 73445046), VECTOR2I( 111654954, 73774520), VECTOR2I( 111710480, 73774520), VECTOR2I( 111710480, 75226197), VECTOR2I( 111973803, 75489520), VECTOR2I( 112346197, 75489520), VECTOR2I( 112609520, 75226197), VECTOR2I( 112609520, 74417243)}, false );;
|
||||
|
||||
// hull-cw-1
|
||||
//auto hull = SHAPE_LINE_CHAIN( { VECTOR2I( 111435489, 74692234), VECTOR2I( 111812234, 74315489), VECTOR2I( 112507766, 74315489), VECTOR2I( 112884511, 74692234), VECTOR2I( 112884511, 75387766), VECTOR2I( 112507766, 75764511), VECTOR2I( 111812234, 75764511), VECTOR2I( 111435489, 75387766)}, true );;
|
||||
// path-cw-1
|
||||
//auto path = SHAPE_LINE_CHAIN( { VECTOR2I( 112609520, 74417243), VECTOR2I( 112609520, 75226197), VECTOR2I( 112346197, 75489520), VECTOR2I( 111973803, 75489520), VECTOR2I( 111710480, 75226197), VECTOR2I( 111710480, 72566303), VECTOR2I( 111973803, 72302980), VECTOR2I( 112346197, 72302980), VECTOR2I( 112609520, 72566303), VECTOR2I( 112609520, 74417243)}, false );;
|
||||
// hull
|
||||
auto hull = SHAPE_LINE_CHAIN( { VECTOR2I( 106035489, 85253452), VECTOR2I( 106313452, 84975489), VECTOR2I( 106706548, 84975489), VECTOR2I( 106984511, 85253452), VECTOR2I( 106984511, 85976548), VECTOR2I( 106706548, 86254511), VECTOR2I( 106313452, 86254511), VECTOR2I( 106035489, 85976548)}, true );;
|
||||
// path
|
||||
auto path = SHAPE_LINE_CHAIN( { VECTOR2I( 101092000, 85246500), VECTOR2I( 101971211, 86125711), VECTOR2I( 106380778, 86125711), VECTOR2I( 106509572, 86254505)}, false );;
|
||||
|
||||
SHAPE_LINE_CHAIN path_w;
|
||||
|
||||
std::vector<SHAPE_ARC> arcs;
|
||||
int n_arcs = sizeof( test_data ) / sizeof( ARC_DATA );
|
||||
|
||||
#if 0
|
||||
arcs.push_back (SHAPE_ARC ( VECTOR2I( 10000, 10000 ), VECTOR2I( -10000000, -500000 ), 50.0 ) );
|
||||
arcs.push_back (SHAPE_ARC ( VECTOR2I( 0, 600 ), VECTOR2I( 1000000, 300000 ), 180.0 ) );
|
||||
BOX2I vp;
|
||||
|
||||
VECTOR2I offset( -3000000, 0 );
|
||||
|
||||
arcs.push_back (SHAPE_ARC ( VECTOR2I( 10000, 10000 ) + offset, VECTOR2I( -550000, -500000 ) + offset, -80.0 ) );
|
||||
arcs.push_back (SHAPE_ARC ( VECTOR2I( -10000, 600 ) + offset, VECTOR2I( 550000 / 2, 500000/ 2 ) + offset , -80.0 ) );
|
||||
|
||||
BOX2D bb ( arcs[0].BBox().GetPosition(), arcs[0].BBox().GetSize() );
|
||||
|
||||
arcs.push_back (SHAPE_ARC ( VECTOR2I( 10000, 10000 ) + offset, VECTOR2I( -550000, -500000 ) + offset, 350.0 ) );
|
||||
arcs.push_back (SHAPE_ARC ( VECTOR2I( -100000, 600 ) + offset, VECTOR2I( 500000, 440000 ) + offset , 350.0 ) );
|
||||
#endif
|
||||
|
||||
FILE *f = fopen("/tmp/arcs.txt","r");
|
||||
|
||||
while( !feof(f) )
|
||||
for( int i = 0; i < n_arcs; i++ )
|
||||
{
|
||||
char str[1024];
|
||||
if( fgets(str, sizeof(str), f ) == NULL )
|
||||
break;
|
||||
const ARC_DATA& d = test_data[i];
|
||||
|
||||
if( str[0] == '#' )
|
||||
continue;
|
||||
SHAPE_ARC arc( VECTOR2D( Millimeter2iu( d.sx ), Millimeter2iu( d.sy ) ),
|
||||
VECTOR2D( Millimeter2iu( d.ex ), Millimeter2iu( d.ey ) ), d.ca, d.w );
|
||||
|
||||
printf("str '%s'\n", str);
|
||||
|
||||
double sx, sy, ex, ey, a, w = 1000.0;
|
||||
if( sscanf(str,"%lf %lf %lf %lf %lf", &sx, &sy, &ex, &ey, &a) != 5 )
|
||||
break;
|
||||
|
||||
|
||||
|
||||
SHAPE_ARC arc( VECTOR2D( Millimeter2iu( sx ), Millimeter2iu( sy ) ), VECTOR2D( Millimeter2iu( ex ), Millimeter2iu( ey ) ), a, w );
|
||||
|
||||
arcs.push_back(arc);
|
||||
arcs.push_back( arc );
|
||||
|
||||
if( i == 0 )
|
||||
vp = arc.BBox();
|
||||
else
|
||||
vp.Merge( arc.BBox() );
|
||||
}
|
||||
fclose(f);
|
||||
|
||||
printf("Read %d arcs\n", arcs.size() );
|
||||
printf("Read %lu arcs\n", arcs.size() );
|
||||
|
||||
LABEL_MANAGER labelMgr( frame->GetPanel()->GetGAL() );
|
||||
|
||||
//frame->GetPanel()->GetView()->SetViewport(bb);
|
||||
frame->GetPanel()->GetView()->SetViewport( BOX2D( vp.GetOrigin(), vp.GetSize() ) );
|
||||
|
||||
overlay->SetStrokeColor( WHITE );
|
||||
overlay->SetLineWidth( 10000.0 );
|
||||
|
|
@ -483,8 +350,6 @@ int playground_main_func( int argc, char* argv[] )
|
|||
|
||||
if( collides )
|
||||
{
|
||||
|
||||
|
||||
overlay->SetStrokeColor( YELLOW );
|
||||
overlay->Line(closestDist.A, closestDist.B);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue