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