diff options
Diffstat (limited to 'src/libavoid/makepath.cpp')
| -rw-r--r-- | src/libavoid/makepath.cpp | 899 |
1 files changed, 529 insertions, 370 deletions
diff --git a/src/libavoid/makepath.cpp b/src/libavoid/makepath.cpp index 3a57f8e4e..4e15dbca9 100644 --- a/src/libavoid/makepath.cpp +++ b/src/libavoid/makepath.cpp @@ -2,43 +2,105 @@ * vim: ts=4 sw=4 et tw=0 wm=0 * * libavoid - Fast, Incremental, Object-avoiding Line Router - * Copyright (C) 2004-2006 Michael Wybrow <mjwybrow@users.sourceforge.net> * - * -------------------------------------------------------------------- - * The dijkstraPath function is based on code published and described - * in "Algorithms in C" (Second Edition), 1990, by Robert Sedgewick. - * -------------------------------------------------------------------- + * Copyright (C) 2004-2009 Monash University * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. + * See the file LICENSE.LGPL distributed with the library. + * + * Licensees holding a valid commercial license may use this file in + * accordance with the commercial license agreement provided with the + * library. * * This library 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 - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * + * Author(s): Michael Wybrow <mjwybrow@users.sourceforge.net> */ + +#include <algorithm> +#include <vector> +#include <climits> +#define _USE_MATH_DEFINES +#include <cmath> + #include "libavoid/vertices.h" #include "libavoid/makepath.h" #include "libavoid/geometry.h" #include "libavoid/connector.h" #include "libavoid/graph.h" #include "libavoid/router.h" -#include <algorithm> -#include <vector> -#include <climits> -#include <limits.h> -#include <math.h> +#include "libavoid/debug.h" +#include "libavoid/assertions.h" +#ifdef ASTAR_DEBUG + #include <SDL_gfxPrimitives.h> +#endif namespace Avoid { +class ANode +{ + public: + VertInf* inf; + double g; // Gone + double h; // Heuristic + double f; // Formula f = g + h + + int prevIndex; // Index into DONE for the previous ANode. + int timeStamp; // Timestamp used to determine explaration order of + // seemingly equal paths during orthogonal routing. + + ANode(VertInf *vinf, int time) + : inf(vinf), + g(0), + h(0), + f(0), + prevIndex(-1), + timeStamp(time) + { + } + ANode() + : inf(NULL), + g(0), + h(0), + f(0), + prevIndex(-1), + timeStamp(-1) + { + } +}; + + +// This returns the opposite result (>) so that when used with stl::make_heap, +// the head node of the heap will be the smallest value, rather than the +// largest. This saves us from having to sort the heap (and then reorder +// it back into a heap) when getting the next node to examine. This way we +// get better complexity -- logarithmic pushs and pops to the heap. +// +bool operator<(const ANode &a, const ANode &b) +{ + if (a.f != b.f) + { + return a.f > b.f; + } + if (a.timeStamp != b.timeStamp) + { + // Tiebreaker, if two paths have equal cost, then choose the one with + // the highest timeStamp. This corresponds to the furthest point + // explored along the straight-line path. When exploring we give the + // directions the following timeStamps; left:1, right:2 and forward:3, + // then we always try to explore forward first. + return a.timeStamp < b.timeStamp; + } + COLA_ASSERT(a.prevIndex != b.prevIndex); + return a.prevIndex > b.prevIndex; +} + static double Dot(const Point& l, const Point& r) { @@ -56,6 +118,13 @@ static double CrossLength(const Point& l, const Point& r) // static double angleBetween(const Point& p1, const Point& p2, const Point& p3) { + if ((p1.x == p2.x && p1.y == p2.y) || (p2.x == p3.x && p2.y == p3.y)) + { + // If two of the points are the same, then we can't say anything + // about the angle between. Treat them as being collinear. + return M_PI; + } + Point v1(p1.x - p2.x, p1.y - p2.y); Point v2(p3.x - p2.x, p3.y - p2.y); @@ -63,21 +132,45 @@ static double angleBetween(const Point& p1, const Point& p2, const Point& p3) } +// Construct a temporary Polygon path given several VertInf's for a connector. +// +static void constructPolygonPath(Polygon& connRoute, VertInf *inf2, + VertInf *inf3, std::vector<ANode>& done, int inf1Index) +{ + int routeSize = 2; + for (int curr = inf1Index; curr >= 0; curr = done[curr].prevIndex) + { + routeSize += 1; + } + connRoute.ps.resize(routeSize); + connRoute.ps[routeSize - 1] = inf3->point; + connRoute.ps[routeSize - 2] = inf2->point; + routeSize -= 3; + for (int curr = inf1Index; curr >= 0; curr = done[curr].prevIndex) + { + connRoute.ps[routeSize] = done[curr].inf->point; + routeSize -= 1; + } +} + + // Given the two points for a new segment of a path (inf2 & inf3) // as well as the distance between these points (dist), as well as // possibly the previous point (inf1) [from inf1--inf2], return a // cost associated with this route. // -double cost(ConnRef *lineRef, const double dist, VertInf *inf1, - VertInf *inf2, VertInf *inf3) +static double cost(ConnRef *lineRef, const double dist, VertInf *inf2, + VertInf *inf3, std::vector<ANode>& done, int inf1Index) { + VertInf *inf1 = (inf1Index >= 0) ? done[inf1Index].inf : NULL; double result = dist; + Polygon connRoute; Router *router = inf2->_router; - if (inf2->pathNext != NULL) + if (inf1 != NULL) { - double& angle_penalty = router->angle_penalty; - double& segmt_penalty = router->segmt_penalty; + const double angle_penalty = router->routingPenalty(anglePenalty); + const double segmt_penalty = router->routingPenalty(segmentPenalty); // This is not the first segment, so there is a bend // between it and the last one in the existing path. @@ -89,29 +182,83 @@ double cost(ConnRef *lineRef, const double dist, VertInf *inf1, double rad = M_PI - angleBetween(p1, p2, p3); - // Make `xval' between 0--10 then take its log so small - // angles are not penalised as much as large ones. - // - double xval = rad * 10 / M_PI; - double yval = xval * log10(xval + 1) / 10.5; - result += (angle_penalty * yval); - //printf("deg from straight: %g\tpenalty: %g\n", - // rad * 180 / M_PI, (angle_penalty * yval)); - - // Don't penalise as an extra segment if there is no turn. - if (rad > 0.0005) + if (rad > 0) + { + // Make `xval' between 0--10 then take its log so small + // angles are not penalised as much as large ones. + // + double xval = rad * 10 / M_PI; + double yval = xval * log10(xval + 1) / 10.5; + result += (angle_penalty * yval); + //db_printf("deg from straight: %g\tpenalty: %g\n", + // rad * 180 / M_PI, (angle_penalty * yval)); + } + + if (rad == M_PI) { + // Needs to double back + result += (2 * segmt_penalty); + } + else if (rad > 0) + { + // Only penalise as an extra segment if the two + // segments are not collinear. result += segmt_penalty; } } } - if (lineRef->doesHateCrossings() && (router->crossing_penalty > 0)) + if (!router->_inCrossingPenaltyReroutingStage) { - Point& a1 = inf2->point; - Point& a2 = inf3->point; + // Return here if we ar not in the postprocessing stage + return result; + } - ConnRefList::iterator curr, finish = router->connRefs.end(); + const double cluster_crossing_penalty = + router->routingPenalty(clusterCrossingPenalty); + // XXX: Clustered routing doesn't yet work with orthogonal connectors. + if (router->ClusteredRouting && !router->clusterRefs.empty() && + (cluster_crossing_penalty > 0) && + (lineRef->routingType() != ConnType_Orthogonal)) + { + if (connRoute.empty()) + { + constructPolygonPath(connRoute, inf2, inf3, done, inf1Index); + } + // There are clusters so do cluster routing. + for (ClusterRefList::const_iterator cl = router->clusterRefs.begin(); + cl != router->clusterRefs.end(); ++cl) + { + ReferencingPolygon& cBoundary = (*cl)->polygon(); + COLA_ASSERT(cBoundary.ps[0] != cBoundary.ps[cBoundary.size() - 1]); + for (size_t j = 0; j < cBoundary.size(); ++j) + { + // Cluster boundary points should correspond to shape + // vertices and hence already be in the list of vertices. + COLA_ASSERT(router->vertices.getVertexByPos(cBoundary.at(j))!=NULL); + } + + bool isConn = false; + Polygon dynamic_c_boundary(cBoundary); + Polygon dynamic_conn_route(connRoute); + const bool finalSegment = (inf3 == lineRef->dst()); + CrossingsInfoPair crossings = countRealCrossings( + dynamic_c_boundary, isConn, dynamic_conn_route, + connRoute.size() - 1, true, finalSegment); + result += (crossings.first * cluster_crossing_penalty); + } + } + + const double shared_path_penalty = + router->routingPenalty(fixedSharedPathPenalty); + if (shared_path_penalty > 0) + { + // Penalises shared paths, except if the connectors shared an endpoint. + if (connRoute.empty()) + { + constructPolygonPath(connRoute, inf2, inf3, done, inf1Index); + } + ConnRefList::const_iterator curr, finish = router->connRefs.end(); for (curr = router->connRefs.begin(); curr != finish; ++curr) { ConnRef *connRef = *curr; @@ -120,306 +267,237 @@ double cost(ConnRef *lineRef, const double dist, VertInf *inf1, { continue; } - Avoid::PolyLine& route2 = connRef->route(); - for (int j = 1; j < route2.pn; ++j) - { - Avoid::Point& b1 = route2.ps[j - 1]; - Avoid::Point& b2 = route2.ps[j]; + const Avoid::PolyLine& route2 = connRef->route(); - if (((a1 == b1) && (a2 == b2)) || - ((a2 == b1) && (a1 == b2))) - { - // Route along same segment: no penalty. We detect - // crossovers when we see the segments diverge. - continue; - } - - if ((a2 == b2) || (a2 == b1) || (b2 == a1)) - { - // Each crossing that is at a vertex in the - // visibility graph gets noticed four times. - // We ignore three of these cases. - // This also catches the case of a shared path, - // but this is one that terminates at a common - // endpoint, so we don't care about it. - continue; - } - - if (a1 == b1) - { - if (j == 1) - { - // common source point. - continue; - } - Avoid::Point& b0 = route2.ps[j - 2]; - // The segments share an endpoint -- a1==b1. - if (a2 == b0) - { - // a2 is not a split, continue. - continue; - } - - // If here, then we know that a2 != b2 - // And a2 and its pair in b are a split. - assert(a2 != b2); - - if (inf2->pathNext == NULL) - { - continue; - } - Avoid::Point& a0 = inf1->point; - - if ((a0 == b0) || (a0 == b2)) - { - //printf("Shared path... "); - bool normal = (a0 == b0) ? true : false; - // Determine direction we have to look through - // the points of connector b. - int dir = normal ? -1 : 1; - - int traceJ = j - 1 + dir; - - int endCornerSide = Avoid::cornerSide( - a0, a1, a2, normal ? b2 : b0); - - - VertInf *traceInf1 = inf2->pathNext; - VertInf *traceInf2 = inf2; - VertInf *traceInf3 = inf3; - while (traceInf1 && - (traceJ >= 0) && (traceJ < route2.pn) && - (traceInf1->point == route2.ps[traceJ])) - { - traceInf3 = traceInf2; - traceInf2 = traceInf1; - traceInf1 = traceInf1->pathNext; - traceJ += dir; - } - - if (!traceInf1 || - (traceJ < 0) || (traceJ >= route2.pn)) - { - //printf("common source or destination.\n"); - // The connectors have a shared path, but it - // comes from a common source point. - // XXX: There might be a better way to - // check this by asking the connectors - // for the IDs of the attached shapes. - continue; - } - - int startCornerSide = Avoid::cornerSide( - traceInf1->point, traceInf2->point, - traceInf3->point, route2.ps[traceJ]); - - if (endCornerSide != startCornerSide) - { - //printf("crosses.\n"); - result += router->crossing_penalty; - } - else - { - //printf("doesn't cross.\n"); - } - } - else - { - // The connectors cross or touch at this point. - //printf("Cross or touch at point... "); - - int side1 = Avoid::cornerSide(a0, a1, a2, b0); - int side2 = Avoid::cornerSide(a0, a1, a2, b2); - - if (side1 != side2) - { - //printf("cross.\n"); - // The connectors cross at this point. - result += router->crossing_penalty; - } - else - { - //printf("touch.\n"); - // The connectors touch at this point. - } - } - continue; - } + bool isConn = true; + Polygon dynamic_route2(route2); + Polygon dynamic_conn_route(connRoute); + CrossingsInfoPair crossings = countRealCrossings( + dynamic_route2, isConn, dynamic_conn_route, + connRoute.size() - 1, false, false, NULL, NULL, + connRef, lineRef); + + if ((crossings.second & CROSSING_SHARES_PATH) && + (crossings.second & CROSSING_SHARES_FIXED_SEGMENT) && + !(crossings.second & CROSSING_SHARES_PATH_AT_END)) + { + // Penalise unecessary shared paths in the middle of + // connectors. + result += shared_path_penalty; + } + } + } - double xc, yc; - int intersectResult = Avoid::segmentIntersectPoint( - a1, a2, b1, b2, &xc, &yc); + const double crossing_penalty = router->routingPenalty(crossingPenalty); + if (lineRef->doesHateCrossings() && (crossing_penalty > 0)) + { + if (connRoute.empty()) + { + constructPolygonPath(connRoute, inf2, inf3, done, inf1Index); + } + ConnRefList::const_iterator curr, finish = router->connRefs.end(); + for (curr = router->connRefs.begin(); curr != finish; ++curr) + { + ConnRef *connRef = *curr; - if (intersectResult == Avoid::DO_INTERSECT) - { - result += router->crossing_penalty; - } + if (connRef->id() == lineRef->id()) + { + continue; } + const Avoid::PolyLine& route2 = connRef->route(); + + bool isConn = true; + Polygon dynamic_route2(route2); + Polygon dynamic_conn_route(connRoute); + CrossingsInfoPair crossings = countRealCrossings( + dynamic_route2, isConn, dynamic_conn_route, + connRoute.size() - 1, true); + result += (crossings.first * crossing_penalty); } } - + return result; } -// Returns the best path from src to tar using the cost function. -// -// The path is worked out via Dijkstra's algorithm, and is encoded via -// pathNext links in each of the VerInfs along the path. -// -// Based on the code of 'matrixpfs'. -// -static void dijkstraPath(ConnRef *lineRef, VertInf *src, VertInf *tar) +static double estimatedCost(ConnRef *lineRef, const Point *last, + const Point& a, const Point& b) { - Router *router = src->_router; - - double unseen = (double) __INT_MAX__; - - // initialize arrays - VertInf *finish = router->vertices.end(); - for (VertInf *t = router->vertices.connsBegin(); t != finish; t = t->lstNext) + if (lineRef->routingType() == ConnType_PolyLine) { - t->pathNext = NULL; - t->pathDist = -unseen; + return euclideanDist(a, b); } - - VertInf *min = src; - while (min != tar) + else // Orthogonal { - VertInf *k = min; - min = NULL; - - k->pathDist *= -1; - if (k->pathDist == unseen) + // XXX: This currently just takes into account the compulsory + // bend but will have to be updated when port direction + // information is available. + int num_penalties = 0; + double xmove = b.x - a.x; + double ymove = b.y - a.y; + if (!last) { - k->pathDist = 0; - } - - EdgeInfList& visList = k->visList; - EdgeInfList::iterator finish = visList.end(); - for (EdgeInfList::iterator edge = visList.begin(); edge != finish; - ++edge) - { - VertInf *t = (*edge)->otherVert(k); - VertID tID = t->id; - - // Only check shape verticies, or endpoints. - if ((t->pathDist < 0) && - ((tID.objID == src->id.objID) || tID.isShape)) + // Just two points. + if ((xmove != 0) && (ymove != 0)) { - double kt_dist = (*edge)->getDist(); - double priority = k->pathDist + - cost(lineRef, kt_dist, k->pathNext, k, t); - - if ((kt_dist != 0) && (t->pathDist < -priority)) - { - t->pathDist = -priority; - t->pathNext = k; - } - if ((min == NULL) || (t->pathDist > min->pathDist)) - { - min = t; - } + num_penalties += 1; } } - EdgeInfList& invisList = k->invisList; - finish = invisList.end(); - for (EdgeInfList::iterator edge = invisList.begin(); edge != finish; - ++edge) + else { - VertInf *t = (*edge)->otherVert(k); - VertID tID = t->id; - - // Only check shape verticies, or endpoints. - if ((t->pathDist < 0) && - ((tID.objID == src->id.objID) || tID.isShape > 0)) + // We have three points, so we know the direction of the + // previous segment. + double rad = M_PI - angleBetween(*last, a, b); + if (rad > (M_PI / 2)) { - if ((min == NULL) || (t->pathDist > min->pathDist)) - { - min = t; - } + // Target point is back in the direction of the first point, + // so at least two bends are required. + num_penalties += 2; + } + else if (rad > 0) + { + // To the side, so at least one bend. + num_penalties += 1; } } + double penalty = num_penalties * + lineRef->router()->routingPenalty(segmentPenalty); + + return manhattanDist(a, b) + penalty; } } -class ANode +class CmpVisEdgeRotation { public: - VertInf* inf; - double g; // Gone - double h; // Heuristic - double f; // Formula f = g + h - VertInf *pp; - - ANode(VertInf *vinf) - : inf(vinf) - , g(0) - , h(0) - , f(0) - , pp(NULL) + CmpVisEdgeRotation(const VertInf* lastPt) + : _lastPt(lastPt) { } - ANode() - : inf(NULL) - , g(0) - , h(0) - , f(0) - , pp(NULL) + bool operator() (const EdgeInf* u, const EdgeInf* v) const { + return u->rotationLessThan(_lastPt, v); } + private: + const VertInf *_lastPt; }; -bool operator<(const ANode &a, const ANode &b) -{ - return a.f < b.f; -} - - -bool operator>(const ANode &a, const ANode &b) -{ - return a.f > b.f; -} - // Returns the best path from src to tar using the cost function. // // The path is worked out using the aStar algorithm, and is encoded via -// pathNext links in each of the VerInfs along the path. +// prevIndex values for each ANode which point back to the previous ANode's +// position in the DONE vector. At completion, this order is written into +// the pathNext links in each of the VerInfs along the path. // // The aStar STL code is based on public domain code available on the // internet. // -static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) +static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar, + VertInf *start) { + bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal); + + double (*dist)(const Point& a, const Point& b) = + (isOrthogonal) ? manhattanDist : euclideanDist; + std::vector<ANode> PENDING; // STL Vectors chosen because of rapid std::vector<ANode> DONE; // insertions/deletions at back, ANode Node, BestNode; // Temporary Node and BestNode bool bNodeFound = false; // Flag if node is found in container + int timestamp = 1; - tar->pathNext = NULL; + if (start == NULL) + { + start = src; + } + + Router *router = lineRef->router(); + if (router->RubberBandRouting && (start != src)) + { + COLA_ASSERT(router->IgnoreRegions == true); + + const PolyLine& currRoute = lineRef->route(); + VertInf *last = NULL; + int rIndx = 0; + while (last != start) + { + const Point& pnt = currRoute.at(rIndx); + bool isShape = (rIndx > 0); + VertID vID(pnt.id, isShape, pnt.vn); + +#ifdef PATHDEBUG + db_printf("/// %d %d %d\n", pnt.id, (int) isShape, pnt.vn); +#endif + VertInf *curr = router->vertices.getVertexByID(vID); + COLA_ASSERT(curr != NULL); + + Node = ANode(curr, timestamp++); + if (!last) + { + Node.g = 0; + Node.h = estimatedCost(lineRef, NULL, Node.inf->point, + tar->point); + Node.f = Node.g + Node.h; + } + else + { + double edgeDist = dist(BestNode.inf->point, curr->point); + + Node.g = BestNode.g + cost(lineRef, edgeDist, BestNode.inf, + Node.inf, DONE, BestNode.prevIndex); + + // Calculate the Heuristic. + Node.h = estimatedCost(lineRef, &(BestNode.inf->point), + Node.inf->point, tar->point); + + // The A* formula + Node.f = Node.g + Node.h; + + // Point parent to last BestNode (pushed onto DONE) + Node.prevIndex = DONE.size() - 1; + } + + if (curr != start) + { + BestNode = Node; + + DONE.push_back(BestNode); + } + else + { + PENDING.push_back(Node); + } - // Create the start node - Node = ANode(src); - Node.g = 0; - Node.h = dist(Node.inf->point, tar->point); - Node.f = Node.g + Node.h; - // Set a null parent, so cost function knows this is the first segment. - Node.pp = NULL; + rIndx++; + last = curr; + } + } + else + { + // Create the start node + Node = ANode(src, timestamp++); + Node.g = 0; + Node.h = estimatedCost(lineRef, NULL, Node.inf->point, tar->point); + Node.f = Node.g + Node.h; + // Set a null parent, so cost function knows this is the first segment. + + // Populate the PENDING container with the first location + PENDING.push_back(Node); + } + + tar->pathNext = NULL; - // Populate the PENDING container with the first location - PENDING.push_back(Node); // Create a heap from PENDING for sorting using std::make_heap; using std::push_heap; using std::pop_heap; make_heap( PENDING.begin(), PENDING.end() ); while (!PENDING.empty()) { - // Ascending sort based on overloaded operators below - sort_heap(PENDING.begin(), PENDING.end()); - - // Set the Node with lowest f value to BESTNODE + // Set the Node with lowest f value to BESTNODE. + // Since the ANode operator< is reversed, the head of the + // heap is the node with the lowest f value. BestNode = PENDING.front(); // Pop off the heap. Actually this moves the @@ -427,38 +505,118 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) // is not actually removed since the pop is to // the heap and not the container. pop_heap(PENDING.begin(), PENDING.end()); - - // Remove node from right (the value we pop_heap'd) PENDING.pop_back(); // Push the BestNode onto DONE - BestNode.inf->pathNext = BestNode.pp; DONE.push_back(BestNode); + VertInf *prevInf = (BestNode.prevIndex >= 0) ? + DONE[BestNode.prevIndex].inf : NULL; #if 0 - printf("Considering... "); - BestNode.ID->print(stdout); - printf(" - g: %3.1f h: %3.1f f: %3.1f back: ", BestNode.g, BestNode.h, - BestNode.f); - BestNode.pp.print(stdout); - printf("\n"); + db_printf("Considering... "); + db_printf(" %g %g ", BestNode.inf->point.x, BestNode.inf->point.y); + BestNode.inf->id.db_print(); + db_printf(" - g: %3.1f h: %3.1f back: ", BestNode.g, BestNode.h); + if (prevInf) + { + db_printf(" %g %g", prevInf->point.x, prevInf->point.y); + //prevInf->id.db_print(); + } + db_printf("\n"); +#endif + +#if defined(ASTAR_DEBUG) + if (router->avoid_screen) + { + int canx = 151; + int cany = 55; + int radius = 5; + ANode curr; + for (curr = BestNode; curr.prevIndex >= 0; + curr = DONE[curr.prevIndex]) + { + filledCircleRGBA(router->avoid_screen, + (int) curr.inf->point.x + canx, + (int) curr.inf->point.y + cany, + radius, 0, 0, 255, 128); + } + filledCircleRGBA(router->avoid_screen, + (int) BestNode.inf->point.x + canx, + (int) BestNode.inf->point.y + cany, + radius, 255, 0, 0, 255); + + SDL_Flip(router->avoid_screen); + //SDL_Delay(500); + + filledCircleRGBA(router->avoid_screen, + (int) BestNode.inf->point.x + canx, + (int) BestNode.inf->point.y + cany, + radius, 255, 255, 255, 255); + filledCircleRGBA(router->avoid_screen, + (int) BestNode.inf->point.x + canx, + (int) BestNode.inf->point.y + cany, + radius, 0, 255, 0, 128); + for (curr = BestNode; curr.prevIndex >= 0; + curr = DONE[curr.prevIndex]) + { + filledCircleRGBA(router->avoid_screen, + (int) curr.inf->point.x + canx, + (int) curr.inf->point.y + cany, + radius, 255, 255, 255, 255); + filledCircleRGBA(router->avoid_screen, + (int) curr.inf->point.x + canx, + (int) curr.inf->point.y + cany, + radius, 0, 255, 0, 128); + } + } #endif // If at destination, break and create path below if (BestNode.inf == tar) { +#ifdef PATHDEBUG + db_printf("Cost: %g\n", BestNode.f); +#endif //bPathFound = true; // arrived at destination... + + // Correct all the pathNext pointers. + ANode curr; + int currIndex = DONE.size() - 1; + for (curr = BestNode; curr.prevIndex > 0; + curr = DONE[curr.prevIndex]) + { + COLA_ASSERT(curr.prevIndex < currIndex); + curr.inf->pathNext = DONE[curr.prevIndex].inf; + currIndex = curr.prevIndex; + } + // Check that we've gone through the complete path. + COLA_ASSERT(curr.prevIndex == 0); + // Fill in the final pathNext pointer. + curr.inf->pathNext = DONE[curr.prevIndex].inf; + break; } // Check adjacent points in graph - EdgeInfList& visList = BestNode.inf->visList; - EdgeInfList::iterator finish = visList.end(); - for (EdgeInfList::iterator edge = visList.begin(); edge != finish; - ++edge) + EdgeInfList& visList = (!isOrthogonal) ? + BestNode.inf->visList : BestNode.inf->orthogVisList; + if (isOrthogonal) + { + // We would like to explore in a structured way, + // so sort the points in the visList... + CmpVisEdgeRotation compare(prevInf); + visList.sort(compare); + } + EdgeInfList::const_iterator finish = visList.end(); + for (EdgeInfList::const_iterator edge = visList.begin(); + edge != finish; ++edge) { - Node.inf = (*edge)->otherVert(BestNode.inf); + Node = ANode((*edge)->otherVert(BestNode.inf), timestamp++); + + // Set the index to the previous ANode that we reached + // this ANode through (the last BestNode pushed onto DONE). + Node.prevIndex = DONE.size() - 1; // Only check shape verticies, or the tar endpoint. if (!(Node.inf->id.isShape) && (Node.inf != tar)) @@ -466,6 +624,15 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) continue; } + VertInf *prevInf = (BestNode.prevIndex >= 0) ? + DONE[BestNode.prevIndex].inf : NULL; + + // Don't bother looking at the segment we just arrived along. + if (prevInf && (prevInf == Node.inf)) + { + continue; + } + double edgeDist = (*edge)->getDist(); if (edgeDist == 0) @@ -473,31 +640,49 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) continue; } - VertInf *prevInf = BestNode.inf->pathNext; + if (!router->_orthogonalRouting && + (!router->RubberBandRouting || (start == src)) && + (validateBendPoint(prevInf, BestNode.inf, Node.inf) == false)) + { + // The bendpoint is not valid, i.e., is a zigzag corner, so... + continue; + // For RubberBand routing we want to allow these routes and + // unwind them later, otherwise instead or unwinding, paths + // can go the *really* long way round. + } - Node.g = BestNode.g + cost(lineRef, edgeDist, prevInf, - BestNode.inf, Node.inf); + Node.g = BestNode.g + cost(lineRef, edgeDist, BestNode.inf, + Node.inf, DONE, BestNode.prevIndex); // Calculate the Heuristic. - Node.h = dist(Node.inf->point, tar->point); + Node.h = estimatedCost(lineRef, &(BestNode.inf->point), + Node.inf->point, tar->point); // The A* formula Node.f = Node.g + Node.h; - // Point parent to last BestNode (pushed onto DONE) - Node.pp = BestNode.inf; + +#if 0 + db_printf("-- Adding: %g %g ", Node.inf->point.x, + Node.inf->point.y); + Node.inf->id.db_print(); + db_printf(" - g: %3.1f h: %3.1f \n", Node.g, Node.h); +#endif bNodeFound = false; // Check to see if already on PENDING for (unsigned int i = 0; i < PENDING.size(); i++) { - if (Node.inf == PENDING.at(i).inf) - { // If already on PENDING - if (Node.g < PENDING.at(i).g) + ANode& ati = PENDING.at(i); + if ((Node.inf == ati.inf) && + (DONE[Node.prevIndex].inf == DONE[ati.prevIndex].inf)) + { + // If already on PENDING + if (Node.g < ati.g) { - PENDING.at(i).g = Node.g; - PENDING.at(i).f = Node.g + PENDING.at(i).h; - PENDING.at(i).pp = Node.pp; + PENDING[i] = Node; + + make_heap( PENDING.begin(), PENDING.end() ); } bNodeFound = true; break; @@ -508,15 +693,14 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) // Check to see if already on DONE for (unsigned int i = 0; i < DONE.size(); i++) { - if (Node.inf == DONE.at(i).inf) + ANode& ati = DONE.at(i); + if ((Node.inf == ati.inf) && + (DONE[Node.prevIndex].inf == DONE[ati.prevIndex].inf)) { // If on DONE, Which has lower gone? - if (Node.g < DONE.at(i).g) + if (Node.g < ati.g) { - DONE.at(i).g = Node.g; - DONE.at(i).f = Node.g + DONE.at(i).h; - DONE.at(i).pp = Node.pp; - DONE.at(i).inf->pathNext = Node.pp; + DONE[i] = Node; } bNodeFound = true; break; @@ -530,26 +714,24 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) PENDING.push_back(Node); // Push NewNode onto heap push_heap( PENDING.begin(), PENDING.end() ); - // Re-Assert heap, or will be short by one - make_heap( PENDING.begin(), PENDING.end() ); #if 0 + using std::cout; using std::endl; // Display PENDING and DONE containers (For Debugging) cout << "PENDING: "; - for (int i = 0; i < PENDING.size(); i++) + for (unsigned int i = 0; i < PENDING.size(); i++) { - cout << PENDING.at(i).x << "," << PENDING.at(i).y << ","; - cout << PENDING.at(i).g << "," << PENDING.at(i).h << " "; + cout << PENDING.at(i).g << "," << PENDING.at(i).h << ","; + cout << PENDING.at(i).inf << "," << PENDING.at(i).pp << " "; } cout << endl; cout << "DONE: "; - for (int i = 0; i < DONE.size(); i++) + for (unsigned int i = 0; i < DONE.size(); i++) { - cout << DONE.at(i).x << "," << DONE.at(i).y << ","; - cout << DONE.at(i).g << "," << DONE.at(i).h << " "; + cout << DONE.at(i).g << "," << DONE.at(i).h << ","; + cout << DONE.at(i).inf << "," << DONE.at(i).pp << " "; } cout << endl << endl; - int ch = _getch(); #endif } } @@ -559,75 +741,52 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) // Returns the best path for the connector referred to by lineRef. // -// The path encoded in the pathNext links in each of the VerInfs +// The path encoded in the pathNext links in each of the VertInfs // backwards along the path, from the tar back to the source. // void makePath(ConnRef *lineRef, bool *flag) { + bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal); Router *router = lineRef->router(); VertInf *src = lineRef->src(); VertInf *tar = lineRef->dst(); + VertInf *start = lineRef->start(); - // If the connector hates crossings then we want to examine direct paths: - bool examineDirectPath = lineRef->doesHateCrossings(); - // TODO: Could be more efficient here. - EdgeInf *directEdge = EdgeInf::existingEdge(src, tar); - if (!(router->IncludeEndpoints) && directVis(src, tar)) - { - Point p = src->point; - Point q = tar->point; - - assert(directEdge == NULL); - - directEdge = new EdgeInf(src, tar); - tar->pathNext = src; - directEdge->setDist(dist(p, q)); - directEdge->addConn(flag); - - return; - } - else if (router->IncludeEndpoints && directEdge && - (directEdge->getDist() > 0) && !examineDirectPath) + if (isOrthogonal) { - tar->pathNext = src; - directEdge->addConn(flag); + aStarPath(lineRef, src, tar, start); } - else + else // if (!isOrthogonal) { - // Mark the path endpoints as not being able to see - // each other. This is true if we are here. - if (!(router->IncludeEndpoints) && router->InvisibilityGrph) - { - if (!directEdge) - { - directEdge = new EdgeInf(src, tar); - } - directEdge->addBlocker(0); - } - - if (router->UseAStarSearch) + EdgeInf *directEdge = EdgeInf::existingEdge(src, tar); + // If the connector hates crossings or there are clusters present, + // then we want to examine direct paths: + bool examineDirectPath = lineRef->doesHateCrossings() || + !(router->clusterRefs.empty()); + + if ((start == src) && directEdge && (directEdge->getDist() > 0) && + !examineDirectPath) { - aStarPath(lineRef, src, tar); + tar->pathNext = src; + directEdge->addConn(flag); } else { - dijkstraPath(lineRef, src, tar); + aStarPath(lineRef, src, tar, start); } + } #if 0 - PointMap::iterator t; - for (VertInf *t = vertices.connsBegin(); t != vertices.end(); - t = t->lstNext) - { - - t->id.print(); - printf(" -> "); - t->pathNext->id.print(); - printf("\n"); - } -#endif + for (VertInf *t = vertices.connsBegin(); t != vertices.end(); + t = t->lstNext) + { + t->id.db_print(); + db_printf(" -> "); + t->pathNext->id.db_print(); + db_printf("\n"); } +#endif } |