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