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