1 /*
2 * vim: ts=4 sw=4 et tw=0 wm=0
3 *
4 * libavoid - Fast, Incremental, Object-avoiding Line Router
5 *
6 * Copyright (C) 2004-2009 Monash University
7 *
8 * --------------------------------------------------------------------
9 * The Visibility Sweep technique is based upon the method described
10 * in Section 5.2 of:
11 * Lee, D.-T. (1978). Proximity and reachability in the plane.,
12 * PhD thesis, Department of Electrical Engineering,
13 * University of Illinois, Urbana, IL.
14 * --------------------------------------------------------------------
15 *
16 * This library is free software; you can redistribute it and/or
17 * modify it under the terms of the GNU Lesser General Public
18 * License as published by the Free Software Foundation; either
19 * version 2.1 of the License, or (at your option) any later version.
20 * See the file LICENSE.LGPL distributed with the library.
21 *
22 * Licensees holding a valid commercial license may use this file in
23 * accordance with the commercial license agreement provided with the
24 * library.
25 *
26 * This library is distributed in the hope that it will be useful,
27 * but WITHOUT ANY WARRANTY; without even the implied warranty of
28 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Author(s): Michael Wybrow <mjwybrow@users.sourceforge.net>
31 */
34 #include <algorithm>
35 #include <cfloat>
36 #define _USE_MATH_DEFINES
37 #include <cmath>
39 #include "libavoid/shape.h"
40 #include "libavoid/debug.h"
41 #include "libavoid/visibility.h"
42 #include "libavoid/vertices.h"
43 #include "libavoid/graph.h"
44 #include "libavoid/geometry.h"
45 #include "libavoid/router.h"
46 #include "libavoid/assertions.h"
48 #ifdef LINEDEBUG
49 #include "SDL_gfxPrimitives.h"
50 #endif
52 namespace Avoid {
55 void shapeVis(ShapeRef *shape)
56 {
57 Router *router = shape->router();
59 if ( !(router->InvisibilityGrph) )
60 {
61 // Clear shape from graph.
62 shape->removeFromGraph();
63 }
65 VertInf *shapeBegin = shape->firstVert();
66 VertInf *shapeEnd = shape->lastVert()->lstNext;
68 VertInf *pointsBegin = router->vertices.connsBegin();
69 for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
70 {
71 bool knownNew = true;
73 db_printf("-- CONSIDERING --\n");
74 curr->id.db_print();
76 db_printf("\tFirst Half:\n");
77 for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
78 {
79 if (j->id == dummyOrthogID)
80 {
81 // Don't include orthogonal dummy vertices.
82 continue;
83 }
84 EdgeInf::checkEdgeVisibility(curr, j, knownNew);
85 }
87 db_printf("\tSecond Half:\n");
88 VertInf *pointsEnd = router->vertices.end();
89 for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext)
90 {
91 if (k->id == dummyOrthogID)
92 {
93 // Don't include orthogonal dummy vertices.
94 continue;
95 }
96 EdgeInf::checkEdgeVisibility(curr, k, knownNew);
97 }
98 }
99 }
102 void shapeVisSweep(ShapeRef *shape)
103 {
104 Router *router = shape->router();
106 if ( !(router->InvisibilityGrph) )
107 {
108 // Clear shape from graph.
109 shape->removeFromGraph();
110 }
112 VertInf *startIter = shape->firstVert();
113 VertInf *endIter = shape->lastVert()->lstNext;
115 for (VertInf *i = startIter; i != endIter; i = i->lstNext)
116 {
117 vertexSweep(i);
118 }
119 }
122 void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
123 const bool gen_contains)
124 {
125 Router *router = point->_router;
126 const VertID& pID = point->id;
128 // Make sure we're only doing ptVis for endpoints.
129 COLA_ASSERT(!(pID.isShape));
131 if ( !(router->InvisibilityGrph) )
132 {
133 point->removeFromGraph();
134 }
136 if (gen_contains && !(pID.isShape))
137 {
138 router->generateContains(point);
139 }
141 if (router->UseLeesAlgorithm)
142 {
143 vertexSweep(point);
144 }
145 else
146 {
147 VertInf *shapesEnd = router->vertices.end();
148 for (VertInf *k = router->vertices.shapesBegin(); k != shapesEnd;
149 k = k->lstNext)
150 {
151 if (k->id == dummyOrthogID)
152 {
153 // Don't include orthogonal dummy vertices.
154 continue;
155 }
156 EdgeInf::checkEdgeVisibility(point, k, knownNew);
157 }
158 if (partner)
159 {
160 EdgeInf::checkEdgeVisibility(point, partner, knownNew);
161 }
162 }
163 }
166 //============================================================================
167 // SWEEP CODE
168 //
170 static VertInf *centerInf;
171 static Point centerPoint;
172 static VertID centerID;
175 class PointPair
176 {
177 public:
178 PointPair(VertInf *inf)
179 : vInf(inf)
180 {
181 double x = vInf->point.x - centerPoint.x;
182 double y = vInf->point.y - centerPoint.y;
184 angle = pos_to_angle(x, y);
185 distance = euclideanDist(centerPoint, vInf->point);
186 }
187 bool operator<(const PointPair& rhs) const
188 {
189 // Firstly order by angle.
190 if (angle == rhs.angle)
191 {
192 // If the points are collinear, then order them in increasing
193 // distance from the point we are sweeping around.
194 if (distance == rhs.distance)
195 {
196 // XXX: Add this assertion back if we require that
197 // connector endpoints have unique IDs. For the
198 // moment it is okay for them to have the same ID.
199 //COLA_ASSERT(vInf->id != rhs.vInf->id);
201 // If comparing two points at the same physical
202 // position, then order them by their VertIDs.
203 return vInf->id < rhs.vInf->id;
204 }
205 return distance < rhs.distance;
206 }
207 return angle < rhs.angle;
208 }
209 static double pos_to_angle(double x, double y)
210 {
211 if (y == 0)
212 {
213 return ((x < 0) ? 180 : 0);
214 }
215 else if (x == 0)
216 {
217 return ((y < 0) ? 270 : 90);
218 }
220 double ang = atan(y / x);
221 ang = (ang * 180) / M_PI;
223 if (x < 0)
224 {
225 ang += 180;
226 }
227 else if (y < 0)
228 {
229 ang += 360;
230 }
231 COLA_ASSERT(ang >= 0);
232 COLA_ASSERT(ang <= 360);
233 return ang;
234 }
236 VertInf *vInf;
237 double angle;
238 double distance;
239 };
242 typedef std::set<PointPair > VertSet;
245 class EdgePair
246 {
247 public:
248 EdgePair() :
249 vInf1(NULL), vInf2(NULL), dist1(0.0), dist2(0.0), angle(0.0),
250 angleDist(0.0)
251 {
252 // The default constuctor should never be called.
253 // This is defined to appease the MSVC compiler.
254 COLA_ASSERT(false);
255 }
256 EdgePair(const PointPair& p1, VertInf *v) :
257 vInf1(p1.vInf),
258 vInf2(v),
259 dist1(p1.distance),
260 dist2(euclideanDist(vInf2->point, centerPoint)),
261 angle(p1.angle),
262 angleDist(p1.distance)
263 {
264 }
265 bool operator<(const EdgePair& rhs) const
266 {
267 COLA_ASSERT(angle == rhs.angle);
268 if (angleDist == rhs.angleDist)
269 {
270 return (dist2 < rhs.dist2);
271 }
272 return (angleDist < rhs.angleDist);
273 }
274 bool operator==(const EdgePair& rhs) const
275 {
276 if (((vInf1->id == rhs.vInf1->id) &&
277 (vInf2->id == rhs.vInf2->id)) ||
278 ((vInf1->id == rhs.vInf2->id) &&
279 (vInf2->id == rhs.vInf1->id)))
280 {
281 return true;
282 }
283 return false;
284 }
285 bool operator!=(const EdgePair& rhs) const
286 {
287 if (((vInf1->id == rhs.vInf1->id) &&
288 (vInf2->id == rhs.vInf2->id)) ||
289 ((vInf1->id == rhs.vInf2->id) &&
290 (vInf2->id == rhs.vInf1->id)))
291 {
292 return false;
293 }
294 return true;
295 }
296 void setNegativeAngle(void)
297 {
298 angle = -1.0;
299 }
300 double setCurrAngle(const PointPair& p)
301 {
302 if (p.vInf->point == vInf1->point)
303 {
304 angleDist = dist1;
305 angle = p.angle;
306 }
307 else if (p.vInf->point == vInf2->point)
308 {
309 angleDist = dist2;
310 angle = p.angle;
311 }
312 else if (p.angle != angle)
313 {
314 COLA_ASSERT(p.angle > angle);
315 angle = p.angle;
316 Point pp;
317 int result = rayIntersectPoint(vInf1->point, vInf2->point,
318 centerPoint, p.vInf->point, &(pp.x), &(pp.y));
319 if (result != DO_INTERSECT)
320 {
321 // This can happen with points that appear to have the
322 // same angle but at are at slightly different positions
323 angleDist = std::min(dist1, dist2);
324 }
325 else
326 {
327 angleDist = euclideanDist(pp, centerPoint);
328 }
329 }
331 return angleDist;
332 }
334 VertInf *vInf1;
335 VertInf *vInf2;
336 double dist1;
337 double dist2;
338 double angle;
339 double angleDist;
340 };
342 typedef std::list<EdgePair> SweepEdgeList;
345 #define AHEAD 1
346 #define BEHIND -1
348 class isBoundingShape
349 {
350 public:
351 // Class instance remembers the ShapeSet.
352 isBoundingShape(ShapeSet& set) :
353 ss(set)
354 { }
355 // The following is an overloading of the function call operator.
356 bool operator () (const PointPair& pp)
357 {
358 if (pp.vInf->id.isShape &&
359 (ss.find(pp.vInf->id.objID) != ss.end()))
360 {
361 return true;
362 }
363 return false;
364 }
365 private:
366 // MSVC wants to generate the assignment operator and the default
367 // constructor, but fails. Therefore we declare them private and
368 // don't implement them.
369 isBoundingShape & operator=(isBoundingShape const &);
370 isBoundingShape();
372 ShapeSet& ss;
373 };
376 static bool sweepVisible(SweepEdgeList& T, const PointPair& point,
377 std::set<unsigned int>& onBorderIDs, int *blocker)
378 {
379 if (T.empty())
380 {
381 // No blocking edges.
382 return true;
383 }
385 Router *router = point.vInf->_router;
386 bool visible = true;
388 SweepEdgeList::const_iterator closestIt = T.begin();
389 SweepEdgeList::const_iterator end = T.end();
390 while (closestIt != end)
391 {
392 if ((point.vInf->point == closestIt->vInf1->point) ||
393 (point.vInf->point == closestIt->vInf2->point))
394 {
395 // If the ray intersects just the endpoint of a
396 // blocking edge then ignore that edge.
397 ++closestIt;
398 continue;
399 }
400 break;
401 }
402 if (closestIt == end)
403 {
404 return true;
405 }
407 if (! point.vInf->id.isShape )
408 {
409 // It's a connector endpoint, so we have to ignore
410 // edges of containing shapes for determining visibility.
411 ShapeSet& rss = router->contains[point.vInf->id];
412 while (closestIt != end)
413 {
414 if (rss.find(closestIt->vInf1->id.objID) == rss.end())
415 {
416 // This is not a containing edge so do the normal
417 // test and then stop.
418 if (point.distance > closestIt->angleDist)
419 {
420 visible = false;
421 }
422 else if ((point.distance == closestIt->angleDist) &&
423 onBorderIDs.find(closestIt->vInf1->id.objID) !=
424 onBorderIDs.end())
425 {
426 // Touching, but centerPoint is on another edge of
427 // shape shape, so count as blocking.
428 visible = false;
429 }
430 break;
431 }
432 // This was a containing edge, so consider the next along.
433 ++closestIt;
434 }
435 }
436 else
437 {
438 // Just test to see if this point is closer than the closest
439 // edge blocking this ray.
440 if (point.distance > closestIt->angleDist)
441 {
442 visible = false;
443 }
444 else if ((point.distance == closestIt->angleDist) &&
445 onBorderIDs.find(closestIt->vInf1->id.objID) !=
446 onBorderIDs.end())
447 {
448 // Touching, but centerPoint is on another edge of
449 // shape shape, so count as blocking.
450 visible = false;
451 }
452 }
454 if (!visible)
455 {
456 *blocker = (*closestIt).vInf1->id.objID;
457 #ifdef LINEDEBUG
458 Point &e1 = (*closestIt).vInf1->point;
459 Point &e2 = (*closestIt).vInf2->point;
461 if (router->avoid_screen)
462 {
463 int canx = 151;
464 int cany = 55;
465 lineRGBA(router->avoid_screen, e1.x + canx, e1.y + cany,
466 e2.x + canx, e2.y + cany, 0, 0, 225, 255);
467 }
468 #endif
469 }
470 return visible;
471 }
474 void vertexSweep(VertInf *vert)
475 {
476 Router *router = vert->_router;
477 VertID& pID = vert->id;
478 Point& pPoint = vert->point;
480 centerInf = vert;
481 centerID = pID;
482 centerPoint = pPoint;
483 Point centerPt = pPoint;
485 // List of shape (and maybe endpt) vertices, except p
486 // Sort list, around
487 VertSet v;
489 // Initialise the vertex list
490 ShapeSet& ss = router->contains[centerID];
491 VertInf *beginVert = router->vertices.connsBegin();
492 VertInf *endVert = router->vertices.end();
493 for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext)
494 {
495 if (inf == centerInf)
496 {
497 // Don't include the center point itself.
498 continue;
499 }
500 else if (inf->id == dummyOrthogID)
501 {
502 // Don't include orthogonal dummy vertices.
503 continue;
504 }
506 if (!(centerID.isShape) && (ss.find(inf->id.objID) != ss.end()))
507 {
508 // Don't include edge points of containing shapes.
509 unsigned int shapeID = inf->id.objID;
510 db_printf("Center is inside shape %u so ignore shape edges.\n",
511 shapeID);
512 continue;
513 }
515 if (inf->id.isShape)
516 {
517 // Add shape vertex.
518 v.insert(inf);
519 }
520 else
521 {
522 // Add connector endpoint.
523 if (centerID.isShape)
524 {
525 // Center is a shape vertex, so add all endpoint vertices.
526 v.insert(inf);
527 }
528 else
529 {
530 // Center is an endpoint, so only include the other
531 // endpoint from the matching connector.
532 VertID partnerID = VertID(centerID.objID, false,
533 (centerID.vn == 1) ? 2 : 1);
534 if (inf->id == partnerID)
535 {
536 v.insert(inf);
537 }
538 }
539 }
540 }
541 std::set<unsigned int> onBorderIDs;
543 // Add edges to T that intersect the initial ray.
544 SweepEdgeList e;
545 VertSet::const_iterator vbegin = v.begin();
546 VertSet::const_iterator vend = v.end();
547 for (VertSet::const_iterator t = vbegin; t != vend; ++t)
548 {
549 VertInf *k = t->vInf;
551 COLA_ASSERT(centerInf != k);
552 COLA_ASSERT(centerID.isShape || (ss.find(k->id.objID) == ss.end()));
554 Point xaxis(DBL_MAX, centerInf->point.y);
556 VertInf *kPrev = k->shPrev;
557 VertInf *kNext = k->shNext;
558 if (kPrev && (kPrev != centerInf) &&
559 (vecDir(centerInf->point, xaxis, kPrev->point) == AHEAD))
560 {
561 if (segmentIntersect(centerInf->point, xaxis, kPrev->point,
562 k->point))
563 {
564 EdgePair intPair = EdgePair(*t, kPrev);
565 e.push_back(intPair);
566 }
567 if ((vecDir(kPrev->point, k->point, centerInf->point) == 0) &&
568 inBetween(kPrev->point, k->point, centerInf->point))
569 {
570 // Record that centerPoint is on an obstacle line.
571 onBorderIDs.insert(k->id.objID);
572 }
573 }
574 else if (kNext && (kNext != centerInf) &&
575 (vecDir(centerInf->point, xaxis, kNext->point) == AHEAD))
576 {
577 if (segmentIntersect(centerInf->point, xaxis, kNext->point,
578 k->point))
579 {
580 EdgePair intPair = EdgePair(*t, kNext);
581 e.push_back(intPair);
582 }
583 if ((vecDir(kNext->point, k->point, centerInf->point) == 0) &&
584 inBetween(kNext->point, k->point, centerInf->point))
585 {
586 // Record that centerPoint is on an obstacle line.
587 onBorderIDs.insert(k->id.objID);
588 }
589 }
590 }
591 for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c)
592 {
593 (*c).setNegativeAngle();
594 }
597 // Start the actual sweep.
598 db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
600 isBoundingShape isBounding(ss);
601 for (VertSet::const_iterator t = vbegin; t != vend; ++t)
602 {
603 VertInf *currInf = (*t).vInf;
604 VertID& currID = currInf->id;
605 Point& currPt = currInf->point;
607 #ifdef LINEDEBUG
608 Sint16 ppx = (int) centerPt.x;
609 Sint16 ppy = (int) centerPt.y;
611 Sint16 cx = (int) currPt.x;
612 Sint16 cy = (int) currPt.y;
614 int canx = 151;
615 int cany = 55;
616 #endif
618 const double& currDist = (*t).distance;
620 EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf);
621 if (edge == NULL)
622 {
623 edge = new EdgeInf(centerInf, currInf);
624 }
626 for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c)
627 {
628 (*c).setCurrAngle(*t);
629 }
630 e.sort();
632 // Check visibility.
633 int blocker = 0;
634 bool currVisible = sweepVisible(e, *t, onBorderIDs, &blocker);
636 bool cone1 = true, cone2 = true;
637 if (centerID.isShape)
638 {
639 cone1 = inValidRegion(router->IgnoreRegions,
640 centerInf->shPrev->point, centerPoint,
641 centerInf->shNext->point, currInf->point);
642 }
643 if (currInf->id.isShape)
644 {
645 cone2 = inValidRegion(router->IgnoreRegions,
646 currInf->shPrev->point, currInf->point,
647 currInf->shNext->point, centerPoint);
648 }
650 if (!cone1 || !cone2)
651 {
652 if (router->InvisibilityGrph)
653 {
654 db_printf("\tSetting invisibility edge... \n\t\t");
655 edge->addBlocker(0);
656 edge->db_print();
657 }
658 }
659 else
660 {
661 if (currVisible)
662 {
663 #ifdef LINEDEBUG
664 if (router->avoid_screen)
665 {
666 lineRGBA(router->avoid_screen, ppx + canx, ppy + cany,
667 cx + canx, cy + cany, 255, 0, 0, 75);
668 SDL_Delay(1000);
669 }
670 #endif
671 db_printf("\tSetting visibility edge... \n\t\t");
672 edge->setDist(currDist);
673 edge->db_print();
674 }
675 else if (router->InvisibilityGrph)
676 {
677 db_printf("\tSetting invisibility edge... \n\t\t");
678 edge->addBlocker(blocker);
679 edge->db_print();
680 }
681 }
683 if (!(edge->added()) && !(router->InvisibilityGrph))
684 {
685 delete edge;
686 edge = NULL;
687 }
689 if (currID.isShape)
690 {
691 // This is a shape edge
693 if (currInf->shPrev != centerInf)
694 {
695 Point& prevPt = currInf->shPrev->point;
696 int prevDir = vecDir(centerPt, currPt, prevPt);
697 EdgePair prevPair = EdgePair(*t, currInf->shPrev);
699 if (prevDir == BEHIND)
700 {
701 e.remove(prevPair);
702 }
703 else if (prevDir == AHEAD)
704 {
705 e.push_front(prevPair);
706 }
707 }
709 if (currInf->shNext != centerInf)
710 {
711 Point& nextPt = currInf->shNext->point;
712 int nextDir = vecDir(centerPt, currPt, nextPt);
713 EdgePair nextPair = EdgePair(*t, currInf->shNext);
715 if (nextDir == BEHIND)
716 {
717 e.remove(nextPair);
718 }
719 else if (nextDir == AHEAD)
720 {
721 e.push_front(nextPair);
722 }
723 }
724 }
725 #ifdef LINEDEBUG
726 if (router->avoid_screen)
727 {
728 SDL_Flip(router->avoid_screen);
729 }
730 #endif
731 }
732 }
735 }