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