/* * 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. * -------------------------------------------------------------------- * * 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. * * 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 * */ #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> namespace Avoid { static double Dot(const Point& l, const Point& r) { return (l.x * r.x) + (l.y * r.y); } static double CrossLength(const Point& l, const Point& r) { return (l.x * r.y) - (l.y * r.x); } // Return the angle between the two line segments made by the // points p1--p2 and p2--p3. Return value is in radians. // static double angleBetween(const Point& p1, const Point& p2, const Point& p3) { Point v1(p1.x - p2.x, p1.y - p2.y); Point v2(p3.x - p2.x, p3.y - p2.y); return fabs(atan2(CrossLength(v1, v2), Dot(v1, v2))); } // 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) { double result = dist; Router *router = inf2->_router; if (inf2->pathNext != NULL) { double& angle_penalty = router->angle_penalty; double& segmt_penalty = router->segmt_penalty; // This is not the first segment, so there is a bend // between it and the last one in the existing path. if ((angle_penalty > 0) || (segmt_penalty > 0)) { Point p1 = inf1->point; Point p2 = inf2->point; Point p3 = inf3->point; 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) { result += segmt_penalty; } } } if (lineRef->doesHateCrossings() && (router->crossing_penalty > 0)) { Point& a1 = inf2->point; Point& a2 = inf3->point; ConnRefList::iterator curr, finish = router->connRefs.end(); for (curr = router->connRefs.begin(); curr != finish; ++curr) { ConnRef *connRef = *curr; if (connRef->id() == lineRef->id()) { 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]; 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; } double xc, yc; int intersectResult = Avoid::segmentIntersectPoint( a1, a2, b1, b2, &xc, &yc); if (intersectResult == Avoid::DO_INTERSECT) { result += router->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) { 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) { t->pathNext = NULL; t->pathDist = -unseen; } VertInf *min = src; while (min != tar) { VertInf *k = min; min = NULL; k->pathDist *= -1; if (k->pathDist == unseen) { 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)) { 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; } } } EdgeInfList& invisList = k->invisList; finish = invisList.end(); for (EdgeInfList::iterator edge = invisList.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 > 0)) { if ((min == NULL) || (t->pathDist > min->pathDist)) { min = t; } } } } } class ANode { 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) { } ANode() : inf(NULL) , g(0) , h(0) , f(0) , pp(NULL) { } }; 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. // // The aStar STL code is based on public domain code available on the // internet. // static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar) { 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 tar->pathNext = NULL; // 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; // 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 BestNode = PENDING.front(); // Pop off the heap. Actually this moves the // far left value to the far right. The node // 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); #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"); #endif // If at destination, break and create path below if (BestNode.inf == tar) { //bPathFound = true; // arrived at destination... 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) { Node.inf = (*edge)->otherVert(BestNode.inf); // Only check shape verticies, or the tar endpoint. if (!(Node.inf->id.isShape) && (Node.inf != tar)) { continue; } double edgeDist = (*edge)->getDist(); if (edgeDist == 0) { continue; } VertInf *prevInf = BestNode.inf->pathNext; Node.g = BestNode.g + cost(lineRef, edgeDist, prevInf, BestNode.inf, Node.inf); // Calculate the Heuristic. Node.h = dist(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; 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) { PENDING.at(i).g = Node.g; PENDING.at(i).f = Node.g + PENDING.at(i).h; PENDING.at(i).pp = Node.pp; } bNodeFound = true; break; } } if (!bNodeFound ) // If Node NOT found on PENDING { // Check to see if already on DONE for (unsigned int i = 0; i < DONE.size(); i++) { if (Node.inf == DONE.at(i).inf) { // If on DONE, Which has lower gone? if (Node.g < DONE.at(i).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; } bNodeFound = true; break; } } } if (!bNodeFound ) // If Node NOT found on PENDING or DONE { // Push NewNode onto PENDING 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 // Display PENDING and DONE containers (For Debugging) cout << "PENDING: "; for (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 << endl; cout << "DONE: "; for (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 << endl << endl; int ch = _getch(); #endif } } } } // Returns the best path for the connector referred to by lineRef. // // The path encoded in the pathNext links in each of the VerInfs // backwards along the path, from the tar back to the source. // void makePath(ConnRef *lineRef, bool *flag) { Router *router = lineRef->router(); VertInf *src = lineRef->src(); VertInf *tar = lineRef->dst(); // 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) { tar->pathNext = src; directEdge->addConn(flag); } else { // 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) { aStarPath(lineRef, src, tar); } else { dijkstraPath(lineRef, src, tar); } #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 } } }