src/libavoid/visibility.cpp

   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 */
  22
  23
  24 #include <algorithm>
  25 #include <cfloat>
  26
  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"
  34
  35 #include <math.h>
  36
  37 #ifdef LINEDEBUG
  38   #include "SDL_gfxPrimitives.h"
  39 #endif
  40
  41 namespace Avoid {
  42
  43
  44 void shapeVis(ShapeRef *shape)
  45 {
  46     Router *router = shape->router();
  47
  48     if ( !(router->InvisibilityGrph) )
  49     {
  50         // Clear shape from graph.
  51         shape->removeFromGraph();
  52     }
  53
  54     VertInf *shapeBegin = shape->firstVert();
  55     VertInf *shapeEnd = shape->lastVert()->lstNext;
  56
  57     VertInf *pointsBegin = NULL;
  58     if (router->IncludeEndpoints)
  59     {
  60         pointsBegin = router->vertices.connsBegin();
  61     }
  62     else
  63     {
  64         pointsBegin = router->vertices.shapesBegin();
  65     }
  66
  67     for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
  68     {
  69         bool knownNew = true;
  70
  71         db_printf("-- CONSIDERING --\n");
  72         curr->id.db_print();
  73
  74         db_printf("\tFirst Half:\n");
  75         for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
  76         {
  77             EdgeInf::checkEdgeVisibility(curr, j, knownNew);
  78         }
  79
  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 }
  88
  89
  90 void shapeVisSweep(ShapeRef *shape)
  91 {
  92     Router *router = shape->router();
  93
  94     if ( !(router->InvisibilityGrph) )
  95     {
  96         // Clear shape from graph.
  97         shape->removeFromGraph();
  98     }
  99
 100     VertInf *startIter = shape->firstVert();
 101     VertInf *endIter = shape->lastVert()->lstNext;
 102
 103     for (VertInf *i = startIter; i != endIter; i = i->lstNext)
 104     {
 105         vertexSweep(i);
 106     }
 107 }
 108
 109
 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;
 115
 116     // Make sure we're only doing ptVis for endpoints.
 117     assert(!(pID.isShape));
 118
 119     if ( !(router->InvisibilityGrph) )
 120     {
 121         point->removeFromGraph();
 122     }
 123
 124     if (gen_contains && !(pID.isShape))
 125     {
 126         router->generateContains(point);
 127     }
 128
 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 }
 147
 148
 149 //============================================================================
 150 //  SWEEP CODE
 151 //
 152
 153 static VertInf *centerInf;
 154 static Point centerPoint;
 155 static VertID centerID;
 156 static double centerAngle;
 157
 158
 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;
 167
 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         }
 192
 193         VertInf    *vInf;
 194         double     angle;
 195 };
 196
 197
 198 typedef std::list<PointPair > VertList;
 199
 200
 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);
 246
 247             //db_printf("SetObsAng: %.2f  (from init %.2f, a %.2f)\n",
 248             //      obsAngle, initangle, a);
 249         }
 250
 251         VertInf *vInf1;
 252         VertInf *vInf2;
 253         double  initdist;
 254         double  initangle;
 255         double  currdist;
 256         double  currangle;
 257         double  obsAngle;
 258 };
 259
 260 typedef std::set<EdgePair> EdgeSet;
 261
 262
 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 }
 274
 275
 276 #define AHEAD    1
 277 #define BEHIND  -1
 278
 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 };
 299
 300
 301 static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf,
 302         bool lastVisible, double lastAngle, int *blocker)
 303 {
 304
 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         {
 311
 312             Point &e1 = (*closestIt).vInf1->point;
 313             Point &e2 = (*closestIt).vInf2->point;
 314
 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;
 339
 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 }
 350
 351
 352 void vertexSweep(VertInf *vert)
 353 {
 354     Router *router = vert->_router;
 355     VertID& pID = vert->id;
 356     Point& pPoint = vert->point;
 357
 358     centerInf = vert;
 359     centerID = pID;
 360     centerPoint = pPoint;
 361     Point centerPt = pPoint;
 362     centerAngle = -1;
 363
 364     // List of shape (and maybe endpt) vertices, except p
 365     // Sort list, around
 366     VertList v;
 367
 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         }
 378
 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);
 409
 410     EdgeSet e;
 411     ShapeSet& ss = router->contains[centerID];
 412
 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         }
 431
 432         VertInf *kPrev = k->shPrev;
 433         if ((centerInf == k) || (centerInf == kPrev))
 434         {
 435             k = k->lstNext;
 436             continue;
 437         }
 438
 439         Point xaxis(DBL_MAX, centerInf->point.y);
 440
 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             }
 452
 453             EdgePair intPair = EdgePair(k, kPrev, distance, 0.0);
 454             e.insert(intPair).first;
 455         }
 456         k = k->lstNext;
 457     }
 458
 459     // Start the actual sweep.
 460     db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
 461
 462     VertInf *lastInf     = NULL;
 463     double   lastAngle   = 0;
 464     bool     lastVisible = false;
 465     int      lastBlocker = 0;
 466
 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;
 476
 477 #ifdef LINEDEBUG
 478         Sint16 ppx = (int) centerPt.x;
 479         Sint16 ppy = (int) centerPt.y;
 480
 481         Sint16 cx = (int) currPt.x;
 482         Sint16 cy = (int) currPt.y;
 483 #endif
 484
 485         double currDist = dist(centerPt, currPt);
 486         db_printf("Dist: %.1f.\n", currDist);
 487
 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         }
 505
 506
 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         }
 520
 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             }
 556
 557             lastVisible = currVisible;
 558             lastInf     = currInf;
 559             lastAngle   = centerAngle;
 560             lastBlocker = blocker;
 561         }
 562
 563         if (currID.isShape)
 564         {
 565             // This is a shape edge
 566             Point& prevPt = currInf->shPrev->point;
 567             Point& nextPt = currInf->shNext->point;
 568
 569             int prevDir = vecDir(centerPt, currPt, prevPt);
 570             EdgePair prevPair = EdgePair(currInf, currInf->shPrev,
 571                     currDist, centerAngle);
 572
 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);
 590
 591                 ePtr = e.insert(prevPair).first;
 592             }
 593
 594
 595             int nextDir = vecDir(centerPt, currPt, nextPt);
 596             EdgePair nextPair = EdgePair(currInf, currInf->shNext,
 597                     currDist, centerAngle);
 598
 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);
 615
 616                 ePtr = e.insert(nextPair).first;
 617             }
 618         }
 619
 620 #ifdef LINEDEBUG
 621         SDL_Flip(avoid_screen);
 622 #endif
 623     }
 624 }
 625
 626
 627 }
 628