/** * 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; }