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;
}
static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance )
{
VECTOR2I nearest = aB.NearestPoint( aA.GetCenter() );
VECTOR2I f (0, 0);
int dist = ( nearest - aA.GetCenter() ).EuclideanNorm();
int min_dist = aClearance + aA.GetRadius();
if( dist < min_dist )
f = ( aA.GetCenter() - nearest ).Resize ( min_dist - dist + 10 );
return f;
}
const SHAPE_LINE_CHAIN PNS_SOLID::Hull( int aClearance, int aWalkaroundThickness ) const
{
int cl = aClearance + aWalkaroundThickness / 2;
switch( m_shape->Type() )
{
case SH_RECT:
{
SHAPE_RECT* rect = static_cast<SHAPE_RECT*>( m_shape );
return OctagonalHull( rect->GetPosition(), rect->GetSize(), cl + 1, 0.2 * cl );
}
case SH_CIRCLE:
{
SHAPE_CIRCLE* circle = static_cast<SHAPE_CIRCLE*>( m_shape );
int r = circle->GetRadius();
return OctagonalHull( circle->GetCenter() - VECTOR2I( r, r ), VECTOR2I( 2 * r, 2 * r ),
cl + 1, 0.52 * ( r + cl ) );
}
case SH_SEGMENT:
{
SHAPE_SEGMENT* seg = static_cast<SHAPE_SEGMENT*> ( m_shape );
return SegmentHull( *seg, aClearance, aWalkaroundThickness );
}
default:
break;
}
return SHAPE_LINE_CHAIN();
}
static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_CIRCLE& aB, int aClearance,
bool aNeedMTV, VECTOR2I& aMTV )
{
ecoord min_dist = aClearance + aA.GetRadius() + aB.GetRadius();
ecoord min_dist_sq = min_dist * min_dist;
const VECTOR2I delta = aB.GetCenter() - aA.GetCenter();
ecoord dist_sq = delta.SquaredEuclideanNorm();
if( dist_sq >= min_dist_sq )
return false;
if( aNeedMTV )
aMTV = delta.Resize( min_dist - sqrt( dist_sq ) + 3 ); // fixme: apparent rounding error
return true;
}
static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_SIMPLE& aB, int aClearance,
bool aNeedMTV, VECTOR2I& aMTV )
{
bool found;
const SHAPE_LINE_CHAIN& lc( aB.Vertices() );
found = lc.Distance( aA.GetCenter() ) <= aClearance + aA.GetRadius();
if( !aNeedMTV || !found )
return found;
SHAPE_CIRCLE cmoved( aA );
VECTOR2I f_total( 0, 0 );
for( int s = 0; s < lc.SegmentCount(); s++ )
{
VECTOR2I f = pushoutForce( cmoved, lc.CSegment( s ), aClearance );
cmoved.SetCenter( cmoved.GetCenter() + f );
f_total += f;
}
aMTV = f_total;
return found;
}
static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance,
bool aNeedMTV, VECTOR2I& aMTV )
{
const VECTOR2I c = aB.GetCenter();
const VECTOR2I p0 = aA.GetPosition();
const VECTOR2I size = aA.GetSize();
const int r = aB.GetRadius();
const int min_dist = aClearance + r;
const VECTOR2I vts[] =
{
VECTOR2I( p0.x, p0.y ),
VECTOR2I( p0.x, p0.y + size.y ),
VECTOR2I( p0.x + size.x, p0.y + size.y ),
VECTOR2I( p0.x + size.x, p0.y ),
VECTOR2I( p0.x, p0.y )
};
int nearest_seg_dist = INT_MAX;
VECTOR2I nearest;
bool inside = c.x >= p0.x && c.x <= ( p0.x + size.x )
&& c.y >= p0.y && c.y <= ( p0.y + size.y );
if( !aNeedMTV && inside )
return true;
for( int i = 0; i < 4; i++ )
{
const SEG seg( vts[i], vts[i + 1] );
VECTOR2I pn = seg.NearestPoint( c );
int d = ( pn - c ).EuclideanNorm();
if( ( d < min_dist ) && !aNeedMTV )
return true;
if( d < nearest_seg_dist )
{
nearest = pn;
nearest_seg_dist = d;
}
}
if( nearest_seg_dist >= min_dist && !inside )
return false;
VECTOR2I delta = c - nearest;
if( !aNeedMTV )
return true;
if( inside )
aMTV = -delta.Resize( abs( min_dist + 1 + nearest_seg_dist ) + 1 );
else
aMTV = delta.Resize( abs( min_dist + 1 - nearest_seg_dist ) + 1 );
return true;
}
static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance,
bool aNeedMTV, VECTOR2I& aMTV )
{
const VECTOR2I c = aB.GetCenter();
const VECTOR2I p0 = aA.GetPosition();
const VECTOR2I size = aA.GetSize();
const ecoord r = aB.GetRadius();
const ecoord min_dist = aClearance + r;
const ecoord min_dist_sq = min_dist * min_dist;
if( aA.BBox( 0 ).Contains( c ) )
return true;
const VECTOR2I vts[] =
{
VECTOR2I( p0.x, p0.y ),
VECTOR2I( p0.x, p0.y + size.y ),
VECTOR2I( p0.x + size.x, p0.y + size.y ),
VECTOR2I( p0.x + size.x, p0.y ),
VECTOR2I( p0.x, p0.y )
};
ecoord nearest_seg_dist_sq = VECTOR2I::ECOORD_MAX;
VECTOR2I nearest;
bool inside = c.x >= p0.x && c.x <= ( p0.x + size.x )
&& c.y >= p0.y && c.y <= ( p0.y + size.y );
if( !inside )
{
for( int i = 0; i < 4; i++ )
{
const SEG seg( vts[i], vts[i + 1] );
ecoord dist_sq = seg.SquaredDistance( c );
if( dist_sq < min_dist_sq )
{
if( !aNeedMTV )
return true;
else
{
nearest = seg.NearestPoint( c );
nearest_seg_dist_sq = dist_sq;
}
}
}
}
if( nearest_seg_dist_sq >= min_dist_sq && !inside )
return false;
VECTOR2I delta = c - nearest;
if( !aNeedMTV )
return true;
if( inside )
aMTV = -delta.Resize( sqrt( abs( r * r + nearest_seg_dist_sq ) + 1 ) );
else
aMTV = delta.Resize( sqrt( abs( r * r - nearest_seg_dist_sq ) + 1 ) );
return true;
}