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