PCB Environment: /home/chr/devel/PhD/Environment/pub/PCBEnv/pcbenv/cxx/AStar.hpp Source File

PCB Environment 2

Loading...
Searching...
No Matches
AStar.hpp
1
2#ifndef GYM_PCB_ASTAR_H
3#define GYM_PCB_ASTAR_H
4
5#include "Log.hpp"
6#include "PCBoard.hpp"
7#include "NavGrid.hpp"
8#include "Path.hpp"
9#include <queue>
10
11// OPTIMIZATIONS TODO:
12// "Online Graph Pruning for Pathfinding on Grid Maps", D. Harabor & A. Grastien, AAAI 2011
13// 1) Don't evaluate nodes that are more quickly reached from the parent.
14// 2) Keep moving in the same direction without expansion as long as we don't
15// find another neighbour (other than straight) that isn't more quickly
16// reached from (dominated by) the parent.
17// 3) The point where we find another neighbour is the jump point that we use
18// as successor to our original node.
19// 4) If we reach an obstacle going straight, we do nothing.
20// The paper proves that this strategy does not affect optimality.
21
23#define ASTAR_ALLOW_XOVER false
24
29 class NavPointRef
30{
31public:
32 NavPointRef(NavPoint *nav, float v) : mPoint(nav), mKey(nav->getScore() + v) { }
33 NavPoint *getPoint() const { return mPoint; }
34 NavPoint *operator->() const { return mPoint; }
35 bool operator<(const NavPointRef &that) const { return mKey >= that.mKey; }
36private:
37 float mKey;
38 NavPoint *mPoint;
39};
40
41 class AStar
42{
43 constexpr static const float mTurnCostPer45Degrees = 1.0f / 1024.0f;
44public:
45 AStar(NavGrid&, const AStarCosts&);
46 ~AStar();
47 bool search(Connection&);
48 const std::vector<Point_25>& violations() const { return mViolationLocs; }
49private:
50 NavGrid &mNav;
51 Point_2 mTargetXY;
52 int mTargetZ[2];
53 std::vector<float> mLayerCost;
54 uint16_t mRouteMask{NAV_POINT_FLAGS_TRACKS_BLOCKED};
55 float mViaCost;
56 float mViolationCost;
57 NavPoint *mTarget{0};
58 Point_2 mSourceXY;
59 std::vector<Point_25> mViolationLocs;
60 AStarCosts mCostParams;
61 float heuristic(const NavPoint&);
62 int getApproxBlockageSearchArea(const Pin *) const;
63 int _search(NavPoint *source, int maxVisits);
64 int _search(NavPoint *source, NavPoint *target, int maxVisits);
65 void initEndPoint(const Point_2&, int z[2], bool dst, bool save);
66 void finiEndPoint(const Point_2&, int z[2]);
67 bool reconstruct(Connection&);
68 void addViolation(const NavPoint *, Real radius);
69 void initCosts(const Connection&);
70 float computeCost(NavPoint *src, NavPoint *dst, const GridDirection d);
71private:
72 void writePoint(const Point_25&, uint16_t add, uint16_t clr, bool save);
73 NavPoint *checkHEdge(const NavPoint *, GridDirection) const;
74 NavPoint *checkVEdge(const NavPoint *, GridDirection) const;
75};
76
77AStar::AStar(NavGrid &nav, const AStarCosts &costs) : mNav(nav), mCostParams(costs)
78{
79}
80AStar::~AStar()
81{
82}
83
84inline void AStar::writePoint(const Point_25 &v, uint16_t add, uint16_t clr, bool save)
85{
86 NavPoint *x = mNav.getPoint(v);
87 if (save)
88 x->saveFlags();
89 x->setFlags(add);
90 x->clearFlags(clr);
91}
92inline void AStar::initEndPoint(const Point_2 &v, int Z[2], bool dst, bool save)
93{
94 const uint w = dst ? NAV_POINT_FLAG_TARGET : NAV_POINT_FLAG_SOURCE;
95 const uint m = dst ? NAV_POINT_FLAG_SOURCE : NAV_POINT_FLAG_TARGET;
96 // If the endpoint goes across multiple layers it will be a pin where we can move freely vertically, so remove via clearance. Leaving the pin is still blocked by the canPlaceVia() check.
97 const uint u = (Z[0] == Z[1]) ? NAV_POINT_FLAGS_TRACK_CLEARANCE : NAV_POINT_FLAGS_CLEARANCE;
98 for (int z = Z[0]; z <= Z[1]; ++z)
99 writePoint(Point_25(v, z), w, NAV_POINT_FLAG_BLOCKED_TEMPORARY | u | m, save);
100 if (dst) {
101 mTargetZ[0] = Z[0];
102 mTargetZ[1] = Z[1];
103 }
104}
105inline void AStar::finiEndPoint(const Point_2 &v, int Z[2])
106{
107 for (int z = Z[0]; z <= Z[1]; ++z)
108 mNav.getPoint(v, z)->restoreFlags();
109}
110
111float AStar::heuristic(const NavPoint &A)
112{
113 //SCORE d = std::sqrt(CGAL::squared_distance(A.getRefPoint(), mTargetXY));
114 float d = geo::distance45(A.getRefPoint(&mNav), mTargetXY);
115
116 uint dz = 0;
117 if (A.getLayer() < mTargetZ[0])
118 dz = mTargetZ[0] - A.getLayer();
119 else if (A.getLayer() > mTargetZ[1])
120 dz = A.getLayer() - mTargetZ[1];
121 if (dz) {
122 if (!A.getBackDirection().isVertical())
123 dz += 1;
124 d += mViaCost * 0.5f * dz;
125 }
126 return d;
127}
128
129// When we add a via:
130// V0 z=0 -> Segment( S,V0)
131// V1 z=0 -> Segment( S,V1)
132// V1 z=1 -> Segment(V1,V1)
133// V2 z=1 -> Segment(V1,V2)
134bool AStar::reconstruct(Connection &route)
135{
136 std::lock_guard wlock(mNav.getPCB().getLock());
137
138 assert(!route.hasTracks() && !route.isRouted());
139 assert(mTarget);
140 const NavPoint *head = mTarget;
141 for (auto next = head->getBackEdge(mNav); next->hasFlags(NAV_POINT_FLAG_TARGET); next = next->getBackEdge(mNav))
142 head = next;
143 Track *T = route.newTrack(head->getRefPoint25(&mNav));
144 const NavPoint *node = head;
145 DEBUG("AStar " << node->str(&mNav));
146 GridDirection d = node->getBackDirection();
147 while (!node->hasFlags(NAV_POINT_FLAG_SOURCE)) {
148 node = node->getBackEdge(mNav);
149 if (!node->canRoute())
150 addViolation(node, T->defaultWidth());
151 DEBUG("AStar " << node->str(&mNav));
152 if (node->getBackDirection() == d && node->getLayer() == head->getLayer() && !node->hasFlags(NAV_POINT_FLAG_SOURCE))
153 continue;
154 if (!d.isVertical())
155 T->_append(head->getSegmentTo(*node, &mNav));
156 head = node;
157 d = node->getBackDirection();
158 }
159 assert(head == node);
160 T->_setEnd(node->getRefPoint25(&mNav));
161 route.forceRouted();
162 T->autocreateVias(T->end() == node->getRefPoint25(&mNav) ? T->end() : route.target());
163 T->computeLength();
164 return true;
165}
166
167void AStar::addViolation(const NavPoint *node, Real radius)
168{
169 const auto v = node->getRefPoint25(&mNav);
170 if (mViolationLocs.empty() ||
171 mViolationLocs.back().z() != v.z() ||
172 CGAL::squared_distance(v.xy(), mViolationLocs.back().xy()) > radius)
173 mViolationLocs.push_back(v);
174}
175
176int AStar::getApproxBlockageSearchArea(const Pin *T) const
177{
178 const auto e = mNav.EdgeLen;
179 if (!T)
180 return 384;
181 const int nx = int((T->getBbox().xmax() - T->getBbox().xmin() + e) / e);
182 const int ny = int((T->getBbox().xmax() - T->getBbox().xmin() + e) / e);
183 const int nA = nx * ny;
184 return std::max(384, std::min(nA * 8, 1024));
185}
186
187inline float AStar::computeCost(NavPoint *src, NavPoint *dst, const GridDirection d)
188{
189 auto moveCost = dst->getCost(0);
190
191 // Moving through the source node is cheaper:
192 if (dst->hasFlags(NAV_POINT_FLAG_SOURCE))
193 moveCost *= 0.125f;
194
195 if (d.isVertical()) {
196 moveCost *= mViaCost;
197 // Using the same via costs less, but we can't make the cost 0 or we'd always explore the whole layer stack rather:
198 if (src->getBackDirection().isVertical())
199 moveCost *= 0.5f;
200 } else {
201 moveCost *= mLayerCost[dst->getLayer()];
202 if (d._isDiagonal())
203 moveCost *= std::sqrt(2.0f);
204 if (dst->hasFlags(NAV_POINT_FLAG_ROUTE_TRACK_CLEARANCE))
205 moveCost *= mViolationCost;
206 moveCost += mTurnCostPer45Degrees * math::squared(d.opposite().get45DegreeStepsBetween(src->getBackDirection()));
207 }
208 return moveCost;
209}
210
211inline NavPoint *AStar::checkHEdge(const NavPoint *ref, GridDirection d) const
212{
213 auto e = ref->getEdge(mNav, d);
214 return (e && !e->hasFlags(mRouteMask)) ? e : 0;
215}
216inline NavPoint *AStar::checkVEdge(const NavPoint *ref, GridDirection d) const
217{
218 if (!ref->canPlaceVia())
219 return 0;
220 auto e = ref->getEdge(mNav, d);
221 return (e && ref->canAddVia(*e)) ? e : 0;
222}
223
224#define PERF_COUNTER_T(i) auto __t##i = std::chrono::high_resolution_clock::now()
225#define PERF_COUNTER_D(a, b) std::chrono::duration_cast<std::chrono::nanoseconds>(__t##b - __t##a).count()
226
227bool AStar::search(Connection &route)
228{
229 DEBUG("A* from " << route.source() << " to " << route.target());
230
231 if (route.hasTracks() || route.isRouted())
232 throw std::runtime_error("Connection pass to A* must have an empty track.");
233
234 // Search in reverse so reconstruct goes in the right direction.
235 auto target = mNav.getPoint(route.source());
236 auto source = mNav.getPoint(route.target());
237 if (!target || !source || source == target)
238 return false;
239 initCosts(route);
240
241 int sourceZ[2];
242 int targetZ[2];
243 sourceZ[0] = route.targetPin() ? route.targetPin()->minLayer() : source->getLayer();
244 targetZ[0] = route.sourcePin() ? route.sourcePin()->minLayer() : target->getLayer();
245 sourceZ[1] = route.targetPin() ? route.targetPin()->maxLayer() : source->getLayer();
246 targetZ[1] = route.sourcePin() ? route.sourcePin()->maxLayer() : target->getLayer();
247
248 // First test if we're blocked off (rv < 0).
249 initEndPoint(target->getRefPoint(&mNav), targetZ, 0, true);
250 initEndPoint(source->getRefPoint(&mNav), sourceZ, 1, true);
251 auto rv = _search(target, source, getApproxBlockageSearchArea(route.sourcePin()));
252 if (rv >= 0) {
253 initEndPoint(source->getRefPoint(&mNav), sourceZ, 0, false);
254 initEndPoint(target->getRefPoint(&mNav), targetZ, 1, false);
255 rv = _search(source, target, std::numeric_limits<int>::max());
256 }
257 DEBUG("A* " << ((rv > 0) ? "finished" : "failed") << " after " << ((rv > 0) ? (std::numeric_limits<int>::max() - rv) : -rv) << " nodes.");
258
259 auto ret = (rv > 0) && reconstruct(route);
260 if (rv <= 0) {
261 std::lock_guard wlock(mNav.getPCB().getLock());
262 route.clearTracks();
263 }
264 finiEndPoint(source->getRefPoint(&mNav), sourceZ);
265 finiEndPoint(target->getRefPoint(&mNav), targetZ);
266 return ret;
267}
268int AStar::_search(NavPoint *source, NavPoint *target, int maxVisits)
269{
270 assert(source && target && source != target);
271 mTargetXY = target->getRefPoint(&mNav);
272 mSourceXY = source->getRefPoint(&mNav);
273 return _search(source, maxVisits);
274}
275int AStar::_search(NavPoint *source, int maxVisits)
276{
277 const uint seq = mNav.nextSearchSeq();
278
279 source->setScore(0);
280 source->setBackDirection(GridDirection::Z().n());
281 source->getVisits().setOpen(seq);
282
283 std::priority_queue<NavPointRef> openList;
284 openList.push(NavPointRef(source, heuristic(*source)));
285 while (!openList.empty()) {
286 auto current = openList.top().getPoint();
287 openList.pop();
288 if (!current->getVisits().isOpen(seq)) // duplicate entry already encountered sooner
289 continue;
290 if (current->hasFlags(NAV_POINT_FLAG_TARGET)) {
291 mTarget = current;
292 return maxVisits;
293 }
294 if (!--maxVisits)
295 break;
296 current->getVisits().setDone(seq);
297
298 // FIXME: The no-cross check is still no sufficient for correctness.
299 // We can have conditions where unroute-reroute does not work:
300 // A B
301 // A B
302 // A BCCC <-- C can be routed or be a via, but then B becomes illegal
303 // CCC
304 // CCC
305 NavPoint *edges[GridDirection::Count];
306 edges[GridDirection::U().n()] = checkHEdge(current, GridDirection::U());
307 edges[GridDirection::D().n()] = checkHEdge(current, GridDirection::D());
308 edges[GridDirection::L().n()] = checkHEdge(current, GridDirection::L());
309 edges[GridDirection::R().n()] = checkHEdge(current, GridDirection::R());
310 edges[GridDirection::UR().n()] = ((edges[GridDirection::U().n()] && edges[GridDirection::R().n()]) || ASTAR_ALLOW_XOVER) ? checkHEdge(current, GridDirection::UR()) : 0;
311 edges[GridDirection::DR().n()] = ((edges[GridDirection::D().n()] && edges[GridDirection::R().n()]) || ASTAR_ALLOW_XOVER) ? checkHEdge(current, GridDirection::DR()) : 0;
312 edges[GridDirection::DL().n()] = ((edges[GridDirection::D().n()] && edges[GridDirection::L().n()]) || ASTAR_ALLOW_XOVER) ? checkHEdge(current, GridDirection::DL()) : 0;
313 edges[GridDirection::UL().n()] = ((edges[GridDirection::U().n()] && edges[GridDirection::L().n()]) || ASTAR_ALLOW_XOVER) ? checkHEdge(current, GridDirection::UL()) : 0;
314 edges[GridDirection::A().n()] = checkVEdge(current, GridDirection::A());
315 edges[GridDirection::V().n()] = checkVEdge(current, GridDirection::V());
316 const auto backd = current->getBackDirection();
317 if (!backd.isZero())
318 edges[backd.n()] = 0; // no need to check going right back
319 if (backd.is2D()) {
320 edges[backd.rotatedCcw45().n()] = 0; // these will never be better as edge costs are node costs and never negative
321 edges[backd.rotatedCw45().n()] = 0;
322 }
323
324 for (auto d = GridDirection::begin(); d != (current->canPlaceVia() ? GridDirection::vend() : GridDirection::hend()); ++d) {
325 NavPoint *node = edges[d.n()];
326 if (!node)
327 continue;
328 const auto score = current->getScore() + computeCost(current, node, d);
329 if (node->getVisits().isSeen(seq) && score >= node->getScore())
330 continue;
331 node->setBackDirection(d.opposite());
332 node->setScore(score);
333 node->getVisits().setOpen(seq);
334 openList.push(NavPointRef(node, heuristic(*node)));
335 }
336#if GYM_PCB_ENABLE_UI
337 mNav.getPCB().setChanged(PCB_CHANGED_NAV_GRID);
338 UserSettings::get().sleepForActionDelay();
339#endif
340 }
341 return -maxVisits;
342}
343
344void AStar::initCosts(const Connection &X)
345{
346 assert(mCostParams.valid());
347 const uint dirMask = (!X.hasNet() || !X.net()->isGroundOrPower()) ? 0x1 : 0x0;
348
349 mLayerCost.resize(mNav.getSize(2), 0.0f);
350 for (uint z = 0; z < mLayerCost.size(); ++z) {
351 if (!X.canRouteOnLayer(z))
352 mLayerCost[z] = mCostParams.MaskedLayer;
353 else if (z & dirMask)
354 mLayerCost[z] = mCostParams.WrongDirection;
355 else
356 mLayerCost[z] = 1.0f;
357 }
358 mViaCost = mCostParams.Via * X.defaultViaDiameter();
359 mViolationCost = mCostParams.Violation;
360
361 if (std::isinf(mCostParams.Violation))
362 mRouteMask |= NAV_POINT_FLAG_ROUTE_TRACK_CLEARANCE;
363 else
364 mRouteMask &= ~NAV_POINT_FLAG_ROUTE_TRACK_CLEARANCE;
365}
366
367#endif // GYM_PCB_ASTAR_H
Connection
Definition Connection.hpp:17
Connection::forceRouted
void forceRouted()
GridDirection
Definition GridDirection.hpp:6
NavGrid
The grid-representation of the board.
Definition NavGrid.hpp:61
NavPoint
Definition NavPoint.hpp:116
Pin
Definition Pin.hpp:18
Point_25
Definition Geometry.hpp:131
AStarCosts
Definition NavGrid.hpp:47