222 lines
5.6 KiB
C++
222 lines
5.6 KiB
C++
|
/*
|
||
|
* KiRouter - a push-and-(sometimes-)shove PCB router
|
||
|
*
|
||
|
* Copyright (C) 2013 CERN
|
||
|
* Author: Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
|
||
|
*
|
||
|
* This program is free software: you can redistribute it and/or modify it
|
||
|
* under the terms of the GNU General Public License as published by the
|
||
|
* Free Software Foundation, either version 3 of the License, or (at your
|
||
|
* option) any later version.
|
||
|
*
|
||
|
* This program is distributed in the hope that it will be useful, but
|
||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||
|
* General Public License for more details.
|
||
|
*
|
||
|
* You should have received a copy of the GNU General Public License along
|
||
|
* with this program. If not, see <http://www.gnu.or/licenses/>.
|
||
|
*/
|
||
|
|
||
|
#include <vector>
|
||
|
|
||
|
#include <boost/foreach.hpp>
|
||
|
#include <boost/optional.hpp>
|
||
|
|
||
|
#include <geometry/shape_line_chain.h>
|
||
|
|
||
|
#include "pns_walkaround.h"
|
||
|
#include "pns_optimizer.h"
|
||
|
#include "pns_utils.h"
|
||
|
#include "pns_router.h"
|
||
|
|
||
|
using namespace std;
|
||
|
using boost::optional;
|
||
|
|
||
|
void PNS_WALKAROUND::start( const PNS_LINE& aInitialPath )
|
||
|
{
|
||
|
m_iteration = 0;
|
||
|
m_iteration_limit = 50;
|
||
|
}
|
||
|
|
||
|
|
||
|
PNS_NODE::OptObstacle PNS_WALKAROUND::nearestObstacle(const PNS_LINE& aPath)
|
||
|
{
|
||
|
return m_world->NearestObstacle ( &aPath, m_solids_only ? (PNS_ITEM::SOLID | PNS_ITEM::VIA) : PNS_ITEM::ANY );
|
||
|
}
|
||
|
|
||
|
|
||
|
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;
|
||
|
}
|