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