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