diff options
Diffstat (limited to 'src/libavoid/visibility.cpp')
| -rw-r--r-- | src/libavoid/visibility.cpp | 579 |
1 files changed, 344 insertions, 235 deletions
diff --git a/src/libavoid/visibility.cpp b/src/libavoid/visibility.cpp index d2b057643..089911f35 100644 --- a/src/libavoid/visibility.cpp +++ b/src/libavoid/visibility.cpp @@ -2,26 +2,39 @@ * vim: ts=4 sw=4 et tw=0 wm=0 * * libavoid - Fast, Incremental, Object-avoiding Line Router - * Copyright (C) 2004-2006 Michael Wybrow <mjwybrow@users.sourceforge.net> + * + * Copyright (C) 2004-2009 Monash University + * + * -------------------------------------------------------------------- + * The Visibility Sweep technique is based upon the method described + * in Section 5.2 of: + * Lee, D.-T. (1978). Proximity and reachability in the plane., + * PhD thesis, Department of Electrical Engineering, + * University of Illinois, Urbana, IL. + * -------------------------------------------------------------------- * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. + * See the file LICENSE.LGPL distributed with the library. + * + * Licensees holding a valid commercial license may use this file in + * accordance with the commercial license agreement provided with the + * library. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * + * Author(s): Michael Wybrow <mjwybrow@users.sourceforge.net> */ + #include <algorithm> #include <cfloat> +#define _USE_MATH_DEFINES +#include <cmath> #include "libavoid/shape.h" #include "libavoid/debug.h" @@ -30,8 +43,7 @@ #include "libavoid/graph.h" #include "libavoid/geometry.h" #include "libavoid/router.h" - -#include <math.h> +#include "libavoid/assertions.h" #ifdef LINEDEBUG #include "SDL_gfxPrimitives.h" @@ -53,16 +65,7 @@ void shapeVis(ShapeRef *shape) VertInf *shapeBegin = shape->firstVert(); VertInf *shapeEnd = shape->lastVert()->lstNext; - VertInf *pointsBegin = NULL; - if (router->IncludeEndpoints) - { - pointsBegin = router->vertices.connsBegin(); - } - else - { - pointsBegin = router->vertices.shapesBegin(); - } - + VertInf *pointsBegin = router->vertices.connsBegin(); for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext) { bool knownNew = true; @@ -73,6 +76,11 @@ void shapeVis(ShapeRef *shape) db_printf("\tFirst Half:\n"); for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext) { + if (j->id == dummyOrthogID) + { + // Don't include orthogonal dummy vertices. + continue; + } EdgeInf::checkEdgeVisibility(curr, j, knownNew); } @@ -80,6 +88,11 @@ void shapeVis(ShapeRef *shape) VertInf *pointsEnd = router->vertices.end(); for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext) { + if (k->id == dummyOrthogID) + { + // Don't include orthogonal dummy vertices. + continue; + } EdgeInf::checkEdgeVisibility(curr, k, knownNew); } } @@ -113,7 +126,7 @@ void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew, const VertID& pID = point->id; // Make sure we're only doing ptVis for endpoints. - assert(!(pID.isShape)); + COLA_ASSERT(!(pID.isShape)); if ( !(router->InvisibilityGrph) ) { @@ -135,9 +148,14 @@ void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew, for (VertInf *k = router->vertices.shapesBegin(); k != shapesEnd; k = k->lstNext) { + if (k->id == dummyOrthogID) + { + // Don't include orthogonal dummy vertices. + continue; + } EdgeInf::checkEdgeVisibility(point, k, knownNew); } - if (router->IncludeEndpoints && partner) + if (partner) { EdgeInf::checkEdgeVisibility(point, partner, knownNew); } @@ -152,7 +170,6 @@ void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew, static VertInf *centerInf; static Point centerPoint; static VertID centerID; -static double centerAngle; class PointPair @@ -165,19 +182,44 @@ class PointPair double y = vInf->point.y - centerPoint.y; angle = pos_to_angle(x, y); + distance = euclideanDist(centerPoint, vInf->point); } - bool operator==(const PointPair& rhs) const + bool operator<(const PointPair& rhs) const { - if (vInf->id == rhs.vInf->id) + // Firstly order by angle. + if (angle == rhs.angle) { - return true; + // If the points are collinear, then order them in increasing + // distance from the point we are sweeping around. + if (distance == rhs.distance) + { + // XXX: Add this assertion back if we require that + // connector endpoints have unique IDs. For the + // moment it is okay for them to have the same ID. + //COLA_ASSERT(vInf->id != rhs.vInf->id); + + // If comparing two points at the same physical + // position, then order them by their VertIDs. + return vInf->id < rhs.vInf->id; + } + return distance < rhs.distance; } - return false; + return angle < rhs.angle; } static double pos_to_angle(double x, double y) { + if (y == 0) + { + return ((x < 0) ? 180 : 0); + } + else if (x == 0) + { + return ((y < 0) ? 270 : 90); + } + double ang = atan(y / x); ang = (ang * 180) / M_PI; + if (x < 0) { ang += 180; @@ -186,36 +228,48 @@ class PointPair { ang += 360; } + COLA_ASSERT(ang >= 0); + COLA_ASSERT(ang <= 360); return ang; } VertInf *vInf; double angle; + double distance; }; -typedef std::list<PointPair > VertList; +typedef std::set<PointPair > VertSet; class EdgePair { public: - EdgePair(VertInf *v1, VertInf *v2, double d, double a) - : vInf1(v1), vInf2(v2), initdist(d), initangle(a) + EdgePair() : + vInf1(NULL), vInf2(NULL), dist1(0.0), dist2(0.0), angle(0.0), + angleDist(0.0) + { + // The default constuctor should never be called. + // This is defined to appease the MSVC compiler. + COLA_ASSERT(false); + } + EdgePair(const PointPair& p1, VertInf *v) : + vInf1(p1.vInf), + vInf2(v), + dist1(p1.distance), + dist2(euclideanDist(vInf2->point, centerPoint)), + angle(p1.angle), + angleDist(p1.distance) { - currdist = initdist; - currangle = initangle; } bool operator<(const EdgePair& rhs) const { - if (initdist == rhs.initdist) + COLA_ASSERT(angle == rhs.angle); + if (angleDist == rhs.angleDist) { - // TODO: This is a bit of a hack, should be - // set by the call to the constructor. - return dist(centerPoint, vInf2->point) < - dist(centerPoint, rhs.vInf2->point); + return (dist2 < rhs.dist2); } - return (initdist < rhs.initdist); + return (angleDist < rhs.angleDist); } bool operator==(const EdgePair& rhs) const { @@ -239,37 +293,53 @@ class EdgePair } return true; } - void SetObsAng(double a) + void setNegativeAngle(void) { - obsAngle = fmod(initangle - (a - 180), 360); + angle = -1.0; + } + double setCurrAngle(const PointPair& p) + { + if (p.vInf->point == vInf1->point) + { + angleDist = dist1; + angle = p.angle; + } + else if (p.vInf->point == vInf2->point) + { + angleDist = dist2; + angle = p.angle; + } + else if (p.angle != angle) + { + COLA_ASSERT(p.angle > angle); + angle = p.angle; + Point pp; + int result = rayIntersectPoint(vInf1->point, vInf2->point, + centerPoint, p.vInf->point, &(pp.x), &(pp.y)); + if (result != DO_INTERSECT) + { + // This can happen with points that appear to have the + // same angle but at are at slightly different positions + angleDist = std::min(dist1, dist2); + } + else + { + angleDist = euclideanDist(pp, centerPoint); + } + } - //db_printf("SetObsAng: %.2f (from init %.2f, a %.2f)\n", - // obsAngle, initangle, a); + return angleDist; } VertInf *vInf1; VertInf *vInf2; - double initdist; - double initangle; - double currdist; - double currangle; - double obsAngle; + double dist1; + double dist2; + double angle; + double angleDist; }; -typedef std::set<EdgePair> EdgeSet; - - -static bool ppCompare(PointPair& pp1, PointPair& pp2) -{ - if (pp1.angle == pp2.angle) - { - // If the points are colinear, then order them in increasing - // distance from the point we are sweeping around. - return dist(centerPoint, pp1.vInf->point) < - dist(centerPoint, pp2.vInf->point); - } - return pp1.angle < pp2.angle; -} +typedef std::list<EdgePair> SweepEdgeList; #define AHEAD 1 @@ -278,11 +348,11 @@ static bool ppCompare(PointPair& pp1, PointPair& pp2) class isBoundingShape { public: - // constructor remembers the value provided - isBoundingShape(ShapeSet& set) - : ss(set) + // Class instance remembers the ShapeSet. + isBoundingShape(ShapeSet& set) : + ss(set) { } - // the following is an overloading of the function call operator + // The following is an overloading of the function call operator. bool operator () (const PointPair& pp) { if (pp.vInf->id.isShape && @@ -293,58 +363,111 @@ class isBoundingShape return false; } private: + // MSVC wants to generate the assignment operator and the default + // constructor, but fails. Therefore we declare them private and + // don't implement them. + isBoundingShape & operator=(isBoundingShape const &); + isBoundingShape(); + ShapeSet& ss; }; -static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf, - bool lastVisible, double lastAngle, int *blocker) +static bool sweepVisible(SweepEdgeList& T, const PointPair& point, + std::set<unsigned int>& onBorderIDs, int *blocker) { + if (T.empty()) + { + // No blocking edges. + return true; + } - if (!lastInf || (lastAngle != centerAngle)) + Router *router = point.vInf->_router; + bool visible = true; + + SweepEdgeList::const_iterator closestIt = T.begin(); + SweepEdgeList::const_iterator end = T.end(); + while (closestIt != end) { - // Nothing before it on the current ray - EdgeSet::iterator closestIt = T.begin(); - if (closestIt != T.end()) + if ((point.vInf->point == closestIt->vInf1->point) || + (point.vInf->point == closestIt->vInf2->point)) { + // If the ray intersects just the endpoint of a + // blocking edge then ignore that edge. + ++closestIt; + continue; + } + break; + } + if (closestIt == end) + { + return true; + } - Point &e1 = (*closestIt).vInf1->point; - Point &e2 = (*closestIt).vInf2->point; - - if (segmentIntersect(centerInf->point, currInf->point, e1, e2)) + if (! point.vInf->id.isShape ) + { + // It's a connector endpoint, so we have to ignore + // edges of containing shapes for determining visibility. + ShapeSet& rss = router->contains[point.vInf->id]; + while (closestIt != end) + { + if (rss.find(closestIt->vInf1->id.objID) == rss.end()) { - *blocker = (*closestIt).vInf1->id.objID; - return false; + // This is not a containing edge so do the normal + // test and then stop. + if (point.distance > closestIt->angleDist) + { + visible = false; + } + else if ((point.distance == closestIt->angleDist) && + onBorderIDs.find(closestIt->vInf1->id.objID) != + onBorderIDs.end()) + { + // Touching, but centerPoint is on another edge of + // shape shape, so count as blocking. + visible = false; + } + break; } + // This was a containing edge, so consider the next along. + ++closestIt; } } else { - // There was another point before this on the ray (lastInf) - if (!lastVisible) + // Just test to see if this point is closer than the closest + // edge blocking this ray. + if (point.distance > closestIt->angleDist) { - *blocker = -1; - return false; + visible = false; } - else + else if ((point.distance == closestIt->angleDist) && + onBorderIDs.find(closestIt->vInf1->id.objID) != + onBorderIDs.end()) { - // Check if there is an edge in T that blocks the ray - // between lastInf and currInf. - EdgeSet::iterator tfin = T.end(); - for (EdgeSet::iterator l = T.begin(); l != tfin; ++l) - { - Point &e1 = (*l).vInf1->point; - Point &e2 = (*l).vInf2->point; + // Touching, but centerPoint is on another edge of + // shape shape, so count as blocking. + visible = false; + } + } - if (segmentIntersect(lastInf->point, currInf->point, e1, e2)) - { - *blocker = (*l).vInf1->id.objID; - return false; - } - } + if (!visible) + { + *blocker = (*closestIt).vInf1->id.objID; +#ifdef LINEDEBUG + Point &e1 = (*closestIt).vInf1->point; + Point &e2 = (*closestIt).vInf2->point; + + if (router->avoid_screen) + { + int canx = 151; + int cany = 55; + lineRGBA(router->avoid_screen, e1.x + canx, e1.y + cany, + e2.x + canx, e2.y + cany, 0, 0, 225, 255); } +#endif } - return true; + return visible; } @@ -358,120 +481,128 @@ void vertexSweep(VertInf *vert) centerID = pID; centerPoint = pPoint; Point centerPt = pPoint; - centerAngle = -1; // List of shape (and maybe endpt) vertices, except p // Sort list, around - VertList v; + VertSet v; // Initialise the vertex list + ShapeSet& ss = router->contains[centerID]; VertInf *beginVert = router->vertices.connsBegin(); VertInf *endVert = router->vertices.end(); for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext) { - if (inf->id == centerID) + if (inf == centerInf) + { + // Don't include the center point itself. + continue; + } + else if (inf->id == dummyOrthogID) { - // Don't include the center point + // Don't include orthogonal dummy vertices. + continue; + } + + if (!(centerID.isShape) && (ss.find(inf->id.objID) != ss.end())) + { + // Don't include edge points of containing shapes. + unsigned int shapeID = inf->id.objID; + db_printf("Center is inside shape %u so ignore shape edges.\n", + shapeID); continue; } if (inf->id.isShape) { - // Add shape vertex - v.push_back(inf); + // Add shape vertex. + v.insert(inf); } else { - if (router->IncludeEndpoints) + // Add connector endpoint. + if (centerID.isShape) { - if (centerID.isShape) - { - // Add endpoint vertex - v.push_back(inf); - } - else + // Center is a shape vertex, so add all endpoint vertices. + v.insert(inf); + } + else + { + // Center is an endpoint, so only include the other + // endpoint from the matching connector. + VertID partnerID = VertID(centerID.objID, false, + (centerID.vn == 1) ? 2 : 1); + if (inf->id == partnerID) { - // Center is an endpoint, so only include the other - // endpoint from the matching connector. - VertID partnerID = VertID(centerID.objID, false, - (centerID.vn == 1) ? 2 : 1); - if (inf->id == partnerID) - { - v.push_back(inf); - } + v.insert(inf); } } } } - // TODO: This should be done with a sorted data type and insertion sort. - v.sort(ppCompare); - - EdgeSet e; - ShapeSet& ss = router->contains[centerID]; + std::set<unsigned int> onBorderIDs; - // And edge to T that intersect the initial ray. - VertInf *last = router->vertices.end(); - for (VertInf *k = router->vertices.shapesBegin(); k != last; ) + // Add edges to T that intersect the initial ray. + SweepEdgeList e; + VertSet::const_iterator vbegin = v.begin(); + VertSet::const_iterator vend = v.end(); + for (VertSet::const_iterator t = vbegin; t != vend; ++t) { - VertID kID = k->id; - if (!(centerID.isShape) && (ss.find(kID.objID) != ss.end())) - { - unsigned int shapeID = kID.objID; - db_printf("Center is inside shape %u so ignore shape edges.\n", - shapeID); - // One of the endpoints is inside this shape so ignore it. - while ((k != last) && (k->id.objID == shapeID)) - { - // And skip the other vertices from this shape. - k = k->lstNext; - } - continue; - } + VertInf *k = t->vInf; - VertInf *kPrev = k->shPrev; - if ((centerInf == k) || (centerInf == kPrev)) - { - k = k->lstNext; - continue; - } + COLA_ASSERT(centerInf != k); + COLA_ASSERT(centerID.isShape || (ss.find(k->id.objID) == ss.end())); Point xaxis(DBL_MAX, centerInf->point.y); - if (segmentIntersect(centerInf->point, xaxis, kPrev->point, k->point)) + VertInf *kPrev = k->shPrev; + VertInf *kNext = k->shNext; + if (kPrev && (kPrev != centerInf) && + (vecDir(centerInf->point, xaxis, kPrev->point) == AHEAD)) { - double distance; - if (vecDir(centerInf->point, xaxis, kPrev->point) == BEHIND) + if (segmentIntersect(centerInf->point, xaxis, kPrev->point, + k->point)) { - distance = dist(centerInf->point, kPrev->point); + EdgePair intPair = EdgePair(*t, kPrev); + e.push_back(intPair); } - else + if ((vecDir(kPrev->point, k->point, centerInf->point) == 0) && + inBetween(kPrev->point, k->point, centerInf->point)) { - distance = dist(centerInf->point, k->point); + // Record that centerPoint is on an obstacle line. + onBorderIDs.insert(k->id.objID); + } + } + else if (kNext && (kNext != centerInf) && + (vecDir(centerInf->point, xaxis, kNext->point) == AHEAD)) + { + if (segmentIntersect(centerInf->point, xaxis, kNext->point, + k->point)) + { + EdgePair intPair = EdgePair(*t, kNext); + e.push_back(intPair); + } + if ((vecDir(kNext->point, k->point, centerInf->point) == 0) && + inBetween(kNext->point, k->point, centerInf->point)) + { + // Record that centerPoint is on an obstacle line. + onBorderIDs.insert(k->id.objID); } - - EdgePair intPair = EdgePair(k, kPrev, distance, 0.0); - e.insert(intPair).first; } - k = k->lstNext; } + for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c) + { + (*c).setNegativeAngle(); + } + // Start the actual sweep. db_printf("SWEEP: "); centerID.db_print(); db_printf("\n"); - VertInf *lastInf = NULL; - double lastAngle = 0; - bool lastVisible = false; - int lastBlocker = 0; - - isBoundingShape isBounding(router->contains[centerID]); - VertList::iterator vfst = v.begin(); - VertList::iterator vfin = v.end(); - for (VertList::iterator t = vfst; t != vfin; ++t) + isBoundingShape isBounding(ss); + for (VertSet::const_iterator t = vbegin; t != vend; ++t) { VertInf *currInf = (*t).vInf; VertID& currID = currInf->id; Point& currPt = currInf->point; - centerAngle = (*t).angle; #ifdef LINEDEBUG Sint16 ppx = (int) centerPt.x; @@ -479,29 +610,28 @@ void vertexSweep(VertInf *vert) Sint16 cx = (int) currPt.x; Sint16 cy = (int) currPt.y; + + int canx = 151; + int cany = 55; #endif - double currDist = dist(centerPt, currPt); - db_printf("Dist: %.1f.\n", currDist); + const double& currDist = (*t).distance; EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf); if (edge == NULL) { edge = new EdgeInf(centerInf, currInf); } - // Ignore vertices from bounding shapes, if sweeping round an endpoint. - if (!(centerID.isShape) && isBounding(*t)) + + for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c) { - if (router->InvisibilityGrph) - { - // if p and t can't see each other, add blank edge - db_printf("\tSkipping visibility edge... \n\t\t"); - edge->addBlocker(currInf->id.objID); - edge->db_print(); - } - continue; + (*c).setCurrAngle(*t); } + e.sort(); + // Check visibility. + int blocker = 0; + bool currVisible = sweepVisible(e, *t, onBorderIDs, &blocker); bool cone1 = true, cone2 = true; if (centerID.isShape) @@ -519,7 +649,6 @@ void vertexSweep(VertInf *vert) if (!cone1 || !cone2) { - lastInf = NULL; if (router->InvisibilityGrph) { db_printf("\tSetting invisibility edge... \n\t\t"); @@ -529,18 +658,15 @@ void vertexSweep(VertInf *vert) } else { - int blocker = 0; - // Check visibility. - bool currVisible = sweepVisible(e, currInf, - lastInf, lastVisible, lastAngle, &blocker); - if (blocker == -1) - { - blocker = lastBlocker; - } if (currVisible) { #ifdef LINEDEBUG - lineRGBA(avoid_screen, ppx, ppy, cx, cy, 255, 0, 0, 32); + if (router->avoid_screen) + { + lineRGBA(router->avoid_screen, ppx + canx, ppy + cany, + cx + canx, cy + cany, 255, 0, 0, 75); + SDL_Delay(1000); + } #endif db_printf("\tSetting visibility edge... \n\t\t"); edge->setDist(currDist); @@ -552,72 +678,55 @@ void vertexSweep(VertInf *vert) edge->addBlocker(blocker); edge->db_print(); } - - lastVisible = currVisible; - lastInf = currInf; - lastAngle = centerAngle; - lastBlocker = blocker; + } + + if (!(edge->added()) && !(router->InvisibilityGrph)) + { + delete edge; + edge = NULL; } if (currID.isShape) { // This is a shape edge - Point& prevPt = currInf->shPrev->point; - Point& nextPt = currInf->shNext->point; - - int prevDir = vecDir(centerPt, currPt, prevPt); - EdgePair prevPair = EdgePair(currInf, currInf->shPrev, - currDist, centerAngle); - EdgeSet::iterator ePtr; - if (prevDir == BEHIND) + if (currInf->shPrev != centerInf) { - // XXX: Strangely e.find does not return the correct results. - // ePtr = e.find(prevPair); - ePtr = std::find(e.begin(), e.end(), prevPair); - if (ePtr != e.end()) + Point& prevPt = currInf->shPrev->point; + int prevDir = vecDir(centerPt, currPt, prevPt); + EdgePair prevPair = EdgePair(*t, currInf->shPrev); + + if (prevDir == BEHIND) { - e.erase(ePtr); + e.remove(prevPair); } - } - else if ((prevDir == AHEAD) && (currInf->shPrev != centerInf)) - { - double x = prevPt.x - currPt.x; - double y = prevPt.y - currPt.y; - double angle = PointPair::pos_to_angle(x, y); - prevPair.SetObsAng(angle); - - ePtr = e.insert(prevPair).first; - } - - - int nextDir = vecDir(centerPt, currPt, nextPt); - EdgePair nextPair = EdgePair(currInf, currInf->shNext, - currDist, centerAngle); - - if (nextDir == BEHIND) - { - // XXX: Strangely e.find does not return the correct results. - // ePtr = e.find(nextPair); - ePtr = std::find(e.begin(), e.end(), nextPair); - if (ePtr != e.end()) + else if (prevDir == AHEAD) { - e.erase(ePtr); + e.push_front(prevPair); } } - else if ((nextDir == AHEAD) && (currInf->shNext != centerInf)) + + if (currInf->shNext != centerInf) { - double x = nextPt.x - currPt.x; - double y = nextPt.y - currPt.y; - double angle = PointPair::pos_to_angle(x, y); - nextPair.SetObsAng(angle); + Point& nextPt = currInf->shNext->point; + int nextDir = vecDir(centerPt, currPt, nextPt); + EdgePair nextPair = EdgePair(*t, currInf->shNext); - ePtr = e.insert(nextPair).first; + if (nextDir == BEHIND) + { + e.remove(nextPair); + } + else if (nextDir == AHEAD) + { + e.push_front(nextPair); + } } } - #ifdef LINEDEBUG - SDL_Flip(avoid_screen); + if (router->avoid_screen) + { + SDL_Flip(router->avoid_screen); + } #endif } } |