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 */
28 #include "libavoid/connector.h"
29 #include "libavoid/graph.h"
30 #include <vector>
31 #include <math.h>
33 namespace Avoid {
36 double segmt_penalty = 0;
37 double angle_penalty = 0;
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(const double dist, VertInf *inf1,
69 VertInf *inf2, VertInf *inf3)
70 {
71 double result = dist;
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;
83 double rad = M_PI - angleBetween(p1, p2, p3);
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));
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 }
96 }
98 return result;
99 }
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;
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 }
121 VertInf *min = src;
122 while (min != tar)
123 {
124 VertInf *k = min;
125 min = NULL;
127 k->pathDist *= -1;
128 if (k->pathDist == unseen)
129 {
130 k->pathDist = 0;
131 }
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;
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);
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;
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 }
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;
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 };
209 bool operator<(const ANode &a, const ANode &b)
210 {
211 return a.f < b.f;
212 }
215 bool operator>(const ANode &a, const ANode &b)
216 {
217 return a.f > b.f;
218 }
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
236 tar->pathNext = NULL;
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;
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() );
252 while (!PENDING.empty())
253 {
254 // Ascending sort based on overloaded operators below
255 sort_heap(PENDING.begin(), PENDING.end());
257 // Set the Node with lowest f value to BESTNODE
258 BestNode = PENDING.front();
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());
267 // Remove node from right (the value we pop_heap'd)
268 PENDING.pop_back();
270 // Push the BestNode onto DONE
271 BestNode.inf->pathNext = BestNode.pp;
272 DONE.push_back(BestNode);
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
283 // If at destination, break and create path below
284 if (BestNode.inf == tar)
285 {
286 //bPathFound = true; // arrived at destination...
287 break;
288 }
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);
298 // Only check shape verticies, or the tar endpoint.
299 if (!(Node.inf->id.isShape) && (Node.inf != tar))
300 {
301 continue;
302 }
304 double edgeDist = (*edge)->getDist();
306 if (edgeDist == 0)
307 {
308 continue;
309 }
311 VertInf *prevInf = BestNode.inf->pathNext;
313 Node.g = BestNode.g + cost(edgeDist, prevInf,
314 BestNode.inf, Node.inf);
316 // Calculate the Heuristic.
317 Node.h = dist(Node.inf->point, tar->point);
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;
324 bNodeFound = false;
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 }
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() );
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 }
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();
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;
412 assert(directEdge == NULL);
414 directEdge = new EdgeInf(src, tar);
415 tar->pathNext = src;
416 directEdge->setDist(dist(p, q));
417 directEdge->addConn(flag);
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 directEdge->addBlocker(0);
433 }
435 if (UseAStarSearch)
436 {
437 aStarPath(src, tar);
438 }
439 else
440 {
441 dijkstraPath(src, tar);
442 }
444 #if 0
445 PointMap::iterator t;
446 for (VertInf *t = vertices.connsBegin(); t != vertices.end();
447 t = t->lstNext)
448 {
450 t->id.print();
451 printf(" -> ");
452 t->pathNext->id.print();
453 printf("\n");
454 }
455 #endif
456 }
457 }
460 }