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 * This library is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU Lesser General Public
9 * License as published by the Free Software Foundation; either
10 * version 2.1 of the License, or (at your option) any later version.
11 *
12 * This library is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 * Lesser General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public
18 * License along with this library; if not, write to the Free Software
19 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
20 *
21 */
24 #include <cfloat>
26 #include "libavoid/shape.h"
27 #include "libavoid/debug.h"
28 #include "libavoid/visibility.h"
29 #include "libavoid/vertices.h"
30 #include "libavoid/graph.h"
31 #include "libavoid/geometry.h"
33 #include <math.h>
35 #ifdef LINEDEBUG
36 #include "SDL_gfxPrimitives.h"
37 #endif
39 namespace Avoid {
42 bool UseAStarSearch = true;
43 bool IgnoreRegions = true;
44 bool SelectiveReroute = true;
45 bool IncludeEndpoints = true;
46 bool UseLeesAlgorithm = false;
47 bool InvisibilityGrph = true;
48 bool PartialFeedback = false;
50 bool PartialTime = false;
53 void computeCompleteVis(void)
54 {
55 VertInf *beginVert = vertices.shapesBegin();
56 VertInf *endVert = vertices.end();
57 for (VertInf *i = beginVert; i != endVert; i = i->lstNext)
58 {
59 db_printf("-- CONSIDERING --\n");
60 i->id.db_print();
62 for (VertInf *j = i->lstPrev ; j != NULL; j = j->lstPrev)
63 {
64 bool knownNew = true;
65 EdgeInf::checkEdgeVisibility(i, j, knownNew);
66 }
67 }
68 }
71 void shapeVis(ShapeRef *shape)
72 {
73 if (!InvisibilityGrph)
74 {
75 // Clear shape from graph.
76 shape->removeFromGraph();
77 }
79 VertInf *shapeBegin = shape->firstVert();
80 VertInf *shapeEnd = shape->lastVert()->lstNext;
82 VertInf *pointsBegin = NULL;
83 if (IncludeEndpoints)
84 {
85 pointsBegin = vertices.connsBegin();
86 }
87 else
88 {
89 pointsBegin = vertices.shapesBegin();
90 }
92 for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
93 {
94 bool knownNew = true;
96 db_printf("-- CONSIDERING --\n");
97 curr->id.db_print();
99 db_printf("\tFirst Half:\n");
100 for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
101 {
102 EdgeInf::checkEdgeVisibility(curr, j, knownNew);
103 }
105 db_printf("\tSecond Half:\n");
106 VertInf *pointsEnd = vertices.end();
107 for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext)
108 {
109 EdgeInf::checkEdgeVisibility(curr, k, knownNew);
110 }
111 }
112 }
115 void shapeVisSweep(ShapeRef *shape)
116 {
117 if (!InvisibilityGrph)
118 {
119 // Clear shape from graph.
120 shape->removeFromGraph();
121 }
123 VertInf *startIter = shape->firstVert();
124 VertInf *endIter = shape->lastVert()->lstNext;
126 for (VertInf *i = startIter; i != endIter; i = i->lstNext)
127 {
128 vertexSweep(i);
129 }
130 }
133 void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
134 const bool gen_contains)
135 {
136 const VertID& pID = point->id;
138 // Make sure we're only doing ptVis for endpoints.
139 assert(!(pID.isShape));
141 if (!InvisibilityGrph)
142 {
143 point->removeFromGraph();
144 }
146 if (gen_contains && !(pID.isShape))
147 {
148 generateContains(point);
149 }
151 if (UseLeesAlgorithm)
152 {
153 vertexSweep(point);
154 }
155 else
156 {
157 VertInf *shapesEnd = vertices.end();
158 for (VertInf *k = vertices.shapesBegin(); k != shapesEnd;
159 k = k->lstNext)
160 {
161 EdgeInf::checkEdgeVisibility(point, k, knownNew);
162 }
163 if (IncludeEndpoints && partner)
164 {
165 EdgeInf::checkEdgeVisibility(point, partner, knownNew);
166 }
167 }
168 }
171 //============================================================================
172 // SWEEP CODE
173 //
175 static VertInf *centerInf;
176 static Point centerPoint;
177 static VertID centerID;
178 static double centerAngle;
180 #ifdef LINEDEBUG
181 SDL_Surface *avoid_screen = NULL;
182 #endif
185 class PointPair
186 {
187 public:
188 PointPair(VertInf *inf)
189 : vInf(inf)
190 {
191 double x = vInf->point.x - centerPoint.x;
192 double y = vInf->point.y - centerPoint.y;
194 angle = pos_to_angle(x, y);
195 }
196 bool operator==(const PointPair& rhs) const
197 {
198 if (vInf->id == rhs.vInf->id)
199 {
200 return true;
201 }
202 return false;
203 }
204 static double pos_to_angle(double x, double y)
205 {
206 double ang = atan(y / x);
207 ang = (ang * 180) / M_PI;
208 if (x < 0)
209 {
210 ang += 180;
211 }
212 else if (y < 0)
213 {
214 ang += 360;
215 }
216 return ang;
217 }
219 VertInf *vInf;
220 double angle;
221 };
224 typedef std::list<PointPair > VertList;
227 class EdgePair
228 {
229 public:
230 EdgePair(VertInf *v1, VertInf *v2, double d, double a)
231 : vInf1(v1), vInf2(v2), initdist(d), initangle(a)
232 {
233 currdist = initdist;
234 currangle = initangle;
235 }
236 bool operator<(const EdgePair& rhs) const
237 {
238 if (initdist == rhs.initdist)
239 {
240 // TODO: This is a bit of a hack, should be
241 // set by the call to the constructor.
242 return dist(centerPoint, vInf2->point) <
243 dist(centerPoint, rhs.vInf2->point);
244 }
245 return (initdist < rhs.initdist);
246 }
247 bool operator==(const EdgePair& rhs) const
248 {
249 if (((vInf1->id == rhs.vInf1->id) &&
250 (vInf2->id == rhs.vInf2->id)) ||
251 ((vInf1->id == rhs.vInf2->id) &&
252 (vInf2->id == rhs.vInf1->id)))
253 {
254 return true;
255 }
256 return false;
257 }
258 bool operator!=(const EdgePair& rhs) const
259 {
260 if (((vInf1->id == rhs.vInf1->id) &&
261 (vInf2->id == rhs.vInf2->id)) ||
262 ((vInf1->id == rhs.vInf2->id) &&
263 (vInf2->id == rhs.vInf1->id)))
264 {
265 return false;
266 }
267 return true;
268 }
269 void SetObsAng(double a)
270 {
271 obsAngle = fmod(initangle - (a - 180), 360);
273 //db_printf("SetObsAng: %.2f (from init %.2f, a %.2f)\n",
274 // obsAngle, initangle, a);
275 }
277 VertInf *vInf1;
278 VertInf *vInf2;
279 double initdist;
280 double initangle;
281 double currdist;
282 double currangle;
283 double obsAngle;
284 };
286 typedef std::set<EdgePair> EdgeSet;
289 static bool ppCompare(PointPair& pp1, PointPair& pp2)
290 {
291 if (pp1.angle == pp2.angle)
292 {
293 // If the points are colinear, then order them in increasing
294 // distance from the point we are sweeping around.
295 return dist(centerPoint, pp1.vInf->point) <
296 dist(centerPoint, pp2.vInf->point);
297 }
298 return pp1.angle < pp2.angle;
299 }
302 #define AHEAD 1
303 #define BEHIND -1
305 class isBoundingShape
306 {
307 public:
308 // constructor remembers the value provided
309 isBoundingShape(ShapeSet& set)
310 : ss(set)
311 { }
312 // the following is an overloading of the function call operator
313 bool operator () (const PointPair& pp)
314 {
315 if (pp.vInf->id.isShape &&
316 (ss.find(pp.vInf->id.objID) != ss.end()))
317 {
318 return true;
319 }
320 return false;
321 }
322 private:
323 ShapeSet& ss;
324 };
327 static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf,
328 bool lastVisible, double lastAngle, int *blocker)
329 {
331 if (!lastInf || (lastAngle != centerAngle))
332 {
333 // Nothing before it on the current ray
334 EdgeSet::iterator closestIt = T.begin();
335 if (closestIt != T.end())
336 {
338 Point &e1 = (*closestIt).vInf1->point;
339 Point &e2 = (*closestIt).vInf2->point;
341 if (segmentIntersect(centerInf->point, currInf->point, e1, e2))
342 {
343 *blocker = (*closestIt).vInf1->id.objID;
344 return false;
345 }
346 else
347 {
348 return true;
349 }
350 }
351 else
352 {
353 return true;
354 }
355 }
356 else
357 {
358 // There was another point before this on the ray (lastInf)
359 if (!lastVisible)
360 {
361 *blocker = -1;
362 return false;
363 }
364 else
365 {
366 // Check if there is an edge in T that blocks the ray
367 // between lastInf and currInf.
368 EdgeSet::iterator tfin = T.end();
369 for (EdgeSet::iterator l = T.begin(); l != tfin; ++l)
370 {
371 Point &e1 = (*l).vInf1->point;
372 Point &e2 = (*l).vInf2->point;
374 if (segmentIntersect(lastInf->point, currInf->point, e1, e2))
375 {
376 *blocker = (*l).vInf1->id.objID;
377 return false;
378 }
379 }
380 return true;
381 }
382 }
383 }
386 void vertexSweep(VertInf *vert)
387 {
388 VertID& pID = vert->id;
389 Point& pPoint = vert->point;
391 centerInf = vert;
392 centerID = pID;
393 centerPoint = pPoint;
394 Point centerPt = pPoint;
395 centerAngle = -1;
397 // List of shape (and maybe endpt) vertices, except p
398 // Sort list, around
399 VertList v;
401 // Initialise the vertex list
402 VertInf *beginVert = vertices.connsBegin();
403 VertInf *endVert = vertices.end();
404 for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext)
405 {
406 if (inf->id == centerID)
407 {
408 // Don't include the center point
409 continue;
410 }
412 if (inf->id.isShape)
413 {
414 // Add shape vertex
415 v.push_back(inf);
416 }
417 else
418 {
419 if (IncludeEndpoints)
420 {
421 if (centerID.isShape)
422 {
423 // Add endpoint vertex
424 v.push_back(inf);
425 }
426 else
427 {
428 // Center is an endpoint, so only include the other
429 // endpoint from the matching connector.
430 VertID partnerID = VertID(centerID.objID, false,
431 (centerID.vn == 1) ? 2 : 1);
432 if (inf->id == partnerID)
433 {
434 v.push_back(inf);
435 }
436 }
437 }
438 }
439 }
440 // TODO: This should be done with a sorted data type and insertion sort.
441 v.sort(ppCompare);
443 EdgeSet e;
444 ShapeSet& ss = contains[centerID];
446 // And edge to T that intersect the initial ray.
447 VertInf *last = vertices.end();
448 for (VertInf *k = vertices.shapesBegin(); k != last; )
449 {
450 VertID kID = k->id;
451 if (!(centerID.isShape) && (ss.find(kID.objID) != ss.end()))
452 {
453 unsigned int shapeID = kID.objID;
454 db_printf("Center is inside shape %u so ignore shape edges.\n",
455 shapeID);
456 // One of the endpoints is inside this shape so ignore it.
457 while ((k != last) && (k->id.objID == shapeID))
458 {
459 // And skip the other vertices from this shape.
460 k = k->lstNext;
461 }
462 continue;
463 }
465 VertInf *kPrev = k->shPrev;
466 if ((centerInf == k) || (centerInf == kPrev))
467 {
468 k = k->lstNext;
469 continue;
470 }
472 Point xaxis = { DBL_MAX, centerInf->point.y };
474 if (segmentIntersect(centerInf->point, xaxis, kPrev->point, k->point))
475 {
476 double distance;
477 if (vecDir(centerInf->point, xaxis, kPrev->point) == BEHIND)
478 {
479 distance = dist(centerInf->point, kPrev->point);
480 }
481 else
482 {
483 distance = dist(centerInf->point, k->point);
484 }
486 EdgePair intPair = EdgePair(k, kPrev, distance, 0.0);
487 e.insert(intPair).first;
488 }
489 k = k->lstNext;
490 }
492 // Start the actual sweep.
493 db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
495 VertInf *lastInf = NULL;
496 double lastAngle = 0;
497 bool lastVisible = false;
498 int lastBlocker = 0;
500 isBoundingShape isBounding(contains[centerID]);
501 VertList::iterator vfst = v.begin();
502 VertList::iterator vfin = v.end();
503 for (VertList::iterator t = vfst; t != vfin; ++t)
504 {
505 VertInf *currInf = (*t).vInf;
506 VertID& currID = currInf->id;
507 Point& currPt = currInf->point;
508 centerAngle = (*t).angle;
510 #ifdef LINEDEBUG
511 Sint16 ppx = (int) centerPt.x;
512 Sint16 ppy = (int) centerPt.y;
514 Sint16 cx = (int) currPt.x;
515 Sint16 cy = (int) currPt.y;
516 #endif
518 double currDist = dist(centerPt, currPt);
519 db_printf("Dist: %.1f.\n", currDist);
521 EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf);
522 if (edge == NULL)
523 {
524 edge = new EdgeInf(centerInf, currInf);
525 }
526 // Ignore vertices from bounding shapes, if sweeping round an endpoint.
527 if (!(centerID.isShape) && isBounding(*t))
528 {
529 if (InvisibilityGrph)
530 {
531 // if p and t can't see each other, add blank edge
532 db_printf("\tSkipping visibility edge... \n\t\t");
533 edge->addBlocker(currInf->id.objID);
534 edge->db_print();
535 }
536 continue;
537 }
540 bool cone1 = true, cone2 = true;
541 if (centerID.isShape)
542 {
543 cone1 = inValidRegion(centerInf->shPrev->point, centerPoint,
544 centerInf->shNext->point, currInf->point);
545 }
546 if (currInf->id.isShape)
547 {
548 cone2 = inValidRegion(currInf->shPrev->point, currInf->point,
549 currInf->shNext->point, centerPoint);
550 }
552 if (!cone1 || !cone2)
553 {
554 lastInf = NULL;
555 if (InvisibilityGrph)
556 {
557 db_printf("\tSetting invisibility edge... \n\t\t");
558 edge->addBlocker(0);
559 edge->db_print();
560 }
561 }
562 else
563 {
564 int blocker = 0;
565 // Check visibility.
566 bool currVisible = sweepVisible(e, currInf,
567 lastInf, lastVisible, lastAngle, &blocker);
568 if (blocker == -1)
569 {
570 blocker = lastBlocker;
571 }
572 if (currVisible)
573 {
574 #ifdef LINEDEBUG
575 lineRGBA(avoid_screen, ppx, ppy, cx, cy, 255, 0, 0, 32);
576 #endif
577 db_printf("\tSetting visibility edge... \n\t\t");
578 edge->setDist(currDist);
579 edge->db_print();
580 }
581 else if (InvisibilityGrph)
582 {
583 db_printf("\tSetting invisibility edge... \n\t\t");
584 edge->addBlocker(blocker);
585 edge->db_print();
586 }
588 lastVisible = currVisible;
589 lastInf = currInf;
590 lastAngle = centerAngle;
591 lastBlocker = blocker;
592 }
594 if (currID.isShape)
595 {
596 // This is a shape edge
597 Point& prevPt = currInf->shPrev->point;
598 Point& nextPt = currInf->shNext->point;
600 int prevDir = vecDir(centerPt, currPt, prevPt);
601 EdgePair prevPair = EdgePair(currInf, currInf->shPrev,
602 currDist, centerAngle);
604 EdgeSet::iterator ePtr;
605 if (prevDir == BEHIND)
606 {
607 // XXX: Strangely e.find does not return the correct results.
608 // ePtr = e.find(prevPair);
609 ePtr = std::find(e.begin(), e.end(), prevPair);
610 if (ePtr != e.end())
611 {
612 e.erase(ePtr);
613 }
614 }
615 else if ((prevDir == AHEAD) && (currInf->shPrev != centerInf))
616 {
617 double x = prevPt.x - currPt.x;
618 double y = prevPt.y - currPt.y;
619 double angle = PointPair::pos_to_angle(x, y);
620 prevPair.SetObsAng(angle);
622 ePtr = e.insert(prevPair).first;
623 }
626 int nextDir = vecDir(centerPt, currPt, nextPt);
627 EdgePair nextPair = EdgePair(currInf, currInf->shNext,
628 currDist, centerAngle);
630 if (nextDir == BEHIND)
631 {
632 // XXX: Strangely e.find does not return the correct results.
633 // ePtr = e.find(nextPair);
634 ePtr = std::find(e.begin(), e.end(), nextPair);
635 if (ePtr != e.end())
636 {
637 e.erase(ePtr);
638 }
639 }
640 else if ((nextDir == AHEAD) && (currInf->shNext != centerInf))
641 {
642 double x = nextPt.x - currPt.x;
643 double y = nextPt.y - currPt.y;
644 double angle = PointPair::pos_to_angle(x, y);
645 nextPair.SetObsAng(angle);
647 ePtr = e.insert(nextPair).first;
648 }
649 }
651 #ifdef LINEDEBUG
652 SDL_Flip(avoid_screen);
653 #endif
654 }
655 }
658 }