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 <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 #include "libavoid/router.h"
  33
  34 #include <math.h>
  35
  36 #ifdef LINEDEBUG
  37   #include "SDL_gfxPrimitives.h"
  38 #endif
  39
  40 namespace Avoid {
  41
  42
  43 void shapeVis(ShapeRef *shape)
  44 {
  45     Router *router = shape->router();
  46
  47     if ( !(router->InvisibilityGrph) )
  48     {
  49         // Clear shape from graph.
  50         shape->removeFromGraph();
  51     }
  52
  53     VertInf *shapeBegin = shape->firstVert();
  54     VertInf *shapeEnd = shape->lastVert()->lstNext;
  55
  56     VertInf *pointsBegin = NULL;
  57     if (router->IncludeEndpoints)
  58     {
  59         pointsBegin = router->vertices.connsBegin();
  60     }
  61     else
  62     {
  63         pointsBegin = router->vertices.shapesBegin();
  64     }
  65
  66     for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
  67     {
  68         bool knownNew = true;
  69
  70         db_printf("-- CONSIDERING --\n");
  71         curr->id.db_print();
  72
  73         db_printf("\tFirst Half:\n");
  74         for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
  75         {
  76             EdgeInf::checkEdgeVisibility(curr, j, knownNew);
  77         }
  78
  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 }
  87
  88
  89 void shapeVisSweep(ShapeRef *shape)
  90 {
  91     Router *router = shape->router();
  92
  93     if ( !(router->InvisibilityGrph) )
  94     {
  95         // Clear shape from graph.
  96         shape->removeFromGraph();
  97     }
  98
  99     VertInf *startIter = shape->firstVert();
 100     VertInf *endIter = shape->lastVert()->lstNext;
 101
 102     for (VertInf *i = startIter; i != endIter; i = i->lstNext)
 103     {
 104         vertexSweep(i);
 105     }
 106 }
 107
 108
 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;
 114
 115     // Make sure we're only doing ptVis for endpoints.
 116     assert(!(pID.isShape));
 117
 118     if ( !(router->InvisibilityGrph) )
 119     {
 120         point->removeFromGraph();
 121     }
 122
 123     if (gen_contains && !(pID.isShape))
 124     {
 125         router->generateContains(point);
 126     }
 127
 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 }
 146
 147
 148 //============================================================================
 149 //  SWEEP CODE
 150 //
 151
 152 static VertInf *centerInf;
 153 static Point centerPoint;
 154 static VertID centerID;
 155 static double centerAngle;
 156
 157
 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;
 166
 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         }
 191
 192         VertInf    *vInf;
 193         double     angle;
 194 };
 195
 196
 197 typedef std::list<PointPair > VertList;
 198
 199
 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);
 245
 246             //db_printf("SetObsAng: %.2f  (from init %.2f, a %.2f)\n",
 247             //      obsAngle, initangle, a);
 248         }
 249
 250         VertInf *vInf1;
 251         VertInf *vInf2;
 252         double  initdist;
 253         double  initangle;
 254         double  currdist;
 255         double  currangle;
 256         double  obsAngle;
 257 };
 258
 259 typedef std::set<EdgePair> EdgeSet;
 260
 261
 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 }
 273
 274
 275 #define AHEAD    1
 276 #define BEHIND  -1
 277
 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 };
 298
 299
 300 static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf,
 301         bool lastVisible, double lastAngle, int *blocker)
 302 {
 303
 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         {
 310
 311             Point &e1 = (*closestIt).vInf1->point;
 312             Point &e2 = (*closestIt).vInf2->point;
 313
 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;
 346
 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 }
 357
 358
 359 void vertexSweep(VertInf *vert)
 360 {
 361     Router *router = vert->_router;
 362     VertID& pID = vert->id;
 363     Point& pPoint = vert->point;
 364
 365     centerInf = vert;
 366     centerID = pID;
 367     centerPoint = pPoint;
 368     Point centerPt = pPoint;
 369     centerAngle = -1;
 370
 371     // List of shape (and maybe endpt) vertices, except p
 372     // Sort list, around
 373     VertList v;
 374
 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         }
 385
 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);
 416
 417     EdgeSet e;
 418     ShapeSet& ss = router->contains[centerID];
 419
 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         }
 438
 439         VertInf *kPrev = k->shPrev;
 440         if ((centerInf == k) || (centerInf == kPrev))
 441         {
 442             k = k->lstNext;
 443             continue;
 444         }
 445
 446         Point xaxis = { DBL_MAX, centerInf->point.y };
 447
 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             }
 459
 460             EdgePair intPair = EdgePair(k, kPrev, distance, 0.0);
 461             e.insert(intPair).first;
 462         }
 463         k = k->lstNext;
 464     }
 465
 466     // Start the actual sweep.
 467     db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
 468
 469     VertInf *lastInf     = NULL;
 470     double   lastAngle   = 0;
 471     bool     lastVisible = false;
 472     int      lastBlocker = 0;
 473
 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;
 483
 484 #ifdef LINEDEBUG
 485         Sint16 ppx = (int) centerPt.x;
 486         Sint16 ppy = (int) centerPt.y;
 487
 488         Sint16 cx = (int) currPt.x;
 489         Sint16 cy = (int) currPt.y;
 490 #endif
 491
 492         double currDist = dist(centerPt, currPt);
 493         db_printf("Dist: %.1f.\n", currDist);
 494
 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         }
 512
 513
 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         }
 527
 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             }
 563
 564             lastVisible = currVisible;
 565             lastInf     = currInf;
 566             lastAngle   = centerAngle;
 567             lastBlocker = blocker;
 568         }
 569
 570         if (currID.isShape)
 571         {
 572             // This is a shape edge
 573             Point& prevPt = currInf->shPrev->point;
 574             Point& nextPt = currInf->shNext->point;
 575
 576             int prevDir = vecDir(centerPt, currPt, prevPt);
 577             EdgePair prevPair = EdgePair(currInf, currInf->shPrev,
 578                     currDist, centerAngle);
 579
 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);
 597
 598                 ePtr = e.insert(prevPair).first;
 599             }
 600
 601
 602             int nextDir = vecDir(centerPt, currPt, nextPt);
 603             EdgePair nextPair = EdgePair(currInf, currInf->shNext,
 604                     currDist, centerAngle);
 605
 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);
 622
 623                 ePtr = e.insert(nextPair).first;
 624             }
 625         }
 626
 627 #ifdef LINEDEBUG
 628         SDL_Flip(avoid_screen);
 629 #endif
 630     }
 631 }
 632
 633
 634 }
 635