Merge GSoC2009 Connectors into trunk

(bzr r8855)

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
     }
 }