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-2005  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/connector.h"
  29 #include "libavoid/graph.h"
  30 #include <vector>
  31 #include <math.h>
  32
  33 namespace Avoid {
  34
  35
  36 double segmt_penalty = 0;
  37 double angle_penalty = 0;
  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(const double dist, VertInf *inf1,
  69         VertInf *inf2, VertInf *inf3)
  70 {
  71     double result = dist;
  72
  73     if (inf2->pathNext != NULL)
  74     {
  75         // This is not the first segment, so there is a bend
  76         // between it and the last one in the existing path.
  77         if ((angle_penalty > 0) || (segmt_penalty > 0))
  78         {
  79             Point p1 = inf1->point;
  80             Point p2 = inf2->point;
  81             Point p3 = inf3->point;
  82
  83             double rad = M_PI - angleBetween(p1, p2, p3);
  84
  85             // Make `rad' between 0--10 then take its log so small
  86             // angles are not penalised as much as large ones.
  87             result += (angle_penalty * log((rad * 10 / M_PI) + 1));
  88
  89             // Don't penalise as an extra segment if there is no turn.
  90             if (rad > 0.0005)
  91             {
  92                 result += segmt_penalty;
  93             }
  94         }
  95
  96     }
  97
  98     return result;
  99 }
 100
 101
 102 // Returns the best path from src to tar using the cost function.
 103 //
 104 // The path is worked out via Dijkstra's algorithm, and is encoded via
 105 // pathNext links in each of the VerInfs along the path.
 106 //
 107 // Based on the code of 'matrixpfs'.
 108 //
 109 static void dijkstraPath(VertInf *src, VertInf *tar)
 110 {
 111     double unseen = (double) INT_MAX;
 112
 113     // initialize arrays
 114     VertInf *finish = vertices.end();
 115     for (VertInf *t = vertices.connsBegin(); t != finish; t = t->lstNext)
 116     {
 117         t->pathNext = NULL;
 118         t->pathDist = -unseen;
 119     }
 120
 121     VertInf *min = src;
 122     while (min != tar)
 123     {
 124         VertInf *k = min;
 125         min = NULL;
 126
 127         k->pathDist *= -1;
 128         if (k->pathDist == unseen)
 129         {
 130             k->pathDist = 0;
 131         }
 132
 133         EdgeInfList& visList = k->visList;
 134         EdgeInfList::iterator finish = visList.end();
 135         for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
 136                 ++edge)
 137         {
 138             VertInf *t = (*edge)->otherVert(k);
 139             VertID tID = t->id;
 140
 141             // Only check shape verticies, or endpoints.
 142             if ((t->pathDist < 0) &&
 143                     ((tID.objID == src->id.objID) || tID.isShape))
 144             {
 145                 double kt_dist = (*edge)->getDist();
 146                 double priority = k->pathDist +
 147                         cost(kt_dist, k->pathNext, k, t);
 148
 149                 if ((kt_dist != 0) && (t->pathDist < -priority))
 150                 {
 151                     t->pathDist = -priority;
 152                     t->pathNext = k;
 153                 }
 154                 if ((min == NULL) || (t->pathDist > min->pathDist))
 155                 {
 156                     min = t;
 157                 }
 158             }
 159         }
 160         EdgeInfList& invisList = k->invisList;
 161         finish = invisList.end();
 162         for (EdgeInfList::iterator edge = invisList.begin(); edge != finish;
 163                 ++edge)
 164         {
 165             VertInf *t = (*edge)->otherVert(k);
 166             VertID tID = t->id;
 167
 168             // Only check shape verticies, or endpoints.
 169             if ((t->pathDist < 0) &&
 170                     ((tID.objID == src->id.objID) || tID.isShape > 0))
 171             {
 172                 if ((min == NULL) || (t->pathDist > min->pathDist))
 173                 {
 174                     min = t;
 175                 }
 176             }
 177         }
 178     }
 179 }
 180
 181
 182 class ANode
 183 {
 184     public:
 185         VertInf* inf;
 186         double g;        // Gone
 187         double h;        // Heuristic
 188         double f;        // Formula f = g + h
 189         VertInf *pp;
 190
 191         ANode(VertInf *vinf)
 192             : inf(vinf)
 193             , g(0)
 194             , h(0)
 195             , f(0)
 196             , pp(NULL)
 197         {
 198         }
 199         ANode()
 200             : inf(NULL)
 201             , g(0)
 202             , h(0)
 203             , f(0)
 204             , pp(NULL)
 205         {
 206         }
 207 };
 208
 209 bool operator<(const ANode &a, const ANode &b)
 210 {
 211     return a.f < b.f;
 212 }
 213
 214
 215 bool operator>(const ANode &a, const ANode &b)
 216 {
 217     return a.f > b.f;
 218 }
 219
 220
 221 // Returns the best path from src to tar using the cost function.
 222 //
 223 // The path is worked out using the aStar algorithm, and is encoded via
 224 // pathNext links in each of the VerInfs along the path.
 225 //
 226 // The aStar STL code is based on public domain code available on the
 227 // internet.
 228 //
 229 static void aStarPath(VertInf *src, VertInf *tar)
 230 {
 231     std::vector<ANode> PENDING;     // STL Vectors chosen because of rapid
 232     std::vector<ANode> DONE;        // insertions/deletions at back,
 233     ANode Node, BestNode;           // Temporary Node and BestNode
 234     bool bNodeFound = false;        // Flag if node is found in container
 235
 236     tar->pathNext = NULL;
 237
 238     // Create the start node
 239     Node = ANode(src);
 240     Node.g = 0;
 241     Node.h = dist(Node.inf->point, tar->point);
 242     Node.f = Node.g + Node.h;
 243     // Set a null parent, so cost function knows this is the first segment.
 244     Node.pp = NULL;
 245
 246     // Populate the PENDING container with the first location
 247     PENDING.push_back(Node);
 248     // Create a heap from PENDING for sorting
 249     using std::make_heap; using std::push_heap; using std::pop_heap;
 250     make_heap( PENDING.begin(), PENDING.end() );
 251
 252     while (!PENDING.empty())
 253     {
 254         // Ascending sort based on overloaded operators below
 255         sort_heap(PENDING.begin(), PENDING.end());
 256
 257         // Set the Node with lowest f value to BESTNODE
 258         BestNode = PENDING.front();
 259
 260         // Pop off the heap.  Actually this moves the
 261         // far left value to the far right.  The node
 262         // is not actually removed since the pop is to
 263         // the heap and not the container.
 264         pop_heap(PENDING.begin(), PENDING.end());
 265
 266
 267         // Remove node from right (the value we pop_heap'd)
 268         PENDING.pop_back();
 269
 270         // Push the BestNode onto DONE
 271         BestNode.inf->pathNext = BestNode.pp;
 272         DONE.push_back(BestNode);
 273
 274 #if 0
 275         printf("Considering... ");
 276         BestNode.ID->print(stdout);
 277         printf(" - g: %3.1f h: %3.1f f: %3.1f back: ", BestNode.g, BestNode.h,
 278                 BestNode.f);
 279         BestNode.pp.print(stdout);
 280         printf("\n");
 281 #endif
 282
 283         // If at destination, break and create path below
 284         if (BestNode.inf == tar)
 285         {
 286             //bPathFound = true; // arrived at destination...
 287             break;
 288         }
 289
 290         // Check adjacent points in graph
 291         EdgeInfList& visList = BestNode.inf->visList;
 292         EdgeInfList::iterator finish = visList.end();
 293         for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
 294                 ++edge)
 295         {
 296             Node.inf = (*edge)->otherVert(BestNode.inf);
 297
 298             // Only check shape verticies, or the tar endpoint.
 299             if (!(Node.inf->id.isShape) && (Node.inf != tar))
 300             {
 301                 continue;
 302             }
 303
 304             double edgeDist = (*edge)->getDist();
 305
 306             if (edgeDist == 0)
 307             {
 308                 continue;
 309             }
 310
 311             VertInf *prevInf = BestNode.inf->pathNext;
 312
 313             Node.g = BestNode.g + cost(edgeDist, prevInf,
 314                     BestNode.inf, Node.inf);
 315
 316             // Calculate the Heuristic.
 317             Node.h = dist(Node.inf->point, tar->point);
 318
 319             // The A* formula
 320             Node.f = Node.g + Node.h;
 321             // Point parent to last BestNode (pushed onto DONE)
 322             Node.pp = BestNode.inf;
 323
 324             bNodeFound = false;
 325
 326             // Check to see if already on PENDING
 327             for (unsigned int i = 0; i < PENDING.size(); i++)
 328             {
 329                 if (Node.inf == PENDING.at(i).inf)
 330                 {   // If already on PENDING
 331                     if (Node.g < PENDING.at(i).g)
 332                     {
 333                         PENDING.at(i).g = Node.g;
 334                         PENDING.at(i).f = Node.g + PENDING.at(i).h;
 335                         PENDING.at(i).pp = Node.pp;
 336                     }
 337                     bNodeFound = true;
 338                     break;
 339                 }
 340             }
 341             if (!bNodeFound ) // If Node NOT found on PENDING
 342             {
 343                 // Check to see if already on DONE
 344                 for (unsigned int i = 0; i < DONE.size(); i++)
 345                 {
 346                     if (Node.inf == DONE.at(i).inf)
 347                     {
 348                         // If on DONE, Which has lower gone?
 349                         if (Node.g < DONE.at(i).g)
 350                         {
 351                             DONE.at(i).g = Node.g;
 352                             DONE.at(i).f = Node.g + DONE.at(i).h;
 353                             DONE.at(i).pp = Node.pp;
 354                             DONE.at(i).inf->pathNext = Node.pp;
 355                         }
 356                         bNodeFound = true;
 357                         break;
 358                     }
 359                 }
 360             }
 361
 362             if (!bNodeFound ) // If Node NOT found on PENDING or DONE
 363             {
 364                 // Push NewNode onto PENDING
 365                 PENDING.push_back(Node);
 366                 // Push NewNode onto heap
 367                 push_heap( PENDING.begin(), PENDING.end() );
 368                 // Re-Assert heap, or will be short by one
 369                 make_heap( PENDING.begin(), PENDING.end() );
 370
 371 #if 0
 372                 // Display PENDING and DONE containers (For Debugging)
 373                 cout << "PENDING:   ";
 374                 for (int i = 0; i < PENDING.size(); i++)
 375                 {
 376                     cout << PENDING.at(i).x << "," << PENDING.at(i).y << ",";
 377                     cout << PENDING.at(i).g << "," << PENDING.at(i).h << "  ";
 378                 }
 379                 cout << endl;
 380                 cout << "DONE:   ";
 381                 for (int i = 0; i < DONE.size(); i++)
 382                 {
 383                     cout << DONE.at(i).x << "," << DONE.at(i).y << ",";
 384                     cout << DONE.at(i).g << "," << DONE.at(i).h << "  ";
 385                 }
 386                 cout << endl << endl;
 387                 int ch = _getch();
 388 #endif
 389             }
 390         }
 391     }
 392 }
 393
 394
 395 // Returns the best path for the connector referred to by lineRef.
 396 //
 397 // The path encoded in the pathNext links in each of the VerInfs
 398 // backwards along the path, from the tar back to the source.
 399 //
 400 void makePath(ConnRef *lineRef, bool *flag)
 401 {
 402     VertInf *src = lineRef->src();
 403     VertInf *tar = lineRef->dst();
 404
 405     // TODO: Could be more efficient here.
 406     EdgeInf *directEdge = EdgeInf::existingEdge(src, tar);
 407     if (!IncludeEndpoints && directVis(src, tar))
 408     {
 409         Point p = src->point;
 410         Point q = tar->point;
 411
 412         assert(directEdge == NULL);
 413
 414         directEdge = new EdgeInf(src, tar);
 415         tar->pathNext = src;
 416         directEdge->setDist(dist(p, q));
 417         directEdge->addConn(flag);
 418
 419         return;
 420     }
 421     else if (IncludeEndpoints && directEdge && (directEdge->getDist() > 0))
 422     {
 423         tar->pathNext = src;
 424         directEdge->addConn(flag);
 425     }
 426     else
 427     {
 428         // Mark the path endpoints as not being able to see
 429         // each other.  This is true if we are here.
 430         if (!IncludeEndpoints && InvisibilityGrph)
 431         {
 432             if (!directEdge)
 433             {
 434                 directEdge = new EdgeInf(src, tar);
 435             }
 436             directEdge->addBlocker(0);
 437         }
 438
 439         if (UseAStarSearch)
 440         {
 441             aStarPath(src, tar);
 442         }
 443         else
 444         {
 445             dijkstraPath(src, tar);
 446         }
 447
 448 #if 0
 449         PointMap::iterator t;
 450         for (VertInf *t = vertices.connsBegin(); t != vertices.end();
 451                 t = t->lstNext)
 452         {
 453
 454             t->id.print();
 455             printf(" -> ");
 456             t->pathNext->id.print();
 457             printf("\n");
 458         }
 459 #endif
 460     }
 461 }
 462
 463
 464 }
 465
 466