/**
* Predicate to check expected distance between two segments
* @param aSegA the first #SEG
* @param aSegB the second #SEG
* @param aExp expected distance
* @return does the distance calculated agree?
*/
bool SegDistanceCorrect( const SEG& aSegA, const SEG& aSegB, int aExp )
{
const int AtoB = aSegA.Distance( aSegB );
const int BtoA = aSegB.Distance( aSegA );
bool ok = ( AtoB == aExp ) && ( BtoA == aExp );
if( AtoB != BtoA )
{
std::stringstream ss;
ss << "Segment distance is not the same in both directions: expected " << aExp << ", got "
<< AtoB << " & " << BtoA;
BOOST_TEST_INFO( ss.str() );
}
else if( !ok )
{
std::stringstream ss;
ss << "Distance incorrect: expected " << aExp << ", got " << AtoB;
BOOST_TEST_INFO( ss.str() );
}
// Sanity check: the collision should be consistent with the distance
ok = ok && SegCollideCorrect( aSegA, aSegB, 0, aExp == 0 );
return ok;
}
bool SHAPE_ARC::Collide( const SEG& aSeg, int aClearance ) const
{
int minDist = aClearance + m_width / 2;
auto centerDist = aSeg.Distance( m_pc );
auto p1 = GetP1();
if( centerDist < minDist )
return true;
auto ab = (aSeg.B - aSeg.A );
auto ac = ( m_pc - aSeg.A );
auto lenAbSq = ab.SquaredEuclideanNorm();
auto lambda = (double) ac.Dot( ab ) / (double) lenAbSq;
if( lambda >= 0.0 && lambda <= 1.0 )
{
VECTOR2I p;
p.x = (double) aSeg.A.x * lambda + (double) aSeg.B.x * (1.0 - lambda);
p.y = (double) aSeg.A.y * lambda + (double) aSeg.B.y * (1.0 - lambda);
auto p0pdist = ( m_p0 - p ).EuclideanNorm();
if( p0pdist < minDist )
return true;
auto p1pdist = ( p1 - p ).EuclideanNorm();
if( p1pdist < minDist )
return true;
}
auto p0dist = aSeg.Distance( m_p0 );
if( p0dist > minDist )
return true;
auto p1dist = aSeg.Distance( p1 );
if( p1dist > minDist )
return false;
return true;
}
static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance )
{
VECTOR2I f( 0, 0 );
const VECTOR2I c = aA.GetCenter();
const VECTOR2I nearest = aB.NearestPoint( c );
const int r = aA.GetRadius();
int dist = ( nearest - c ).EuclideanNorm();
int min_dist = aClearance + r;
if( dist < min_dist )
{
for( int corr = 0; corr < 5; corr++ )
{
f = ( aA.GetCenter() - nearest ).Resize( min_dist - dist + corr );
if( aB.Distance( c + f ) >= min_dist )
break;
}
}
return f;
}
int SHAPE_LINE_CHAIN::Split( const VECTOR2I& aP )
{
int ii = -1;
int min_dist = 2;
ii = Find( aP );
if( ii >= 0 )
return ii;
for( int s = 0; s < SegmentCount(); s++ )
{
const SEG seg = CSegment( s );
int dist = seg.Distance( aP );
// make sure we are not producing a 'slightly concave' primitive. This might happen
// if aP lies very close to one of already existing points.
if( dist < min_dist && seg.A != aP && seg.B != aP )
{
min_dist = dist;
ii = s;
}
}
if( ii >= 0 )
{
m_points.insert( m_points.begin() + ii + 1, aP );
return ii + 1;
}
return -1;
}
/**
* Predicate to check expected distance between a segment and a point
* @param aSegA the segment
* @param aVec the vector (point)
* @param aExp expected distance
* @return does the distance calculated agree?
*/
bool SegVecDistanceCorrect( const SEG& aSeg, const VECTOR2I& aVec, int aExp )
{
const int dist = aSeg.Distance( aVec );
bool ok = ( dist == aExp );
if( !ok )
{
std::stringstream ss;
ss << "Distance incorrect: expected " << aExp << ", got " << dist;
BOOST_TEST_INFO( ss.str() );
}
return ok;
}
bool SHAPE_LINE_CHAIN::PointOnEdge( const VECTOR2I& aP ) const
{
if( SegmentCount() < 1 )
return m_points[0] == aP;
for( int i = 1; i < SegmentCount(); i++ )
{
const SEG s = CSegment( i );
if( s.A == aP || s.B == aP )
return true;
if( s.Distance( aP ) <= 1 )
return true;
}
return false;
}
int SHAPE_LINE_CHAIN::PathLength( const VECTOR2I& aP ) const
{
int sum = 0;
for( int i = 0; i < SegmentCount(); i++ )
{
const SEG seg = CSegment( i );
int d = seg.Distance( aP );
if( d <= 1 )
{
sum += ( aP - seg.A ).EuclideanNorm();
return sum;
}
else
sum += seg.Length();
}
return -1;
}
bool SHAPE_LINE_CHAIN::CheckClearance( const VECTOR2I& aP, const int aDist) const
{
if( !PointCount() )
return false;
else if( PointCount() == 1 )
return m_points[0] == aP;
for( int i = 0; i < SegmentCount(); i++ )
{
const SEG s = CSegment( i );
if( s.A == aP || s.B == aP )
return true;
if( s.Distance( aP ) <= aDist )
return true;
}
return false;
}
int SHAPE_LINE_CHAIN::EdgeContainingPoint( const VECTOR2I& aPt, int aAccuracy ) const
{
if( !PointCount() )
return -1;
else if( PointCount() == 1 )
{
VECTOR2I dist = m_points[0] - aPt;
return ( hypot( dist.x, dist.y ) <= aAccuracy + 1 ) ? 0 : -1;
}
for( int i = 0; i < SegmentCount(); i++ )
{
const SEG s = CSegment( i );
if( s.A == aPt || s.B == aPt )
return i;
if( s.Distance( aPt ) <= aAccuracy + 1 )
return i;
}
return -1;
}