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 else
320 {
321 return true;
322 }
323 }
324 else
325 {
326 return true;
327 }
328 }
329 else
330 {
331 // There was another point before this on the ray (lastInf)
332 if (!lastVisible)
333 {
334 *blocker = -1;
335 return false;
336 }
337 else
338 {
339 // Check if there is an edge in T that blocks the ray
340 // between lastInf and currInf.
341 EdgeSet::iterator tfin = T.end();
342 for (EdgeSet::iterator l = T.begin(); l != tfin; ++l)
343 {
344 Point &e1 = (*l).vInf1->point;
345 Point &e2 = (*l).vInf2->point;
347 if (segmentIntersect(lastInf->point, currInf->point, e1, e2))
348 {
349 *blocker = (*l).vInf1->id.objID;
350 return false;
351 }
352 }
353 return true;
354 }
355 }
356 }
359 void vertexSweep(VertInf *vert)
360 {
361 Router *router = vert->_router;
362 VertID& pID = vert->id;
363 Point& pPoint = vert->point;
365 centerInf = vert;
366 centerID = pID;
367 centerPoint = pPoint;
368 Point centerPt = pPoint;
369 centerAngle = -1;
371 // List of shape (and maybe endpt) vertices, except p
372 // Sort list, around
373 VertList v;
375 // Initialise the vertex list
376 VertInf *beginVert = router->vertices.connsBegin();
377 VertInf *endVert = router->vertices.end();
378 for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext)
379 {
380 if (inf->id == centerID)
381 {
382 // Don't include the center point
383 continue;
384 }
386 if (inf->id.isShape)
387 {
388 // Add shape vertex
389 v.push_back(inf);
390 }
391 else
392 {
393 if (router->IncludeEndpoints)
394 {
395 if (centerID.isShape)
396 {
397 // Add endpoint vertex
398 v.push_back(inf);
399 }
400 else
401 {
402 // Center is an endpoint, so only include the other
403 // endpoint from the matching connector.
404 VertID partnerID = VertID(centerID.objID, false,
405 (centerID.vn == 1) ? 2 : 1);
406 if (inf->id == partnerID)
407 {
408 v.push_back(inf);
409 }
410 }
411 }
412 }
413 }
414 // TODO: This should be done with a sorted data type and insertion sort.
415 v.sort(ppCompare);
417 EdgeSet e;
418 ShapeSet& ss = router->contains[centerID];
420 // And edge to T that intersect the initial ray.
421 VertInf *last = router->vertices.end();
422 for (VertInf *k = router->vertices.shapesBegin(); k != last; )
423 {
424 VertID kID = k->id;
425 if (!(centerID.isShape) && (ss.find(kID.objID) != ss.end()))
426 {
427 unsigned int shapeID = kID.objID;
428 db_printf("Center is inside shape %u so ignore shape edges.\n",
429 shapeID);
430 // One of the endpoints is inside this shape so ignore it.
431 while ((k != last) && (k->id.objID == shapeID))
432 {
433 // And skip the other vertices from this shape.
434 k = k->lstNext;
435 }
436 continue;
437 }
439 VertInf *kPrev = k->shPrev;
440 if ((centerInf == k) || (centerInf == kPrev))
441 {
442 k = k->lstNext;
443 continue;
444 }
446 Point xaxis = { DBL_MAX, centerInf->point.y };
448 if (segmentIntersect(centerInf->point, xaxis, kPrev->point, k->point))
449 {
450 double distance;
451 if (vecDir(centerInf->point, xaxis, kPrev->point) == BEHIND)
452 {
453 distance = dist(centerInf->point, kPrev->point);
454 }
455 else
456 {
457 distance = dist(centerInf->point, k->point);
458 }
460 EdgePair intPair = EdgePair(k, kPrev, distance, 0.0);
461 e.insert(intPair).first;
462 }
463 k = k->lstNext;
464 }
466 // Start the actual sweep.
467 db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
469 VertInf *lastInf = NULL;
470 double lastAngle = 0;
471 bool lastVisible = false;
472 int lastBlocker = 0;
474 isBoundingShape isBounding(router->contains[centerID]);
475 VertList::iterator vfst = v.begin();
476 VertList::iterator vfin = v.end();
477 for (VertList::iterator t = vfst; t != vfin; ++t)
478 {
479 VertInf *currInf = (*t).vInf;
480 VertID& currID = currInf->id;
481 Point& currPt = currInf->point;
482 centerAngle = (*t).angle;
484 #ifdef LINEDEBUG
485 Sint16 ppx = (int) centerPt.x;
486 Sint16 ppy = (int) centerPt.y;
488 Sint16 cx = (int) currPt.x;
489 Sint16 cy = (int) currPt.y;
490 #endif
492 double currDist = dist(centerPt, currPt);
493 db_printf("Dist: %.1f.\n", currDist);
495 EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf);
496 if (edge == NULL)
497 {
498 edge = new EdgeInf(centerInf, currInf);
499 }
500 // Ignore vertices from bounding shapes, if sweeping round an endpoint.
501 if (!(centerID.isShape) && isBounding(*t))
502 {
503 if (router->InvisibilityGrph)
504 {
505 // if p and t can't see each other, add blank edge
506 db_printf("\tSkipping visibility edge... \n\t\t");
507 edge->addBlocker(currInf->id.objID);
508 edge->db_print();
509 }
510 continue;
511 }
514 bool cone1 = true, cone2 = true;
515 if (centerID.isShape)
516 {
517 cone1 = inValidRegion(router->IgnoreRegions,
518 centerInf->shPrev->point, centerPoint,
519 centerInf->shNext->point, currInf->point);
520 }
521 if (currInf->id.isShape)
522 {
523 cone2 = inValidRegion(router->IgnoreRegions,
524 currInf->shPrev->point, currInf->point,
525 currInf->shNext->point, centerPoint);
526 }
528 if (!cone1 || !cone2)
529 {
530 lastInf = NULL;
531 if (router->InvisibilityGrph)
532 {
533 db_printf("\tSetting invisibility edge... \n\t\t");
534 edge->addBlocker(0);
535 edge->db_print();
536 }
537 }
538 else
539 {
540 int blocker = 0;
541 // Check visibility.
542 bool currVisible = sweepVisible(e, currInf,
543 lastInf, lastVisible, lastAngle, &blocker);
544 if (blocker == -1)
545 {
546 blocker = lastBlocker;
547 }
548 if (currVisible)
549 {
550 #ifdef LINEDEBUG
551 lineRGBA(avoid_screen, ppx, ppy, cx, cy, 255, 0, 0, 32);
552 #endif
553 db_printf("\tSetting visibility edge... \n\t\t");
554 edge->setDist(currDist);
555 edge->db_print();
556 }
557 else if (router->InvisibilityGrph)
558 {
559 db_printf("\tSetting invisibility edge... \n\t\t");
560 edge->addBlocker(blocker);
561 edge->db_print();
562 }
564 lastVisible = currVisible;
565 lastInf = currInf;
566 lastAngle = centerAngle;
567 lastBlocker = blocker;
568 }
570 if (currID.isShape)
571 {
572 // This is a shape edge
573 Point& prevPt = currInf->shPrev->point;
574 Point& nextPt = currInf->shNext->point;
576 int prevDir = vecDir(centerPt, currPt, prevPt);
577 EdgePair prevPair = EdgePair(currInf, currInf->shPrev,
578 currDist, centerAngle);
580 EdgeSet::iterator ePtr;
581 if (prevDir == BEHIND)
582 {
583 // XXX: Strangely e.find does not return the correct results.
584 // ePtr = e.find(prevPair);
585 ePtr = std::find(e.begin(), e.end(), prevPair);
586 if (ePtr != e.end())
587 {
588 e.erase(ePtr);
589 }
590 }
591 else if ((prevDir == AHEAD) && (currInf->shPrev != centerInf))
592 {
593 double x = prevPt.x - currPt.x;
594 double y = prevPt.y - currPt.y;
595 double angle = PointPair::pos_to_angle(x, y);
596 prevPair.SetObsAng(angle);
598 ePtr = e.insert(prevPair).first;
599 }
602 int nextDir = vecDir(centerPt, currPt, nextPt);
603 EdgePair nextPair = EdgePair(currInf, currInf->shNext,
604 currDist, centerAngle);
606 if (nextDir == BEHIND)
607 {
608 // XXX: Strangely e.find does not return the correct results.
609 // ePtr = e.find(nextPair);
610 ePtr = std::find(e.begin(), e.end(), nextPair);
611 if (ePtr != e.end())
612 {
613 e.erase(ePtr);
614 }
615 }
616 else if ((nextDir == AHEAD) && (currInf->shNext != centerInf))
617 {
618 double x = nextPt.x - currPt.x;
619 double y = nextPt.y - currPt.y;
620 double angle = PointPair::pos_to_angle(x, y);
621 nextPair.SetObsAng(angle);
623 ePtr = e.insert(nextPair).first;
624 }
625 }
627 #ifdef LINEDEBUG
628 SDL_Flip(avoid_screen);
629 #endif
630 }
631 }
634 }