src/libavoid/makepath.cpp

   1 /*
   2  * vim: ts=4 sw=4 et tw=0 wm=0
   3  *
   4  * libavoid - Fast, Incremental, Object-avoiding Line Router
   5  * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
   6  *
   7  * --------------------------------------------------------------------
   8  * The dijkstraPath function is based on code published and described
   9  * in "Algorithms in C" (Second Edition), 1990, by Robert Sedgewick.
  10  * --------------------------------------------------------------------
  11  *
  12  * This library is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU Lesser General Public
  14  * License as published by the Free Software Foundation; either
  15  * version 2.1 of the License, or (at your option) any later version.
  16  *
  17  * This library is distributed in the hope that it will be useful,
  18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  20  * Lesser General Public License for more details.
  21  *
  22  * You should have received a copy of the GNU Lesser General Public
  23  * License along with this library; if not, write to the Free Software
  24  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  25  *
  26 */
  27
  28 #include "libavoid/vertices.h"
  29 #include "libavoid/makepath.h"
  30 #include "libavoid/geometry.h"
  31 #include "libavoid/connector.h"
  32 #include "libavoid/graph.h"
  33 #include "libavoid/router.h"
  34 #include <vector>
  35 #include <math.h>
  36
  37 namespace Avoid {
  38
  39
  40 double segmt_penalty = 0;
  41 double angle_penalty = 0;
  42
  43
  44 static double Dot(const Point& l, const Point& r)
  45 {
  46     return (l.x * r.x) + (l.y * r.y);
  47 }
  48
  49 static double CrossLength(const Point& l, const Point& r)
  50 {
  51     return (l.x * r.y) - (l.y * r.x);
  52 }
  53
  54
  55 // Return the angle between the two line segments made by the
  56 // points p1--p2 and p2--p3.  Return value is in radians.
  57 //
  58 static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
  59 {
  60     Point v1 = { p1.x - p2.x, p1.y - p2.y };
  61     Point v2 = { p3.x - p2.x, p3.y - p2.y };
  62
  63     return fabs(atan2(CrossLength(v1, v2), Dot(v1, v2)));
  64 }
  65
  66
  67 // Given the two points for a new segment of a path (inf2 & inf3)
  68 // as well as the distance between these points (dist), as well as
  69 // possibly the previous point (inf1) [from inf1--inf2], return a
  70 // cost associated with this route.
  71 //
  72 double cost(const double dist, VertInf *inf1,
  73         VertInf *inf2, VertInf *inf3)
  74 {
  75     double result = dist;
  76
  77     if (inf2->pathNext != NULL)
  78     {
  79         // This is not the first segment, so there is a bend
  80         // between it and the last one in the existing path.
  81         if ((angle_penalty > 0) || (segmt_penalty > 0))
  82         {
  83             Point p1 = inf1->point;
  84             Point p2 = inf2->point;
  85             Point p3 = inf3->point;
  86
  87             double rad = M_PI - angleBetween(p1, p2, p3);
  88
  89             // Make `rad' between 0--10 then take its log so small
  90             // angles are not penalised as much as large ones.
  91             result += (angle_penalty * log((rad * 10 / M_PI) + 1));
  92
  93             // Don't penalise as an extra segment if there is no turn.
  94             if (rad > 0.0005)
  95             {
  96                 result += segmt_penalty;
  97             }
  98         }
  99
 100     }
 101
 102     return result;
 103 }
 104
 105
 106 // Returns the best path from src to tar using the cost function.
 107 //
 108 // The path is worked out via Dijkstra's algorithm, and is encoded via
 109 // pathNext links in each of the VerInfs along the path.
 110 //
 111 // Based on the code of 'matrixpfs'.
 112 //
 113 static void dijkstraPath(VertInf *src, VertInf *tar)
 114 {
 115     Router *router = src->_router;
 116
 117     double unseen = (double) INT_MAX;
 118
 119     // initialize arrays
 120     VertInf *finish = router->vertices.end();
 121     for (VertInf *t = router->vertices.connsBegin(); t != finish; t = t->lstNext)
 122     {
 123         t->pathNext = NULL;
 124         t->pathDist = -unseen;
 125     }
 126
 127     VertInf *min = src;
 128     while (min != tar)
 129     {
 130         VertInf *k = min;
 131         min = NULL;
 132
 133         k->pathDist *= -1;
 134         if (k->pathDist == unseen)
 135         {
 136             k->pathDist = 0;
 137         }
 138
 139         EdgeInfList& visList = k->visList;
 140         EdgeInfList::iterator finish = visList.end();
 141         for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
 142                 ++edge)
 143         {
 144             VertInf *t = (*edge)->otherVert(k);
 145             VertID tID = t->id;
 146
 147             // Only check shape verticies, or endpoints.
 148             if ((t->pathDist < 0) &&
 149                     ((tID.objID == src->id.objID) || tID.isShape))
 150             {
 151                 double kt_dist = (*edge)->getDist();
 152                 double priority = k->pathDist +
 153                         cost(kt_dist, k->pathNext, k, t);
 154
 155                 if ((kt_dist != 0) && (t->pathDist < -priority))
 156                 {
 157                     t->pathDist = -priority;
 158                     t->pathNext = k;
 159                 }
 160                 if ((min == NULL) || (t->pathDist > min->pathDist))
 161                 {
 162                     min = t;
 163                 }
 164             }
 165         }
 166         EdgeInfList& invisList = k->invisList;
 167         finish = invisList.end();
 168         for (EdgeInfList::iterator edge = invisList.begin(); edge != finish;
 169                 ++edge)
 170         {
 171             VertInf *t = (*edge)->otherVert(k);
 172             VertID tID = t->id;
 173
 174             // Only check shape verticies, or endpoints.
 175             if ((t->pathDist < 0) &&
 176                     ((tID.objID == src->id.objID) || tID.isShape > 0))
 177             {
 178                 if ((min == NULL) || (t->pathDist > min->pathDist))
 179                 {
 180                     min = t;
 181                 }
 182             }
 183         }
 184     }
 185 }
 186
 187
 188 class ANode
 189 {
 190     public:
 191         VertInf* inf;
 192         double g;        // Gone
 193         double h;        // Heuristic
 194         double f;        // Formula f = g + h
 195         VertInf *pp;
 196
 197         ANode(VertInf *vinf)
 198             : inf(vinf)
 199             , g(0)
 200             , h(0)
 201             , f(0)
 202             , pp(NULL)
 203         {
 204         }
 205         ANode()
 206             : inf(NULL)
 207             , g(0)
 208             , h(0)
 209             , f(0)
 210             , pp(NULL)
 211         {
 212         }
 213 };
 214
 215 bool operator<(const ANode &a, const ANode &b)
 216 {
 217     return a.f < b.f;
 218 }
 219
 220
 221 bool operator>(const ANode &a, const ANode &b)
 222 {
 223     return a.f > b.f;
 224 }
 225
 226
 227 // Returns the best path from src to tar using the cost function.
 228 //
 229 // The path is worked out using the aStar algorithm, and is encoded via
 230 // pathNext links in each of the VerInfs along the path.
 231 //
 232 // The aStar STL code is based on public domain code available on the
 233 // internet.
 234 //
 235 static void aStarPath(VertInf *src, VertInf *tar)
 236 {
 237     std::vector<ANode> PENDING;     // STL Vectors chosen because of rapid
 238     std::vector<ANode> DONE;        // insertions/deletions at back,
 239     ANode Node, BestNode;           // Temporary Node and BestNode
 240     bool bNodeFound = false;        // Flag if node is found in container
 241
 242     tar->pathNext = NULL;
 243
 244     // Create the start node
 245     Node = ANode(src);
 246     Node.g = 0;
 247     Node.h = dist(Node.inf->point, tar->point);
 248     Node.f = Node.g + Node.h;
 249     // Set a null parent, so cost function knows this is the first segment.
 250     Node.pp = NULL;
 251
 252     // Populate the PENDING container with the first location
 253     PENDING.push_back(Node);
 254     // Create a heap from PENDING for sorting
 255     using std::make_heap; using std::push_heap; using std::pop_heap;
 256     make_heap( PENDING.begin(), PENDING.end() );
 257
 258     while (!PENDING.empty())
 259     {
 260         // Ascending sort based on overloaded operators below
 261         sort_heap(PENDING.begin(), PENDING.end());
 262
 263         // Set the Node with lowest f value to BESTNODE
 264         BestNode = PENDING.front();
 265
 266         // Pop off the heap.  Actually this moves the
 267         // far left value to the far right.  The node
 268         // is not actually removed since the pop is to
 269         // the heap and not the container.
 270         pop_heap(PENDING.begin(), PENDING.end());
 271
 272
 273         // Remove node from right (the value we pop_heap'd)
 274         PENDING.pop_back();
 275
 276         // Push the BestNode onto DONE
 277         BestNode.inf->pathNext = BestNode.pp;
 278         DONE.push_back(BestNode);
 279
 280 #if 0
 281         printf("Considering... ");
 282         BestNode.ID->print(stdout);
 283         printf(" - g: %3.1f h: %3.1f f: %3.1f back: ", BestNode.g, BestNode.h,
 284                 BestNode.f);
 285         BestNode.pp.print(stdout);
 286         printf("\n");
 287 #endif
 288
 289         // If at destination, break and create path below
 290         if (BestNode.inf == tar)
 291         {
 292             //bPathFound = true; // arrived at destination...
 293             break;
 294         }
 295
 296         // Check adjacent points in graph
 297         EdgeInfList& visList = BestNode.inf->visList;
 298         EdgeInfList::iterator finish = visList.end();
 299         for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
 300                 ++edge)
 301         {
 302             Node.inf = (*edge)->otherVert(BestNode.inf);
 303
 304             // Only check shape verticies, or the tar endpoint.
 305             if (!(Node.inf->id.isShape) && (Node.inf != tar))
 306             {
 307                 continue;
 308             }
 309
 310             double edgeDist = (*edge)->getDist();
 311
 312             if (edgeDist == 0)
 313             {
 314                 continue;
 315             }
 316
 317             VertInf *prevInf = BestNode.inf->pathNext;
 318
 319             Node.g = BestNode.g + cost(edgeDist, prevInf,
 320                     BestNode.inf, Node.inf);
 321
 322             // Calculate the Heuristic.
 323             Node.h = dist(Node.inf->point, tar->point);
 324
 325             // The A* formula
 326             Node.f = Node.g + Node.h;
 327             // Point parent to last BestNode (pushed onto DONE)
 328             Node.pp = BestNode.inf;
 329
 330             bNodeFound = false;
 331
 332             // Check to see if already on PENDING
 333             for (unsigned int i = 0; i < PENDING.size(); i++)
 334             {
 335                 if (Node.inf == PENDING.at(i).inf)
 336                 {   // If already on PENDING
 337                     if (Node.g < PENDING.at(i).g)
 338                     {
 339                         PENDING.at(i).g = Node.g;
 340                         PENDING.at(i).f = Node.g + PENDING.at(i).h;
 341                         PENDING.at(i).pp = Node.pp;
 342                     }
 343                     bNodeFound = true;
 344                     break;
 345                 }
 346             }
 347             if (!bNodeFound ) // If Node NOT found on PENDING
 348             {
 349                 // Check to see if already on DONE
 350                 for (unsigned int i = 0; i < DONE.size(); i++)
 351                 {
 352                     if (Node.inf == DONE.at(i).inf)
 353                     {
 354                         // If on DONE, Which has lower gone?
 355                         if (Node.g < DONE.at(i).g)
 356                         {
 357                             DONE.at(i).g = Node.g;
 358                             DONE.at(i).f = Node.g + DONE.at(i).h;
 359                             DONE.at(i).pp = Node.pp;
 360                             DONE.at(i).inf->pathNext = Node.pp;
 361                         }
 362                         bNodeFound = true;
 363                         break;
 364                     }
 365                 }
 366             }
 367
 368             if (!bNodeFound ) // If Node NOT found on PENDING or DONE
 369             {
 370                 // Push NewNode onto PENDING
 371                 PENDING.push_back(Node);
 372                 // Push NewNode onto heap
 373                 push_heap( PENDING.begin(), PENDING.end() );
 374                 // Re-Assert heap, or will be short by one
 375                 make_heap( PENDING.begin(), PENDING.end() );
 376
 377 #if 0
 378                 // Display PENDING and DONE containers (For Debugging)
 379                 cout << "PENDING:   ";
 380                 for (int i = 0; i < PENDING.size(); i++)
 381                 {
 382                     cout << PENDING.at(i).x << "," << PENDING.at(i).y << ",";
 383                     cout << PENDING.at(i).g << "," << PENDING.at(i).h << "  ";
 384                 }
 385                 cout << endl;
 386                 cout << "DONE:   ";
 387                 for (int i = 0; i < DONE.size(); i++)
 388                 {
 389                     cout << DONE.at(i).x << "," << DONE.at(i).y << ",";
 390                     cout << DONE.at(i).g << "," << DONE.at(i).h << "  ";
 391                 }
 392                 cout << endl << endl;
 393                 int ch = _getch();
 394 #endif
 395             }
 396         }
 397     }
 398 }
 399
 400
 401 // Returns the best path for the connector referred to by lineRef.
 402 //
 403 // The path encoded in the pathNext links in each of the VerInfs
 404 // backwards along the path, from the tar back to the source.
 405 //
 406 void makePath(ConnRef *lineRef, bool *flag)
 407 {
 408     Router *router = lineRef->router();
 409     VertInf *src = lineRef->src();
 410     VertInf *tar = lineRef->dst();
 411
 412     // TODO: Could be more efficient here.
 413     EdgeInf *directEdge = EdgeInf::existingEdge(src, tar);
 414     if (!(router->IncludeEndpoints) && directVis(src, tar))
 415     {
 416         Point p = src->point;
 417         Point q = tar->point;
 418
 419         assert(directEdge == NULL);
 420
 421         directEdge = new EdgeInf(src, tar);
 422         tar->pathNext = src;
 423         directEdge->setDist(dist(p, q));
 424         directEdge->addConn(flag);
 425
 426         return;
 427     }
 428     else if (router->IncludeEndpoints && directEdge &&
 429             (directEdge->getDist() > 0))
 430     {
 431         tar->pathNext = src;
 432         directEdge->addConn(flag);
 433     }
 434     else
 435     {
 436         // Mark the path endpoints as not being able to see
 437         // each other.  This is true if we are here.
 438         if (!(router->IncludeEndpoints) && router->InvisibilityGrph)
 439         {
 440             if (!directEdge)
 441             {
 442                 directEdge = new EdgeInf(src, tar);
 443             }
 444             directEdge->addBlocker(0);
 445         }
 446
 447         if (router->UseAStarSearch)
 448         {
 449             aStarPath(src, tar);
 450         }
 451         else
 452         {
 453             dijkstraPath(src, tar);
 454         }
 455
 456 #if 0
 457         PointMap::iterator t;
 458         for (VertInf *t = vertices.connsBegin(); t != vertices.end();
 459                 t = t->lstNext)
 460         {
 461
 462             t->id.print();
 463             printf(" -> ");
 464             t->pathNext->id.print();
 465             printf("\n");
 466         }
 467 #endif
 468     }
 469 }
 470
 471
 472 }
 473
 474