diff options
Diffstat (limited to 'src/3rdparty/adaptagrams/libavoid/visibility.cpp')
| -rw-r--r-- | src/3rdparty/adaptagrams/libavoid/visibility.cpp | 676 |
1 files changed, 676 insertions, 0 deletions
diff --git a/src/3rdparty/adaptagrams/libavoid/visibility.cpp b/src/3rdparty/adaptagrams/libavoid/visibility.cpp new file mode 100644 index 0000000..3adadd4 --- /dev/null +++ b/src/3rdparty/adaptagrams/libavoid/visibility.cpp @@ -0,0 +1,676 @@ +/* + * vim: ts=4 sw=4 et tw=0 wm=0 + * + * libavoid - Fast, Incremental, Object-avoiding Line Router + * + * 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. + * + * Author(s): Michael Wybrow +*/ + + +#include <algorithm> +#include <cfloat> + +#include "libavoid/shape.h" +#include "libavoid/debug.h" +#include "libavoid/visibility.h" +#include "libavoid/vertices.h" +#include "libavoid/graph.h" +#include "libavoid/geometry.h" +#include "libavoid/router.h" +#include "libavoid/assertions.h" + + +namespace Avoid { + + +static void vertexSweep(VertInf *vert); + +void Obstacle::computeVisibilityNaive(void) +{ + if ( !(router()->InvisibilityGrph) ) + { + // Clear shape from graph. + removeFromGraph(); + } + + VertInf *shapeBegin = firstVert(); + VertInf *shapeEnd = lastVert()->lstNext; + + VertInf *pointsBegin = router()->vertices.connsBegin(); + for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext) + { + bool knownNew = true; + + db_printf("-- CONSIDERING --\n"); + curr->id.db_print(); + + 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); + } + + db_printf("\tSecond Half:\n"); + 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); + } + } +} + + +void Obstacle::computeVisibilitySweep(void) +{ + if ( !(router()->InvisibilityGrph) ) + { + // Clear shape from graph. + removeFromGraph(); + } + + VertInf *startIter = firstVert(); + VertInf *endIter = lastVert()->lstNext; + + for (VertInf *i = startIter; i != endIter; i = i->lstNext) + { + vertexSweep(i); + } +} + + +void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew, + const bool gen_contains) +{ + Router *router = point->_router; + const VertID& pID = point->id; + + // Make sure we're only doing ptVis for endpoints. + COLA_ASSERT(pID.isConnPt()); + + if ( !(router->InvisibilityGrph) ) + { + point->removeFromGraph(); + } + + if (gen_contains && pID.isConnPt()) + { + router->generateContains(point); + } + + if (router->UseLeesAlgorithm) + { + vertexSweep(point); + } + else + { + VertInf *shapesEnd = router->vertices.end(); + for (VertInf *k = router->vertices.connsBegin(); k != shapesEnd; + k = k->lstNext) + { + if (k->id == dummyOrthogID) + { + // Don't include orthogonal dummy vertices. + continue; + } + else if (k->id.isConnPt() && !k->id.isConnectionPin() && + !(k->id.isConnCheckpoint() && k->id.objID == pID.objID)) + { + // Include connection pins, but not connectors. + // Also include checkpoints with same ID as sweep point. + continue; + } + EdgeInf::checkEdgeVisibility(point, k, knownNew); + } + if (partner) + { + EdgeInf::checkEdgeVisibility(point, partner, knownNew); + } + } +} + + +//============================================================================ +// SWEEP CODE +// + + +class PointPair +{ + public: + PointPair(const Point& centerPoint, VertInf *inf) + : vInf(inf), + centerPoint(centerPoint) + { + angle = rotationalAngle(vInf->point - centerPoint); + distance = euclideanDist(centerPoint, vInf->point); + } + bool operator<(const PointPair& rhs) const + { + // Firstly order by angle. + if (angle == rhs.angle) + { + // 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 angle < rhs.angle; + } + + VertInf *vInf; + double angle; + double distance; + Point centerPoint; +}; + +typedef std::set<PointPair > VertSet; + + +class EdgePair +{ + public: + EdgePair() + : vInf1(nullptr), + vInf2(nullptr), + 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, p1.centerPoint)), + angle(p1.angle), + angleDist(p1.distance), + centerPoint(p1.centerPoint) + { + } + bool operator<(const EdgePair& rhs) const + { + COLA_ASSERT(angle == rhs.angle); + if (angleDist == rhs.angleDist) + { + return (dist2 < rhs.dist2); + } + return (angleDist < rhs.angleDist); + } + bool operator==(const EdgePair& rhs) const + { + if (((vInf1->id == rhs.vInf1->id) && + (vInf2->id == rhs.vInf2->id)) || + ((vInf1->id == rhs.vInf2->id) && + (vInf2->id == rhs.vInf1->id))) + { + return true; + } + return false; + } + bool operator!=(const EdgePair& rhs) const + { + if (((vInf1->id == rhs.vInf1->id) && + (vInf2->id == rhs.vInf2->id)) || + ((vInf1->id == rhs.vInf2->id) && + (vInf2->id == rhs.vInf1->id))) + { + return false; + } + return true; + } + void setNegativeAngle(void) + { + 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); + } + } + + return angleDist; + } + + VertInf *vInf1; + VertInf *vInf2; + double dist1; + double dist2; + double angle; + double angleDist; + Point centerPoint; +}; + +typedef std::list<EdgePair> SweepEdgeList; + + +#define AHEAD 1 +#define BEHIND -1 + +class isBoundingShape +{ + public: + // Class instance remembers the ShapeSet. + isBoundingShape(ShapeSet& set) : + ss(set) + { } + // The following is an overloading of the function call operator. + bool operator () (const PointPair& pp) + { + if (!(pp.vInf->id.isConnPt()) && + (ss.find(pp.vInf->id.objID) != ss.end())) + { + return true; + } + 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(SweepEdgeList& T, const PointPair& point, + std::set<unsigned int>& onBorderIDs, int *blocker) +{ + if (T.empty()) + { + // No blocking edges. + return true; + } + + Router *router = point.vInf->_router; + bool visible = true; + + SweepEdgeList::const_iterator closestIt = T.begin(); + SweepEdgeList::const_iterator end = T.end(); + while (closestIt != 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; + } + + if (point.vInf->id.isConnPt()) + { + // 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()) + { + // 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 + { + // Just test to see if this point is closer than the closest + // edge blocking this ray. + 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; + } + } + + if (!visible) + { + *blocker = (*closestIt).vInf1->id.objID; + } + return visible; +} + + +static void vertexSweep(VertInf *vert) +{ + Router *router = vert->_router; + VertID& pID = vert->id; + Point& pPoint = vert->point; + + VertInf *centerInf = vert; + VertID centerID = pID; + Point centerPoint = pPoint; + + // List of shape (and maybe endpt) vertices, except p + // Sort list, around + 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 == centerInf) + { + // Don't include the center point itself. + continue; + } + else if (inf->id == dummyOrthogID) + { + // Don't include orthogonal dummy vertices. + continue; + } + + if (centerID.isConnPt() && (ss.find(inf->id.objID) != ss.end()) && + !inf->id.isConnPt()) + { + // 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.isConnPt()) + { + // Add connector endpoint. + if (centerID.isConnPt()) + { + if (inf->id.isConnectionPin()) + { + v.insert(PointPair(centerPoint, inf)); + } + else if (centerID.isConnectionPin()) + { + // Connection pins have visibility to everything. + v.insert(PointPair(centerPoint, inf)); + } + else if (inf->id.objID == centerID.objID) + { + // Center is an endpoint, so only include the other + // endpoints or checkpoints from the matching connector. + v.insert(PointPair(centerPoint, inf)); + } + } + else + { + // Center is a shape vertex, so add all endpoint vertices. + v.insert(PointPair(centerPoint, inf)); + } + } + else + { + // Add shape vertex. + v.insert(PointPair(centerPoint, inf)); + } + } + std::set<unsigned int> onBorderIDs; + + // Add edges to T that intersect the initial ray. + SweepEdgeList e; + VertSet::const_iterator vbegin = v.begin(); + VertSet::const_iterator vend = v.end(); + const Point xaxis(DBL_MAX, centerInf->point.y); + for (VertSet::const_iterator t = vbegin; t != vend; ++t) + { + VertInf *k = t->vInf; + + COLA_ASSERT(centerInf != k); + + VertInf *kPrev = k->shPrev; + VertInf *kNext = k->shNext; + if (kPrev && (kPrev != centerInf) && + (vecDir(centerInf->point, xaxis, kPrev->point) == AHEAD)) + { + if (segmentIntersect(centerInf->point, xaxis, kPrev->point, + k->point)) + { + EdgePair intPair = EdgePair(*t, kPrev); + e.push_back(intPair); + } + if (pointOnLine(kPrev->point, k->point, centerInf->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 (pointOnLine(kNext->point, k->point, centerInf->point)) + { + // Record that centerPoint is on an obstacle line. + onBorderIDs.insert(k->id.objID); + } + } + } + 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"); + + isBoundingShape isBounding(ss); + for (VertSet::const_iterator t = vbegin; t != vend; ++t) + { + VertInf *currInf = (*t).vInf; + VertID& currID = currInf->id; + Point& currPt = currInf->point; + + const double& currDist = (*t).distance; + + EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf); + if (edge == nullptr) + { + edge = new EdgeInf(centerInf, currInf); + } + + for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c) + { + (*c).setCurrAngle(*t); + } + e.sort(); + + // Check visibility. + int blocker = 0; + bool currVisible = sweepVisible(e, *t, onBorderIDs, &blocker); + + bool cone1 = true, cone2 = true; + if (!(centerID.isConnPt())) + { + cone1 = inValidRegion(router->IgnoreRegions, + centerInf->shPrev->point, centerPoint, + centerInf->shNext->point, currInf->point); + } + if (!(currInf->id.isConnPt())) + { + cone2 = inValidRegion(router->IgnoreRegions, + currInf->shPrev->point, currInf->point, + currInf->shNext->point, centerPoint); + } + + if (!cone1 || !cone2) + { + if (router->InvisibilityGrph) + { + db_printf("\tSetting invisibility edge... \n\t\t"); + edge->addBlocker(0); + edge->db_print(); + } + } + else + { + if (currVisible) + { + db_printf("\tSetting visibility edge... \n\t\t"); + edge->setDist(currDist); + edge->db_print(); + } + else if (router->InvisibilityGrph) + { + db_printf("\tSetting invisibility edge... \n\t\t"); + edge->addBlocker(blocker); + edge->db_print(); + } + } + + if (!(edge->added()) && !(router->InvisibilityGrph)) + { + delete edge; + edge = nullptr; + } + + if (!(currID.isConnPt())) + { + // This is a shape edge + + if (currInf->shPrev != centerInf) + { + Point& prevPt = currInf->shPrev->point; + int prevDir = vecDir(centerPoint, currPt, prevPt); + EdgePair prevPair = EdgePair(*t, currInf->shPrev); + + if (prevDir == BEHIND) + { + e.remove(prevPair); + } + else if (prevDir == AHEAD) + { + e.push_front(prevPair); + } + } + + if (currInf->shNext != centerInf) + { + Point& nextPt = currInf->shNext->point; + int nextDir = vecDir(centerPoint, currPt, nextPt); + EdgePair nextPair = EdgePair(*t, currInf->shNext); + + if (nextDir == BEHIND) + { + e.remove(nextPair); + } + else if (nextDir == AHEAD) + { + e.push_front(nextPair); + } + } + } + } +} + + +} + |