bool PNS_LINE_PLACER::buildInitialLine( const VECTOR2I& aP, PNS_LINE& aHead ) { SHAPE_LINE_CHAIN l; if( m_p_start == aP ) { l.Clear(); } else { if( Settings().GetFreeAngleMode() && Settings().Mode() == RM_MarkObstacles ) { l = SHAPE_LINE_CHAIN( m_p_start, aP ); } else { l = m_direction.BuildInitialTrace( m_p_start, aP ); } if( l.SegmentCount() > 1 && m_orthoMode ) { VECTOR2I newLast = l.CSegment( 0 ).LineProject( l.CPoint( -1 ) ); l.Remove( -1, -1 ); l.Point( 1 ) = newLast; } } aHead.SetShape( l ); if( !m_placingVia ) return true; PNS_VIA v( makeVia( aP ) ); v.SetNet( aHead.Net() ); if( m_currentMode == RM_MarkObstacles ) { aHead.AppendVia( v ); return true; } VECTOR2I force; VECTOR2I lead = aP - m_p_start; bool solidsOnly = ( m_currentMode != RM_Walkaround ); if( v.PushoutForce( m_currentNode, lead, force, solidsOnly, 40 ) ) { SHAPE_LINE_CHAIN line = m_direction.BuildInitialTrace( m_p_start, aP + force ); aHead = PNS_LINE( aHead, line ); v.SetPos( v.Pos() + force ); return true; } return false; // via placement unsuccessful }
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; }
PNS_WALKAROUND::WALKAROUND_STATUS 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.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(); PNS_LINE walk_path( aPath, path_walk[1] ); bool alt_collides = m_world->CheckColliding( &walk_path, m_itemMask ); 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] ); if( !path_post[1].PointCount() || !path_walk[1].PointCount() ) current_obs = nearestObstacle( PNS_LINE( aPath, path_pre[1] ) ); else 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] ); if( !path_post[0].PointCount() || !path_walk[0].PointCount() ) current_obs = nearestObstacle( PNS_LINE( aPath, path_pre[0] ) ); else 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; }