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 <algorithm>
  35 #include <vector>
  36 #include <limits.h>
  37 #include <math.h>
  38
  39 namespace Avoid {
  40
  41
  42 static double Dot(const Point& l, const Point& r)
  43 {
  44     return (l.x * r.x) + (l.y * r.y);
  45 }
  46
  47 static double CrossLength(const Point& l, const Point& r)
  48 {
  49     return (l.x * r.y) - (l.y * r.x);
  50 }
  51
  52
  53 // Return the angle between the two line segments made by the
  54 // points p1--p2 and p2--p3.  Return value is in radians.
  55 //
  56 static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
  57 {
  58     Point v1(p1.x - p2.x, p1.y - p2.y);
  59     Point v2(p3.x - p2.x, p3.y - p2.y);
  60
  61     return fabs(atan2(CrossLength(v1, v2), Dot(v1, v2)));
  62 }
  63
  64
  65 // Given the two points for a new segment of a path (inf2 & inf3)
  66 // as well as the distance between these points (dist), as well as
  67 // possibly the previous point (inf1) [from inf1--inf2], return a
  68 // cost associated with this route.
  69 //
  70 double cost(ConnRef *lineRef, const double dist, VertInf *inf1,
  71         VertInf *inf2, VertInf *inf3)
  72 {
  73     double result = dist;
  74
  75     Router *router = inf2->_router;
  76     if (inf2->pathNext != NULL)
  77     {
  78         double& angle_penalty = router->angle_penalty;
  79         double& segmt_penalty = router->segmt_penalty;
  80
  81         // This is not the first segment, so there is a bend
  82         // between it and the last one in the existing path.
  83         if ((angle_penalty > 0) || (segmt_penalty > 0))
  84         {
  85             Point p1 = inf1->point;
  86             Point p2 = inf2->point;
  87             Point p3 = inf3->point;
  88
  89             double rad = M_PI - angleBetween(p1, p2, p3);
  90
  91             // Make `xval' between 0--10 then take its log so small
  92             // angles are not penalised as much as large ones.
  93             //
  94             double xval = rad * 10 / M_PI;
  95             double yval = xval * log10(xval + 1) / 10.5;
  96             result += (angle_penalty * yval);
  97             //printf("deg from straight: %g\tpenalty: %g\n",
  98             //        rad * 180 / M_PI, (angle_penalty * yval));
  99
 100             // Don't penalise as an extra segment if there is no turn.
 101             if (rad > 0.0005)
 102             {
 103                 result += segmt_penalty;
 104             }
 105         }
 106     }
 107
 108     if (lineRef->doesHateCrossings() && (router->crossing_penalty > 0))
 109     {
 110         Point& a1 = inf2->point;
 111         Point& a2 = inf3->point;
 112
 113         ConnRefList::iterator curr, finish = router->connRefs.end();
 114         for (curr = router->connRefs.begin(); curr != finish; ++curr)
 115         {
 116             ConnRef *connRef = *curr;
 117
 118             if (connRef->id() == lineRef->id())
 119             {
 120                 continue;
 121             }
 122             Avoid::PolyLine& route2 = connRef->route();
 123             for (int j = 1; j < route2.pn; ++j)
 124             {
 125                 Avoid::Point& b1 = route2.ps[j - 1];
 126                 Avoid::Point& b2 = route2.ps[j];
 127
 128                 if (((a1 == b1) && (a2 == b2)) ||
 129                     ((a2 == b1) && (a1 == b2)))
 130                 {
 131                     // Route along same segment: no penalty.  We detect
 132                     // crossovers when we see the segments diverge.
 133                     continue;
 134                 }
 135
 136                 if ((a2 == b2) || (a2 == b1) || (b2 == a1))
 137                 {
 138                     // Each crossing that is at a vertex in the
 139                     // visibility graph gets noticed four times.
 140                     // We ignore three of these cases.
 141                     // This also catches the case of a shared path,
 142                     // but this is one that terminates at a common
 143                     // endpoint, so we don't care about it.
 144                     continue;
 145                 }
 146
 147                 if (a1 == b1)
 148                 {
 149                     if (j == 1)
 150                     {
 151                         // common source point.
 152                         continue;
 153                     }
 154                     Avoid::Point& b0 = route2.ps[j - 2];
 155                     // The segments share an endpoint -- a1==b1.
 156                     if (a2 == b0)
 157                     {
 158                         // a2 is not a split, continue.
 159                         continue;
 160                     }
 161
 162                     // If here, then we know that a2 != b2
 163                     // And a2 and its pair in b are a split.
 164                     assert(a2 != b2);
 165
 166                     if (inf2->pathNext == NULL)
 167                     {
 168                         continue;
 169                     }
 170                     Avoid::Point& a0 = inf1->point;
 171
 172                     if ((a0 == b0) || (a0 == b2))
 173                     {
 174                         //printf("Shared path... ");
 175                         bool normal = (a0 == b0) ? true : false;
 176                         // Determine direction we have to look through
 177                         // the points of connector b.
 178                         int dir = normal ? -1 : 1;
 179
 180                         int traceJ = j - 1 + dir;
 181
 182                         int endCornerSide = Avoid::cornerSide(
 183                                 a0, a1, a2, normal ? b2 : b0);
 184
 185
 186                         VertInf *traceInf1 = inf2->pathNext;
 187                         VertInf *traceInf2 = inf2;
 188                         VertInf *traceInf3 = inf3;
 189                         while (traceInf1 &&
 190                                 (traceJ >= 0) && (traceJ < route2.pn) &&
 191                                 (traceInf1->point == route2.ps[traceJ]))
 192                         {
 193                             traceInf3 = traceInf2;
 194                             traceInf2 = traceInf1;
 195                             traceInf1 = traceInf1->pathNext;
 196                             traceJ += dir;
 197                         }
 198
 199                         if (!traceInf1 ||
 200                                 (traceJ < 0) || (traceJ >= route2.pn))
 201                         {
 202                             //printf("common source or destination.\n");
 203                             // The connectors have a shared path, but it
 204                             // comes from a common source point.
 205                             // XXX: There might be a better way to
 206                             //      check this by asking the connectors
 207                             //      for the IDs of the attached shapes.
 208                             continue;
 209                         }
 210
 211                         int startCornerSide = Avoid::cornerSide(
 212                                 traceInf1->point, traceInf2->point,
 213                                 traceInf3->point, route2.ps[traceJ]);
 214
 215                         if (endCornerSide != startCornerSide)
 216                         {
 217                             //printf("crosses.\n");
 218                             result += router->crossing_penalty;
 219                         }
 220                         else
 221                         {
 222                             //printf("doesn't cross.\n");
 223                         }
 224                     }
 225                     else
 226                     {
 227                         // The connectors cross or touch at this point.
 228                         //printf("Cross or touch at point... ");
 229
 230                         int side1 = Avoid::cornerSide(a0, a1, a2, b0);
 231                         int side2 = Avoid::cornerSide(a0, a1, a2, b2);
 232
 233                         if (side1 != side2)
 234                         {
 235                             //printf("cross.\n");
 236                             // The connectors cross at this point.
 237                             result += router->crossing_penalty;
 238                         }
 239                         else
 240                         {
 241                             //printf("touch.\n");
 242                             // The connectors touch at this point.
 243                         }
 244                     }
 245                     continue;
 246                 }
 247
 248                 double xc, yc;
 249                 int intersectResult = Avoid::segmentIntersectPoint(
 250                         a1, a2, b1, b2, &xc, &yc);
 251
 252                 if (intersectResult == Avoid::DO_INTERSECT)
 253                 {
 254                     result += router->crossing_penalty;
 255                 }
 256             }
 257         }
 258     }
 259
 260     return result;
 261 }
 262
 263
 264 // Returns the best path from src to tar using the cost function.
 265 //
 266 // The path is worked out via Dijkstra's algorithm, and is encoded via
 267 // pathNext links in each of the VerInfs along the path.
 268 //
 269 // Based on the code of 'matrixpfs'.
 270 //
 271 static void dijkstraPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
 272 {
 273     Router *router = src->_router;
 274
 275     double unseen = (double) INT_MAX;
 276
 277     // initialize arrays
 278     VertInf *finish = router->vertices.end();
 279     for (VertInf *t = router->vertices.connsBegin(); t != finish; t = t->lstNext)
 280     {
 281         t->pathNext = NULL;
 282         t->pathDist = -unseen;
 283     }
 284
 285     VertInf *min = src;
 286     while (min != tar)
 287     {
 288         VertInf *k = min;
 289         min = NULL;
 290
 291         k->pathDist *= -1;
 292         if (k->pathDist == unseen)
 293         {
 294             k->pathDist = 0;
 295         }
 296
 297         EdgeInfList& visList = k->visList;
 298         EdgeInfList::iterator finish = visList.end();
 299         for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
 300                 ++edge)
 301         {
 302             VertInf *t = (*edge)->otherVert(k);
 303             VertID tID = t->id;
 304
 305             // Only check shape verticies, or endpoints.
 306             if ((t->pathDist < 0) &&
 307                     ((tID.objID == src->id.objID) || tID.isShape))
 308             {
 309                 double kt_dist = (*edge)->getDist();
 310                 double priority = k->pathDist +
 311                         cost(lineRef, kt_dist, k->pathNext, k, t);
 312
 313                 if ((kt_dist != 0) && (t->pathDist < -priority))
 314                 {
 315                     t->pathDist = -priority;
 316                     t->pathNext = k;
 317                 }
 318                 if ((min == NULL) || (t->pathDist > min->pathDist))
 319                 {
 320                     min = t;
 321                 }
 322             }
 323         }
 324         EdgeInfList& invisList = k->invisList;
 325         finish = invisList.end();
 326         for (EdgeInfList::iterator edge = invisList.begin(); edge != finish;
 327                 ++edge)
 328         {
 329             VertInf *t = (*edge)->otherVert(k);
 330             VertID tID = t->id;
 331
 332             // Only check shape verticies, or endpoints.
 333             if ((t->pathDist < 0) &&
 334                     ((tID.objID == src->id.objID) || tID.isShape > 0))
 335             {
 336                 if ((min == NULL) || (t->pathDist > min->pathDist))
 337                 {
 338                     min = t;
 339                 }
 340             }
 341         }
 342     }
 343 }
 344
 345
 346 class ANode
 347 {
 348     public:
 349         VertInf* inf;
 350         double g;        // Gone
 351         double h;        // Heuristic
 352         double f;        // Formula f = g + h
 353         VertInf *pp;
 354
 355         ANode(VertInf *vinf)
 356             : inf(vinf)
 357             , g(0)
 358             , h(0)
 359             , f(0)
 360             , pp(NULL)
 361         {
 362         }
 363         ANode()
 364             : inf(NULL)
 365             , g(0)
 366             , h(0)
 367             , f(0)
 368             , pp(NULL)
 369         {
 370         }
 371 };
 372
 373 bool operator<(const ANode &a, const ANode &b)
 374 {
 375     return a.f < b.f;
 376 }
 377
 378
 379 bool operator>(const ANode &a, const ANode &b)
 380 {
 381     return a.f > b.f;
 382 }
 383
 384
 385 // Returns the best path from src to tar using the cost function.
 386 //
 387 // The path is worked out using the aStar algorithm, and is encoded via
 388 // pathNext links in each of the VerInfs along the path.
 389 //
 390 // The aStar STL code is based on public domain code available on the
 391 // internet.
 392 //
 393 static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
 394 {
 395     std::vector<ANode> PENDING;     // STL Vectors chosen because of rapid
 396     std::vector<ANode> DONE;        // insertions/deletions at back,
 397     ANode Node, BestNode;           // Temporary Node and BestNode
 398     bool bNodeFound = false;        // Flag if node is found in container
 399
 400     tar->pathNext = NULL;
 401
 402     // Create the start node
 403     Node = ANode(src);
 404     Node.g = 0;
 405     Node.h = dist(Node.inf->point, tar->point);
 406     Node.f = Node.g + Node.h;
 407     // Set a null parent, so cost function knows this is the first segment.
 408     Node.pp = NULL;
 409
 410     // Populate the PENDING container with the first location
 411     PENDING.push_back(Node);
 412     // Create a heap from PENDING for sorting
 413     using std::make_heap; using std::push_heap; using std::pop_heap;
 414     make_heap( PENDING.begin(), PENDING.end() );
 415
 416     while (!PENDING.empty())
 417     {
 418         // Ascending sort based on overloaded operators below
 419         sort_heap(PENDING.begin(), PENDING.end());
 420
 421         // Set the Node with lowest f value to BESTNODE
 422         BestNode = PENDING.front();
 423
 424         // Pop off the heap.  Actually this moves the
 425         // far left value to the far right.  The node
 426         // is not actually removed since the pop is to
 427         // the heap and not the container.
 428         pop_heap(PENDING.begin(), PENDING.end());
 429
 430
 431         // Remove node from right (the value we pop_heap'd)
 432         PENDING.pop_back();
 433
 434         // Push the BestNode onto DONE
 435         BestNode.inf->pathNext = BestNode.pp;
 436         DONE.push_back(BestNode);
 437
 438 #if 0
 439         printf("Considering... ");
 440         BestNode.ID->print(stdout);
 441         printf(" - g: %3.1f h: %3.1f f: %3.1f back: ", BestNode.g, BestNode.h,
 442                 BestNode.f);
 443         BestNode.pp.print(stdout);
 444         printf("\n");
 445 #endif
 446
 447         // If at destination, break and create path below
 448         if (BestNode.inf == tar)
 449         {
 450             //bPathFound = true; // arrived at destination...
 451             break;
 452         }
 453
 454         // Check adjacent points in graph
 455         EdgeInfList& visList = BestNode.inf->visList;
 456         EdgeInfList::iterator finish = visList.end();
 457         for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
 458                 ++edge)
 459         {
 460             Node.inf = (*edge)->otherVert(BestNode.inf);
 461
 462             // Only check shape verticies, or the tar endpoint.
 463             if (!(Node.inf->id.isShape) && (Node.inf != tar))
 464             {
 465                 continue;
 466             }
 467
 468             double edgeDist = (*edge)->getDist();
 469
 470             if (edgeDist == 0)
 471             {
 472                 continue;
 473             }
 474
 475             VertInf *prevInf = BestNode.inf->pathNext;
 476
 477             Node.g = BestNode.g + cost(lineRef, edgeDist, prevInf,
 478                     BestNode.inf, Node.inf);
 479
 480             // Calculate the Heuristic.
 481             Node.h = dist(Node.inf->point, tar->point);
 482
 483             // The A* formula
 484             Node.f = Node.g + Node.h;
 485             // Point parent to last BestNode (pushed onto DONE)
 486             Node.pp = BestNode.inf;
 487
 488             bNodeFound = false;
 489
 490             // Check to see if already on PENDING
 491             for (unsigned int i = 0; i < PENDING.size(); i++)
 492             {
 493                 if (Node.inf == PENDING.at(i).inf)
 494                 {   // If already on PENDING
 495                     if (Node.g < PENDING.at(i).g)
 496                     {
 497                         PENDING.at(i).g = Node.g;
 498                         PENDING.at(i).f = Node.g + PENDING.at(i).h;
 499                         PENDING.at(i).pp = Node.pp;
 500                     }
 501                     bNodeFound = true;
 502                     break;
 503                 }
 504             }
 505             if (!bNodeFound ) // If Node NOT found on PENDING
 506             {
 507                 // Check to see if already on DONE
 508                 for (unsigned int i = 0; i < DONE.size(); i++)
 509                 {
 510                     if (Node.inf == DONE.at(i).inf)
 511                     {
 512                         // If on DONE, Which has lower gone?
 513                         if (Node.g < DONE.at(i).g)
 514                         {
 515                             DONE.at(i).g = Node.g;
 516                             DONE.at(i).f = Node.g + DONE.at(i).h;
 517                             DONE.at(i).pp = Node.pp;
 518                             DONE.at(i).inf->pathNext = Node.pp;
 519                         }
 520                         bNodeFound = true;
 521                         break;
 522                     }
 523                 }
 524             }
 525
 526             if (!bNodeFound ) // If Node NOT found on PENDING or DONE
 527             {
 528                 // Push NewNode onto PENDING
 529                 PENDING.push_back(Node);
 530                 // Push NewNode onto heap
 531                 push_heap( PENDING.begin(), PENDING.end() );
 532                 // Re-Assert heap, or will be short by one
 533                 make_heap( PENDING.begin(), PENDING.end() );
 534
 535 #if 0
 536                 // Display PENDING and DONE containers (For Debugging)
 537                 cout << "PENDING:   ";
 538                 for (int i = 0; i < PENDING.size(); i++)
 539                 {
 540                     cout << PENDING.at(i).x << "," << PENDING.at(i).y << ",";
 541                     cout << PENDING.at(i).g << "," << PENDING.at(i).h << "  ";
 542                 }
 543                 cout << endl;
 544                 cout << "DONE:   ";
 545                 for (int i = 0; i < DONE.size(); i++)
 546                 {
 547                     cout << DONE.at(i).x << "," << DONE.at(i).y << ",";
 548                     cout << DONE.at(i).g << "," << DONE.at(i).h << "  ";
 549                 }
 550                 cout << endl << endl;
 551                 int ch = _getch();
 552 #endif
 553             }
 554         }
 555     }
 556 }
 557
 558
 559 // Returns the best path for the connector referred to by lineRef.
 560 //
 561 // The path encoded in the pathNext links in each of the VerInfs
 562 // backwards along the path, from the tar back to the source.
 563 //
 564 void makePath(ConnRef *lineRef, bool *flag)
 565 {
 566     Router *router = lineRef->router();
 567     VertInf *src = lineRef->src();
 568     VertInf *tar = lineRef->dst();
 569
 570     // If the connector hates crossings then we want to examine direct paths:
 571     bool examineDirectPath = lineRef->doesHateCrossings();
 572
 573     // TODO: Could be more efficient here.
 574     EdgeInf *directEdge = EdgeInf::existingEdge(src, tar);
 575     if (!(router->IncludeEndpoints) && directVis(src, tar))
 576     {
 577         Point p = src->point;
 578         Point q = tar->point;
 579
 580         assert(directEdge == NULL);
 581
 582         directEdge = new EdgeInf(src, tar);
 583         tar->pathNext = src;
 584         directEdge->setDist(dist(p, q));
 585         directEdge->addConn(flag);
 586
 587         return;
 588     }
 589     else if (router->IncludeEndpoints && directEdge &&
 590             (directEdge->getDist() > 0) && !examineDirectPath)
 591     {
 592         tar->pathNext = src;
 593         directEdge->addConn(flag);
 594     }
 595     else
 596     {
 597         // Mark the path endpoints as not being able to see
 598         // each other.  This is true if we are here.
 599         if (!(router->IncludeEndpoints) && router->InvisibilityGrph)
 600         {
 601             if (!directEdge)
 602             {
 603                 directEdge = new EdgeInf(src, tar);
 604             }
 605             directEdge->addBlocker(0);
 606         }
 607
 608         if (router->UseAStarSearch)
 609         {
 610             aStarPath(lineRef, src, tar);
 611         }
 612         else
 613         {
 614             dijkstraPath(lineRef, src, tar);
 615         }
 616
 617 #if 0
 618         PointMap::iterator t;
 619         for (VertInf *t = vertices.connsBegin(); t != vertices.end();
 620                 t = t->lstNext)
 621         {
 622
 623             t->id.print();
 624             printf(" -> ");
 625             t->pathNext->id.print();
 626             printf("\n");
 627         }
 628 #endif
 629     }
 630 }
 631
 632
 633 }
 634
 635