Exemplo n.º 1
0
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();
}
Exemplo n.º 2
0
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( &current_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;
}
Exemplo n.º 7
0
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;
}