From 179fa413b047bede6e32109e2ce82437c5fb8d34 Mon Sep 17 00:00:00 2001 From: MenTaLguY Date: Mon, 16 Jan 2006 02:36:01 +0000 Subject: moving trunk for module inkscape (bzr r1) --- src/libavoid/makepath.cpp | 462 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 462 insertions(+) create mode 100644 src/libavoid/makepath.cpp (limited to 'src/libavoid/makepath.cpp') diff --git a/src/libavoid/makepath.cpp b/src/libavoid/makepath.cpp new file mode 100644 index 000000000..776ffd307 --- /dev/null +++ b/src/libavoid/makepath.cpp @@ -0,0 +1,462 @@ +/* + * vim: ts=4 sw=4 et tw=0 wm=0 + * + * libavoid - Fast, Incremental, Object-avoiding Line Router + * Copyright (C) 2004-2005 Michael Wybrow + * + * -------------------------------------------------------------------- + * 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/connector.h" +#include "libavoid/graph.h" +#include +#include + +namespace Avoid { + + +double segmt_penalty = 0; +double angle_penalty = 0; + + +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(const double dist, VertInf *inf1, + VertInf *inf2, VertInf *inf3) +{ + double result = dist; + + if (inf2->pathNext != NULL) + { + // 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 `rad' between 0--10 then take its log so small + // angles are not penalised as much as large ones. + result += (angle_penalty * log((rad * 10 / M_PI) + 1)); + + // Don't penalise as an extra segment if there is no turn. + if (rad > 0.0005) + { + result += segmt_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(VertInf *src, VertInf *tar) +{ + double unseen = (double) INT_MAX; + + // initialize arrays + VertInf *finish = vertices.end(); + for (VertInf *t = 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(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(VertInf *src, VertInf *tar) +{ + std::vector PENDING; // STL Vectors chosen because of rapid + std::vector 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(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) +{ + VertInf *src = lineRef->src(); + VertInf *tar = lineRef->dst(); + + // TODO: Could be more efficient here. + EdgeInf *directEdge = EdgeInf::existingEdge(src, tar); + if (!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 (IncludeEndpoints && directEdge && (directEdge->getDist() > 0)) + { + 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 (!IncludeEndpoints && InvisibilityGrph) + { + directEdge->addBlocker(0); + } + + if (UseAStarSearch) + { + aStarPath(src, tar); + } + else + { + dijkstraPath(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 + } +} + + +} + + -- cgit v1.2.3