1 /*
2 * vim: ts=4 sw=4 et tw=0 wm=0
3 *
4 * libavoid - Fast, Incremental, Object-avoiding Line Router
5 *
6 * Copyright (C) 2004-2014 Monash University
7 *
8 * This library is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU Lesser General Public
10 * License as published by the Free Software Foundation; either
11 * version 2.1 of the License, or (at your option) any later version.
12 * See the file LICENSE.LGPL distributed with the library.
13 *
14 * Licensees holding a valid commercial license may use this file in
15 * accordance with the commercial license agreement provided with the
16 * library.
17 *
18 * This library is distributed in the hope that it will be useful,
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
21 *
22 * Author(s): Michael Wybrow
23 */
24
25 // For M_PI.
26 // This should be first include for MSVC.
27 #define _USE_MATH_DEFINES
28 #include <cmath>
29
30 #include <algorithm>
31 #include <vector>
32 #include <climits>
33 #include <cfloat>
34
35 #include "libavoid/makepath.h"
36 #include "libavoid/vertices.h"
37 #include "libavoid/geometry.h"
38 #include "libavoid/connector.h"
39 #include "libavoid/viscluster.h"
40 #include "libavoid/graph.h"
41 #include "libavoid/router.h"
42 #include "libavoid/debug.h"
43 #include "libavoid/assertions.h"
44 #include "libavoid/debughandler.h"
45
46 //#define ESTIMATED_COST_DEBUG
47
48 namespace Avoid {
49
50 class ANode
51 {
52 public:
53 VertInf* inf;
54 double g; // Gone
55 double h; // Heuristic
56 double f; // Formula f = g + h
57
58 ANode *prevNode; // VertInf for the previous ANode.
59 int timeStamp; // Time-stamp used to determine exploration order of
60 // seemingly equal paths during orthogonal routing.
61
ANode(VertInf * vinf,int time)62 ANode(VertInf *vinf, int time)
63 : inf(vinf),
64 g(0),
65 h(0),
66 f(0),
67 prevNode(nullptr),
68 timeStamp(time)
69 {
70 }
ANode()71 ANode()
72 : inf(nullptr),
73 g(0),
74 h(0),
75 f(0),
76 prevNode(nullptr),
77 timeStamp(-1)
78 {
79 }
80 };
81
82 class AStarPathPrivate
83 {
84 public:
AStarPathPrivate()85 AStarPathPrivate()
86 : m_available_nodes(),
87 m_available_array_size(0),
88 m_available_array_index(0),
89 m_available_node_index(0)
90 {
91 }
~AStarPathPrivate()92 ~AStarPathPrivate()
93 {
94 // Free memory
95 for (size_t i = 0; i < m_available_nodes.size(); ++i)
96 {
97 delete[] m_available_nodes[i];
98 }
99 }
100 // Returns a pointer to an ANode for aStar search, but allocates
101 // these in blocks
newANode(const ANode & node,const bool addToPending=true)102 ANode *newANode(const ANode& node, const bool addToPending = true)
103 {
104 const size_t blockSize = 5000;
105 if ((m_available_array_index + 1 > m_available_array_size) ||
106 (m_available_node_index >= blockSize))
107 {
108 m_available_nodes.push_back(new ANode[blockSize]);
109 ++m_available_array_size;
110 m_available_node_index = 0;
111 m_available_array_index = m_available_array_size - 1;
112 }
113
114 ANode *nodes = m_available_nodes[m_available_array_index];
115 ANode *newNode = &(nodes[m_available_node_index++]);
116 *newNode = node;
117 if (addToPending)
118 {
119 node.inf->aStarPendingNodes.push_back(newNode);
120 }
121 return newNode;
122 }
123 void search(ConnRef *lineRef, VertInf *src, VertInf *tar,
124 VertInf *start);
125
126 private:
127 void determineEndPointLocation(double dist, VertInf *start,
128 VertInf *target, VertInf *other, int level);
129 double estimatedCost(ConnRef *lineRef, const Point *last,
130 const Point& curr) const;
131
132 std::vector<ANode *> m_available_nodes;
133 size_t m_available_array_size;
134 size_t m_available_array_index;
135 size_t m_available_node_index;
136
137 // For determining estimated cost target.
138 std::vector<VertInf *> m_cost_targets;
139 std::vector<unsigned int> m_cost_targets_directions;
140 std::vector<double> m_cost_targets_displacements;
141 };
142
143
144
145 // This returns the opposite result (>) so that when used with stl::make_heap,
146 // the head node of the heap will be the smallest value, rather than the
147 // largest. This saves us from having to sort the heap (and then reorder
148 // it back into a heap) when getting the next node to examine. This way we
149 // get better complexity -- logarithmic pushes and pops to the heap.
150 //
151 class ANodeCmp
152 {
153 public:
ANodeCmp()154 ANodeCmp()
155 {
156 }
operator ()(const ANode * a,const ANode * b)157 bool operator()(const ANode *a, const ANode *b)
158 {
159 // We need to use an epsilon here since otherwise the multiple addition
160 // of floating point numbers that makes up the 'f' values cause a problem
161 // with routings occasionally being non-deterministic.
162 if (fabs(a->f - b->f) > 0.0000001)
163 {
164 return a->f > b->f;
165 }
166 if (a->timeStamp != b->timeStamp)
167 {
168 // Tiebreaker, if two paths have equal cost, then choose the one with
169 // the highest timeStamp. This corresponds to the furthest point
170 // explored along the straight-line path. When exploring we give the
171 // directions the following timeStamps; left:1, right:2 and forward:3,
172 // then we always try to explore forward first.
173 return a->timeStamp < b->timeStamp;
174 }
175 return false;
176 }
177 };
178
179
Dot(const Point & l,const Point & r)180 static double Dot(const Point& l, const Point& r)
181 {
182 return (l.x * r.x) + (l.y * r.y);
183 }
184
CrossLength(const Point & l,const Point & r)185 static double CrossLength(const Point& l, const Point& r)
186 {
187 return (l.x * r.y) - (l.y * r.x);
188 }
189
190
191 // Return the angle between the two line segments made by the
192 // points p1--p2 and p2--p3. Return value is in radians.
193 //
angleBetween(const Point & p1,const Point & p2,const Point & p3)194 static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
195 {
196 if ((p1.x == p2.x && p1.y == p2.y) || (p2.x == p3.x && p2.y == p3.y))
197 {
198 // If two of the points are the same, then we can't say anything
199 // about the angle between. Treat them as being collinear.
200 return M_PI;
201 }
202
203 Point v1(p1.x - p2.x, p1.y - p2.y);
204 Point v2(p3.x - p2.x, p3.y - p2.y);
205
206 return fabs(atan2(CrossLength(v1, v2), Dot(v1, v2)));
207 }
208
209
210 // Construct a temporary Polygon path given several VertInf's for a connector.
211 //
constructPolygonPath(Polygon & connRoute,VertInf * inf2,VertInf * inf3,ANode * inf1Node)212 static void constructPolygonPath(Polygon& connRoute, VertInf *inf2,
213 VertInf *inf3, ANode *inf1Node)
214 {
215 // Don't include colinear points.
216 bool simplified = true;
217
218 int routeSize = 2;
219 for (ANode *curr = inf1Node; curr != nullptr; curr = curr->prevNode)
220 {
221 routeSize += 1;
222 }
223 connRoute.ps.resize(routeSize);
224 int arraySize = routeSize;
225 connRoute.ps[routeSize - 1] = inf3->point;
226 connRoute.ps[routeSize - 2] = inf2->point;
227 routeSize -= 3;
228 for (ANode *curr = inf1Node; curr != nullptr; curr = curr->prevNode)
229 {
230 // For connection pins, we stop and don't include the fake shape
231 // center as part of this path.
232 bool isConnectionPin = curr->inf->id.isConnectionPin();
233
234 if (!simplified)
235 {
236 // If this is non-simplified, we don't need to do anything
237 // clever and can simply add the new point.
238 connRoute.ps[routeSize] = curr->inf->point;
239 routeSize -= 1;
240
241 if (isConnectionPin)
242 {
243 // Stop at the connection pin.
244 break;
245 }
246 continue;
247 }
248
249 if ((curr == inf1Node) ||
250 vecDir(curr->inf->point, connRoute.ps[routeSize + 1],
251 connRoute.ps[routeSize + 2]) != 0)
252 {
253 // Add new point if this is the earlier than the last segment
254 // and it is not colinear with the other points.
255 // Note, you can't collapse the 'last' segment with previous
256 // segments, or if this just intersects another line you risk
257 // penalising it once for each collapsed line segment.
258 connRoute.ps[routeSize] = curr->inf->point;
259 routeSize -= 1;
260 }
261 else
262 {
263 // The last point is inline with this one, so update it.
264 connRoute.ps[routeSize + 1] = curr->inf->point;
265 }
266
267 if (isConnectionPin)
268 {
269 // Stop at the connection pin.
270 break;
271 }
272 }
273
274 // If the vector is not filled, move entries to the beginning and
275 // remove the unused end of the vector.
276 int diff = routeSize + 1;
277 COLA_ASSERT(simplified || (diff == 0));
278 if (diff > 0)
279 {
280 for (int i = diff; i < arraySize; ++i)
281 {
282 connRoute.ps[i - diff] = connRoute.ps[i];
283 }
284 connRoute.ps.resize(connRoute.size() - diff);
285 }
286 }
287
288 // Used to get an indication of if a diffence is positive (1),
289 // negative (-1) or no different (0).
dimDirection(double difference)290 static inline int dimDirection(double difference)
291 {
292 if (difference > 0)
293 {
294 return 1;
295 }
296 else if (difference < 0)
297 {
298 return -1;
299 }
300 return 0;
301 }
302
303 // Given the two points for a new segment of a path (inf2 & inf3)
304 // as well as the distance between these points (dist), as well as
305 // possibly the previous point (inf1) [from inf1--inf2], return a
306 // cost associated with this route.
307 //
cost(ConnRef * lineRef,const double dist,VertInf * inf2,VertInf * inf3,ANode * inf1Node)308 static double cost(ConnRef *lineRef, const double dist, VertInf *inf2,
309 VertInf *inf3, ANode *inf1Node)
310 {
311 bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);
312 VertInf *inf1 = (inf1Node) ? inf1Node->inf : nullptr;
313 double result = dist;
314 Polygon connRoute;
315
316 Router *router = inf2->_router;
317 if (inf1 != nullptr)
318 {
319 const double angle_penalty = router->routingParameter(anglePenalty);
320 const double segmt_penalty = router->routingParameter(segmentPenalty);
321
322 // This is not the first segment, so there is a bend
323 // between it and the last one in the existing path.
324 if ((angle_penalty > 0) || (segmt_penalty > 0))
325 {
326 Point p1 = inf1->point;
327 Point p2 = inf2->point;
328 Point p3 = inf3->point;
329
330 double rad = M_PI - angleBetween(p1, p2, p3);
331
332 if ((rad > 0) && !isOrthogonal)
333 {
334 // Make `xval' between 0--10 then take its log so small
335 // angles are not penalised as much as large ones.
336 //
337 double xval = rad * 10 / M_PI;
338 double yval = xval * log10(xval + 1) / 10.5;
339 result += (angle_penalty * yval);
340 //db_printf("deg from straight: %g\tpenalty: %g\n",
341 // rad * 180 / M_PI, (angle_penalty * yval));
342 }
343
344 if (rad == M_PI)
345 {
346 // Needs to double back
347 result += (2 * segmt_penalty);
348 }
349 else if (rad > 0)
350 {
351 // Only penalise as an extra segment if the two
352 // segments are not collinear.
353 result += segmt_penalty;
354 }
355 }
356 }
357
358 const double cluster_crossing_penalty =
359 router->routingParameter(clusterCrossingPenalty);
360 // XXX: Clustered routing doesn't yet work with orthogonal connectors.
361 if (router->ClusteredRouting && !router->clusterRefs.empty() &&
362 (cluster_crossing_penalty > 0))
363 {
364 if (connRoute.empty())
365 {
366 constructPolygonPath(connRoute, inf2, inf3, inf1Node);
367 }
368 // There are clusters so do cluster routing.
369 for (ClusterRefList::const_iterator cl = router->clusterRefs.begin();
370 cl != router->clusterRefs.end(); ++cl)
371 {
372 Polygon cBoundary = (isOrthogonal) ?
373 (*cl)->rectangularPolygon() : (*cl)->polygon();
374 if (cBoundary.size() <= 2)
375 {
376 continue;
377 }
378 COLA_ASSERT(cBoundary.ps[0] != cBoundary.ps[cBoundary.size() - 1]);
379 for (size_t j = 0; j < cBoundary.size(); ++j)
380 {
381 // Non-orthogonal cluster boundary points should correspond to
382 // shape vertices and hence already be in the list of vertices.
383 COLA_ASSERT(isOrthogonal ||
384 router->vertices.getVertexByPos(cBoundary.at(j)));
385 }
386
387 bool isConn = false;
388 Polygon dynamic_conn_route(connRoute);
389 const bool finalSegment = (inf3 == lineRef->dst());
390 ConnectorCrossings cross(cBoundary, isConn, dynamic_conn_route);
391 cross.checkForBranchingSegments = true;
392 cross.countForSegment(connRoute.size() - 1, finalSegment);
393
394 result += (cross.crossingCount * cluster_crossing_penalty);
395 }
396 }
397
398 // This penalty penalises route segments that head in a direction opposite
399 // of the direction(s) toward the target point.
400 const double reversePenalty = router->routingParameter(
401 reverseDirectionPenalty);
402 if (reversePenalty)
403 {
404 // X and Y direction of destination from source point.
405 const Point& srcPoint = lineRef->src()->point;
406 const Point& dstPoint = lineRef->dst()->point;
407 int xDir = dimDirection(dstPoint.x - srcPoint.x);
408 int yDir = dimDirection(dstPoint.y - srcPoint.y);
409
410 bool doesReverse = false;
411
412 if ((xDir != 0) &&
413 (-xDir == dimDirection(inf3->point.x - inf2->point.x)))
414 {
415 // Connector has an X component and the segment heads in the
416 // opposite direction.
417 doesReverse |= true;
418 }
419
420 if ((yDir != 0) &&
421 (-yDir == dimDirection(inf3->point.y - inf2->point.y)))
422 {
423 // Connector has an Y component and the segment heads in the
424 // opposite direction.
425 doesReverse |= true;
426 }
427
428 if (doesReverse)
429 {
430 result += reversePenalty;
431 }
432 }
433
434 if (!router->isInCrossingPenaltyReroutingStage())
435 {
436 // Return here if we are not in the post-processing stage
437 return result;
438 }
439
440 const double crossing_penalty = router->routingParameter(crossingPenalty);
441 const double shared_path_penalty =
442 router->routingParameter(fixedSharedPathPenalty);
443 if ((shared_path_penalty > 0) || (crossing_penalty > 0))
444 {
445 if (connRoute.empty())
446 {
447 constructPolygonPath(connRoute, inf2, inf3, inf1Node);
448 }
449 ConnRefList::const_iterator curr, finish = router->connRefs.end();
450 for (curr = router->connRefs.begin(); curr != finish; ++curr)
451 {
452 ConnRef *connRef = *curr;
453
454 if (connRef->id() == lineRef->id())
455 {
456 continue;
457 }
458 const Avoid::PolyLine& route2 = connRef->displayRoute();
459
460 bool isConn = true;
461 Polygon dynamic_route2(route2);
462 Polygon dynamic_conn_route(connRoute);
463 const bool finalSegment = (inf3->point == lineRef->dst()->point);
464 ConnectorCrossings cross(dynamic_route2, isConn,
465 dynamic_conn_route, connRef, lineRef);
466 cross.checkForBranchingSegments = true;
467 cross.countForSegment(connRoute.size() - 1, finalSegment);
468
469 if ((cross.crossingFlags & CROSSING_SHARES_PATH) &&
470 (cross.crossingFlags & CROSSING_SHARES_FIXED_SEGMENT) &&
471 (router->routingOption(
472 penaliseOrthogonalSharedPathsAtConnEnds) ||
473 !(cross.crossingFlags & CROSSING_SHARES_PATH_AT_END)))
474 {
475 // Penalise unnecessary shared paths in the middle of
476 // connectors.
477 result += shared_path_penalty;
478 }
479 result += (cross.crossingCount * crossing_penalty);
480 }
481 }
482
483 return result;
484 }
485
486 // Directions for estimated orthgonal cost, as bitflags.
487 static const unsigned int CostDirectionN = 1;
488 static const unsigned int CostDirectionE = 2;
489 static const unsigned int CostDirectionS = 4;
490 static const unsigned int CostDirectionW = 8;
491
492 #ifdef ESTIMATED_COST_DEBUG
printDirections(FILE * fp,unsigned int directions)493 static void printDirections(FILE *fp, unsigned int directions)
494 {
495 if (directions & CostDirectionN)
496 {
497 fprintf(fp, "N ");
498 }
499 if (directions & CostDirectionE)
500 {
501 fprintf(fp, "E ");
502 }
503 if (directions & CostDirectionS)
504 {
505 fprintf(fp, "S ");
506 }
507 if (directions & CostDirectionW)
508 {
509 fprintf(fp, "W ");
510 }
511 }
512 #endif
513
514 // Returns the number of directions for the argument.
orthogonalDirectionsCount(const unsigned int directions)515 static unsigned int orthogonalDirectionsCount(const unsigned int directions)
516 {
517 unsigned int count = 0;
518 if (directions & CostDirectionN)
519 {
520 ++count;
521 }
522 if (directions & CostDirectionE)
523 {
524 ++count;
525 }
526 if (directions & CostDirectionS)
527 {
528 ++count;
529 }
530 if (directions & CostDirectionW)
531 {
532 ++count;
533 }
534 return count;
535 }
536
537 // Returns the directions of point b from point a.
orthogonalDirection(const Point & a,const Point & b)538 static unsigned int orthogonalDirection(const Point &a, const Point &b)
539 {
540 unsigned int result = 0;
541
542 if (b.y > a.y)
543 {
544 result |= CostDirectionS;
545 }
546 else if (b.y < a.y)
547 {
548 result |= CostDirectionN;
549 }
550
551 if (b.x > a.x)
552 {
553 result |= CostDirectionE;
554 }
555 else if (b.x < a.x)
556 {
557 result |= CostDirectionW;
558 }
559
560 return result;
561 }
562
563 // Returns the direction to the right of the given direction.
dirRight(unsigned int direction)564 static unsigned int dirRight(unsigned int direction)
565 {
566 if (direction == CostDirectionN)
567 {
568 return CostDirectionE;
569 }
570 else if (direction == CostDirectionE)
571 {
572 return CostDirectionS;
573 }
574 else if (direction == CostDirectionS)
575 {
576 return CostDirectionW;
577 }
578 else if (direction == CostDirectionW)
579 {
580 return CostDirectionN;
581 }
582
583 // Should not be possible to reach here.
584 COLA_ASSERT(false);
585 return direction;
586 }
587
588 // Returns the direction to the left of the given direction.
dirLeft(unsigned int direction)589 static unsigned int dirLeft(unsigned int direction)
590 {
591 if (direction == CostDirectionN)
592 {
593 return CostDirectionW;
594 }
595 else if (direction == CostDirectionE)
596 {
597 return CostDirectionN;
598 }
599 else if (direction == CostDirectionS)
600 {
601 return CostDirectionE;
602 }
603 else if (direction == CostDirectionW)
604 {
605 return CostDirectionS;
606 }
607
608 // Should not be possible to reach here.
609 COLA_ASSERT(false);
610 return direction;
611 }
612
613 // Returns the reverse direction to the given direction.
dirReverse(unsigned int direction)614 static unsigned int dirReverse(unsigned int direction)
615 {
616 if (direction == CostDirectionN)
617 {
618 return CostDirectionS;
619 }
620 else if (direction == CostDirectionE)
621 {
622 return CostDirectionW;
623 }
624 else if (direction == CostDirectionS)
625 {
626 return CostDirectionN;
627 }
628 else if (direction == CostDirectionW)
629 {
630 return CostDirectionE;
631 }
632
633 // Should not be possible to reach here.
634 COLA_ASSERT(false);
635 return direction;
636 }
637
638 // Given Point curr with a direction of currDir, returns the nimimum number
639 // of bends to reach Point dest with the entry direction of destDir
640 //
641 // This is used for estimating the bend penalty cost to the target point
642 // from the current point of the search. The geometry was described in the
643 // "Orthogonal Connector Routing" paper, although the version described
644 // there is incorrect.
645 //
bends(const Point & curr,unsigned int currDir,const Point & dest,unsigned int destDir)646 int bends(const Point& curr, unsigned int currDir, const Point& dest,
647 unsigned int destDir)
648 {
649 // Bend counts from 'o' to 'D' should be:
650 //
651 // 1 1 3
652 // v v v
653 // 2 > o < 2 2 > o < 2 4 > o < 2
654 // ^ ^ ^
655 // 3 3 3
656 //
657 // 0 > o < 4 D--> 4 > o < 4
658 // ^ ^
659 // 1 3
660 //
661 COLA_ASSERT(currDir != 0);
662 unsigned int currToDestDir = orthogonalDirection(curr, dest);
663 unsigned int reverseDestDir = dirReverse(destDir);
664 bool currDirPerpendicularToDestDir =
665 (currDir == dirLeft(destDir)) || (currDir == dirRight(destDir));
666
667 if ((currDir == destDir) &&
668 (currToDestDir == currDir))
669 {
670 //
671 // 0 > o D-->
672 //
673 return 0;
674 }
675 else if (currDirPerpendicularToDestDir &&
676 (currToDestDir == (destDir | currDir)))
677 {
678 //
679 // 1
680 // v
681 // o
682 //
683 //
684 // D-->
685 //
686 return 1;
687 }
688 else if (currDirPerpendicularToDestDir &&
689 (currToDestDir == currDir))
690 {
691 //
692 // 1
693 // v
694 // o
695 //
696 //
697 // D-->
698 //
699 return 1;
700 }
701 else if (currDirPerpendicularToDestDir &&
702 (currToDestDir == destDir))
703 {
704 //
705 // o D-->
706 // ^
707 // 1
708 //
709 return 1;
710 }
711 else if ((currDir == destDir) &&
712 (currToDestDir != currDir) &&
713 !(currToDestDir & reverseDestDir))
714 {
715 //
716 // 2 > o 2 > o
717 //
718 //
719 // D-->
720 //
721 return 2;
722 }
723 else if (currDir == reverseDestDir &&
724 (currToDestDir != destDir) &&
725 (currToDestDir != currDir))
726 {
727 //
728 // o < 2 o < 2 o < 2
729 //
730 //
731 // D-->
732 //
733 return 2;
734 }
735 else if (currDirPerpendicularToDestDir &&
736 (currToDestDir != (destDir | currDir)) &&
737 (currToDestDir != currDir))
738 {
739 //
740 // 3
741 // v
742 // o o o
743 // ^ ^ ^
744 // 3 3 3
745 //
746 // D--> o
747 // ^
748 // 3
749 //
750 return 3;
751 }
752 else if ((currDir == reverseDestDir) &&
753 ((currToDestDir == destDir) || (currToDestDir == currDir)))
754 {
755 //
756 //
757 //
758 // o < 4 D--> o < 4
759 //
760 return 4;
761 }
762 else if ((currDir == destDir) &&
763 (currToDestDir & reverseDestDir))
764 {
765 //
766 // 4 > o
767 //
768 //
769 // D--> 4 > o
770 //
771 return 4;
772 }
773
774 // Should not be possible to reach here.
775 COLA_ASSERT(false);
776 return 0;
777 }
778
779
estimatedCostSpecific(ConnRef * lineRef,const Point * last,const Point & curr,const VertInf * costTar,const unsigned int costTarDirs)780 static double estimatedCostSpecific(ConnRef *lineRef, const Point *last,
781 const Point& curr, const VertInf *costTar,
782 const unsigned int costTarDirs)
783 {
784 Point costTarPoint = costTar->point;
785
786 if (lineRef->routingType() == ConnType_PolyLine)
787 {
788 return euclideanDist(curr, costTarPoint);
789 }
790 else // Orthogonal
791 {
792 // Really doesn't make sense to route orthogonal paths without
793 // a segment penalty.
794 COLA_ASSERT(lineRef->router()->routingParameter(segmentPenalty) > 0);
795
796 double dist = manhattanDist(curr, costTarPoint);
797
798 int bendCount = 0;
799 double xmove = costTarPoint.x - curr.x;
800 double ymove = costTarPoint.y - curr.y;
801 if (last == nullptr)
802 {
803 // This is just the initial point. Penalise it simply if it is
804 // not inline with the target in either the x- or y-dimension.
805 if ((xmove != 0) && (ymove != 0))
806 {
807 bendCount += 1;
808 }
809 }
810 else if (dist > 0)
811 {
812 // We have two points and a non-zero distance, so we know
813 // the segment direction.
814
815 unsigned int currDir = orthogonalDirection(*last, curr);
816 if ((currDir > 0) && (orthogonalDirectionsCount(currDir) == 1))
817 {
818 // Suitably high value, then we find the minimum.
819 bendCount = 10;
820
821 // Find the minimum bent penalty given all the possible
822 // directions at the target point.
823 if (costTarDirs & CostDirectionN)
824 {
825 bendCount = std::min(bendCount,
826 bends(curr, currDir, costTarPoint, CostDirectionN));
827 }
828 if (costTarDirs & CostDirectionE)
829 {
830 bendCount = std::min(bendCount,
831 bends(curr, currDir, costTarPoint, CostDirectionE));
832 }
833 if (costTarDirs & CostDirectionS)
834 {
835 bendCount = std::min(bendCount,
836 bends(curr, currDir, costTarPoint, CostDirectionS));
837 }
838 if (costTarDirs & CostDirectionW)
839 {
840 bendCount = std::min(bendCount,
841 bends(curr, currDir, costTarPoint, CostDirectionW));
842 }
843 }
844 }
845 double penalty = bendCount *
846 lineRef->router()->routingParameter(segmentPenalty);
847
848 return dist + penalty;
849 }
850 }
851
852
853
estimatedCost(ConnRef * lineRef,const Point * last,const Point & curr) const854 double AStarPathPrivate::estimatedCost(ConnRef *lineRef, const Point *last,
855 const Point& curr) const
856 {
857 double estimate = DBL_MAX;
858 COLA_ASSERT(m_cost_targets.size() > 0);
859
860 // Find the minimum cost from the estimates to each of the possible
861 // target points from this current point.
862 for (size_t i = 0; i < m_cost_targets.size(); ++i)
863 {
864 double iEstimate = estimatedCostSpecific(lineRef, last,
865 curr, m_cost_targets[i], m_cost_targets_directions[i]);
866
867 // Add on the distance to the real target, otherwise this difference
868 // might may make the comparisons unfair if they vary between targets.
869 iEstimate += m_cost_targets_displacements[i];
870
871 estimate = std::min(estimate, iEstimate);
872 }
873 return estimate;
874 }
875
876
877 class CmpVisEdgeRotation
878 {
879 public:
CmpVisEdgeRotation(const VertInf * lastPt)880 CmpVisEdgeRotation(const VertInf* lastPt)
881 : _lastPt(lastPt)
882 {
883 }
operator ()(const EdgeInf * u,const EdgeInf * v) const884 bool operator() (const EdgeInf* u, const EdgeInf* v) const
885 {
886 // Dummy ShapeConnectionPin edges are not orthogonal and
887 // therefore can't be compared in the same way.
888 if (u->isOrthogonal() && v->isOrthogonal())
889 {
890 return u->rotationLessThan(_lastPt, v);
891 }
892 return u < v;
893 }
894 private:
895 const VertInf *_lastPt;
896 };
897
898
pointAlignedWithOneOf(const Point & point,const std::vector<Point> & points,const size_t dim)899 static inline bool pointAlignedWithOneOf(const Point& point,
900 const std::vector<Point>& points, const size_t dim)
901 {
902 for (size_t i = 0; i < points.size(); ++i)
903 {
904 if (point[dim] == points[i][dim])
905 {
906 return true;
907 }
908 }
909 return false;
910 }
911
AStarPath(void)912 AStarPath::AStarPath(void)
913 : m_private(new AStarPathPrivate())
914 {
915 }
916
~AStarPath(void)917 AStarPath::~AStarPath(void)
918 {
919 delete m_private;
920 }
921
search(ConnRef * lineRef,VertInf * src,VertInf * tar,VertInf * start)922 void AStarPath::search(ConnRef *lineRef, VertInf *src, VertInf *tar, VertInf *start)
923 {
924 m_private->search(lineRef, src, tar, start);
925 }
926
determineEndPointLocation(double dist,VertInf * start,VertInf * target,VertInf * other,int level)927 void AStarPathPrivate::determineEndPointLocation(double dist, VertInf *start,
928 VertInf *target, VertInf *other, int level)
929 {
930 COLA_UNUSED(dist);
931 COLA_UNUSED(start);
932 COLA_UNUSED(level);
933
934 Point otherPoint = other->point;
935 unsigned int thisDirs = orthogonalDirection(otherPoint, target->point);
936 COLA_ASSERT(orthogonalDirectionsCount(thisDirs) > 0);
937 double displacement = manhattanDist(otherPoint, target->point);
938
939 m_cost_targets.push_back(other);
940 m_cost_targets_directions.push_back(thisDirs);
941 m_cost_targets_displacements.push_back(displacement);
942
943 #ifdef ESTIMATED_COST_DEBUG
944 fprintf(stderr," - %g %g ", otherPoint.x, otherPoint.y);
945 if (manhattanDist(start->point, otherPoint) > dist)
946 {
947 fprintf(stderr,"far ");
948 }
949 fprintf(stderr, "%s", (level == 1) ? "--" : "- ");
950 printDirections(stderr, thisDirs);
951 fprintf(stderr,"\n");
952 #endif
953 }
954
955 // Returns the best path from src to tar using the cost function.
956 //
957 // The path is worked out using the aStar algorithm, and is encoded via
958 // prevNode values for each ANode which point back to the previous ANode.
959 // At completion, this order is written into the pathNext links in each
960 // of the VerInfs along the path.
961 //
962 // The aStar STL code is originally based on public domain code available
963 // on the internet.
964 //
search(ConnRef * lineRef,VertInf * src,VertInf * tar,VertInf * start)965 void AStarPathPrivate::search(ConnRef *lineRef, VertInf *src, VertInf *tar, VertInf *start)
966 {
967 ANodeCmp pendingCmp;
968
969 bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);
970
971 if (start == nullptr)
972 {
973 start = src;
974 }
975
976 #ifdef DEBUGHANDLER
977 if (lineRef->router()->debugHandler())
978 {
979 lineRef->router()->debugHandler()->beginningSearchWithEndpoints(start, tar);
980 }
981 #endif
982
983 // Find a target point to use for cost estimate for orthogonal routing.
984 //
985 // If the connectivity is only on the far side we need to estimate to the
986 // point on the far side. Otherwise for orthogonal routing we can explore
987 // all the space in between before we pay the extra cost to explore this
988 // area. This is especially true given many orthogonal routes have
989 // equivalent costs.
990 #ifdef ESTIMATED_COST_DEBUG
991 fprintf(stderr,"== aStar %g %g ==\n", tar->point.x, tar->point.y);
992 #endif
993 if (isOrthogonal && tar->id.isConnPt() && !tar->id.isConnCheckpoint())
994 {
995 // The target is a connector endpoint and the connector is orthogonal.
996 double dist = manhattanDist(start->point, tar->point);
997 for (EdgeInfList::const_iterator it = tar->orthogVisList.begin();
998 it != tar->orthogVisList.end(); ++it)
999 {
1000 // For each edge from the target endpoint, find the other vertex.
1001 EdgeInf *edge = *it;
1002 VertInf *other = edge->otherVert(tar);
1003 if (other->id.isConnectionPin())
1004 {
1005 // If this is a connection pin we need to do this process
1006 // another time since the current edge will be a dummy
1007 // zero-length edge.
1008 VertInf *replacementTar = other;
1009 for (EdgeInfList::const_iterator it =
1010 replacementTar->orthogVisList.begin();
1011 it != replacementTar->orthogVisList.end(); ++it)
1012 {
1013 EdgeInf *edge = *it;
1014 VertInf *other = edge->otherVert(replacementTar);
1015 if ((other == tar) ||
1016 (other->point == tar->point))
1017 {
1018 // Ignore edge we came from, or zero-length edges.
1019 continue;
1020 }
1021
1022 // Determine possible target endpoint directions and
1023 // position.
1024 determineEndPointLocation(dist, start, replacementTar,
1025 other, 2);
1026 }
1027 continue;
1028 }
1029
1030 // Determine possible target endpoint directions and position.
1031 determineEndPointLocation(dist, start, tar, other, 1);
1032 }
1033 }
1034
1035
1036 if (m_cost_targets.empty())
1037 {
1038 m_cost_targets.push_back(tar);
1039 // For polyline routing, assume target has visibility is all
1040 // directions for the purpose of cost estimations.
1041 m_cost_targets_directions.push_back(CostDirectionN |
1042 CostDirectionE | CostDirectionS | CostDirectionW);
1043 m_cost_targets_displacements.push_back(0.0);
1044 }
1045
1046 #ifdef ESTIMATED_COST_DEBUG
1047 fprintf(stderr, "------------\n");
1048 for (size_t i = 0; i < m_cost_targets.size(); ++i)
1049 {
1050 fprintf(stderr,"== %g %g - ", m_cost_targets[i]->point.x,
1051 m_cost_targets[i]->point.y);
1052 printDirections(stderr, m_cost_targets_directions[i]);
1053 fprintf(stderr,"\n");
1054 }
1055 #endif
1056
1057
1058 double (*dist)(const Point& a, const Point& b) =
1059 (isOrthogonal) ? manhattanDist : euclideanDist;
1060
1061 // We need to know the possible endpoints for doing an orthogonal
1062 // routing optimisation where we only turn when we are heading beside
1063 // a shape or are in line with a possible endpoint.
1064 std::vector<Point> endPoints;
1065 if (isOrthogonal)
1066 {
1067 endPoints = lineRef->possibleDstPinPoints();
1068 }
1069 endPoints.push_back(tar->point);
1070
1071 // Heap of PENDING nodes.
1072 std::vector<ANode *> PENDING;
1073 PENDING.reserve(1000);
1074
1075 size_t exploredCount = 0;
1076 ANode node, ati;
1077 ANode *bestNode = nullptr; // Temporary bestNode
1078 bool bNodeFound = false; // Flag if node is found in container
1079 int timestamp = 1;
1080
1081 Router *router = lineRef->router();
1082 if (router->RubberBandRouting && (start != src))
1083 {
1084 COLA_ASSERT(router->IgnoreRegions == true);
1085
1086 const PolyLine& currRoute = lineRef->route();
1087 VertInf *last = nullptr;
1088 int rIndx = 0;
1089 while (last != start)
1090 {
1091 const Point& pnt = currRoute.at(rIndx);
1092 VertIDProps props = (rIndx > 0) ? 0 : VertID::PROP_ConnPoint;
1093 VertID vID(pnt.id, pnt.vn, props);
1094
1095 #ifdef PATHDEBUG
1096 db_printf("/// %d %d\n", pnt.id, pnt.vn);
1097 #endif
1098 VertInf *curr = router->vertices.getVertexByID(vID);
1099 COLA_ASSERT(curr != nullptr);
1100
1101 node = ANode(curr, timestamp++);
1102 if (!last)
1103 {
1104 node.inf = src;
1105 node.g = 0;
1106 node.h = estimatedCost(lineRef, nullptr, node.inf->point);
1107
1108 node.f = node.g + node.h;
1109 }
1110 else
1111 {
1112 double edgeDist = dist(bestNode->inf->point, curr->point);
1113
1114 node.g = bestNode->g + cost(lineRef, edgeDist, bestNode->inf,
1115 node.inf, bestNode->prevNode);
1116
1117 // Calculate the Heuristic.
1118 node.h = estimatedCost(lineRef, &(bestNode->inf->point),
1119 node.inf->point);
1120
1121 // The A* formula
1122 node.f = node.g + node.h;
1123
1124 // Point parent to last bestNode
1125 node.prevNode = bestNode;
1126 }
1127
1128 if (curr != start)
1129 {
1130 bool addToPending = false;
1131 bestNode = newANode(node, addToPending);
1132 bestNode->inf->aStarDoneNodes.push_back(bestNode);
1133 ++exploredCount;
1134 }
1135 else
1136 {
1137 ANode * newNode = newANode(node);
1138 PENDING.push_back(newNode);
1139 }
1140
1141 rIndx++;
1142 last = curr;
1143 }
1144 }
1145 else
1146 {
1147 if (start->pathNext)
1148 {
1149 // If we are doing checkpoint routing and have already done one
1150 // path, then we have an existing segment to consider for the
1151 // cost of the choice from the start node, so we add a dummy
1152 // nodes as if they were already in the Done set. This causes
1153 // us to first search in a collinear direction from the previous
1154 // segment.
1155 bool addToPending = false;
1156 bestNode = newANode(ANode(start->pathNext, timestamp++),
1157 addToPending);
1158 bestNode->inf->aStarDoneNodes.push_back(bestNode);
1159 ++exploredCount;
1160 }
1161
1162 // Create the start node
1163 node = ANode(src, timestamp++);
1164 node.g = 0;
1165 node.h = estimatedCost(lineRef, nullptr, node.inf->point);
1166 node.f = node.g + node.h;
1167 // Set a nullptr parent, so cost function knows this is the first segment.
1168 node.prevNode = bestNode;
1169
1170 // Populate the PENDING container with the first location
1171 ANode *newNode = newANode(node);
1172 PENDING.push_back(newNode);
1173 }
1174
1175 tar->pathNext = nullptr;
1176
1177 // Create a heap from PENDING for sorting
1178 using std::make_heap; using std::push_heap; using std::pop_heap;
1179 make_heap( PENDING.begin(), PENDING.end(), pendingCmp);
1180
1181 // Continue until the queue is empty.
1182 while (!PENDING.empty())
1183 {
1184 TIMER_VAR_ADD(router, 0, 1);
1185 // Set the Node with lowest f value to BESTNODE.
1186 // Since the ANode operator< is reversed, the head of the
1187 // heap is the node with the lowest f value.
1188 bestNode = PENDING.front();
1189 VertInf *bestNodeInf = bestNode->inf;
1190
1191 #ifdef DEBUGHANDLER
1192 if (router->debugHandler())
1193 {
1194 PolyLine currentSearchPath;
1195
1196 ANode *curr = bestNode;
1197 while (curr)
1198 {
1199 currentSearchPath.ps.push_back(curr->inf->point);
1200 curr = curr->prevNode;
1201 }
1202 router->debugHandler()->updateCurrentSearchPath(currentSearchPath);
1203 }
1204 #endif
1205
1206 // Remove this node from the aStarPendingList
1207 std::list<ANode *>::iterator finishIt =
1208 bestNodeInf->aStarPendingNodes.end();
1209 for (std::list<ANode *>::iterator currInd =
1210 bestNodeInf->aStarPendingNodes.begin(); currInd != finishIt;
1211 ++currInd)
1212 {
1213 if (*currInd == bestNode)
1214 {
1215 bestNodeInf->aStarPendingNodes.erase(currInd);
1216 break;
1217 }
1218 }
1219
1220 // Pop off the heap. Actually this moves the
1221 // far left value to the far right. The node
1222 // is not actually removed since the pop is to
1223 // the heap and not the container.
1224 pop_heap(PENDING.begin(), PENDING.end(), pendingCmp);
1225 // Remove node from right (the value we pop_heap'd)
1226 PENDING.pop_back();
1227
1228 // Add the bestNode into the Done set.
1229 bestNodeInf->aStarDoneNodes.push_back(bestNode);
1230 ++exploredCount;
1231
1232 VertInf *prevInf = (bestNode->prevNode) ? bestNode->prevNode->inf : nullptr;
1233 #ifdef ASTAR_DEBUG
1234 db_printf("Considering... ");
1235 db_printf(" %g %g ", bestNodeInf->point.x, bestNodeInf->point.y);
1236 bestNodeInf->id.db_print();
1237 db_printf(" - g: %3.1f h: %3.1f back: ", bestNode->g, bestNode->h);
1238 if (prevInf)
1239 {
1240 db_printf(" %g %g", prevInf->point.x, prevInf->point.y);
1241 //prevInf->id.db_print();
1242 }
1243 db_printf("\n");
1244 #endif
1245
1246 if (bestNodeInf == tar)
1247 {
1248 TIMER_VAR_ADD(router, 1, PENDING.size());
1249 // This node is our goal.
1250 #ifdef ASTAR_DEBUG
1251 db_printf("LINE %10d Steps: %4d Cost: %g\n", lineRef->id(),
1252 (int) exploredCount, bestNode->f);
1253 #endif
1254
1255 // Correct all the pathNext pointers.
1256 for (ANode *curr = bestNode; curr->prevNode; curr = curr->prevNode)
1257 {
1258 #ifdef ASTAR_DEBUG
1259 db_printf("[%.12f, %.12f]\n", curr->inf->point.x, curr->inf->point.y);
1260 #endif
1261 curr->inf->pathNext = curr->prevNode->inf;
1262 }
1263 #ifdef ASTAR_DEBUG
1264 db_printf("\n");
1265 #endif
1266
1267 // Exit from the search
1268 break;
1269 }
1270
1271 // Check adjacent points in graph and add them to the queue.
1272 EdgeInfList& visList = (!isOrthogonal) ?
1273 bestNodeInf->visList : bestNodeInf->orthogVisList;
1274 if (isOrthogonal)
1275 {
1276 // We would like to explore in a structured way,
1277 // so sort the points in the visList...
1278 CmpVisEdgeRotation compare(prevInf);
1279 visList.sort(compare);
1280 }
1281 EdgeInfList::const_iterator finish = visList.end();
1282 for (EdgeInfList::const_iterator edge = visList.begin();
1283 edge != finish; ++edge)
1284 {
1285 if ((*edge)->isDisabled())
1286 {
1287 // Skip disabled edges.
1288 continue;
1289 }
1290
1291 node = ANode((*edge)->otherVert(bestNodeInf), timestamp++);
1292
1293 // Set the index to the previous ANode that we reached
1294 // this ANode via.
1295 node.prevNode = bestNode;
1296
1297 VertInf *prevInf = (bestNode->prevNode) ?
1298 bestNode->prevNode->inf : nullptr;
1299
1300 // Don't bother looking at the segment we just arrived along.
1301 if (prevInf && (prevInf == node.inf))
1302 {
1303 continue;
1304 }
1305 if (node.inf->id.isConnectionPin() &&
1306 !node.inf->id.isConnCheckpoint())
1307 {
1308 if ( !( (bestNodeInf == lineRef->src()) &&
1309 lineRef->src()->id.isDummyPinHelper()
1310 ) &&
1311 !( node.inf->hasNeighbour(lineRef->dst(), isOrthogonal) &&
1312 lineRef->dst()->id.isDummyPinHelper())
1313 )
1314 {
1315 // Don't check connection pins if they don't have the
1316 // target vertex as a direct neighbour, or are directly
1317 // leaving the source vertex.
1318 continue;
1319 }
1320 }
1321 else if (node.inf->id.isConnPt())
1322 {
1323 if ((node.inf != tar))
1324 {
1325 // Don't check connector endpoints vertices unless they
1326 // are the target endpoint.
1327 continue;
1328 }
1329 }
1330
1331 if (isOrthogonal && !(*edge)->isDummyConnection())
1332 {
1333 // Orthogonal routing optimisation.
1334 // Skip the edges that don't lead to shape edges, or the
1335 // connection point we are looking for. Though allow them
1336 // if we haven't yet turned from the source point, since it
1337 // may be a free-floating endpoint with directional visibility.
1338 // Also, don't check if the previous point was a dummy for a
1339 // connection pin and this happens to be placed diagonally
1340 // from here, i.e., when both of notInline{X,Y} are true.
1341 Point& bestPt = bestNodeInf->point;
1342 Point& nextPt = node.inf->point;
1343
1344 bool notInlineX = prevInf && (prevInf->point.x != bestPt.x);
1345 bool notInlineY = prevInf && (prevInf->point.y != bestPt.y);
1346 if ((bestPt.x == nextPt.x) && notInlineX && !notInlineY &&
1347 (bestPt[YDIM] != src->point[YDIM]))
1348 {
1349 if (nextPt.y < bestPt.y)
1350 {
1351 if (!(bestNodeInf->orthogVisPropFlags & YL_EDGE) &&
1352 !pointAlignedWithOneOf(bestPt, endPoints, XDIM))
1353 {
1354 continue;
1355 }
1356 }
1357 else if (nextPt.y > bestPt.y)
1358 {
1359 if (!(bestNodeInf->orthogVisPropFlags & YH_EDGE) &&
1360 !pointAlignedWithOneOf(bestPt, endPoints, XDIM))
1361 {
1362 continue;
1363 }
1364 }
1365 }
1366 if ((bestPt.y == nextPt.y) && notInlineY && !notInlineX &&
1367 (bestPt[XDIM] != src->point[XDIM]))
1368 {
1369 if (nextPt.x < bestPt.x)
1370 {
1371 if (!(bestNodeInf->orthogVisPropFlags & XL_EDGE) &&
1372 !pointAlignedWithOneOf(bestPt, endPoints, YDIM))
1373 {
1374 continue;
1375 }
1376 }
1377 else if (nextPt.x > bestPt.x)
1378 {
1379 if (!(bestNodeInf->orthogVisPropFlags & XH_EDGE) &&
1380 !pointAlignedWithOneOf(bestPt, endPoints, YDIM))
1381 {
1382 continue;
1383 }
1384 }
1385 }
1386 }
1387
1388 double edgeDist = (*edge)->getDist();
1389
1390 if (edgeDist == 0)
1391 {
1392 continue;
1393 }
1394
1395 if (!isOrthogonal &&
1396 (!router->RubberBandRouting || (start == src)) &&
1397 (validateBendPoint(prevInf, bestNodeInf, node.inf) == false))
1398 {
1399 // The bendpoint is not valid, i.e., is a zigzag corner, so...
1400 continue;
1401 // For RubberBand routing we want to allow these routes and
1402 // unwind them later, otherwise instead or unwinding, paths
1403 // can go the *really* long way round.
1404 }
1405
1406 // Figure out if we are at one of the cost targets.
1407 bool atCostTarget = false;
1408 for (size_t i = 0; i < m_cost_targets.size(); ++i)
1409 {
1410 if (bestNode->inf == m_cost_targets[i])
1411
1412 {
1413 atCostTarget = true;
1414 break;
1415 }
1416 }
1417
1418 if (atCostTarget &&
1419 (node.inf->id.isConnectionPin() || (node.inf == tar)))
1420 {
1421 // This is a point on the side of an obstacle that connects
1422 // to the target or a connection pin. It should have no
1423 // further cost and the heuristic should be zero.
1424 node.g = bestNode->g;
1425 node.h = 0;
1426 }
1427 else
1428 {
1429 if (node.inf == tar)
1430 {
1431 // We've reached the target. The heuristic should be zero.
1432 node.h = 0;
1433 }
1434 else
1435 {
1436 // Otherwise, calculate the heuristic value.
1437 node.h = estimatedCost(lineRef, &(bestNodeInf->point),
1438 node.inf->point);
1439 }
1440
1441 if (node.inf->id.isDummyPinHelper())
1442 {
1443 // This is connecting to a connection pin helper vertex.
1444 // There should be no additional cost for this step.
1445 node.g = bestNode->g;
1446 }
1447 else
1448 {
1449 // Otherwise, calculate the cost of this step.
1450 node.g = bestNode->g + cost(lineRef, edgeDist, bestNodeInf,
1451 node.inf, bestNode->prevNode);
1452 }
1453 }
1454
1455 // The A* formula
1456 node.f = node.g + node.h;
1457
1458 #ifdef ASTAR_DEBUG
1459 db_printf("-- Adding: %g %g ", node.inf->point.x,
1460 node.inf->point.y);
1461 node.inf->id.db_print();
1462 db_printf(" - g: %3.1f h: %3.1f \n", node.g, node.h);
1463 #endif
1464
1465 bNodeFound = false;
1466
1467
1468 // Check to see if already on PENDING
1469 std::list<ANode *>::const_iterator finish = node.inf->aStarPendingNodes.end();
1470 for (std::list<ANode *>::const_iterator currInd =
1471 node.inf->aStarPendingNodes.begin(); currInd != finish; ++currInd)
1472 {
1473 ati = **currInd;
1474 // The (node.prevNode == ati.prevNode) is redundant, but may
1475 // save checking the mosre costly prevNode->inf test if the
1476 // Nodes are the same.
1477 if ((node.inf == ati.inf) &&
1478 ((node.prevNode == ati.prevNode) ||
1479 (node.prevNode->inf == ati.prevNode->inf)))
1480 {
1481 // If already on PENDING
1482 if (node.g < ati.g)
1483 {
1484 // Replace the existing node in PENDING
1485 **currInd = node;
1486 make_heap( PENDING.begin(), PENDING.end(), pendingCmp);
1487 }
1488 bNodeFound = true;
1489 break;
1490 }
1491 }
1492 if ( !bNodeFound ) // If Node NOT found on PENDING
1493 {
1494 // Check to see if it is already in the Done set for this
1495 // vertex.
1496 for (std::list<ANode *>::const_iterator currInd =
1497 node.inf->aStarDoneNodes.begin();
1498 currInd != node.inf->aStarDoneNodes.end(); ++currInd)
1499 {
1500 ati = **currInd;
1501 // The (node.prevNode == ati.prevNode) is redundant, but may
1502 // save checking the mosre costly prevNode->inf test if the
1503 // Nodes are the same.
1504 if ((node.inf == ati.inf) && ati.prevNode &&
1505 ((node.prevNode == ati.prevNode) ||
1506 (node.prevNode->inf == ati.prevNode->inf)))
1507 {
1508 //COLA_ASSERT(node.g >= (ati.g - 10e-10));
1509 // This node is already in the Done set and the
1510 // current node also has a higher g-value, so we
1511 // don't need to consider this node.
1512 bNodeFound = true;
1513 break;
1514 }
1515 }
1516 }
1517
1518 if (!bNodeFound ) // If Node NOT in either Pending or Done.
1519 {
1520 // Push NewNode onto PENDING
1521 ANode *newNode = newANode(node);
1522 PENDING.push_back(newNode);
1523 // Push NewNode onto heap
1524 push_heap( PENDING.begin(), PENDING.end(), pendingCmp);
1525
1526 #if 0
1527 using std::cout; using std::endl;
1528 // Display PENDING container (For Debugging)
1529 cout << "PENDING: ";
1530 for (unsigned int i = 0; i < PENDING.size(); i++)
1531 {
1532 cout << PENDING[i]->g << "," << PENDING[i]->h << ",";
1533 cout << PENDING[i]->inf << "," << PENDING[i]->pp << " ";
1534 }
1535 cout << endl << endl;
1536 #endif
1537 }
1538 }
1539 }
1540
1541 // Cleanup lists used to store Done and Pending sets for each vertex.
1542 VertInf *endVert = router->vertices.end();
1543 for (VertInf *k = router->vertices.connsBegin(); k != endVert;
1544 k = k->lstNext)
1545 {
1546 k->aStarDoneNodes.clear();
1547 k->aStarPendingNodes.clear();
1548 }
1549 }
1550
1551
1552 }
1553
1554
1555