void PNS_LINE::Walkaround( const SHAPE_LINE_CHAIN& aObstacle,
SHAPE_LINE_CHAIN& aPath,
bool aCw ) const
{
SHAPE_LINE_CHAIN walk, post;
Walkaround( aObstacle, aPath, walk, post, aCw );
aPath.Append( walk );
aPath.Append( post );
aPath.Simplify();
}
void PNS_LINE::DragCorner ( const VECTOR2I& aP, int aIndex, int aSnappingThreshold )
{
SHAPE_LINE_CHAIN path;
VECTOR2I snapped = snapDraggedCorner( m_line, aP, aIndex, aSnappingThreshold );
if( aIndex == 0 )
path = dragCornerInternal( m_line.Reverse(), snapped ).Reverse();
else if ( aIndex == m_line.SegmentCount() )
path = dragCornerInternal( m_line, snapped );
else
{
// fixme: awkward behaviour for "outwards" drags
path = dragCornerInternal( m_line.Slice( 0, aIndex ), snapped );
SHAPE_LINE_CHAIN path_rev = dragCornerInternal( m_line.Slice( aIndex, -1 ).Reverse(),
snapped ).Reverse();
path.Append( path_rev );
}
path.Simplify();
m_line = path;
}
PNS_WALKAROUND::WalkaroundStatus PNS_WALKAROUND::singleStep( PNS_LINE& aPath,
bool aWindingDirection )
{
optional<PNS_OBSTACLE>& current_obs =
aWindingDirection ? m_currentObstacle[0] : m_currentObstacle[1];
bool& prev_recursive = aWindingDirection ? m_recursiveCollision[0] : m_recursiveCollision[1];
if( !current_obs )
return DONE;
SHAPE_LINE_CHAIN path_pre[2], path_walk[2], path_post[2];
VECTOR2I last = aPath.GetCLine().CPoint( -1 );
if( ( current_obs->hull ).PointInside( last ) )
{
m_recursiveBlockageCount++;
if( m_recursiveBlockageCount < 3 )
aPath.GetLine().Append( current_obs->hull.NearestPoint( last ) );
else
{
aPath = aPath.ClipToNearestObstacle( m_world );
return STUCK;
}
}
aPath.NewWalkaround( current_obs->hull, path_pre[0], path_walk[0],
path_post[0], aWindingDirection );
aPath.NewWalkaround( current_obs->hull, path_pre[1], path_walk[1],
path_post[1], !aWindingDirection );
int len_pre = path_walk[0].Length();
int len_alt = path_walk[1].Length();
PNS_LINE walk_path( aPath, path_walk[1] );
bool alt_collides = m_world->CheckColliding( &walk_path,
m_solids_only ? PNS_ITEM::SOLID : PNS_ITEM::ANY );
SHAPE_LINE_CHAIN pnew;
if( !m_forceSingleDirection && len_alt < len_pre && !alt_collides && !prev_recursive )
{
pnew = path_pre[1];
pnew.Append( path_walk[1] );
pnew.Append( path_post[1] );
current_obs = nearestObstacle( PNS_LINE( aPath, path_post[1] ) );
prev_recursive = false;
}
else
{
pnew = path_pre[0];
pnew.Append( path_walk[0] );
pnew.Append( path_post[0] );
current_obs = nearestObstacle( PNS_LINE( aPath, path_walk[0] ) );
if( !current_obs )
{
prev_recursive = false;
current_obs = nearestObstacle( PNS_LINE( aPath, path_post[0] ) );
}
else
prev_recursive = true;
}
pnew.Simplify();
aPath.SetShape( pnew );
return IN_PROGRESS;
}
PNS_WALKAROUND::WalkaroundStatus PNS_WALKAROUND::Route( const PNS_LINE& aInitialPath,
PNS_LINE& aWalkPath,
bool aOptimize )
{
PNS_LINE path_cw( aInitialPath ), path_ccw( aInitialPath );
WalkaroundStatus s_cw = IN_PROGRESS, s_ccw = IN_PROGRESS;
SHAPE_LINE_CHAIN best_path;
start( aInitialPath );
m_currentObstacle[0] = m_currentObstacle[1] = nearestObstacle( aInitialPath );
m_recursiveBlockageCount = 0;
aWalkPath = aInitialPath;
while( m_iteration < m_iteration_limit )
{
if( s_cw != STUCK )
s_cw = singleStep( path_cw, true );
if( s_ccw != STUCK )
s_ccw = singleStep( path_ccw, false );
if( ( s_cw == DONE && s_ccw == DONE ) || ( s_cw == STUCK && s_ccw == STUCK ) )
{
int len_cw = path_cw.GetCLine().Length();
int len_ccw = path_ccw.GetCLine().Length();
if( m_forceLongerPath )
aWalkPath = (len_cw > len_ccw ? path_cw : path_ccw);
else
aWalkPath = (len_cw < len_ccw ? path_cw : path_ccw);
break;
}
else if( s_cw == DONE && !m_forceLongerPath )
{
aWalkPath = path_cw;
break;
}
else if( s_ccw == DONE && !m_forceLongerPath )
{
aWalkPath = path_ccw;
break;
}
m_iteration++;
}
if( m_iteration == m_iteration_limit )
{
int len_cw = path_cw.GetCLine().Length();
int len_ccw = path_ccw.GetCLine().Length();
if( m_forceLongerPath )
aWalkPath = (len_cw > len_ccw ? path_cw : path_ccw);
else
aWalkPath = (len_cw < len_ccw ? path_cw : path_ccw);
}
if( m_cursorApproachMode )
{
// int len_cw = path_cw.GetCLine().Length();
// int len_ccw = path_ccw.GetCLine().Length();
bool found = false;
SHAPE_LINE_CHAIN l = aWalkPath.GetCLine();
for( int i = 0; i < l.SegmentCount(); i++ )
{
const SEG s = l.Segment( i );
VECTOR2I nearest = s.NearestPoint( m_cursorPos );
VECTOR2I::extended_type dist_a = ( s.A - m_cursorPos ).SquaredEuclideanNorm();
VECTOR2I::extended_type dist_b = ( s.B - m_cursorPos ).SquaredEuclideanNorm();
VECTOR2I::extended_type dist_n = ( nearest - m_cursorPos ).SquaredEuclideanNorm();
if( dist_n <= dist_a && dist_n < dist_b )
{
// PNSDisplayDebugLine( l, 3 );
l.Remove( i + 1, -1 );
l.Append( nearest );
l.Simplify();
found = true;
break;
}
}
if( found )
{
aWalkPath = aInitialPath;
aWalkPath.SetShape( l );
}
}
aWalkPath.SetWorld( m_world );
aWalkPath.GetLine().Simplify();
WalkaroundStatus st = s_ccw == DONE || s_cw == DONE ? DONE : STUCK;
if( aOptimize && st == DONE )
PNS_OPTIMIZER::Optimize( &aWalkPath, PNS_OPTIMIZER::MERGE_OBTUSE, m_world );
return st;
}
WALKAROUND::WALKAROUND_STATUS WALKAROUND::singleStep( LINE& aPath,
bool aWindingDirection )
{
optional<OBSTACLE>& current_obs =
aWindingDirection ? m_currentObstacle[0] : m_currentObstacle[1];
bool& prev_recursive = aWindingDirection ? m_recursiveCollision[0] : m_recursiveCollision[1];
if( !current_obs )
return DONE;
SHAPE_LINE_CHAIN path_pre[2], path_walk[2], path_post[2];
VECTOR2I last = aPath.CPoint( -1 );
if( ( current_obs->m_hull ).PointInside( last ) || ( current_obs->m_hull ).PointOnEdge( last ) )
{
m_recursiveBlockageCount++;
if( m_recursiveBlockageCount < 3 )
aPath.Line().Append( current_obs->m_hull.NearestPoint( last ) );
else
{
aPath = aPath.ClipToNearestObstacle( m_world );
return DONE;
}
}
aPath.Walkaround( current_obs->m_hull, path_pre[0], path_walk[0],
path_post[0], aWindingDirection );
aPath.Walkaround( current_obs->m_hull, path_pre[1], path_walk[1],
path_post[1], !aWindingDirection );
#ifdef DEBUG
m_logger.NewGroup( aWindingDirection ? "walk-cw" : "walk-ccw", m_iteration );
m_logger.Log( &path_walk[0], 0, "path-walk" );
m_logger.Log( &path_pre[0], 1, "path-pre" );
m_logger.Log( &path_post[0], 4, "path-post" );
m_logger.Log( ¤t_obs->m_hull, 2, "hull" );
m_logger.Log( current_obs->m_item, 3, "item" );
#endif
int len_pre = path_walk[0].Length();
int len_alt = path_walk[1].Length();
LINE walk_path( aPath, path_walk[1] );
bool alt_collides = static_cast<bool>( m_world->CheckColliding( &walk_path, m_itemMask ) );
SHAPE_LINE_CHAIN pnew;
if( !m_forceLongerPath && len_alt < len_pre && !alt_collides && !prev_recursive )
{
pnew = path_pre[1];
pnew.Append( path_walk[1] );
pnew.Append( path_post[1] );
if( !path_post[1].PointCount() || !path_walk[1].PointCount() )
current_obs = nearestObstacle( LINE( aPath, path_pre[1] ) );
else
current_obs = nearestObstacle( LINE( aPath, path_post[1] ) );
prev_recursive = false;
}
else
{
pnew = path_pre[0];
pnew.Append( path_walk[0] );
pnew.Append( path_post[0] );
if( !path_post[0].PointCount() || !path_walk[0].PointCount() )
current_obs = nearestObstacle( LINE( aPath, path_pre[0] ) );
else
current_obs = nearestObstacle( LINE( aPath, path_walk[0] ) );
if( !current_obs )
{
prev_recursive = false;
current_obs = nearestObstacle( LINE( aPath, path_post[0] ) );
}
else
prev_recursive = true;
}
pnew.Simplify();
aPath.SetShape( pnew );
return IN_PROGRESS;
}
WALKAROUND::WALKAROUND_STATUS WALKAROUND::Route( const LINE& aInitialPath,
LINE& aWalkPath, bool aOptimize )
{
LINE path_cw( aInitialPath ), path_ccw( aInitialPath );
WALKAROUND_STATUS s_cw = IN_PROGRESS, s_ccw = IN_PROGRESS;
SHAPE_LINE_CHAIN best_path;
// special case for via-in-the-middle-of-track placement
if( aInitialPath.PointCount() <= 1 )
{
if( aInitialPath.EndsWithVia() && m_world->CheckColliding( &aInitialPath.Via(), m_itemMask ) )
return STUCK;
aWalkPath = aInitialPath;
return DONE;
}
start( aInitialPath );
m_currentObstacle[0] = m_currentObstacle[1] = nearestObstacle( aInitialPath );
m_recursiveBlockageCount = 0;
aWalkPath = aInitialPath;
if( m_forceWinding )
{
s_cw = m_forceCw ? IN_PROGRESS : STUCK;
s_ccw = m_forceCw ? STUCK : IN_PROGRESS;
m_forceSingleDirection = true;
} else {
m_forceSingleDirection = false;
}
while( m_iteration < m_iterationLimit )
{
if( s_cw != STUCK )
s_cw = singleStep( path_cw, true );
if( s_ccw != STUCK )
s_ccw = singleStep( path_ccw, false );
if( ( s_cw == DONE && s_ccw == DONE ) || ( s_cw == STUCK && s_ccw == STUCK ) )
{
int len_cw = path_cw.CLine().Length();
int len_ccw = path_ccw.CLine().Length();
if( m_forceLongerPath )
aWalkPath = ( len_cw > len_ccw ? path_cw : path_ccw );
else
aWalkPath = ( len_cw < len_ccw ? path_cw : path_ccw );
break;
}
else if( s_cw == DONE && !m_forceLongerPath )
{
aWalkPath = path_cw;
break;
}
else if( s_ccw == DONE && !m_forceLongerPath )
{
aWalkPath = path_ccw;
break;
}
m_iteration++;
}
if( m_iteration == m_iterationLimit )
{
int len_cw = path_cw.CLine().Length();
int len_ccw = path_ccw.CLine().Length();
if( m_forceLongerPath )
aWalkPath = ( len_cw > len_ccw ? path_cw : path_ccw );
else
aWalkPath = ( len_cw < len_ccw ? path_cw : path_ccw );
}
if( m_cursorApproachMode )
{
// int len_cw = path_cw.GetCLine().Length();
// int len_ccw = path_ccw.GetCLine().Length();
bool found = false;
SHAPE_LINE_CHAIN l = aWalkPath.CLine();
for( int i = 0; i < l.SegmentCount(); i++ )
{
const SEG s = l.Segment( i );
VECTOR2I nearest = s.NearestPoint( m_cursorPos );
VECTOR2I::extended_type dist_a = ( s.A - m_cursorPos ).SquaredEuclideanNorm();
VECTOR2I::extended_type dist_b = ( s.B - m_cursorPos ).SquaredEuclideanNorm();
VECTOR2I::extended_type dist_n = ( nearest - m_cursorPos ).SquaredEuclideanNorm();
if( dist_n <= dist_a && dist_n < dist_b )
{
l.Remove( i + 1, -1 );
l.Append( nearest );
l.Simplify();
found = true;
break;
}
}
if( found )
{
aWalkPath = aInitialPath;
aWalkPath.SetShape( l );
}
}
aWalkPath.Line().Simplify();
if( aWalkPath.SegmentCount() < 1 )
return STUCK;
if( aWalkPath.CPoint( -1 ) != aInitialPath.CPoint( -1 ) )
return STUCK;
if( aWalkPath.CPoint( 0 ) != aInitialPath.CPoint( 0 ) )
return STUCK;
WALKAROUND_STATUS st = s_ccw == DONE || s_cw == DONE ? DONE : STUCK;
if( st == DONE )
{
if( aOptimize )
OPTIMIZER::Optimize( &aWalkPath, OPTIMIZER::MERGE_OBTUSE, m_world );
}
return st;
}
bool PNS_LINE::Walkaround( SHAPE_LINE_CHAIN aObstacle, SHAPE_LINE_CHAIN& aPre,
SHAPE_LINE_CHAIN& aWalk, SHAPE_LINE_CHAIN& aPost, bool aCw ) const
{
const SHAPE_LINE_CHAIN& line ( CLine() );
VECTOR2I ip_start;
VECTOR2I ip_end;
if( line.SegmentCount() < 1 )
return false;
if( aObstacle.PointInside( line.CPoint( 0 ) ) || aObstacle.PointInside( line.CPoint( -1 ) ) )
return false;
SHAPE_LINE_CHAIN::INTERSECTIONS ips, ips2;
line.Intersect( aObstacle, ips );
int nearest_dist = INT_MAX;
int farthest_dist = 0;
SHAPE_LINE_CHAIN::INTERSECTION nearest, farthest;
for( int i = 0; i < (int) ips.size(); i++ )
{
const VECTOR2I p = ips[i].p;
int dist = line.PathLength( p );
if( dist < 0 )
return false;
if( dist <= nearest_dist )
{
nearest_dist = dist;
nearest = ips[i];
}
if( dist >= farthest_dist )
{
farthest_dist = dist;
farthest = ips[i];
}
}
if( ips.size() <= 1 || nearest.p == farthest.p )
{
aPre = line;
return true;
}
aPre = line.Slice( 0, nearest.our.Index() );
aPre.Append( nearest.p );
aPre.Simplify();
aWalk.Clear();
aWalk.SetClosed( false );
aWalk.Append( nearest.p );
int i = nearest.their.Index();
assert( nearest.their.Index() >= 0 );
assert( farthest.their.Index() >= 0 );
assert( nearest_dist <= farthest_dist );
aObstacle.Split( nearest.p );
aObstacle.Split( farthest.p );
int i_first = aObstacle.Find( nearest.p );
int i_last = aObstacle.Find( farthest.p );
i = i_first;
while( i != i_last )
{
aWalk.Append( aObstacle.CPoint( i ) );
i += ( aCw ? 1 : -1 );
if( i < 0 )
i = aObstacle.PointCount() - 1;
else if( i == aObstacle.PointCount() )
i = 0;
}
aWalk.Append( farthest.p );
aWalk.Simplify();
aPost.Clear();
aPost.Append( farthest.p );
aPost.Append( line.Slice( farthest.our.Index() + 1, -1 ) );
aPost.Simplify();
return true;
}
