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 <vector>
35 #include <math.h>
37 namespace Avoid {
40 static double Dot(const Point& l, const Point& r)
41 {
42 return (l.x * r.x) + (l.y * r.y);
43 }
45 static double CrossLength(const Point& l, const Point& r)
46 {
47 return (l.x * r.y) - (l.y * r.x);
48 }
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);
59 return fabs(atan2(CrossLength(v1, v2), Dot(v1, v2)));
60 }
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;
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;
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;
87 double rad = M_PI - angleBetween(p1, p2, p3);
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));
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 }
106 if (lineRef->doesHateCrossings() && (router->crossing_penalty > 0))
107 {
108 Point& a1 = inf2->point;
109 Point& a2 = inf3->point;
111 ConnRefList::iterator curr, finish = router->connRefs.end();
112 for (curr = router->connRefs.begin(); curr != finish; ++curr)
113 {
114 ConnRef *connRef = *curr;
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];
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 }
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 }
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 }
160 // If here, then we know that a2 != b2
161 // And a2 and its pair in b are a split.
162 assert(a2 != b2);
164 if (inf2->pathNext == NULL)
165 {
166 continue;
167 }
168 Avoid::Point& a0 = inf1->point;
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;
178 int traceJ = j - 1 + dir;
180 int endCornerSide = Avoid::cornerSide(
181 a0, a1, a2, normal ? b2 : b0);
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 }
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 }
209 int startCornerSide = Avoid::cornerSide(
210 traceInf1->point, traceInf2->point,
211 traceInf3->point, route2.ps[traceJ]);
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... ");
228 int side1 = Avoid::cornerSide(a0, a1, a2, b0);
229 int side2 = Avoid::cornerSide(a0, a1, a2, b2);
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 }
246 double xc, yc;
247 int intersectResult = Avoid::segmentIntersectPoint(
248 a1, a2, b1, b2, &xc, &yc);
250 if (intersectResult == Avoid::DO_INTERSECT)
251 {
252 result += router->crossing_penalty;
253 }
254 }
255 }
256 }
258 return result;
259 }
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;
273 double unseen = (double) INT_MAX;
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 }
283 VertInf *min = src;
284 while (min != tar)
285 {
286 VertInf *k = min;
287 min = NULL;
289 k->pathDist *= -1;
290 if (k->pathDist == unseen)
291 {
292 k->pathDist = 0;
293 }
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;
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);
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;
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 }
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;
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 };
371 bool operator<(const ANode &a, const ANode &b)
372 {
373 return a.f < b.f;
374 }
377 bool operator>(const ANode &a, const ANode &b)
378 {
379 return a.f > b.f;
380 }
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
398 tar->pathNext = NULL;
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;
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() );
414 while (!PENDING.empty())
415 {
416 // Ascending sort based on overloaded operators below
417 sort_heap(PENDING.begin(), PENDING.end());
419 // Set the Node with lowest f value to BESTNODE
420 BestNode = PENDING.front();
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());
429 // Remove node from right (the value we pop_heap'd)
430 PENDING.pop_back();
432 // Push the BestNode onto DONE
433 BestNode.inf->pathNext = BestNode.pp;
434 DONE.push_back(BestNode);
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
445 // If at destination, break and create path below
446 if (BestNode.inf == tar)
447 {
448 //bPathFound = true; // arrived at destination...
449 break;
450 }
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);
460 // Only check shape verticies, or the tar endpoint.
461 if (!(Node.inf->id.isShape) && (Node.inf != tar))
462 {
463 continue;
464 }
466 double edgeDist = (*edge)->getDist();
468 if (edgeDist == 0)
469 {
470 continue;
471 }
473 VertInf *prevInf = BestNode.inf->pathNext;
475 Node.g = BestNode.g + cost(lineRef, edgeDist, prevInf,
476 BestNode.inf, Node.inf);
478 // Calculate the Heuristic.
479 Node.h = dist(Node.inf->point, tar->point);
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;
486 bNodeFound = false;
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 }
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() );
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 }
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();
568 // If the connector hates crossings then we want to examine direct paths:
569 bool examineDirectPath = lineRef->doesHateCrossings();
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;
578 assert(directEdge == NULL);
580 directEdge = new EdgeInf(src, tar);
581 tar->pathNext = src;
582 directEdge->setDist(dist(p, q));
583 directEdge->addConn(flag);
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 }
606 if (router->UseAStarSearch)
607 {
608 aStarPath(lineRef, src, tar);
609 }
610 else
611 {
612 dijkstraPath(lineRef, src, tar);
613 }
615 #if 0
616 PointMap::iterator t;
617 for (VertInf *t = vertices.connsBegin(); t != vertices.end();
618 t = t->lstNext)
619 {
621 t->id.print();
622 printf(" -> ");
623 t->pathNext->id.print();
624 printf("\n");
625 }
626 #endif
627 }
628 }
631 }