diff --git a/include/core/kicad_algo.h b/include/core/kicad_algo.h index ff2cef0566..a16150ab16 100644 --- a/include/core/kicad_algo.h +++ b/include/core/kicad_algo.h @@ -26,6 +26,7 @@ #define INCLUDE_CORE_KICAD_ALGO_H_ #include +#include #include // std::function #include // std::pair @@ -114,6 +115,47 @@ bool pair_contains( const std::pair<_Type, _Type> __pair, _Value __value ) || __pair.second == static_cast<_Type>( __value ); } +/** + * @brief Test if __val lies within __minval and __maxval in a wrapped range. + * + * @param __val A value to test + * @param __minval Lowest permissible value within the wrapped range + * @param __maxval Highest permissible value within the wrapped range + * @param __wrap Value at which the range wraps around itself (must be positive) + * @return true if @p __val lies in the wrapped range + */ +template +bool within_wrapped_range( T __val, T __minval, T __maxval, T __wrap ) +{ + assert( __wrap > 0 ); // Wrap must be positive! + + if( __wrap <= 0 ) + return false; + + while( __maxval >= __wrap ) + __maxval -= __wrap; + + while( __maxval < 0 ) + __maxval += __wrap; + + while( __minval >= __wrap ) + __minval -= __wrap; + + while( __minval < 0 ) + __minval += __wrap; + + while( __val < 0 ) + __val += __wrap; + + while( __val >= __wrap ) + __val -= __wrap; + + if( __maxval > __minval ) + return __val >= __minval && __val <= __maxval; + else + return __val >= __minval || __val <= __maxval; +} + } // namespace alg diff --git a/libs/kimath/include/geometry/shape_arc.h b/libs/kimath/include/geometry/shape_arc.h index d15ee6a922..1cc801b9cc 100644 --- a/libs/kimath/include/geometry/shape_arc.h +++ b/libs/kimath/include/geometry/shape_arc.h @@ -127,6 +127,25 @@ public: return SHAPE::Collide( aShape, aClearance, aActual, aLocation ); } + /** + * Find intersection points between this arc and aSeg, treating aSeg as an infinite line. + * Ignores arc width. + * + * @param aSeg Line to intersect against (treated as an infinite line) + * @param aIpsBuffer Buffer to store the resulting intersection points (if any) + * @return Number of intersection points found + */ + int IntersectLine( const SEG& aSeg, std::vector* aIpsBuffer ) const; + + /** + * Find intersection points between this arc and aArc. Ignores arc width. + * + * @param aSeg + * @param aIpsBuffer Buffer to store the resulting intersection points (if any) + * @return Number of intersection points found + */ + int Intersect( const SHAPE_ARC& aArc, std::vector* aIpsBuffer ) const; + bool IsClockwise() const; void SetWidth( int aWidth ) @@ -233,6 +252,8 @@ private: void update_bbox(); + bool sliceContainsPoint( const VECTOR2I& p ) const; + private: VECTOR2I m_start; VECTOR2I m_mid; diff --git a/libs/kimath/src/geometry/shape_arc.cpp b/libs/kimath/src/geometry/shape_arc.cpp index d2abe1737c..e1f1dd5026 100644 --- a/libs/kimath/src/geometry/shape_arc.cpp +++ b/libs/kimath/src/geometry/shape_arc.cpp @@ -23,6 +23,8 @@ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA */ +#include +#include #include #include // for SEG #include @@ -296,6 +298,43 @@ bool SHAPE_ARC::Collide( const SEG& aSeg, int aClearance, int* aActual, VECTOR2I } +int SHAPE_ARC::IntersectLine( const SEG& aSeg, std::vector* aIpsBuffer ) const +{ + CIRCLE circ( GetCenter(), GetRadius() ); + + std::vector intersections = circ.IntersectLine( aSeg ); + + size_t originalSize = aIpsBuffer->size(); + + for( const VECTOR2I& intersection : intersections ) + { + if( sliceContainsPoint( intersection ) ) + aIpsBuffer->push_back( intersection ); + } + + return aIpsBuffer->size() - originalSize; +} + + +int SHAPE_ARC::Intersect( const SHAPE_ARC& aArc, std::vector* aIpsBuffer ) const +{ + CIRCLE thiscirc( GetCenter(), GetRadius() ); + CIRCLE othercirc( aArc.GetCenter(), aArc.GetRadius() ); + + std::vector intersections = thiscirc.Intersect( othercirc ); + + size_t originalSize = aIpsBuffer->size(); + + for( const VECTOR2I& intersection : intersections ) + { + if( sliceContainsPoint( intersection ) && aArc.sliceContainsPoint( intersection ) ) + aIpsBuffer->push_back( intersection ); + } + + return aIpsBuffer->size() - originalSize; +} + + void SHAPE_ARC::update_bbox() { std::vector points; @@ -586,3 +625,25 @@ SHAPE_ARC SHAPE_ARC::Reversed() const { return SHAPE_ARC( m_end, m_mid, m_start, m_width ); } + + +bool SHAPE_ARC::sliceContainsPoint( const VECTOR2I& p ) const +{ + VECTOR2I center = GetCenter(); + double phi = 180.0 / M_PI * atan2( p.y - center.y, p.x - center.x ); + double ca = GetCentralAngle(); + double sa = GetStartAngle(); + double ea; + + if( ca >= 0 ) + { + ea = sa + ca; + } + else + { + ea = sa; + sa += ca; + } + + return alg::within_wrapped_range( phi, sa, ea, 360.0 ); +} diff --git a/libs/kimath/src/geometry/shape_collisions.cpp b/libs/kimath/src/geometry/shape_collisions.cpp index 8d585e1138..eca244e584 100644 --- a/libs/kimath/src/geometry/shape_collisions.cpp +++ b/libs/kimath/src/geometry/shape_collisions.cpp @@ -552,14 +552,82 @@ static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_ARC& aB, int aClear aA.Type(), aB.Type() ) ); - const SHAPE_LINE_CHAIN lcA = aA.ConvertToPolyline(); - const SHAPE_LINE_CHAIN lcB = aB.ConvertToPolyline(); - int widths = ( aA.GetWidth() / 2 ) + ( aB.GetWidth() / 2 ); + SEG mediatrix( aA.GetCenter(), aB.GetCenter() ); - bool rv = Collide( lcA, lcB, aClearance + widths, aActual, aLocation, aMTV ); + std::vector ips; + + // Basic case - arcs intersect + if( aA.Intersect( aB, &ips ) > 0 ) + { + if( aActual ) + *aActual = 0; + + if( aLocation ) + *aLocation = ips[0]; // Pick the first intersection point + + return true; + } + + // Arcs don't intersect, build a list of points to check + std::vector ptsA; + std::vector ptsB; + + bool cocentered = ( mediatrix.A == mediatrix.B ); + + // 1: Interior points of both arcs, which are on the line segment between the two centres + if( !cocentered ) + { + aA.IntersectLine( mediatrix, &ptsA ); + aB.IntersectLine( mediatrix, &ptsB ); + } + + // 2: Check arc end points + ptsA.push_back( aA.GetP0() ); + ptsA.push_back( aA.GetP1() ); + ptsB.push_back( aB.GetP0() ); + ptsB.push_back( aB.GetP1() ); + + // 3: Endpoint of one and "projected" point on the other, which is on the + // line segment through that endpoint and the centre of the other arc + aA.IntersectLine( SEG( aB.GetP0(), aA.GetCenter() ), &ptsA ); + aA.IntersectLine( SEG( aB.GetP1(), aA.GetCenter() ), &ptsA ); + + aB.IntersectLine( SEG( aA.GetP0(), aB.GetCenter() ), &ptsB ); + aB.IntersectLine( SEG( aA.GetP1(), aB.GetCenter() ), &ptsB ); + + double minDist = std::numeric_limits::max(); + SEG minDistSeg; + bool rv = false; + + int widths = ( aA.GetWidth() / 2 ) + ( aB.GetWidth() / 2 ); + + // @todo performance could be improved by only checking certain points (e.g only check end + // points against other end points or their corresponding "projected" points) + for( const VECTOR2I& ptA : ptsA ) + { + for( const VECTOR2I& ptB : ptsB ) + { + SEG candidateMinDist( ptA, ptB ); + int dist = candidateMinDist.Length() - widths; + + if( dist < aClearance ) + { + if( !rv || dist < minDist ) + { + minDist = dist; + minDistSeg = candidateMinDist; + } + + rv = true; + } + } + } if( rv && aActual ) - *aActual = std::max( 0, *aActual - widths ); + *aActual = std::max( 0, minDistSeg.Length() - widths ); + + if( rv && aLocation ) + *aLocation = minDistSeg.Center(); return rv; } diff --git a/qa/libs/kimath/geometry/test_shape_arc.cpp b/qa/libs/kimath/geometry/test_shape_arc.cpp index ac4269483a..3005b30d97 100644 --- a/qa/libs/kimath/geometry/test_shape_arc.cpp +++ b/qa/libs/kimath/geometry/test_shape_arc.cpp @@ -639,9 +639,9 @@ struct ARC_DATA_MM SHAPE_ARC GenerateArc() const { - SHAPE_ARC arc( VECTOR2D( PcbMillimeter2iu( m_center_x ), PcbMillimeter2iu( m_center_y ) ), - VECTOR2D( PcbMillimeter2iu( m_start_x ), PcbMillimeter2iu( m_start_y ) ), - m_center_angle, PcbMillimeter2iu( m_width ) ); + SHAPE_ARC arc( VECTOR2D( PcbMm2iu( m_center_x ), PcbMm2iu( m_center_y ) ), + VECTOR2D( PcbMm2iu( m_start_x ), PcbMm2iu( m_start_y ) ), + m_center_angle, PcbMm2iu( m_width ) ); return arc; } @@ -717,8 +717,19 @@ static const std::vector arc_arc_collide_cases = { { "case 12: Simulated differential pair meander", { 94.6551, 88.295989, 95.6551, 88.295989, 90.0, 0.1 }, { 94.6551, 88.295989, 95.8551, 88.295989, 90.0, 0.1 }, - 0.1 - SHAPE_ARC::DefaultAccuracyForPCB() / PCB_IU_PER_MM, // remove offset when support for true arc-arc intersection + // Offset needed due to rounding errors of integer coordinates + 0.1 - PcbIu2mm( SHAPE_ARC::MIN_PRECISION_IU ), false }, + { "case 13: One arc fully enclosed in other, non-concentric", + { 73.77532, 93.413654, 75.70532, 93.883054, 60.0, 0.1 }, + { 73.86532, 93.393054, 75.86532, 93.393054, 90.0, 0.3 }, + 0, + true }, + { "case 14: One arc fully enclosed in other, concentric", + { 79.87532, 93.413654, 81.64532, 94.113054, 60.0, 0.1 }, + { 79.87532, 93.413654, 81.86532, 93.393054, 90.0, 0.3 }, + 0, + true }, }; @@ -734,7 +745,7 @@ BOOST_AUTO_TEST_CASE( CollideArc ) int actual = 0; VECTOR2I location; - bool result = arc1.Collide( &arc2, PcbMillimeter2iu( c.m_clearance ), &actual, &location ); + bool result = arc1.Collide( &arc2, PcbMm2iu( c.m_clearance ), &actual, &location ); BOOST_CHECK_EQUAL( result, c.m_exp_result ); } @@ -776,9 +787,8 @@ BOOST_AUTO_TEST_CASE( CollideArcToPolygonApproximation ) int actual = 0; VECTOR2I location; - int tol = SHAPE_ARC::DefaultAccuracyForPCB(); + int tol = SHAPE_ARC::MIN_PRECISION_IU; - //@todo remove "- tol" from collision check below once true arc collisions have been implemented BOOST_CHECK_EQUAL( zoneFill.Collide( &arc, clearance - tol, &actual, &location ), false ); BOOST_CHECK_EQUAL( zoneFill.Collide( &arc, clearance * 2, &actual, &location ), true ); diff --git a/qa/pns/playground.cpp b/qa/pns/playground.cpp index 8d1b787c42..2ad6cf98ac 100644 --- a/qa/pns/playground.cpp +++ b/qa/pns/playground.cpp @@ -36,153 +36,53 @@ std::shared_ptr overlay; -template -static T within_range(T x, T minval, T maxval, T wrap) -{ - int rv; - - while (maxval >= wrap) - maxval -= wrap; - - while (maxval < 0) - maxval += wrap; - - while (minval >= wrap) - minval -= wrap; - - while (minval < 0) - minval += wrap; - - while (x < 0) - x += wrap; - - while (x >= wrap) - x -= wrap; - - if (maxval > minval) - rv = (x >= minval && x <= maxval) ? 1 : 0; - else - rv = (x >= minval || x <= maxval) ? 1 : 0; - - return rv; -} - -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 ca = a.GetCentralAngle(); - double sa = a.GetStartAngle(); - double ea; - if( ca >= 0 ) - { - ea = sa + ca; - } - else - { - ea = sa; - sa += ca; - } - - - return within_range( phi, sa, ea, 360.0 ); -} - -static int arclineIntersect( const SHAPE_ARC& a, const SEG& s, std::vector* ips ) -{ - CIRCLE circ( a.GetCenter(), a.GetRadius() ); - - std::vector intersections = circ.IntersectLine( s ); - - size_t originalSize = ips->size(); - - for( const VECTOR2I& intersection : intersections ) - { - if( sliceContainsPoint( a, intersection ) ) - ips->push_back( intersection ); - } - - return ips->size() - originalSize; -} - -int intersectArc2Arc( const SHAPE_ARC& a1, const SHAPE_ARC& a2, std::vector* ips ) -{ - CIRCLE circ1( a1.GetCenter(), a1.GetRadius() ); - CIRCLE circ2( a2.GetCenter(), a2.GetRadius() ); - - std::vector intersections = circ1.Intersect( circ2 ); - - size_t originalSize = ips->size(); - - for( const VECTOR2I& intersection : intersections ) - { - if( sliceContainsPoint( a1, intersection ) && sliceContainsPoint( a2, intersection ) ) - ips->push_back( intersection ); - } - - return ips->size() - originalSize; -} - - bool collideArc2Arc( const SHAPE_ARC& a1, const SHAPE_ARC& a2, int clearance, SEG& minDistSeg ) { SEG mediatrix( a1.GetCenter(), a2.GetCenter() ); std::vector ips; - if( intersectArc2Arc( a1, a2, &ips ) > 0 ) + // Basic case - arcs intersect + if( a1.Intersect( a2, &ips ) > 0 ) { minDistSeg.A = minDistSeg.B = ips[0]; return true; } + // Arcs don't intersect, build a list of points to check std::vector ptsA; std::vector ptsB; bool cocentered = ( mediatrix.A == mediatrix.B ); - // arcs don't have the same center point + // 1: Interior points of both arcs, which are on the line segment between the two centres if( !cocentered ) { - arclineIntersect( a1, mediatrix, &ptsA ); - arclineIntersect( a2, mediatrix, &ptsB ); + a1.IntersectLine( mediatrix, &ptsA ); + a2.IntersectLine( mediatrix, &ptsB ); } - arclineIntersect( a1, SEG( a2.GetP0(), a2.GetCenter() ), &ptsA ); - arclineIntersect( a1, SEG( a2.GetP1(), a2.GetCenter() ), &ptsA ); - - arclineIntersect( a2, SEG( a1.GetP0(), a1.GetCenter() ), &ptsB ); - arclineIntersect( a2, SEG( a1.GetP1(), a1.GetCenter() ), &ptsB ); - + // 2: Check arc end points ptsA.push_back( a1.GetP0() ); ptsA.push_back( a1.GetP1() ); ptsB.push_back( a2.GetP0() ); ptsB.push_back( a2.GetP1() ); - std::vector nearestA, nearestB; + // 3: Endpoint of one and "projected" point on the other, which is on the + // line segment through that endpoint and the centre of the other arc + a1.IntersectLine( SEG( a2.GetP0(), a1.GetCenter() ), &ptsA ); + a1.IntersectLine( SEG( a2.GetP1(), a1.GetCenter() ), &ptsA ); - // this probably can be made with less points, but still... - CIRCLE circ1( a1.GetCenter(), a1.GetRadius() ); - for( auto pB: ptsB ) - { - auto nearest = circ1.NearestPoint( pB ); - if( sliceContainsPoint( a1, nearest ) ) - nearestA.push_back( nearest ); - } - CIRCLE circ2( a2.GetCenter(), a2.GetRadius() ); - for( auto pA: ptsA ) - { - auto nearest = circ2.NearestPoint( pA ); - if( sliceContainsPoint( a2, nearest ) ) - nearestB.push_back( nearest ); - } - - ptsA.insert( ptsA.end(), nearestA.begin(), nearestA.end() ); - ptsB.insert( ptsB.end(), nearestB.begin(), nearestB.end() ); + a2.IntersectLine( SEG( a1.GetP0(), a2.GetCenter() ), &ptsB ); + a2.IntersectLine( SEG( a1.GetP1(), a2.GetCenter() ), &ptsB ); double minDist = std::numeric_limits::max(); bool minDistFound = false; - for( const VECTOR2I& ptA: ptsA ) + // @todo performance might be improved by only checking certain points (e.g only check end + // points against other end points or their corresponding "projected" points) + for( const VECTOR2I& ptA : ptsA ) + { for( const VECTOR2I& ptB : ptsB ) { double dist = ( ptA - ptB ).EuclideanNorm() - a1.GetWidth() / 2.0 - a2.GetWidth() / 2.0; @@ -198,7 +98,7 @@ bool collideArc2Arc( const SHAPE_ARC& a1, const SHAPE_ARC& a2, int clearance, SE minDistFound = true; } } - + } return minDistFound; } @@ -241,6 +141,10 @@ int playground_main_func( int argc, char* argv[] ) {86.063537, 88.295989, 84.968628, 87.581351, -255.5, 0.2}, {94.6551, 88.295989, 95.6551, 88.295989, 90.0, 0.2 }, // simulating diff pair {94.6551, 88.295989, 95.8551, 88.295989, 90.0, 0.2 }, + {73.77532, 93.413654, 75.70532, 93.883054, 60.0, 0.1 }, // one arc fully enclosed in other + {73.86532, 93.393054, 75.86532, 93.393054, 90.0, 0.3 }, + {79.87532, 93.413654, 81.64532, 94.113054, 60.0, 0.1 }, // concentric + {79.87532, 93.413654, 81.86532, 93.393054, 90.0, 0.3 } }; @@ -281,7 +185,7 @@ int playground_main_func( int argc, char* argv[] ) SEG closestDist; std::vector ips; bool collides = collideArc2Arc( arcs[i], arcs[i+1], 0, closestDist ); - int ni = intersectArc2Arc( arcs[i], arcs[i+1], &ips ); + int ni = arcs[i].Intersect( arcs[i+1], &ips ); overlay->SetLineWidth( 10000.0 ); overlay->SetStrokeColor( GREEN ); @@ -295,7 +199,7 @@ int playground_main_func( int argc, char* argv[] ) overlay->Line( closestDist.A, closestDist.B ); overlay->SetLineWidth( 10000.0 ); overlay->SetGlyphSize( { 100000.0, 100000.0 } ); - overlay->BitmapText( wxString::Format( "dist: %d", closestDist.Length() ), + overlay->BitmapText( wxString::Format( "dist=%d, l=%d", closestDist.Length() ), closestDist.A + VECTOR2I( 0, -arcs[i].GetWidth() ), 0 ); }