diff --git a/qa/pns/playground.cpp b/qa/pns/playground.cpp index 9898abd490..5d0eecc084 100644 --- a/qa/pns/playground.cpp +++ b/qa/pns/playground.cpp @@ -33,15 +33,11 @@ #include -namespace PNS { - extern SHAPE_LINE_CHAIN g_pnew, g_hnew; -}; - std::shared_ptr overlay; template -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 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); }