diff options
Diffstat (limited to 'src/libavoid/visibility.cpp')
| -rw-r--r-- | src/libavoid/visibility.cpp | 653 |
1 files changed, 653 insertions, 0 deletions
diff --git a/src/libavoid/visibility.cpp b/src/libavoid/visibility.cpp new file mode 100644 index 000000000..e13656f5f --- /dev/null +++ b/src/libavoid/visibility.cpp @@ -0,0 +1,653 @@ +/* + * vim: ts=4 sw=4 et tw=0 wm=0 + * + * libavoid - Fast, Incremental, Object-avoiding Line Router + * Copyright (C) 2004-2005 Michael Wybrow <mjwybrow@users.sourceforge.net> + * + * 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. + * + * 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 + * +*/ + + +#include <cfloat> + +#include "libavoid/shape.h" +#include "libavoid/debug.h" +#include "libavoid/visibility.h" +#include "libavoid/graph.h" + +#include <math.h> + +#ifdef LINEDEBUG + #include "SDL_gfxPrimitives.h" +#endif + +namespace Avoid { + + +bool UseAStarSearch = true; +bool IgnoreRegions = true; +bool SelectiveReroute = true; +bool IncludeEndpoints = true; +bool UseLeesAlgorithm = false; +bool InvisibilityGrph = true; +bool PartialFeedback = false; + +bool PartialTime = false; + + +void computeCompleteVis(void) +{ + VertInf *beginVert = vertices.shapesBegin(); + VertInf *endVert = vertices.end(); + for (VertInf *i = beginVert; i != endVert; i = i->lstNext) + { + db_printf("-- CONSIDERING --\n"); + i->id.db_print(); + + for (VertInf *j = i->lstPrev ; j != NULL; j = j->lstPrev) + { + bool knownNew = true; + EdgeInf::checkEdgeVisibility(i, j, knownNew); + } + } +} + + +void shapeVis(ShapeRef *shape) +{ + if (!InvisibilityGrph) + { + // Clear shape from graph. + shape->removeFromGraph(); + } + + VertInf *shapeBegin = shape->firstVert(); + VertInf *shapeEnd = shape->lastVert()->lstNext; + + VertInf *pointsBegin = NULL; + if (IncludeEndpoints) + { + pointsBegin = vertices.connsBegin(); + } + else + { + pointsBegin = vertices.shapesBegin(); + } + + 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) + { + EdgeInf::checkEdgeVisibility(curr, j, knownNew); + } + + db_printf("\tSecond Half:\n"); + VertInf *pointsEnd = vertices.end(); + for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext) + { + EdgeInf::checkEdgeVisibility(curr, k, knownNew); + } + } +} + + +void shapeVisSweep(ShapeRef *shape) +{ + if (!InvisibilityGrph) + { + // Clear shape from graph. + shape->removeFromGraph(); + } + + VertInf *startIter = shape->firstVert(); + VertInf *endIter = shape->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) +{ + const VertID& pID = point->id; + + // Make sure we're only doing ptVis for endpoints. + assert(!(pID.isShape)); + + if (!InvisibilityGrph) + { + point->removeFromGraph(); + } + + if (gen_contains && !(pID.isShape)) + { + generateContains(point); + } + + if (UseLeesAlgorithm) + { + vertexSweep(point); + } + else + { + VertInf *shapesEnd = vertices.end(); + for (VertInf *k = vertices.shapesBegin(); k != shapesEnd; + k = k->lstNext) + { + EdgeInf::checkEdgeVisibility(point, k, knownNew); + } + if (IncludeEndpoints && partner) + { + EdgeInf::checkEdgeVisibility(point, partner, knownNew); + } + } +} + + +//============================================================================ +// SWEEP CODE +// + +static VertInf *centerInf; +static Point centerPoint; +static VertID centerID; +static double centerAngle; + +#ifdef LINEDEBUG + SDL_Surface *avoid_screen = NULL; +#endif + + +class PointPair +{ + public: + PointPair(VertInf *inf) + : vInf(inf) + { + double x = vInf->point.x - centerPoint.x; + double y = vInf->point.y - centerPoint.y; + + angle = pos_to_angle(x, y); + } + bool operator==(const PointPair& rhs) const + { + if (vInf->id == rhs.vInf->id) + { + return true; + } + return false; + } + static double pos_to_angle(double x, double y) + { + double ang = atan(y / x); + ang = (ang * 180) / M_PI; + if (x < 0) + { + ang += 180; + } + else if (y < 0) + { + ang += 360; + } + return ang; + } + + VertInf *vInf; + double angle; +}; + + +typedef std::list<PointPair > VertList; + + +class EdgePair +{ + public: + EdgePair(VertInf *v1, VertInf *v2, double d, double a) + : vInf1(v1), vInf2(v2), initdist(d), initangle(a) + { + currdist = initdist; + currangle = initangle; + } + bool operator<(const EdgePair& rhs) const + { + if (initdist == rhs.initdist) + { + // 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 (initdist < rhs.initdist); + } + 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 SetObsAng(double a) + { + obsAngle = fmod(initangle - (a - 180), 360); + + //db_printf("SetObsAng: %.2f (from init %.2f, a %.2f)\n", + // obsAngle, initangle, a); + } + + VertInf *vInf1; + VertInf *vInf2; + double initdist; + double initangle; + double currdist; + double currangle; + double obsAngle; +}; + +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; +} + + +#define AHEAD 1 +#define BEHIND -1 + +class isBoundingShape +{ + public: + // constructor remembers the value provided + isBoundingShape(ShapeSet& set) + : ss(set) + { } + // the following is an overloading of the function call operator + bool operator () (const PointPair& pp) + { + if (pp.vInf->id.isShape && + (ss.find(pp.vInf->id.objID) != ss.end())) + { + return true; + } + return false; + } + private: + ShapeSet& ss; +}; + + +static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf, + bool lastVisible, double lastAngle, int *blocker) +{ + + if (!lastInf || (lastAngle != centerAngle)) + { + // Nothing before it on the current ray + EdgeSet::iterator closestIt = T.begin(); + if (closestIt != T.end()) + { + + Point &e1 = (*closestIt).vInf1->point; + Point &e2 = (*closestIt).vInf2->point; + + if (segmentIntersect(centerInf->point, currInf->point, e1, e2)) + { + *blocker = (*closestIt).vInf1->id.objID; + return false; + } + else + { + return true; + } + } + else + { + return true; + } + } + else + { + // There was another point before this on the ray (lastInf) + if (!lastVisible) + { + *blocker = -1; + return false; + } + else + { + // 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; + + if (segmentIntersect(lastInf->point, currInf->point, e1, e2)) + { + *blocker = (*l).vInf1->id.objID; + return false; + } + } + return true; + } + } +} + + +void vertexSweep(VertInf *vert) +{ + VertID& pID = vert->id; + Point& pPoint = vert->point; + + centerInf = vert; + centerID = pID; + centerPoint = pPoint; + Point centerPt = pPoint; + centerAngle = -1; + + // List of shape (and maybe endpt) vertices, except p + // Sort list, around + VertList v; + + // Initialise the vertex list + VertInf *beginVert = vertices.connsBegin(); + VertInf *endVert = vertices.end(); + for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext) + { + if (inf->id == centerID) + { + // Don't include the center point + continue; + } + + if (inf->id.isShape) + { + // Add shape vertex + v.push_back(inf); + } + else + { + if (IncludeEndpoints) + { + if (centerID.isShape) + { + // Add endpoint vertex + v.push_back(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) + { + v.push_back(inf); + } + } + } + } + } + // TODO: This should be done with a sorted data type and insertion sort. + v.sort(ppCompare); + + EdgeSet e; + ShapeSet& ss = contains[centerID]; + + // And edge to T that intersect the initial ray. + VertInf *last = vertices.end(); + for (VertInf *k = vertices.shapesBegin(); k != last; ) + { + VertID kID = k->id; + if (!(centerID.isShape) && (ss.find(kID.objID) != ss.end())) + { + uint 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 *kPrev = k->shPrev; + if ((centerInf == k) || (centerInf == kPrev)) + { + k = k->lstNext; + continue; + } + + Point xaxis = { DBL_MAX, centerInf->point.y }; + + if (segmentIntersect(centerInf->point, xaxis, kPrev->point, k->point)) + { + double distance; + if (vecDir(centerInf->point, xaxis, kPrev->point) == BEHIND) + { + distance = dist(centerInf->point, kPrev->point); + } + else + { + distance = dist(centerInf->point, k->point); + } + + EdgePair intPair = EdgePair(k, kPrev, distance, 0.0); + e.insert(intPair).first; + } + k = k->lstNext; + } + + // 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(contains[centerID]); + VertList::iterator vfst = v.begin(); + VertList::iterator vfin = v.end(); + for (VertList::iterator t = vfst; t != vfin; ++t) + { + VertInf *currInf = (*t).vInf; + VertID& currID = currInf->id; + Point& currPt = currInf->point; + centerAngle = (*t).angle; + +#ifdef LINEDEBUG + Sint16 ppx = (int) centerPt.x; + Sint16 ppy = (int) centerPt.y; + + Sint16 cx = (int) currPt.x; + Sint16 cy = (int) currPt.y; +#endif + + double currDist = dist(centerPt, currPt); + db_printf("Dist: %.1f.\n", currDist); + + 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)) + { + if (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; + } + + + bool cone1 = true, cone2 = true; + if (centerID.isShape) + { + cone1 = inValidRegion(centerInf->shPrev->point, centerPoint, + centerInf->shNext->point, currInf->point); + } + if (currInf->id.isShape) + { + cone2 = inValidRegion(currInf->shPrev->point, currInf->point, + currInf->shNext->point, centerPoint); + } + + if (!cone1 || !cone2) + { + lastInf = NULL; + if (InvisibilityGrph) + { + db_printf("\tSetting invisibility edge... \n\t\t"); + edge->addBlocker(0); + edge->db_print(); + } + } + 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); +#endif + db_printf("\tSetting visibility edge... \n\t\t"); + edge->setDist(currDist); + edge->db_print(); + } + else if (InvisibilityGrph) + { + db_printf("\tSetting invisibility edge... \n\t\t"); + edge->addBlocker(blocker); + edge->db_print(); + } + + lastVisible = currVisible; + lastInf = currInf; + lastAngle = centerAngle; + lastBlocker = blocker; + } + + 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) + { + ePtr = e.find(prevPair); + if (ePtr != e.end()) + { + e.erase(ePtr); + } + } + 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) + { + ePtr = e.find(nextPair); + if (ePtr != e.end()) + { + e.erase(ePtr); + } + } + else if ((nextDir == AHEAD) && (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); + + ePtr = e.insert(nextPair).first; + } + } + +#ifdef LINEDEBUG + SDL_Flip(avoid_screen); +#endif + } +} + + +} + |