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  *
   6  * Copyright (C) 2004-2009  Monash University
   7  *
   8  * --------------------------------------------------------------------
   9  * The Visibility Sweep technique is based upon the method described
  10  * in Section 5.2 of:
  11  *     Lee, D.-T. (1978). Proximity and reachability in the plane.,
  12  *     PhD thesis, Department of Electrical Engineering,
  13  *     University of Illinois, Urbana, IL.
  14  * --------------------------------------------------------------------
  15  *
  16  * This library is free software; you can redistribute it and/or
  17  * modify it under the terms of the GNU Lesser General Public
  18  * License as published by the Free Software Foundation; either
  19  * version 2.1 of the License, or (at your option) any later version.
  20  * See the file LICENSE.LGPL distributed with the library.
  21  *
  22  * Licensees holding a valid commercial license may use this file in
  23  * accordance with the commercial license agreement provided with the
  24  * library.
  25  *
  26  * This library is distributed in the hope that it will be useful,
  27  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  28  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  29  *
  30  * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
  31 */
  32
  33
  34 #include <algorithm>
  35 #include <cfloat>
  36 #define _USE_MATH_DEFINES
  37 #include <cmath>
  38
  39 #include "libavoid/shape.h"
  40 #include "libavoid/debug.h"
  41 #include "libavoid/visibility.h"
  42 #include "libavoid/vertices.h"
  43 #include "libavoid/graph.h"
  44 #include "libavoid/geometry.h"
  45 #include "libavoid/router.h"
  46 #include "libavoid/assertions.h"
  47
  48 #ifdef LINEDEBUG
  49   #include "SDL_gfxPrimitives.h"
  50 #endif
  51
  52 namespace Avoid {
  53
  54
  55 void shapeVis(ShapeRef *shape)
  56 {
  57     Router *router = shape->router();
  58
  59     if ( !(router->InvisibilityGrph) )
  60     {
  61         // Clear shape from graph.
  62         shape->removeFromGraph();
  63     }
  64
  65     VertInf *shapeBegin = shape->firstVert();
  66     VertInf *shapeEnd = shape->lastVert()->lstNext;
  67
  68     VertInf *pointsBegin = router->vertices.connsBegin();
  69     for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
  70     {
  71         bool knownNew = true;
  72
  73         db_printf("-- CONSIDERING --\n");
  74         curr->id.db_print();
  75
  76         db_printf("\tFirst Half:\n");
  77         for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
  78         {
  79             if (j->id == dummyOrthogID)
  80             {
  81                 // Don't include orthogonal dummy vertices.
  82                 continue;
  83             }
  84             EdgeInf::checkEdgeVisibility(curr, j, knownNew);
  85         }
  86
  87         db_printf("\tSecond Half:\n");
  88         VertInf *pointsEnd = router->vertices.end();
  89         for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext)
  90         {
  91             if (k->id == dummyOrthogID)
  92             {
  93                 // Don't include orthogonal dummy vertices.
  94                 continue;
  95             }
  96             EdgeInf::checkEdgeVisibility(curr, k, knownNew);
  97         }
  98     }
  99 }
 100
 101
 102 void shapeVisSweep(ShapeRef *shape)
 103 {
 104     Router *router = shape->router();
 105
 106     if ( !(router->InvisibilityGrph) )
 107     {
 108         // Clear shape from graph.
 109         shape->removeFromGraph();
 110     }
 111
 112     VertInf *startIter = shape->firstVert();
 113     VertInf *endIter = shape->lastVert()->lstNext;
 114
 115     for (VertInf *i = startIter; i != endIter; i = i->lstNext)
 116     {
 117         vertexSweep(i);
 118     }
 119 }
 120
 121
 122 void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
 123         const bool gen_contains)
 124 {
 125     Router *router = point->_router;
 126     const VertID& pID = point->id;
 127
 128     // Make sure we're only doing ptVis for endpoints.
 129     COLA_ASSERT(!(pID.isShape));
 130
 131     if ( !(router->InvisibilityGrph) )
 132     {
 133         point->removeFromGraph();
 134     }
 135
 136     if (gen_contains && !(pID.isShape))
 137     {
 138         router->generateContains(point);
 139     }
 140
 141     if (router->UseLeesAlgorithm)
 142     {
 143         vertexSweep(point);
 144     }
 145     else
 146     {
 147         VertInf *shapesEnd = router->vertices.end();
 148         for (VertInf *k = router->vertices.shapesBegin(); k != shapesEnd;
 149                 k = k->lstNext)
 150         {
 151             if (k->id == dummyOrthogID)
 152             {
 153                 // Don't include orthogonal dummy vertices.
 154                 continue;
 155             }
 156             EdgeInf::checkEdgeVisibility(point, k, knownNew);
 157         }
 158         if (partner)
 159         {
 160             EdgeInf::checkEdgeVisibility(point, partner, knownNew);
 161         }
 162     }
 163 }
 164
 165
 166 //============================================================================
 167 //  SWEEP CODE
 168 //
 169
 170 static VertInf *centerInf;
 171 static Point centerPoint;
 172 static VertID centerID;
 173
 174
 175 class PointPair
 176 {
 177     public:
 178         PointPair(VertInf *inf)
 179             : vInf(inf)
 180         {
 181             double x = vInf->point.x - centerPoint.x;
 182             double y = vInf->point.y - centerPoint.y;
 183
 184             angle = pos_to_angle(x, y);
 185             distance = euclideanDist(centerPoint, vInf->point);
 186         }
 187         bool operator<(const PointPair& rhs) const
 188         {
 189             // Firstly order by angle.
 190             if (angle == rhs.angle)
 191             {
 192                 // If the points are collinear, then order them in increasing
 193                 // distance from the point we are sweeping around.
 194                 if (distance == rhs.distance)
 195                 {
 196                     // XXX: Add this assertion back if we require that
 197                     //      connector endpoints have unique IDs. For the
 198                     //      moment it is okay for them to have the same ID.
 199                     //COLA_ASSERT(vInf->id != rhs.vInf->id);
 200
 201                     // If comparing two points at the same physical
 202                     // position, then order them by their VertIDs.
 203                     return vInf->id < rhs.vInf->id;
 204                 }
 205                 return distance < rhs.distance;
 206             }
 207             return angle < rhs.angle;
 208         }
 209         static double pos_to_angle(double x, double y)
 210         {
 211             if (y == 0)
 212             {
 213                 return ((x < 0) ? 180 : 0);
 214             }
 215             else if (x == 0)
 216             {
 217                 return ((y < 0) ? 270 : 90);
 218             }
 219
 220             double ang = atan(y / x);
 221             ang = (ang * 180) / M_PI;
 222
 223             if (x < 0)
 224             {
 225                 ang += 180;
 226             }
 227             else if (y < 0)
 228             {
 229                 ang += 360;
 230             }
 231             COLA_ASSERT(ang >= 0);
 232             COLA_ASSERT(ang <= 360);
 233             return ang;
 234         }
 235
 236         VertInf    *vInf;
 237         double     angle;
 238         double     distance;
 239 };
 240
 241
 242 typedef std::set<PointPair > VertSet;
 243
 244
 245 class EdgePair
 246 {
 247     public:
 248         EdgePair() :
 249             vInf1(NULL), vInf2(NULL), dist1(0.0), dist2(0.0), angle(0.0),
 250             angleDist(0.0)
 251         {
 252             // The default constuctor should never be called.
 253             // This is defined to appease the MSVC compiler.
 254             COLA_ASSERT(false);
 255         }
 256         EdgePair(const PointPair& p1, VertInf *v) :
 257                 vInf1(p1.vInf),
 258                 vInf2(v),
 259                 dist1(p1.distance),
 260                 dist2(euclideanDist(vInf2->point, centerPoint)),
 261                 angle(p1.angle),
 262                 angleDist(p1.distance)
 263         {
 264         }
 265         bool operator<(const EdgePair& rhs) const
 266         {
 267             COLA_ASSERT(angle == rhs.angle);
 268             if (angleDist == rhs.angleDist)
 269             {
 270                 return (dist2 < rhs.dist2);
 271             }
 272             return (angleDist < rhs.angleDist);
 273         }
 274         bool operator==(const EdgePair& rhs) const
 275         {
 276             if (((vInf1->id == rhs.vInf1->id) &&
 277                         (vInf2->id == rhs.vInf2->id)) ||
 278                 ((vInf1->id == rhs.vInf2->id) &&
 279                         (vInf2->id == rhs.vInf1->id)))
 280             {
 281                 return true;
 282             }
 283             return false;
 284         }
 285         bool operator!=(const EdgePair& rhs) const
 286         {
 287             if (((vInf1->id == rhs.vInf1->id) &&
 288                         (vInf2->id == rhs.vInf2->id)) ||
 289                 ((vInf1->id == rhs.vInf2->id) &&
 290                         (vInf2->id == rhs.vInf1->id)))
 291             {
 292                 return false;
 293             }
 294             return true;
 295         }
 296         void setNegativeAngle(void)
 297         {
 298             angle = -1.0;
 299         }
 300         double setCurrAngle(const PointPair& p)
 301         {
 302             if (p.vInf->point == vInf1->point)
 303             {
 304                 angleDist = dist1;
 305                 angle = p.angle;
 306             }
 307             else if (p.vInf->point == vInf2->point)
 308             {
 309                 angleDist = dist2;
 310                 angle = p.angle;
 311             }
 312             else if (p.angle != angle)
 313             {
 314                 COLA_ASSERT(p.angle > angle);
 315                 angle = p.angle;
 316                 Point pp;
 317                 int result = rayIntersectPoint(vInf1->point, vInf2->point,
 318                         centerPoint, p.vInf->point, &(pp.x), &(pp.y));
 319                 if (result != DO_INTERSECT)
 320                 {
 321                     // This can happen with points that appear to have the
 322                     // same angle but at are at slightly different positions
 323                     angleDist = std::min(dist1, dist2);
 324                 }
 325                 else
 326                 {
 327                     angleDist = euclideanDist(pp, centerPoint);
 328                 }
 329             }
 330
 331             return angleDist;
 332         }
 333
 334         VertInf *vInf1;
 335         VertInf *vInf2;
 336         double dist1;
 337         double dist2;
 338         double angle;
 339         double angleDist;
 340 };
 341
 342 typedef std::list<EdgePair> SweepEdgeList;
 343
 344
 345 #define AHEAD    1
 346 #define BEHIND  -1
 347
 348 class isBoundingShape
 349 {
 350     public:
 351         // Class instance remembers the ShapeSet.
 352         isBoundingShape(ShapeSet& set) :
 353             ss(set)
 354         { }
 355         // The following is an overloading of the function call operator.
 356         bool operator () (const PointPair& pp)
 357         {
 358             if (pp.vInf->id.isShape &&
 359                     (ss.find(pp.vInf->id.objID) != ss.end()))
 360             {
 361                 return true;
 362             }
 363             return false;
 364         }
 365     private:
 366         // MSVC wants to generate the assignment operator and the default
 367         // constructor, but fails.  Therefore we declare them private and
 368         // don't implement them.
 369         isBoundingShape & operator=(isBoundingShape const &);
 370         isBoundingShape();
 371
 372         ShapeSet& ss;
 373 };
 374
 375
 376 static bool sweepVisible(SweepEdgeList& T, const PointPair& point,
 377         std::set<unsigned int>& onBorderIDs, int *blocker)
 378 {
 379     if (T.empty())
 380     {
 381         // No blocking edges.
 382         return true;
 383     }
 384
 385     Router *router = point.vInf->_router;
 386     bool visible = true;
 387
 388     SweepEdgeList::const_iterator closestIt = T.begin();
 389     SweepEdgeList::const_iterator end = T.end();
 390     while (closestIt != end)
 391     {
 392         if ((point.vInf->point == closestIt->vInf1->point) ||
 393                 (point.vInf->point == closestIt->vInf2->point))
 394         {
 395             // If the ray intersects just the endpoint of a
 396             // blocking edge then ignore that edge.
 397             ++closestIt;
 398             continue;
 399         }
 400         break;
 401     }
 402     if (closestIt == end)
 403     {
 404         return true;
 405     }
 406
 407     if (! point.vInf->id.isShape )
 408     {
 409         // It's a connector endpoint, so we have to ignore
 410         // edges of containing shapes for determining visibility.
 411         ShapeSet& rss = router->contains[point.vInf->id];
 412         while (closestIt != end)
 413         {
 414             if (rss.find(closestIt->vInf1->id.objID) == rss.end())
 415             {
 416                 // This is not a containing edge so do the normal
 417                 // test and then stop.
 418                 if (point.distance > closestIt->angleDist)
 419                 {
 420                     visible = false;
 421                 }
 422                 else if ((point.distance == closestIt->angleDist) &&
 423                         onBorderIDs.find(closestIt->vInf1->id.objID) !=
 424                                 onBorderIDs.end())
 425                 {
 426                     // Touching, but centerPoint is on another edge of
 427                     // shape shape, so count as blocking.
 428                     visible = false;
 429                 }
 430                 break;
 431             }
 432             // This was a containing edge, so consider the next along.
 433             ++closestIt;
 434         }
 435     }
 436     else
 437     {
 438         // Just test to see if this point is closer than the closest
 439         // edge blocking this ray.
 440         if (point.distance > closestIt->angleDist)
 441         {
 442             visible =  false;
 443         }
 444         else if ((point.distance == closestIt->angleDist) &&
 445                 onBorderIDs.find(closestIt->vInf1->id.objID) !=
 446                         onBorderIDs.end())
 447         {
 448             // Touching, but centerPoint is on another edge of
 449             // shape shape, so count as blocking.
 450             visible = false;
 451         }
 452     }
 453
 454     if (!visible)
 455     {
 456         *blocker = (*closestIt).vInf1->id.objID;
 457 #ifdef LINEDEBUG
 458         Point &e1 = (*closestIt).vInf1->point;
 459         Point &e2 = (*closestIt).vInf2->point;
 460
 461         if (router->avoid_screen)
 462         {
 463             int canx = 151;
 464             int cany = 55;
 465             lineRGBA(router->avoid_screen, e1.x + canx, e1.y + cany,
 466                     e2.x + canx, e2.y + cany, 0, 0, 225, 255);
 467         }
 468 #endif
 469     }
 470     return visible;
 471 }
 472
 473
 474 void vertexSweep(VertInf *vert)
 475 {
 476     Router *router = vert->_router;
 477     VertID& pID = vert->id;
 478     Point& pPoint = vert->point;
 479
 480     centerInf = vert;
 481     centerID = pID;
 482     centerPoint = pPoint;
 483     Point centerPt = pPoint;
 484
 485     // List of shape (and maybe endpt) vertices, except p
 486     // Sort list, around
 487     VertSet v;
 488
 489     // Initialise the vertex list
 490     ShapeSet& ss = router->contains[centerID];
 491     VertInf *beginVert = router->vertices.connsBegin();
 492     VertInf *endVert = router->vertices.end();
 493     for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext)
 494     {
 495         if (inf == centerInf)
 496         {
 497             // Don't include the center point itself.
 498             continue;
 499         }
 500         else if (inf->id == dummyOrthogID)
 501         {
 502             // Don't include orthogonal dummy vertices.
 503             continue;
 504         }
 505
 506         if (!(centerID.isShape) && (ss.find(inf->id.objID) != ss.end()))
 507         {
 508             // Don't include edge points of containing shapes.
 509             unsigned int shapeID = inf->id.objID;
 510             db_printf("Center is inside shape %u so ignore shape edges.\n",
 511                     shapeID);
 512             continue;
 513         }
 514
 515         if (inf->id.isShape)
 516         {
 517             // Add shape vertex.
 518             v.insert(inf);
 519         }
 520         else
 521         {
 522             // Add connector endpoint.
 523             if (centerID.isShape)
 524             {
 525                 // Center is a shape vertex, so add all endpoint vertices.
 526                 v.insert(inf);
 527             }
 528             else
 529             {
 530                 // Center is an endpoint, so only include the other
 531                 // endpoint from the matching connector.
 532                 VertID partnerID = VertID(centerID.objID, false,
 533                         (centerID.vn == 1) ? 2 : 1);
 534                 if (inf->id == partnerID)
 535                 {
 536                     v.insert(inf);
 537                 }
 538             }
 539         }
 540     }
 541     std::set<unsigned int> onBorderIDs;
 542
 543     // Add edges to T that intersect the initial ray.
 544     SweepEdgeList e;
 545     VertSet::const_iterator vbegin = v.begin();
 546     VertSet::const_iterator vend = v.end();
 547     for (VertSet::const_iterator t = vbegin; t != vend; ++t)
 548     {
 549         VertInf *k = t->vInf;
 550
 551         COLA_ASSERT(centerInf != k);
 552         COLA_ASSERT(centerID.isShape || (ss.find(k->id.objID) == ss.end()));
 553
 554         Point xaxis(DBL_MAX, centerInf->point.y);
 555
 556         VertInf *kPrev = k->shPrev;
 557         VertInf *kNext = k->shNext;
 558         if (kPrev && (kPrev != centerInf) &&
 559                 (vecDir(centerInf->point, xaxis, kPrev->point) == AHEAD))
 560         {
 561             if (segmentIntersect(centerInf->point, xaxis, kPrev->point,
 562                         k->point))
 563             {
 564                 EdgePair intPair = EdgePair(*t, kPrev);
 565                 e.push_back(intPair);
 566             }
 567             if ((vecDir(kPrev->point, k->point, centerInf->point) == 0) &&
 568                     inBetween(kPrev->point, k->point, centerInf->point))
 569             {
 570                 // Record that centerPoint is on an obstacle line.
 571                 onBorderIDs.insert(k->id.objID);
 572             }
 573         }
 574         else if (kNext && (kNext != centerInf) &&
 575                 (vecDir(centerInf->point, xaxis, kNext->point) == AHEAD))
 576         {
 577             if (segmentIntersect(centerInf->point, xaxis, kNext->point,
 578                         k->point))
 579             {
 580                 EdgePair intPair = EdgePair(*t, kNext);
 581                 e.push_back(intPair);
 582             }
 583             if ((vecDir(kNext->point, k->point, centerInf->point) == 0) &&
 584                     inBetween(kNext->point, k->point, centerInf->point))
 585             {
 586                 // Record that centerPoint is on an obstacle line.
 587                 onBorderIDs.insert(k->id.objID);
 588             }
 589         }
 590     }
 591     for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c)
 592     {
 593         (*c).setNegativeAngle();
 594     }
 595
 596
 597     // Start the actual sweep.
 598     db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
 599
 600     isBoundingShape isBounding(ss);
 601     for (VertSet::const_iterator t = vbegin; t != vend; ++t)
 602     {
 603         VertInf *currInf = (*t).vInf;
 604         VertID& currID = currInf->id;
 605         Point&  currPt = currInf->point;
 606
 607 #ifdef LINEDEBUG
 608         Sint16 ppx = (int) centerPt.x;
 609         Sint16 ppy = (int) centerPt.y;
 610
 611         Sint16 cx = (int) currPt.x;
 612         Sint16 cy = (int) currPt.y;
 613
 614         int canx = 151;
 615         int cany = 55;
 616 #endif
 617
 618         const double& currDist = (*t).distance;
 619
 620         EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf);
 621         if (edge == NULL)
 622         {
 623             edge = new EdgeInf(centerInf, currInf);
 624         }
 625
 626         for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c)
 627         {
 628             (*c).setCurrAngle(*t);
 629         }
 630         e.sort();
 631
 632         // Check visibility.
 633         int blocker = 0;
 634         bool currVisible = sweepVisible(e, *t, onBorderIDs, &blocker);
 635
 636         bool cone1 = true, cone2 = true;
 637         if (centerID.isShape)
 638         {
 639             cone1 = inValidRegion(router->IgnoreRegions,
 640                     centerInf->shPrev->point, centerPoint,
 641                     centerInf->shNext->point, currInf->point);
 642         }
 643         if (currInf->id.isShape)
 644         {
 645             cone2 = inValidRegion(router->IgnoreRegions,
 646                     currInf->shPrev->point, currInf->point,
 647                     currInf->shNext->point, centerPoint);
 648         }
 649
 650         if (!cone1 || !cone2)
 651         {
 652             if (router->InvisibilityGrph)
 653             {
 654                 db_printf("\tSetting invisibility edge... \n\t\t");
 655                 edge->addBlocker(0);
 656                 edge->db_print();
 657             }
 658         }
 659         else
 660         {
 661             if (currVisible)
 662             {
 663 #ifdef LINEDEBUG
 664                 if (router->avoid_screen)
 665                 {
 666                     lineRGBA(router->avoid_screen, ppx + canx, ppy + cany,
 667                             cx + canx, cy + cany, 255, 0, 0, 75);
 668                     SDL_Delay(1000);
 669                 }
 670 #endif
 671                 db_printf("\tSetting visibility edge... \n\t\t");
 672                 edge->setDist(currDist);
 673                 edge->db_print();
 674             }
 675             else if (router->InvisibilityGrph)
 676             {
 677                 db_printf("\tSetting invisibility edge... \n\t\t");
 678                 edge->addBlocker(blocker);
 679                 edge->db_print();
 680             }
 681         }
 682
 683         if (!(edge->added()) && !(router->InvisibilityGrph))
 684         {
 685             delete edge;
 686             edge = NULL;
 687         }
 688
 689         if (currID.isShape)
 690         {
 691             // This is a shape edge
 692
 693             if (currInf->shPrev != centerInf)
 694             {
 695                 Point& prevPt = currInf->shPrev->point;
 696                 int prevDir = vecDir(centerPt, currPt, prevPt);
 697                 EdgePair prevPair = EdgePair(*t, currInf->shPrev);
 698
 699                 if (prevDir == BEHIND)
 700                 {
 701                     e.remove(prevPair);
 702                 }
 703                 else if (prevDir == AHEAD)
 704                 {
 705                     e.push_front(prevPair);
 706                 }
 707             }
 708
 709             if (currInf->shNext != centerInf)
 710             {
 711                 Point& nextPt = currInf->shNext->point;
 712                 int nextDir = vecDir(centerPt, currPt, nextPt);
 713                 EdgePair nextPair = EdgePair(*t, currInf->shNext);
 714
 715                 if (nextDir == BEHIND)
 716                 {
 717                     e.remove(nextPair);
 718                 }
 719                 else if (nextDir == AHEAD)
 720                 {
 721                     e.push_front(nextPair);
 722                 }
 723             }
 724         }
 725 #ifdef LINEDEBUG
 726         if (router->avoid_screen)
 727         {
 728             SDL_Flip(router->avoid_screen);
 729         }
 730 #endif
 731     }
 732 }
 733
 734
 735 }
 736