Simplify & move new arc collision code into Kimath library

Don't need to check intersections with the circle, just fix the
calculated "projected" point from the end points.

Fixes https://gitlab.com/kicad/code/kicad/-/issues/8234
This commit is contained in:
Roberto Fernandez Bautista 2021-07-25 20:46:00 +01:00
parent b6be10f05c
commit dd65ce9523
6 changed files with 238 additions and 132 deletions

View File

@ -26,6 +26,7 @@
#define INCLUDE_CORE_KICAD_ALGO_H_
#include <algorithm>
#include <assert.h>
#include <functional> // std::function
#include <utility> // 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 <class T>
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

View File

@ -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<VECTOR2I>* 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<VECTOR2I>* 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;

View File

@ -23,6 +23,8 @@
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
*/
#include <core/kicad_algo.h>
#include <geometry/circle.h>
#include <geometry/geometry_utils.h>
#include <geometry/seg.h> // for SEG
#include <geometry/shape_arc.h>
@ -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<VECTOR2I>* aIpsBuffer ) const
{
CIRCLE circ( GetCenter(), GetRadius() );
std::vector<VECTOR2I> 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<VECTOR2I>* aIpsBuffer ) const
{
CIRCLE thiscirc( GetCenter(), GetRadius() );
CIRCLE othercirc( aArc.GetCenter(), aArc.GetRadius() );
std::vector<VECTOR2I> 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<VECTOR2I> 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 );
}

View File

@ -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<VECTOR2I> 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<VECTOR2I> ptsA;
std::vector<VECTOR2I> 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<double>::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;
}

View File

@ -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_CASE> 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 );

View File

@ -36,153 +36,53 @@
std::shared_ptr<PNS_LOG_VIEWER_OVERLAY> overlay;
template<class T>
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<VECTOR2I>* ips )
{
CIRCLE circ( a.GetCenter(), a.GetRadius() );
std::vector<VECTOR2I> 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<VECTOR2I>* ips )
{
CIRCLE circ1( a1.GetCenter(), a1.GetRadius() );
CIRCLE circ2( a2.GetCenter(), a2.GetRadius() );
std::vector<VECTOR2I> 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<VECTOR2I> 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<VECTOR2I> ptsA;
std::vector<VECTOR2I> 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<VECTOR2I> 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<double>::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<VECTOR2I> 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 );
}