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/graph.h"
31 #include <math.h>
33 #ifdef LINEDEBUG
34 #include "SDL_gfxPrimitives.h"
35 #endif
37 namespace Avoid {
40 bool UseAStarSearch = true;
41 bool IgnoreRegions = true;
42 bool SelectiveReroute = true;
43 bool IncludeEndpoints = true;
44 bool UseLeesAlgorithm = false;
45 bool InvisibilityGrph = true;
46 bool PartialFeedback = false;
48 bool PartialTime = false;
51 void computeCompleteVis(void)
52 {
53 VertInf *beginVert = vertices.shapesBegin();
54 VertInf *endVert = vertices.end();
55 for (VertInf *i = beginVert; i != endVert; i = i->lstNext)
56 {
57 db_printf("-- CONSIDERING --\n");
58 i->id.db_print();
60 for (VertInf *j = i->lstPrev ; j != NULL; j = j->lstPrev)
61 {
62 bool knownNew = true;
63 EdgeInf::checkEdgeVisibility(i, j, knownNew);
64 }
65 }
66 }
69 void shapeVis(ShapeRef *shape)
70 {
71 if (!InvisibilityGrph)
72 {
73 // Clear shape from graph.
74 shape->removeFromGraph();
75 }
77 VertInf *shapeBegin = shape->firstVert();
78 VertInf *shapeEnd = shape->lastVert()->lstNext;
80 VertInf *pointsBegin = NULL;
81 if (IncludeEndpoints)
82 {
83 pointsBegin = vertices.connsBegin();
84 }
85 else
86 {
87 pointsBegin = vertices.shapesBegin();
88 }
90 for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
91 {
92 bool knownNew = true;
94 db_printf("-- CONSIDERING --\n");
95 curr->id.db_print();
97 db_printf("\tFirst Half:\n");
98 for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
99 {
100 EdgeInf::checkEdgeVisibility(curr, j, knownNew);
101 }
103 db_printf("\tSecond Half:\n");
104 VertInf *pointsEnd = vertices.end();
105 for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext)
106 {
107 EdgeInf::checkEdgeVisibility(curr, k, knownNew);
108 }
109 }
110 }
113 void shapeVisSweep(ShapeRef *shape)
114 {
115 if (!InvisibilityGrph)
116 {
117 // Clear shape from graph.
118 shape->removeFromGraph();
119 }
121 VertInf *startIter = shape->firstVert();
122 VertInf *endIter = shape->lastVert()->lstNext;
124 for (VertInf *i = startIter; i != endIter; i = i->lstNext)
125 {
126 vertexSweep(i);
127 }
128 }
131 void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
132 const bool gen_contains)
133 {
134 const VertID& pID = point->id;
136 // Make sure we're only doing ptVis for endpoints.
137 assert(!(pID.isShape));
139 if (!InvisibilityGrph)
140 {
141 point->removeFromGraph();
142 }
144 if (gen_contains && !(pID.isShape))
145 {
146 generateContains(point);
147 }
149 if (UseLeesAlgorithm)
150 {
151 vertexSweep(point);
152 }
153 else
154 {
155 VertInf *shapesEnd = vertices.end();
156 for (VertInf *k = vertices.shapesBegin(); k != shapesEnd;
157 k = k->lstNext)
158 {
159 EdgeInf::checkEdgeVisibility(point, k, knownNew);
160 }
161 if (IncludeEndpoints && partner)
162 {
163 EdgeInf::checkEdgeVisibility(point, partner, knownNew);
164 }
165 }
166 }
169 //============================================================================
170 // SWEEP CODE
171 //
173 static VertInf *centerInf;
174 static Point centerPoint;
175 static VertID centerID;
176 static double centerAngle;
178 #ifdef LINEDEBUG
179 SDL_Surface *avoid_screen = NULL;
180 #endif
183 class PointPair
184 {
185 public:
186 PointPair(VertInf *inf)
187 : vInf(inf)
188 {
189 double x = vInf->point.x - centerPoint.x;
190 double y = vInf->point.y - centerPoint.y;
192 angle = pos_to_angle(x, y);
193 }
194 bool operator==(const PointPair& rhs) const
195 {
196 if (vInf->id == rhs.vInf->id)
197 {
198 return true;
199 }
200 return false;
201 }
202 static double pos_to_angle(double x, double y)
203 {
204 double ang = atan(y / x);
205 ang = (ang * 180) / M_PI;
206 if (x < 0)
207 {
208 ang += 180;
209 }
210 else if (y < 0)
211 {
212 ang += 360;
213 }
214 return ang;
215 }
217 VertInf *vInf;
218 double angle;
219 };
222 typedef std::list<PointPair > VertList;
225 class EdgePair
226 {
227 public:
228 EdgePair(VertInf *v1, VertInf *v2, double d, double a)
229 : vInf1(v1), vInf2(v2), initdist(d), initangle(a)
230 {
231 currdist = initdist;
232 currangle = initangle;
233 }
234 bool operator<(const EdgePair& rhs) const
235 {
236 if (initdist == rhs.initdist)
237 {
238 // TODO: This is a bit of a hack, should be
239 // set by the call to the constructor.
240 return dist(centerPoint, vInf2->point) <
241 dist(centerPoint, rhs.vInf2->point);
242 }
243 return (initdist < rhs.initdist);
244 }
245 bool operator==(const EdgePair& rhs) const
246 {
247 if (((vInf1->id == rhs.vInf1->id) &&
248 (vInf2->id == rhs.vInf2->id)) ||
249 ((vInf1->id == rhs.vInf2->id) &&
250 (vInf2->id == rhs.vInf1->id)))
251 {
252 return true;
253 }
254 return false;
255 }
256 bool operator!=(const EdgePair& rhs) const
257 {
258 if (((vInf1->id == rhs.vInf1->id) &&
259 (vInf2->id == rhs.vInf2->id)) ||
260 ((vInf1->id == rhs.vInf2->id) &&
261 (vInf2->id == rhs.vInf1->id)))
262 {
263 return false;
264 }
265 return true;
266 }
267 void SetObsAng(double a)
268 {
269 obsAngle = fmod(initangle - (a - 180), 360);
271 //db_printf("SetObsAng: %.2f (from init %.2f, a %.2f)\n",
272 // obsAngle, initangle, a);
273 }
275 VertInf *vInf1;
276 VertInf *vInf2;
277 double initdist;
278 double initangle;
279 double currdist;
280 double currangle;
281 double obsAngle;
282 };
284 typedef std::set<EdgePair> EdgeSet;
287 static bool ppCompare(PointPair& pp1, PointPair& pp2)
288 {
289 if (pp1.angle == pp2.angle)
290 {
291 // If the points are colinear, then order them in increasing
292 // distance from the point we are sweeping around.
293 return dist(centerPoint, pp1.vInf->point) <
294 dist(centerPoint, pp2.vInf->point);
295 }
296 return pp1.angle < pp2.angle;
297 }
300 #define AHEAD 1
301 #define BEHIND -1
303 class isBoundingShape
304 {
305 public:
306 // constructor remembers the value provided
307 isBoundingShape(ShapeSet& set)
308 : ss(set)
309 { }
310 // the following is an overloading of the function call operator
311 bool operator () (const PointPair& pp)
312 {
313 if (pp.vInf->id.isShape &&
314 (ss.find(pp.vInf->id.objID) != ss.end()))
315 {
316 return true;
317 }
318 return false;
319 }
320 private:
321 ShapeSet& ss;
322 };
325 static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf,
326 bool lastVisible, double lastAngle, int *blocker)
327 {
329 if (!lastInf || (lastAngle != centerAngle))
330 {
331 // Nothing before it on the current ray
332 EdgeSet::iterator closestIt = T.begin();
333 if (closestIt != T.end())
334 {
336 Point &e1 = (*closestIt).vInf1->point;
337 Point &e2 = (*closestIt).vInf2->point;
339 if (segmentIntersect(centerInf->point, currInf->point, e1, e2))
340 {
341 *blocker = (*closestIt).vInf1->id.objID;
342 return false;
343 }
344 else
345 {
346 return true;
347 }
348 }
349 else
350 {
351 return true;
352 }
353 }
354 else
355 {
356 // There was another point before this on the ray (lastInf)
357 if (!lastVisible)
358 {
359 *blocker = -1;
360 return false;
361 }
362 else
363 {
364 // Check if there is an edge in T that blocks the ray
365 // between lastInf and currInf.
366 EdgeSet::iterator tfin = T.end();
367 for (EdgeSet::iterator l = T.begin(); l != tfin; ++l)
368 {
369 Point &e1 = (*l).vInf1->point;
370 Point &e2 = (*l).vInf2->point;
372 if (segmentIntersect(lastInf->point, currInf->point, e1, e2))
373 {
374 *blocker = (*l).vInf1->id.objID;
375 return false;
376 }
377 }
378 return true;
379 }
380 }
381 }
384 void vertexSweep(VertInf *vert)
385 {
386 VertID& pID = vert->id;
387 Point& pPoint = vert->point;
389 centerInf = vert;
390 centerID = pID;
391 centerPoint = pPoint;
392 Point centerPt = pPoint;
393 centerAngle = -1;
395 // List of shape (and maybe endpt) vertices, except p
396 // Sort list, around
397 VertList v;
399 // Initialise the vertex list
400 VertInf *beginVert = vertices.connsBegin();
401 VertInf *endVert = vertices.end();
402 for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext)
403 {
404 if (inf->id == centerID)
405 {
406 // Don't include the center point
407 continue;
408 }
410 if (inf->id.isShape)
411 {
412 // Add shape vertex
413 v.push_back(inf);
414 }
415 else
416 {
417 if (IncludeEndpoints)
418 {
419 if (centerID.isShape)
420 {
421 // Add endpoint vertex
422 v.push_back(inf);
423 }
424 else
425 {
426 // Center is an endpoint, so only include the other
427 // endpoint from the matching connector.
428 VertID partnerID = VertID(centerID.objID, false,
429 (centerID.vn == 1) ? 2 : 1);
430 if (inf->id == partnerID)
431 {
432 v.push_back(inf);
433 }
434 }
435 }
436 }
437 }
438 // TODO: This should be done with a sorted data type and insertion sort.
439 v.sort(ppCompare);
441 EdgeSet e;
442 ShapeSet& ss = contains[centerID];
444 // And edge to T that intersect the initial ray.
445 VertInf *last = vertices.end();
446 for (VertInf *k = vertices.shapesBegin(); k != last; )
447 {
448 VertID kID = k->id;
449 if (!(centerID.isShape) && (ss.find(kID.objID) != ss.end()))
450 {
451 unsigned int shapeID = kID.objID;
452 db_printf("Center is inside shape %u so ignore shape edges.\n",
453 shapeID);
454 // One of the endpoints is inside this shape so ignore it.
455 while ((k != last) && (k->id.objID == shapeID))
456 {
457 // And skip the other vertices from this shape.
458 k = k->lstNext;
459 }
460 continue;
461 }
463 VertInf *kPrev = k->shPrev;
464 if ((centerInf == k) || (centerInf == kPrev))
465 {
466 k = k->lstNext;
467 continue;
468 }
470 Point xaxis = { DBL_MAX, centerInf->point.y };
472 if (segmentIntersect(centerInf->point, xaxis, kPrev->point, k->point))
473 {
474 double distance;
475 if (vecDir(centerInf->point, xaxis, kPrev->point) == BEHIND)
476 {
477 distance = dist(centerInf->point, kPrev->point);
478 }
479 else
480 {
481 distance = dist(centerInf->point, k->point);
482 }
484 EdgePair intPair = EdgePair(k, kPrev, distance, 0.0);
485 e.insert(intPair).first;
486 }
487 k = k->lstNext;
488 }
490 // Start the actual sweep.
491 db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
493 VertInf *lastInf = NULL;
494 double lastAngle = 0;
495 bool lastVisible = false;
496 int lastBlocker = 0;
498 isBoundingShape isBounding(contains[centerID]);
499 VertList::iterator vfst = v.begin();
500 VertList::iterator vfin = v.end();
501 for (VertList::iterator t = vfst; t != vfin; ++t)
502 {
503 VertInf *currInf = (*t).vInf;
504 VertID& currID = currInf->id;
505 Point& currPt = currInf->point;
506 centerAngle = (*t).angle;
508 #ifdef LINEDEBUG
509 Sint16 ppx = (int) centerPt.x;
510 Sint16 ppy = (int) centerPt.y;
512 Sint16 cx = (int) currPt.x;
513 Sint16 cy = (int) currPt.y;
514 #endif
516 double currDist = dist(centerPt, currPt);
517 db_printf("Dist: %.1f.\n", currDist);
519 EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf);
520 if (edge == NULL)
521 {
522 edge = new EdgeInf(centerInf, currInf);
523 }
524 // Ignore vertices from bounding shapes, if sweeping round an endpoint.
525 if (!(centerID.isShape) && isBounding(*t))
526 {
527 if (InvisibilityGrph)
528 {
529 // if p and t can't see each other, add blank edge
530 db_printf("\tSkipping visibility edge... \n\t\t");
531 edge->addBlocker(currInf->id.objID);
532 edge->db_print();
533 }
534 continue;
535 }
538 bool cone1 = true, cone2 = true;
539 if (centerID.isShape)
540 {
541 cone1 = inValidRegion(centerInf->shPrev->point, centerPoint,
542 centerInf->shNext->point, currInf->point);
543 }
544 if (currInf->id.isShape)
545 {
546 cone2 = inValidRegion(currInf->shPrev->point, currInf->point,
547 currInf->shNext->point, centerPoint);
548 }
550 if (!cone1 || !cone2)
551 {
552 lastInf = NULL;
553 if (InvisibilityGrph)
554 {
555 db_printf("\tSetting invisibility edge... \n\t\t");
556 edge->addBlocker(0);
557 edge->db_print();
558 }
559 }
560 else
561 {
562 int blocker = 0;
563 // Check visibility.
564 bool currVisible = sweepVisible(e, currInf,
565 lastInf, lastVisible, lastAngle, &blocker);
566 if (blocker == -1)
567 {
568 blocker = lastBlocker;
569 }
570 if (currVisible)
571 {
572 #ifdef LINEDEBUG
573 lineRGBA(avoid_screen, ppx, ppy, cx, cy, 255, 0, 0, 32);
574 #endif
575 db_printf("\tSetting visibility edge... \n\t\t");
576 edge->setDist(currDist);
577 edge->db_print();
578 }
579 else if (InvisibilityGrph)
580 {
581 db_printf("\tSetting invisibility edge... \n\t\t");
582 edge->addBlocker(blocker);
583 edge->db_print();
584 }
586 lastVisible = currVisible;
587 lastInf = currInf;
588 lastAngle = centerAngle;
589 lastBlocker = blocker;
590 }
592 if (currID.isShape)
593 {
594 // This is a shape edge
595 Point& prevPt = currInf->shPrev->point;
596 Point& nextPt = currInf->shNext->point;
598 int prevDir = vecDir(centerPt, currPt, prevPt);
599 EdgePair prevPair = EdgePair(currInf, currInf->shPrev,
600 currDist, centerAngle);
602 EdgeSet::iterator ePtr;
603 if (prevDir == BEHIND)
604 {
605 ePtr = e.find(prevPair);
606 if (ePtr != e.end())
607 {
608 e.erase(ePtr);
609 }
610 }
611 else if ((prevDir == AHEAD) && (currInf->shPrev != centerInf))
612 {
613 double x = prevPt.x - currPt.x;
614 double y = prevPt.y - currPt.y;
615 double angle = PointPair::pos_to_angle(x, y);
616 prevPair.SetObsAng(angle);
618 ePtr = e.insert(prevPair).first;
619 }
622 int nextDir = vecDir(centerPt, currPt, nextPt);
623 EdgePair nextPair = EdgePair(currInf, currInf->shNext,
624 currDist, centerAngle);
626 if (nextDir == BEHIND)
627 {
628 ePtr = e.find(nextPair);
629 if (ePtr != e.end())
630 {
631 e.erase(ePtr);
632 }
633 }
634 else if ((nextDir == AHEAD) && (currInf->shNext != centerInf))
635 {
636 double x = nextPt.x - currPt.x;
637 double y = nextPt.y - currPt.y;
638 double angle = PointPair::pos_to_angle(x, y);
639 nextPair.SetObsAng(angle);
641 ePtr = e.insert(nextPair).first;
642 }
643 }
645 #ifdef LINEDEBUG
646 SDL_Flip(avoid_screen);
647 #endif
648 }
649 }
652 }