Merge GSoC2009 Connectors into trunk

(bzr r8855)

29 files changed, 5536 insertions, 2754 deletions

diff --git a/src/libavoid/CMakeLists.txt b/src/libavoid/CMakeLists.txt
index df532c564..3f408074c 100644
--- a/src/libavoid/CMakeLists.txt
+++ b/src/libavoid/CMakeLists.txt
@@ -11,6 +11,8 @@ static.cpp
 timer.cpp
 vertices.cpp
 visibility.cpp
+orthogonal.cpp
+vpsc.cpp
 )
 ADD_LIBRARY(avoid STATIC ${libavoid_SRC})
 TARGET_LINK_LIBRARIES(avoid
diff --git a/src/libavoid/Makefile_insert b/src/libavoid/Makefile_insert
index f75470e26..77728499c 100644
--- a/src/libavoid/Makefile_insert
+++ b/src/libavoid/Makefile_insert
@@ -11,24 +11,26 @@ libavoid_libavoid_a_SOURCES =	\
 	libavoid/debug.h	\
 	libavoid/geometry.cpp	\
 	libavoid/geometry.h	\
+	libavoid/geomtypes.cpp	\
 	libavoid/geomtypes.h	\
 	libavoid/graph.cpp	\
 	libavoid/graph.h	\
 	libavoid/makepath.cpp	\
 	libavoid/makepath.h	\
-	libavoid/polyutil.cpp	\
-	libavoid/polyutil.h	\
+	libavoid/orthogonal.cpp	\
+	libavoid/orthogonal.h	\
+	libavoid/vpsc.cpp	\
+	libavoid/vpsc.h		\
 	libavoid/router.cpp	\
 	libavoid/router.h	\
 	libavoid/shape.cpp	\
 	libavoid/shape.h	\
-	libavoid/static.cpp	\
-	libavoid/static.h	\
 	libavoid/timer.cpp	\
 	libavoid/timer.h	\
 	libavoid/vertices.cpp	\
 	libavoid/vertices.h	\
 	libavoid/visibility.cpp	\
 	libavoid/visibility.h	\
-	libavoid/libavoid.h	\
-	libavoid/region.h
+	libavoid/viscluster.cpp \
+	libavoid/viscluster.h   \
+	libavoid/libavoid.h
diff --git a/src/libavoid/connector.cpp b/src/libavoid/connector.cpp
index 647303371..d9088dfe7 100644
--- a/src/libavoid/connector.cpp
+++ b/src/libavoid/connector.cpp
@@ -2,95 +2,236 @@
  * 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
  *
  * 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 <cstring>
+#include <cfloat>
+#include <cmath>
 #include <cstdlib>
+
 #include "libavoid/graph.h"
 #include "libavoid/connector.h"
 #include "libavoid/makepath.h"
 #include "libavoid/visibility.h"
 #include "libavoid/debug.h"
 #include "libavoid/router.h"
+#include "libavoid/assertions.h"
 
 
 namespace Avoid {
 
+    
+ConnEnd::ConnEnd(const Point& point) 
+    : _point(point),
+      _directions(ConnDirAll),
+      _shapeRef(NULL)
+{
+}
+
+
+ConnEnd::ConnEnd(const Point& point, const ConnDirFlags visDirs) 
+    : _point(point),
+      _directions(visDirs),
+      _shapeRef(NULL)
+{
+}
+
+ConnEnd::ConnEnd(ShapeRef *shapeRef, const double x_pos, const double y_pos,
+        const double insideOffset, const ConnDirFlags visDirs) 
+    : _directions(visDirs),
+      _shapeRef(shapeRef),
+      _xPosition(x_pos),
+      _yPosition(y_pos),
+      _insideOffset(insideOffset)
+{
+}
+
+const Point ConnEnd::point(void) const
+{
+    if (_shapeRef)
+    {
+        const Polygon& poly = _shapeRef->polygon();
+
+        double x_min = DBL_MAX;
+        double x_max = -DBL_MAX;
+        double y_min = DBL_MAX;
+        double y_max = -DBL_MAX;
+        for (size_t i = 0; i < poly.size(); ++i)
+        {
+            x_min = std::min(x_min, poly.ps[i].x);
+            x_max = std::max(x_max, poly.ps[i].x);
+            y_min = std::min(y_min, poly.ps[i].y);
+            y_max = std::max(y_max, poly.ps[i].y);
+        }
+
+        Point point;
+
+        // We want to place connection points on the edges of shapes, 
+        // or possibly slightly inside them (if _insideOfset is set).
+
+        point.vn = kUnassignedVertexNumber;
+        if (_xPosition == ATTACH_POS_LEFT)
+        {
+            point.x = x_min + _insideOffset;
+            point.vn = 6;
+        }
+        else if (_xPosition == ATTACH_POS_RIGHT)
+        {
+            point.x = x_max - _insideOffset;
+            point.vn = 4;
+        }
+        else
+        {
+            point.x = x_min + (_xPosition * (x_max - x_min));
+        }
+
+        if (_yPosition == ATTACH_POS_TOP)
+        {
+            point.y = y_max - _insideOffset;
+            point.vn = 5;
+        }
+        else if (_yPosition == ATTACH_POS_BOTTOM)
+        {
+            point.y = y_min + _insideOffset;
+            point.vn = 7;
+        }
+        else
+        {
+            point.y = y_min + (_yPosition * (y_max - y_min));
+            point.vn = kUnassignedVertexNumber;
+        }
+
+        return point;
+    }
+    else
+    {
+        return _point;
+    }
+}
+
+
+ConnDirFlags ConnEnd::directions(void) const
+{
+    if (_shapeRef)
+    {
+        ConnDirFlags visDir = _directions;
+        if (_directions == ConnDirNone)
+        {
+            // None is set, use the defaults:
+            if (_xPosition == ATTACH_POS_LEFT)
+            {
+                visDir = ConnDirLeft;
+            }
+            else if (_xPosition == ATTACH_POS_RIGHT)
+            {
+                visDir = ConnDirRight;
+            }
+            if (_yPosition == ATTACH_POS_TOP)
+            {
+                visDir = ConnDirDown;
+            }
+            else if (_yPosition == ATTACH_POS_BOTTOM)
+            {
+                visDir = ConnDirUp;
+            }
+
+            if (visDir == ConnDirNone)
+            {
+                visDir = ConnDirAll;
+            }
+        }
+        return visDir;
+    }
+    else
+    {
+        return _directions;
+    }
+}
+
+
 ConnRef::ConnRef(Router *router, const unsigned int id)
-    : _router(router)
-    , _id(id)
-    , _type(ConnType_PolyLine)
-    , _srcId(0)
-    , _dstId(0)
-    , _needs_reroute_flag(true)
-    , _false_path(false)
-    , _active(false)
-    , _route_dist(0)
-    , _srcVert(NULL)
-    , _dstVert(NULL)
-    , _initialised(false)
-    , _callback(NULL)
-    , _connector(NULL)
-    , _hateCrossings(false)
+    : _router(router),
+      _type(router->validConnType()),
+      _srcId(0),
+      _dstId(0),
+      _needs_reroute_flag(true),
+      _false_path(false),
+      _needs_repaint(false),
+      _active(false),
+      _route_dist(0),
+      _srcVert(NULL),
+      _dstVert(NULL),
+      _startVert(NULL),
+      _initialised(false),
+      _callback(NULL),
+      _connector(NULL),
+      _hateCrossings(false)
 {
+    _id = router->assignId(id);
+
     // TODO: Store endpoints and details.
-    _route.pn = 0;
-    _route.ps = NULL;
-}
-
-
-ConnRef::ConnRef(Router *router, const unsigned int id,
-        const Point& src, const Point& dst)
-    : _router(router)
-    , _id(id)
-    , _type(ConnType_PolyLine)
-    , _srcId(0)
-    , _dstId(0)
-    , _needs_reroute_flag(true)
-    , _false_path(false)
-    , _active(false)
-    , _route_dist(0)
-    , _srcVert(NULL)
-    , _dstVert(NULL)
-    , _initialised(false)
-    , _callback(NULL)
-    , _connector(NULL)
-    , _hateCrossings(false)
-{
-    _route.pn = 0;
-    _route.ps = NULL;
-
-    if (_router->IncludeEndpoints)
-    {
-        bool isShape = false;
-        _srcVert = new VertInf(_router, VertID(id, isShape, 1), src);
-        _dstVert = new VertInf(_router, VertID(id, isShape, 2), dst);
-        _router->vertices.addVertex(_srcVert);
-        _router->vertices.addVertex(_dstVert);
-        makeActive();
-        _initialised = true;
-    }
+    _route.clear();
+}
+
+
+ConnRef::ConnRef(Router *router, const ConnEnd& src, const ConnEnd& dst,
+        const unsigned int id)
+    : _router(router),
+      _type(router->validConnType()),
+      _srcId(0),
+      _dstId(0),
+      _needs_reroute_flag(true),
+      _false_path(false),
+      _needs_repaint(false),
+      _active(false),
+      _route_dist(0),
+      _srcVert(NULL),
+      _dstVert(NULL),
+      _initialised(false),
+      _callback(NULL),
+      _connector(NULL),
+      _hateCrossings(false)
+{
+    _id = router->assignId(id);
+    _route.clear();
+
+    bool isShape = false;
+    _srcVert = new VertInf(_router, VertID(_id, isShape, 1), src.point());
+    _srcVert->visDirections = src.directions();
+    _dstVert = new VertInf(_router, VertID(_id, isShape, 2), dst.point());
+    _dstVert->visDirections = dst.directions();
+    makeActive();
+    _initialised = true;
+    
+    setEndpoints(src, dst);
 }
 
 
 ConnRef::~ConnRef()
 {
-    freeRoute();
+    _router->removeQueuedConnectorActions(this);
+    removeFromGraph();
+
+    freeRoutes();
 
     if (_srcVert)
     {
@@ -106,27 +247,38 @@ ConnRef::~ConnRef()
         _dstVert = NULL;
     }
 
-    if (_active)
-    {
-        makeInactive();
-    }
+    makeInactive();
 }
 
 
-void ConnRef::setType(unsigned int type)
+ConnType ConnRef::routingType(void) const
 {
-    _type = type;
+    return _type;
 }
 
 
-void ConnRef::updateEndPoint(const unsigned int type, const Point& point)
+void ConnRef::setRoutingType(ConnType type)
 {
-    assert((type == (unsigned int) VertID::src) ||
-           (type == (unsigned int) VertID::tar));
-    
-    // XXX: This was commented out.  Is there a case where it isn't true? 
-    assert(_router->IncludeEndpoints);
+    type = _router->validConnType(type);
+    if (_type != type)
+    {
+        _type = type;
+
+        makePathInvalid();
+
+        _router->modifyConnector(this);
+    }
+}
 
+
+void ConnRef::common_updateEndPoint(const unsigned int type, const ConnEnd& connEnd)
+{
+    const Point& point = connEnd.point();
+    //db_printf("common_updateEndPoint(%d,(pid=%d,vn=%d,(%f,%f)))\n",
+    //      type,point.id,point.vn,point.x,point.y);
+    COLA_ASSERT((type == (unsigned int) VertID::src) ||
+                (type == (unsigned int) VertID::tar));
+    
     if (!_initialised)
     {
         makeActive();
@@ -141,28 +293,28 @@ void ConnRef::updateEndPoint(const unsigned int type, const Point& point)
     {
         if (_srcVert)
         {
-            _srcVert->Reset(point);
+            _srcVert->Reset(VertID(_id, isShape, type), point);
         }
         else
         {
             _srcVert = new VertInf(_router, VertID(_id, isShape, type), point);
-            _router->vertices.addVertex(_srcVert);
         }
+        _srcVert->visDirections = connEnd.directions();
         
         altered = _srcVert;
         partner = _dstVert;
     }
-    else // if (type == (unsigned int) VertID::dst)
+    else // if (type == (unsigned int) VertID::tar)
     {
         if (_dstVert)
         {
-            _dstVert->Reset(point);
+            _dstVert->Reset(VertID(_id, isShape, type), point);
         }
         else
         {
             _dstVert = new VertInf(_router, VertID(_id, isShape, type), point);
-            _router->vertices.addVertex(_dstVert);
         }
+        _dstVert->visDirections = connEnd.directions();
         
         altered = _dstVert;
         partner = _srcVert;
@@ -171,8 +323,85 @@ void ConnRef::updateEndPoint(const unsigned int type, const Point& point)
     // XXX: Seems to be faster to just remove the edges and recreate
     bool isConn = true;
     altered->removeFromGraph(isConn);
-    bool knownNew = true;
-    vertexVisibility(altered, partner, knownNew, true);
+
+    makePathInvalid();
+    _router->setStaticGraphInvalidated(true);
+}
+
+
+void ConnRef::setEndpoints(const ConnEnd& srcPoint, const ConnEnd& dstPoint)
+{
+    _router->modifyConnector(this, VertID::src, srcPoint);
+    _router->modifyConnector(this, VertID::tar, dstPoint);
+}
+
+
+void ConnRef::setEndpoint(const unsigned int type, const ConnEnd& connEnd)
+{
+    _router->modifyConnector(this, type, connEnd);
+}
+
+
+void ConnRef::setSourceEndpoint(const ConnEnd& srcPoint)
+{
+    _router->modifyConnector(this, VertID::src, srcPoint);
+}
+
+
+void ConnRef::setDestEndpoint(const ConnEnd& dstPoint)
+{
+    _router->modifyConnector(this, VertID::tar, dstPoint);
+}
+
+
+void ConnRef::updateEndPoint(const unsigned int type, const ConnEnd& connEnd)
+{
+    common_updateEndPoint(type, connEnd);
+
+    if (_router->_polyLineRouting)
+    {
+        bool knownNew = true;
+        bool genContains = true;
+        if (type == (unsigned int) VertID::src)
+        {
+            vertexVisibility(_srcVert, _dstVert, knownNew, genContains);
+        }
+        else
+        {
+            vertexVisibility(_dstVert, _srcVert, knownNew, genContains);
+        }
+    }
+}
+
+
+bool ConnRef::setEndpoint(const unsigned int type, const VertID& pointID,
+        Point *pointSuggestion)
+{
+    VertInf *vInf = _router->vertices.getVertexByID(pointID);
+    if (vInf == NULL)
+    {
+        return false;
+    }
+    Point& point = vInf->point;
+    if (pointSuggestion)
+    {
+        if (euclideanDist(point, *pointSuggestion) > 0.5)
+        {
+            return false;
+        }
+    }
+
+    common_updateEndPoint(type, point);
+
+    // Give this visibility just to the point it is over.
+    EdgeInf *edge = new EdgeInf(
+            (type == VertID::src) ? _srcVert : _dstVert, vInf);
+    // XXX: We should be able to set this to zero, but can't due to 
+    //      assumptions elsewhere in the code.
+    edge->setDist(0.001);
+
+    _router->processTransaction();
+    return true;
 }
 
 
@@ -203,7 +432,7 @@ unsigned int ConnRef::getDstShapeId(void)
 
 void ConnRef::makeActive(void)
 {
-    assert(!_active);
+    COLA_ASSERT(!_active);
     
     // Add to connRefs list.
     _pos = _router->connRefs.insert(_router->connRefs.begin(), this);
@@ -213,7 +442,7 @@ void ConnRef::makeActive(void)
 
 void ConnRef::makeInactive(void)
 {
-    assert(_active);
+    COLA_ASSERT(_active);
     
     // Remove from connRefs list.
     _router->connRefs.erase(_pos);
@@ -221,54 +450,69 @@ void ConnRef::makeInactive(void)
 }
 
 
-void ConnRef::freeRoute(void)
+void ConnRef::freeRoutes(void)
 {
-    if (_route.ps)
-    {
-        _route.pn = 0;
-        std::free(_route.ps);
-        _route.ps = NULL;
-    }
+    _route.clear();
+    _display_route.clear();
 }
     
 
-PolyLine& ConnRef::route(void)
+const PolyLine& ConnRef::route(void) const
 {
     return _route;
 }
 
 
-void ConnRef::calcRouteDist(void)
+PolyLine& ConnRef::routeRef(void)
 {
-    _route_dist = 0;
-    for (int i = 1; i < _route.pn; i++)
+    return _route;
+}
+
+
+void ConnRef::set_route(const PolyLine& route)
+{
+    if (&_display_route == &route)
     {
-        _route_dist += dist(_route.ps[i], _route.ps[i - 1]);
+        db_printf("Error:\tTrying to update libavoid route with itself.\n");
+        return;
     }
+    _display_route.ps = route.ps;
+
+    //_display_route.clear();
 }
 
 
-bool ConnRef::needsReroute(void)
+Polygon& ConnRef::displayRoute(void)
 {
-    return (_false_path || _needs_reroute_flag);
+    if (_display_route.empty())
+    {
+        // No displayRoute is set.  Simplify the current route to get it.
+        _display_route = _route.simplify();
+    }
+    return _display_route;
 }
 
 
-void ConnRef::lateSetup(const Point& src, const Point& dst)
+void ConnRef::calcRouteDist(void)
 {
-    assert(!_initialised);
+    double (*dist)(const Point& a, const Point& b) = 
+            (_type == ConnType_PolyLine) ? euclideanDist : manhattanDist;
 
-    bool isShape = false;
-    _srcVert = new VertInf(_router, VertID(_id, isShape, 1), src);
-    _dstVert = new VertInf(_router, VertID(_id, isShape, 2), dst);
-    _router->vertices.addVertex(_srcVert);
-    _router->vertices.addVertex(_dstVert);
-    makeActive();
-    _initialised = true;
+    _route_dist = 0;
+    for (size_t i = 1; i < _route.size(); ++i)
+    {
+        _route_dist += dist(_route.at(i), _route.at(i - 1));
+    }
 }
 
 
-unsigned int ConnRef::id(void)
+bool ConnRef::needsRepaint(void) const
+{
+    return _needs_repaint;
+}
+
+
+unsigned int ConnRef::id(void) const
 {
     return _id;
 }
@@ -286,6 +530,12 @@ VertInf *ConnRef::dst(void)
 }
 
 
+VertInf *ConnRef::start(void)
+{
+    return _startVert;
+}
+
+
 bool ConnRef::isInitialised(void)
 {
     return _initialised;
@@ -303,29 +553,8 @@ void ConnRef::unInitialise(void)
 
 void ConnRef::removeFromGraph(void)
 {
-    for (VertInf *iter = _srcVert; iter != NULL; )
-    {
-        VertInf *tmp = iter;
-        iter = (iter == _srcVert) ? _dstVert : NULL;
-        
-        // For each vertex.
-        EdgeInfList& visList = tmp->visList;
-        EdgeInfList::iterator finish = visList.end();
-        EdgeInfList::iterator edge;
-        while ((edge = visList.begin()) != finish)
-        {
-            // Remove each visibility edge
-            delete (*edge);
-        }
-
-        EdgeInfList& invisList = tmp->invisList;
-        finish = invisList.end();
-        while ((edge = invisList.begin()) != finish)
-        {
-            // Remove each invisibility edge
-            delete (*edge);
-        }
-    }
+    _srcVert->removeFromGraph();
+    _dstVert->removeFromGraph();
 }
 
 
@@ -336,12 +565,11 @@ void ConnRef::setCallback(void (*cb)(void *), void *ptr)
 }
 
 
-void ConnRef::handleInvalid(void)
+void ConnRef::performCallback(void)
 {
-    if (_false_path || _needs_reroute_flag) {
-        if (_callback) {
-            _callback(_connector);
-        }
+    if (_callback) 
+    {
+        _callback(_connector);
     }
 }
 
@@ -352,79 +580,279 @@ void ConnRef::makePathInvalid(void)
 }
 
 
-Router *ConnRef::router(void)
+Router *ConnRef::router(void) const
 {
     return _router;
 }
 
 
-int ConnRef::generatePath(Point p0, Point p1)
+bool ConnRef::generatePath(Point p0, Point p1)
+{
+    // XXX Code to determine when connectors really need to be rerouted
+    //     does not yet work for orthogonal connectors.
+    if (_type != ConnType_Orthogonal)
+    {
+        if (!_false_path && !_needs_reroute_flag) 
+        {
+            // This connector is up to date.
+            return false;
+        }
+    }
+
+    bool result = generatePath();
+
+    return result;
+}
+
+
+// Validates a bend point on a path to check it does not form a zigzag corner.
+// a, b, c are consecutive points on the path.  d and e are b's neighbours,
+// forming the shape corner d-b-e.
+//
+bool validateBendPoint(VertInf *aInf, VertInf *bInf, VertInf *cInf)
 {
-    if (!_false_path && !_needs_reroute_flag) {
+    bool bendOkay = true;
+
+    if ((aInf == NULL) || (cInf == NULL))
+    {
+        // Not a bendpoint, i.e., the end of the connector, so don't test.
+        return bendOkay;
+    }
+
+    COLA_ASSERT(bInf != NULL);
+    VertInf *dInf = bInf->shPrev;
+    VertInf *eInf = bInf->shNext;
+    COLA_ASSERT(dInf != NULL);
+    COLA_ASSERT(eInf != NULL);
+
+    Point& a = aInf->point;
+    Point& b = bInf->point;
+    Point& c = cInf->point;
+    Point& d = dInf->point;
+    Point& e = eInf->point;
+
+    if ((a == b) || (b == c))
+    {
+        return bendOkay;
+    }
+
+#ifdef PATHDEBUG
+    db_printf("a=(%g, %g)\n", a.x, a.y);
+    db_printf("b=(%g, %g)\n", b.x, b.y);
+    db_printf("c=(%g, %g)\n", c.x, c.y);
+    db_printf("d=(%g, %g)\n", d.x, d.y);
+    db_printf("e=(%g, %g)\n", e.x, e.y);
+#endif
+    // Check angle:
+    int abc = vecDir(a, b, c);
+#ifdef PATHDEBUG
+    db_printf("(abc == %d) ", abc);
+#endif
+   
+    if (abc == 0)
+    {
+        // The three consecutive point on the path are in a line.
+        // Thus, there should always be an equally short path that
+        // skips this bend point.
+        bendOkay = false;
+    }
+    else // (abc != 0)
+    {
+        COLA_ASSERT(vecDir(d, b, e) > 0);
+        int abe = vecDir(a, b, e);
+        int abd = vecDir(a, b, d);
+        int bce = vecDir(b, c, e);
+        int bcd = vecDir(b, c, d);
+#ifdef PATHDEBUG
+        db_printf("&& (abe == %d) && (abd == %d) &&\n(bce == %d) && (bcd == %d)",
+                abe, abd, bce, bcd);
+#endif
+
+        bendOkay = false;
+        if (abe > 0)
+        {
+            if ((abc > 0) && (abd >= 0) && (bce >= 0))
+            {
+                bendOkay = true;
+            }
+        }
+        else if (abd < 0)
+        {
+            if ((abc < 0) && (abe <= 0) && (bcd <= 0))
+            {
+                bendOkay = true;
+            }
+        }
+    }
+#ifdef PATHDEBUG
+    db_printf("\n");
+#endif
+    return bendOkay;
+}
+
+
+bool ConnRef::generatePath(void)
+{
+    if (!_false_path && !_needs_reroute_flag) 
+    {
         // This connector is up to date.
-        return (int) false;
+        return false;
     }
 
+    if (!_dstVert || !_srcVert)
+    {
+        // Connector is not fully initialised..
+        return false;
+    }
+    
+    //COLA_ASSERT(_srcVert->point != _dstVert->point);
+
     _false_path = false;
     _needs_reroute_flag = false;
 
-    VertInf *src = _srcVert;
     VertInf *tar = _dstVert;
+    _startVert = _srcVert;
 
-    if ( !(_router->IncludeEndpoints) )
-    {
-        lateSetup(p0, p1);
-        
-        // Update as they have just been set by lateSetup.
-        src = _srcVert;
-        tar = _dstVert;
+    bool *flag = &(_needs_reroute_flag);
    
-        bool knownNew = true;
-        bool genContains = true;
-        vertexVisibility(src, tar, knownNew, genContains);
-        vertexVisibility(tar, src, knownNew, genContains);
+    size_t existingPathStart = 0;
+    const PolyLine& currRoute = route();
+    if (_router->RubberBandRouting)
+    {
+        COLA_ASSERT(_router->IgnoreRegions == true);
+
+#ifdef PATHDEBUG
+        db_printf("\n");
+        _srcVert->id.db_print();
+        db_printf(": %g, %g\n", _srcVert->point.x, _srcVert->point.y);
+        tar->id.db_print();
+        db_printf(": %g, %g\n", tar->point.x, tar->point.y);
+        for (size_t i = 0; i < currRoute.ps.size(); ++i)
+        {
+            db_printf("%g, %g  ", currRoute.ps[i].x, currRoute.ps[i].y);
+        }
+        db_printf("\n");
+#endif
+        if (currRoute.size() > 2)
+        {
+            if (_srcVert->point == currRoute.ps[0])
+            {
+                existingPathStart = currRoute.size() - 2;
+                COLA_ASSERT(existingPathStart != 0);
+                const Point& pnt = currRoute.at(existingPathStart);
+                bool isShape = true;
+                VertID vID(pnt.id, isShape, pnt.vn);
+
+                _startVert = _router->vertices.getVertexByID(vID);
+            }
+        }
+    }
+    //db_printf("GO\n");
+    //db_printf("src: %X strt: %X dst: %x\n", (int) _srcVert, (int) _startVert, (int) _dstVert);
+    bool found = false;
+    while (!found)
+    {
+        makePath(this, flag);
+        for (VertInf *i = tar; i != NULL; i = i->pathNext)
+        {
+            if (i == _srcVert)
+            {
+                found = true;
+                break;
+            }
+        }
+        if (!found)
+        {
+            if (existingPathStart == 0)
+            {
+                break;
+            }
+#ifdef PATHDEBUG
+            db_printf("BACK\n");
+#endif
+            existingPathStart--;
+            const Point& pnt = currRoute.at(existingPathStart);
+            bool isShape = (existingPathStart > 0);
+            VertID vID(pnt.id, isShape, pnt.vn);
+
+            _startVert = _router->vertices.getVertexByID(vID);
+            COLA_ASSERT(_startVert);
+        }
+        else if (_router->RubberBandRouting)
+        {
+            // found.
+            bool unwind = false;
+
+#ifdef PATHDEBUG
+            db_printf("\n\n\nSTART:\n\n");
+#endif
+            VertInf *prior = NULL;
+            for (VertInf *curr = tar; curr != _startVert->pathNext; 
+                    curr = curr->pathNext)
+            {
+                if (!validateBendPoint(curr->pathNext, curr, prior))
+                {
+                    unwind = true;
+                    break;
+                }
+                prior = curr;
+            }
+            if (unwind)
+            {
+#ifdef PATHDEBUG
+                db_printf("BACK II\n");
+#endif
+                if (existingPathStart == 0)
+                {
+                    break;
+                }
+                existingPathStart--;
+                const Point& pnt = currRoute.at(existingPathStart);
+                bool isShape = (existingPathStart > 0);
+                VertID vID(pnt.id, isShape, pnt.vn);
+
+                _startVert = _router->vertices.getVertexByID(vID);
+                COLA_ASSERT(_startVert);
+
+                found = false;
+            }
+        }
     }
 
-    bool *flag = &(_needs_reroute_flag);
-    
-    makePath(this, flag);
     
     bool result = true;
     
     int pathlen = 1;
-    for (VertInf *i = tar; i != src; i = i->pathNext)
+    for (VertInf *i = tar; i != _srcVert; i = i->pathNext)
     {
         pathlen++;
         if (i == NULL)
         {
             db_printf("Warning: Path not found...\n");
             pathlen = 2;
-            tar->pathNext = src;
-            if (_router->InvisibilityGrph)
+            tar->pathNext = _srcVert;
+            if ((_type == ConnType_PolyLine) && _router->InvisibilityGrph)
             {
                 // TODO:  Could we know this edge already?
-                EdgeInf *edge = EdgeInf::existingEdge(src, tar);
-                assert(edge != NULL);
+                EdgeInf *edge = EdgeInf::existingEdge(_srcVert, tar);
+                COLA_ASSERT(edge != NULL);
                 edge->addCycleBlocker();
             }
-            result = false;
             break;
         }
-        if (pathlen > 100)
-        {
-            fprintf(stderr, "ERROR: Should never be here...\n");
-            exit(1);
-        }
+        // Check we don't have an apparent infinite connector path.
+        COLA_ASSERT(pathlen < 200);
     }
-    Point *path = (Point *) malloc(pathlen * sizeof(Point));
+    std::vector<Point> path(pathlen);
 
     int j = pathlen - 1;
-    for (VertInf *i = tar; i != src; i = i->pathNext)
+    for (VertInf *i = tar; i != _srcVert; i = i->pathNext)
     {
-        if (_router->InvisibilityGrph)
+        if (_router->InvisibilityGrph && (_type == ConnType_PolyLine))
         {
             // TODO: Again, we could know this edge without searching.
             EdgeInf *edge = EdgeInf::existingEdge(i, i->pathNext);
+            COLA_ASSERT(edge != NULL);
             edge->addConn(flag);
         }
         else
@@ -432,25 +860,53 @@ int ConnRef::generatePath(Point p0, Point p1)
             _false_path = true;
         }
         path[j] = i->point;
-        path[j].id = i->id.objID;
+        if (i->id.isShape)
+        {
+            path[j].id = i->id.objID;
+            path[j].vn = i->id.vn;
+        }
+        else
+        {
+            path[j].id = _id;
+            path[j].vn = kUnassignedVertexNumber;
+        }
         j--;
-    }
-    path[0] = src->point;
 
+        if (i->pathNext && (i->pathNext->point == i->point))
+        {
+            if (i->pathNext->id.isShape && i->id.isShape)
+            {
+                // Check for consecutive points on opposite 
+                // corners of two touching shapes.
+                COLA_ASSERT(abs(i->pathNext->id.objID - i->id.objID) != 2);
+            }
+        }
+    }
+    path[0] = _srcVert->point;
+    // Use topbit to differentiate between start and end point of connector.
+    // They need unique IDs for nudging.
+    unsigned int topbit = ((unsigned int) 1) << 31;
+    path[0].id = _id | topbit; 
+    path[0].vn = kUnassignedVertexNumber;
 
     // Would clear visibility for endpoints here if required.
 
-    PolyLine& output_route = route();
-    output_route.pn = pathlen;
+    freeRoutes();
+    PolyLine& output_route = _route;
     output_route.ps = path;
  
-    if ( !(_router->IncludeEndpoints) )
+#ifdef PATHDEBUG
+    db_printf("Output route:\n");
+    for (size_t i = 0; i < output_route.ps.size(); ++i)
     {
-        assert(_initialised);
-        unInitialise();
+        db_printf("[%d,%d] %g, %g   ", output_route.ps[i].id, 
+                output_route.ps[i].vn, output_route.ps[i].x, 
+                output_route.ps[i].y);
     }
-   
-    return (int) result;
+    db_printf("\n\n");
+#endif
+
+    return result;
 }
 
 
@@ -466,6 +922,895 @@ bool ConnRef::doesHateCrossings(void)
 }
 
 
+PtOrder::~PtOrder()
+{
+    // Free the PointRep lists.
+    for (int dim = 0; dim < 2; ++dim)
+    {
+        PointRepList::iterator curr = connList[dim].begin();
+        while (curr != connList[dim].end())
+        {
+            PointRep *doomed = *curr;
+            curr = connList[dim].erase(curr);
+            delete doomed;
+        }
+    }
+}
+
+bool PointRep::follow_inner(PointRep *target)
+{
+    if (this == target)
+    {
+        return true;
+    }
+    else
+    {
+        for (PointRepSet::iterator curr = inner_set.begin(); 
+                curr != inner_set.end(); ++curr)
+        {
+            if ((*curr)->follow_inner(target))
+            {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+
+int PtOrder::positionFor(const ConnRef *conn, const size_t dim) const
+{
+    int position = 0;
+    for (PointRepList::const_iterator curr = connList[dim].begin(); 
+            curr != connList[dim].end(); ++curr)
+    {
+        if ((*curr)->conn == conn)
+        {
+            return position;
+        }
+        ++position;
+    }
+    // Not found.
+    return -1;
+}
+
+
+bool PtOrder::addPoints(const int dim, PtConnPtrPair innerArg, 
+        PtConnPtrPair outerArg, bool swapped)
+{
+    PtConnPtrPair inner = (swapped) ? outerArg : innerArg;
+    PtConnPtrPair outer = (swapped) ? innerArg : outerArg;
+    COLA_ASSERT(inner != outer);
+
+    //printf("addPoints(%d, [%g, %g]-%X, [%g, %g]-%X)\n", dim,
+    //        inner->x, inner->y, (int) inner, outer->x, outer->y, (int) outer);
+
+    PointRep *innerPtr = NULL;
+    PointRep *outerPtr = NULL;
+    for (PointRepList::iterator curr = connList[dim].begin(); 
+            curr != connList[dim].end(); ++curr)
+    {
+        if ((*curr)->point == inner.first)
+        {
+            innerPtr = *curr;
+        }
+        if ((*curr)->point == outer.first)
+        {
+            outerPtr = *curr;
+        }
+    }
+    
+    if (innerPtr == NULL)
+    {
+        innerPtr = new PointRep(inner.first, inner.second);
+        connList[dim].push_back(innerPtr);
+    }
+    
+    if (outerPtr == NULL)
+    {
+        outerPtr = new PointRep(outer.first, outer.second);
+        connList[dim].push_back(outerPtr);
+    }
+    // TODO COLA_ASSERT(innerPtr->inner_set.find(outerPtr) == innerPtr->inner_set.end());
+    bool cycle = innerPtr->follow_inner(outerPtr);
+    if (cycle)
+    {
+        // Must reverse to avoid a cycle.
+        innerPtr->inner_set.insert(outerPtr);
+    }
+    else
+    {
+        outerPtr->inner_set.insert(innerPtr);
+    }
+    return cycle;
+}
+
+
+// Assuming that addPoints has been called for each pair of points in the 
+// shared path at that corner, then the contents of inner_set can be used 
+// to determine the correct ordering.
+static bool pointRepLessThan(PointRep *r1, PointRep *r2)
+{
+    size_t r1less = r1->inner_set.size();
+    size_t r2less = r2->inner_set.size();
+    //COLA_ASSERT(r1less != r2less);
+    
+    return (r1less > r2less);
+}
+
+
+void PtOrder::sort(const int dim)
+{
+    connList[dim].sort(pointRepLessThan);
+}
+
+
+// Returns a vertex number representing a point on the line between 
+// two shape corners, represented by p0 and p1.
+//
+static int midVertexNumber(const Point& p0, const Point& p1, const Point& c)
+{
+    if (c.vn != kUnassignedVertexNumber)
+    {
+        // The split point is a shape corner, so doesn't need its 
+        // vertex number adjusting.
+        return c.vn;
+    }
+    if ((p0.vn >= 4) && (p0.vn < kUnassignedVertexNumber))
+    {
+        // The point next to this has the correct nudging direction,
+        // so use that.
+        return p0.vn;
+    }
+    if ((p1.vn >= 4) && (p1.vn < kUnassignedVertexNumber))
+    {
+        // The point next to this has the correct nudging direction,
+        // so use that.
+        return p1.vn;
+    }
+    if ((p0.vn < 4) && (p1.vn < 4))
+    {
+        if (p0.vn != p1.vn)
+        {
+            return p0.vn;
+        }
+        // Splitting between two ordinary shape corners.
+        int vn_mid = std::min(p0.vn, p1.vn);
+        if ((std::max(p0.vn, p1.vn) == 3) && (vn_mid == 0))
+        {
+            vn_mid = 3; // Next vn is effectively 4.
+        }
+        return vn_mid + 4;
+    }
+    COLA_ASSERT((p0.x == p1.x) || (p0.y == p1.y));
+    if (p0.vn != kUnassignedVertexNumber)
+    {
+        if (p0.x == p1.x)
+        {
+            if ((p0.vn == 2) || (p0.vn == 3))
+            {
+                return 6;
+            }
+            return 4;
+        }
+        else
+        {
+            if ((p0.vn == 0) || (p0.vn == 3))
+            {
+                return 7;
+            }
+            return 5;
+        }
+    }
+    else if (p1.vn != kUnassignedVertexNumber)
+    {
+        if (p0.x == p1.x)
+        {
+            if ((p1.vn == 2) || (p1.vn == 3))
+            {
+                return 6;
+            }
+            return 4;
+        }
+        else
+        {
+            if ((p1.vn == 0) || (p1.vn == 3))
+            {
+                return 7;
+            }
+            return 5;
+        }
+    }
+
+    // Shouldn't both be new (kUnassignedVertexNumber) points.
+    db_printf("midVertexNumber(): p0.vn and p1.vn both = "
+            "kUnassignedVertexNumber\n");
+    db_printf("p0.vn %d p1.vn %d\n", p0.vn, p1.vn);
+    return kUnassignedVertexNumber;
+}
+
+
+// Break up overlapping parallel segments that are not the same edge in 
+// the visibility graph, i.e., where one segment is a subsegment of another.
+void splitBranchingSegments(Avoid::Polygon& poly, bool polyIsConn,
+        Avoid::Polygon& conn, const double tolerance)
+{
+    for (std::vector<Avoid::Point>::iterator i = conn.ps.begin(); 
+            i != conn.ps.end(); ++i)
+    {
+        if (i == conn.ps.begin())
+        {
+            // Skip the first point.
+            // There are points-1 segments in a connector.
+            continue;
+        }
+
+        for (std::vector<Avoid::Point>::iterator j = poly.ps.begin(); 
+                j != poly.ps.end(); )
+        {
+            if (polyIsConn && (j == poly.ps.begin()))
+            {
+                // Skip the first point.
+                // There are points-1 segments in a connector.
+                ++j;
+                continue;
+            }
+            Point& c0 = *(i - 1);
+            Point& c1 = *i;
+
+            Point& p0 = (j == poly.ps.begin()) ? poly.ps.back() : *(j - 1);
+            Point& p1 = *j;
+
+            // Check the first point of the first segment.
+            if (((i - 1) == conn.ps.begin()) && 
+                    pointOnLine(p0, p1, c0, tolerance))
+            {
+                //db_printf("add to poly %g %g\n", c0.x, c0.y);
+                
+                c0.vn = midVertexNumber(p0, p1, c0);
+                j = poly.ps.insert(j, c0);
+                if (j != poly.ps.begin())
+                {
+                    --j;
+                }
+                continue;
+            }
+            // And the second point of every segment.
+            if (pointOnLine(p0, p1, c1, tolerance))
+            {
+                //db_printf("add to poly %g %g\n", c1.x, c1.y);
+                
+                c1.vn = midVertexNumber(p0, p1, c1);
+                j = poly.ps.insert(j, c1);
+                if (j != poly.ps.begin())
+                {
+                    --j;
+                }
+                continue;
+            }
+
+            // Check the first point of the first segment.
+            if (polyIsConn && ((j - 1) == poly.ps.begin()) && 
+                        pointOnLine(c0, c1, p0, tolerance))
+            {
+                //db_printf("add to conn %g %g\n", p0.x, p0.y);
+
+                p0.vn = midVertexNumber(c0, c1, p0);
+                i = conn.ps.insert(i, p0);
+                continue;
+            }
+            // And the second point of every segment.
+            if (pointOnLine(c0, c1, p1, tolerance))
+            {
+                //db_printf("add to conn %g %g\n", p1.x, p1.y);
+
+                p1.vn = midVertexNumber(c0, c1, p1);
+                i = conn.ps.insert(i, p1);
+            }
+            ++j;
+        }
+    }
+}
+
+
+static int segDir(const Point& p1, const Point& p2)
+{
+    int result = 1;
+    if (p1.x == p2.x)
+    {
+        if (p2.y > p1.y)
+        {
+            result = -1;
+        }
+    }
+    else if (p1.y == p2.y)
+    {
+        if (p2.x < p1.x)
+        {
+            result = -1;
+        }
+    }
+    return result;
+}
+
+
+// Works out if the segment conn[cIndex-1]--conn[cIndex] really crosses poly.
+// This does not not count non-crossing shared paths as crossings.
+// poly can be either a connector (polyIsConn = true) or a cluster
+// boundary (polyIsConn = false).
+//
+CrossingsInfoPair countRealCrossings(Avoid::Polygon& poly, 
+        bool polyIsConn, Avoid::Polygon& conn, size_t cIndex, 
+        bool checkForBranchingSegments, const bool finalSegment, 
+        PointSet *crossingPoints, PtOrderMap *pointOrders, 
+        ConnRef *polyConnRef, ConnRef *connConnRef)
+{
+    unsigned int crossingFlags = CROSSING_NONE;
+    if (checkForBranchingSegments)
+    {
+        size_t conn_pn = conn.size();
+        // XXX When doing the pointOnLine test we allow the points to be 
+        // slightly non-collinear.  This addresses a problem with clustered
+        // routing where connectors could otherwise route cheaply through
+        // shape corners that were not quite on the cluster boundary, but
+        // reported to be on there by the line segment intersection code,
+        // which I suspect is not numerically accurate enough.  This occured
+        // for points that only differed by about 10^-12 in the y-dimension.
+        double tolerance = (!polyIsConn) ? 0.00001 : 0.0;
+        splitBranchingSegments(poly, polyIsConn, conn, tolerance);
+        // cIndex is going to be the last, so take into account added points.
+        cIndex += (conn.size() - conn_pn);
+    }
+    COLA_ASSERT(cIndex >= 1);
+    COLA_ASSERT(cIndex < conn.size());
+
+    bool polyIsOrthogonal = (polyConnRef && 
+            (polyConnRef->routingType() == ConnType_Orthogonal));
+    bool connIsOrthogonal = (connConnRef &&
+            (connConnRef->routingType() == ConnType_Orthogonal));
+
+    size_t poly_size = poly.size();
+    int crossingCount = 0;
+    std::vector<Avoid::Point *> c_path;
+    std::vector<Avoid::Point *> p_path;
+
+    Avoid::Point& a1 = conn.ps[cIndex - 1];
+    Avoid::Point& a2 = conn.ps[cIndex];
+    //db_printf("a1: %g %g\n", a1.x, a1.y);
+    //db_printf("a2: %g %g\n", a2.x, a2.y);
+
+    for (size_t j = ((polyIsConn) ? 1 : 0); j < poly_size; ++j)
+    {
+        Avoid::Point& b1 = poly.ps[(j - 1 + poly_size) % poly_size];
+        Avoid::Point& b2 = poly.ps[j];
+        //db_printf("b1: %g %g\n", b1.x, b1.y);
+        //db_printf("b2: %g %g\n", b2.x, b2.y);
+
+        p_path.clear();
+        c_path.clear();
+        bool converging = false;
+
+        const bool a1_eq_b1 = (a1 == b1);
+        const bool a2_eq_b1 = (a2 == b1);
+        const bool a2_eq_b2 = (a2 == b2);
+        const bool a1_eq_b2 = (a1 == b2);
+
+        if ( (a1_eq_b1 && a2_eq_b2) ||
+             (a2_eq_b1 && a1_eq_b2) )
+        {
+            if (finalSegment)
+            {
+                converging = true;
+            }
+            else
+            {
+                // Route along same segment: no penalty.  We detect
+                // crossovers when we see the segments diverge.
+                continue;
+            }
+        }
+        else if (a2_eq_b1 || a2_eq_b2 || a1_eq_b2)
+        {
+            // Each crossing that is at a vertex in the 
+            // visibility graph gets noticed four times.
+            // We ignore three of these cases.
+            // This also catches the case of a shared path,
+            // but this is one that terminates at a common
+            // endpoint, so we don't care about it.
+            continue;
+        }
+    
+        if (a1_eq_b1 || converging)
+        {
+            if (!converging)
+            {
+                if (polyIsConn && (j == 1))
+                {
+                    // Can't be the end of a shared path or crossing path 
+                    // since the common point is the first point of the 
+                    // connector path.  This is not a shared path at all.
+                    continue;
+                }
+
+                Avoid::Point& b0 = poly.ps[(j - 2 + poly_size) % poly_size];
+                // The segments share an endpoint -- a1==b1.
+                if (a2 == b0)
+                {
+                    // a2 is not a split, continue.
+                    continue;
+                }
+            }
+            
+            // If here and not converging, then we know that a2 != b2
+            // And a2 and its pair in b are a split.
+            COLA_ASSERT(converging || !a2_eq_b2);
+
+            bool shared_path = false;
+            
+            // Initial values here don't matter. They are only used after 
+            // being set to sensible values, but we set them to stop a MSVC
+            // warning.
+            bool p_dir_back;
+            int p_dir = 0;
+            int trace_c = 0;
+            int trace_p = 0;
+            
+            if (converging)
+            {
+                // Determine direction we have to look through
+                // the points of connector b.
+                p_dir_back = a2_eq_b2 ? true : false;
+                p_dir = p_dir_back ? -1 : 1;
+                trace_c = (int) cIndex;
+                trace_p = (int) j;
+                if (!p_dir_back)
+                {
+                    if (finalSegment)
+                    {
+                        trace_p--;
+                    }
+                    else
+                    {   
+                        trace_c--;
+                    }
+                }
+
+                shared_path = true;
+            }
+            else if (cIndex >= 2)
+            {
+                Avoid::Point& b0 = poly.ps[(j - 2 + poly_size) % poly_size];
+                Avoid::Point& a0 = conn.ps[cIndex - 2];
+            
+                //db_printf("a0: %g %g\n", a0.x, a0.y);
+                //db_printf("b0: %g %g\n", b0.x, b0.y);
+
+                if ((a0 == b2) || (a0 == b0))
+                {
+                    // Determine direction we have to look through
+                    // the points of connector b.
+                    p_dir_back = (a0 == b0) ? true : false;
+                    p_dir = p_dir_back ? -1 : 1;
+                    trace_c = (int) cIndex;
+                    trace_p = (int) (p_dir_back ? j : j - 2);
+                    
+                    shared_path = true;
+                }
+            }    
+
+            if (shared_path)
+            {
+                crossingFlags |= CROSSING_SHARES_PATH;
+                // Shouldn't be here if p_dir is still equal to zero.
+                COLA_ASSERT(p_dir != 0);
+
+                // Build the shared path, including the diverging points at
+                // each end if the connector does not end at a common point.
+                while ( (trace_c >= 0) && (!polyIsConn || 
+                            ((trace_p >= 0) && (trace_p < (int) poly_size))) )
+                {
+                    // If poly is a cluster boundary, then it is a closed 
+                    // poly-line and so it wraps arounds.
+                    size_t index_p = (size_t)
+                            ((trace_p + (2 * poly_size)) % poly_size);
+                    size_t index_c = (size_t) trace_c;
+                    c_path.push_back(&conn.ps[index_c]);
+                    p_path.push_back(&poly.ps[index_p]);
+                    if ((c_path.size() > 1) && 
+                            (conn.ps[index_c] != poly.ps[index_p]))
+                    {
+                        // Points don't match, so break out of loop.
+                        break;
+                    }
+                    trace_c--;
+                    trace_p += p_dir;
+                }
+
+                // Are there diverging points at the ends of the shared path.
+                bool front_same = (*(c_path.front()) == *(p_path.front()));
+                bool back_same  = (*(c_path.back())  == *(p_path.back()));
+
+                size_t size = c_path.size();
+                
+                // Check to see if these share a fixed segment.
+                if (polyIsOrthogonal && connIsOrthogonal)
+                {
+                    size_t startPt = (front_same) ? 0 : 1;
+                    if (c_path[startPt]->x == c_path[startPt + 1]->x)
+                    {
+                        // Vertical
+                        double xPos = c_path[startPt]->x;
+                        // See if this is inline with either the start
+                        // or end point of both connectors.
+                        if ( ((xPos == poly.ps[0].x) || 
+                                (xPos == poly.ps[poly_size - 1].x)) &&
+                             ((xPos == conn.ps[0].x) || 
+                                (xPos == conn.ps[cIndex].x)) )
+                        {
+                            crossingFlags |= CROSSING_SHARES_FIXED_SEGMENT;
+                        }
+                    }
+                    else
+                    {
+                        // Horizontal
+                        double yPos = c_path[startPt]->y;
+                        // See if this is inline with either the start
+                        // or end point of both connectors.
+                        if ( ((yPos == poly.ps[0].y) || 
+                                (yPos == poly.ps[poly_size - 1].y)) &&
+                             ((yPos == conn.ps[0].y) || 
+                                (yPos == conn.ps[cIndex].y)) )
+                        {
+                            crossingFlags |= CROSSING_SHARES_FIXED_SEGMENT;
+                        }
+                    }
+                }
+
+                int prevTurnDir = -1;
+                int startCornerSide = 1;
+                int endCornerSide = 1;
+                bool reversed = false;
+                if (!front_same)
+                {
+                    // If there is a divergence at the beginning, 
+                    // then order the shared path based on this.
+                    prevTurnDir = vecDir(*c_path[0], *c_path[1], *c_path[2]);
+                    startCornerSide = Avoid::cornerSide(*c_path[0], *c_path[1], 
+                            *c_path[2], *p_path[0]) 
+                        * segDir(*c_path[1], *c_path[2]);
+                    reversed = (startCornerSide != -prevTurnDir);
+                }
+                if (!back_same)
+                {
+                    // If there is a divergence at the end of the path, 
+                    // then order the shared path based on this.
+                    prevTurnDir = vecDir(*c_path[size - 3],
+                            *c_path[size - 2], *c_path[size - 1]);
+                    endCornerSide = Avoid::cornerSide(*c_path[size - 3], 
+                            *c_path[size - 2], *c_path[size - 1], 
+                            *p_path[size - 1])
+                        * segDir(*c_path[size - 3], *c_path[size - 2]);
+                    reversed = (endCornerSide != -prevTurnDir);
+                }
+                else
+                {
+                    endCornerSide = startCornerSide;
+                }
+                if (front_same)
+                {
+                    startCornerSide = endCornerSide;
+                }
+                
+                if (front_same || back_same)
+                {
+                    crossingFlags |= CROSSING_SHARES_PATH_AT_END;
+                }
+                else if (polyIsOrthogonal && connIsOrthogonal)
+                {
+                    int cStartDir = vecDir(*c_path[0], *c_path[1], *c_path[2]);
+                    int pStartDir = vecDir(*p_path[0], *p_path[1], *p_path[2]);
+                    if ((cStartDir != 0) && (cStartDir == -pStartDir))
+                    {
+                        // The start segments diverge at 180 degrees to each 
+                        // other.  So order based on not introducing overlap
+                        // of the diverging segments when these are nudged
+                        // apart.
+                        startCornerSide = -cStartDir * 
+                                segDir(*c_path[1], *c_path[2]);
+                    }
+                    else 
+                    {
+                        int cEndDir = vecDir(*c_path[size - 3], 
+                                *c_path[size - 2], *c_path[size - 1]);
+                        int pEndDir = vecDir(*p_path[size - 3], 
+                                *p_path[size - 2], *p_path[size - 1]);
+                        if ((cEndDir != 0) && (cEndDir == -pEndDir))
+                        {
+                            // The end segments diverge at 180 degrees to 
+                            // each other.  So order based on not introducing 
+                            // overlap of the diverging segments when these 
+                            // are nudged apart.
+                            startCornerSide = -cEndDir * segDir(
+                                    *c_path[size - 3], *c_path[size - 2]);
+                        }
+                    }
+                }
+
+#if 0
+                prevTurnDir = 0;
+                if (pointOrders)
+                {
+                    // Return the ordering for the shared path.
+                    COLA_ASSERT(c_path.size() > 0 || back_same);
+                    size_t adj_size = (c_path.size() - ((back_same) ? 0 : 1));
+                    for (size_t i = (front_same) ? 0 : 1; i < adj_size; ++i)
+                    {
+                        Avoid::Point& an = *(c_path[i]);
+                        Avoid::Point& bn = *(p_path[i]);
+                        int currTurnDir = ((i > 0) && (i < (adj_size - 1))) ?  
+                                vecDir(*c_path[i - 1], an,
+                                       *c_path[i + 1]) : 0;
+                        VertID vID(an.id, true, an.vn);
+                        if ( (currTurnDir == (-1 * prevTurnDir)) &&
+                                (currTurnDir != 0) && (prevTurnDir != 0) )
+                        {
+                            // The connector turns the opposite way around 
+                            // this shape as the previous bend on the path,
+                            // so reverse the order so that the inner path
+                            // become the outer path and vice versa.
+                            reversed = !reversed;
+                        }
+                        bool orderSwapped = (*pointOrders)[an].addPoints(
+                                &bn, &an, reversed);
+                        if (orderSwapped)
+                        {
+                            // Reverse the order for later points.
+                            reversed = !reversed;
+                        }
+                        prevTurnDir = currTurnDir;
+                    }
+                }
+#endif
+                prevTurnDir = 0;
+                if (pointOrders)
+                {
+                    reversed = false;
+                    size_t startPt = (front_same) ? 0 : 1;
+                    
+                    // Orthogonal should always have at least one segment.
+                    COLA_ASSERT(c_path.size() > (startPt + 1));
+                    
+                    if (startCornerSide > 0)
+                    {
+                        reversed = !reversed;
+                    }
+
+                    int prevDir = 0;
+                    // Return the ordering for the shared path.
+                    COLA_ASSERT(c_path.size() > 0 || back_same);
+                    size_t adj_size = (c_path.size() - ((back_same) ? 0 : 1));
+                    for (size_t i = (front_same) ? 0 : 1; i < adj_size; ++i)
+                    {
+                        Avoid::Point& an = *(c_path[i]);
+                        Avoid::Point& bn = *(p_path[i]);
+                        COLA_ASSERT(an == bn);
+
+                        int thisDir = prevDir;
+                        if ((i > 0) && (*(c_path[i - 1]) == *(p_path[i - 1])))
+                        {
+                            thisDir = segDir(*c_path[i - 1], *c_path[i]);
+                        }
+
+                        if (thisDir != prevDir)
+                        {
+                            reversed = !reversed;
+                        }
+                        prevDir = thisDir;
+
+                        if (i > startPt)
+                        {
+                            Avoid::Point& ap = *(c_path[i - 1]);
+                            Avoid::Point& bp = *(p_path[i - 1]);
+                            int orientation = (ap.x == an.x) ? 0 : 1;
+                            //printf("prevOri %d\n", prevOrientation);
+                            //printf("1: %X, %X\n", (int) &(bn), (int) &(an));
+                            bool orderSwapped = (*pointOrders)[an].addPoints(
+                                    orientation, 
+                                    std::make_pair(&bn, polyConnRef), 
+                                    std::make_pair(&an, connConnRef), 
+                                    reversed);
+                            if (orderSwapped)
+                            {
+                                // Reverse the order for later points.
+                                reversed = !reversed;
+                            }
+                            COLA_ASSERT(ap == bp);
+                            //printf("2: %X, %X\n", (int) &bp, (int) &ap);
+                            orderSwapped = (*pointOrders)[ap].addPoints(
+                                    orientation, 
+                                    std::make_pair(&bp, polyConnRef), 
+                                    std::make_pair(&ap, connConnRef), 
+                                    reversed);
+                            COLA_ASSERT(!orderSwapped);
+                        }
+                    }
+                }
+#if 0
+                    int ymod = -1;
+                    if ((id.vn == 1) || (id.vn == 2))
+                    {
+                        // bottom.
+                        ymod = +1;
+                    }
+                    
+                    int xmod = -1;
+                    if ((id.vn == 0) || (id.vn == 1))
+                    {
+                        // right.
+                        xmod = +1;
+                    }
+                    if(id.vn > 3)
+                    {
+                        xmod = ymod = 0;
+                        if (id.vn == 4)
+                        {
+                            // right.
+                            xmod = +1;
+                        }
+                        else if (id.vn == 5)
+                        {
+                            // bottom.
+                            ymod = +1;
+                        }
+                        else if (id.vn == 6)
+                        {
+                            // left.
+                            xmod = -1;
+                        }
+                        else if (id.vn == 7)
+                        {
+                            // top.
+                            ymod = -1;
+                        }
+                    }
+#endif
+ 
+                if (endCornerSide != startCornerSide)
+                {
+                    // Mark that the shared path crosses.
+                    //db_printf("shared path crosses.\n");
+                    crossingCount += 1;
+                    if (crossingPoints)
+                    {
+                        crossingPoints->insert(*c_path[1]);
+                    }
+                }
+                crossingFlags |= CROSSING_TOUCHES;
+            }
+            else if (cIndex >= 2)
+            {
+                // The connectors cross or touch at this point.
+                //db_printf("Cross or touch at point... \n");
+                
+                // Crossing shouldn't be at an endpoint.
+                COLA_ASSERT(cIndex >= 2);
+                COLA_ASSERT(polyIsConn && (j >= 2));
+
+                Avoid::Point& b0 = poly.ps[(j - 2 + poly_size) % poly_size];
+                Avoid::Point& a0 = conn.ps[cIndex - 2];
+            
+                int side1 = Avoid::cornerSide(a0, a1, a2, b0);
+                int side2 = Avoid::cornerSide(a0, a1, a2, b2);
+                if (side1 != side2)
+                {
+                    // The connectors cross at this point.
+                    //db_printf("cross.\n");
+                    crossingCount += 1;
+                    if (crossingPoints)
+                    {
+                        crossingPoints->insert(a1);
+                    }
+                }
+
+                crossingFlags |= CROSSING_TOUCHES;
+                if (pointOrders)
+                {
+                    if (polyIsOrthogonal && connIsOrthogonal)
+                    {
+                        // Orthogonal case:
+                        // Just order based on which comes from the left and
+                        // top in each dimension because this can only be two
+                        // L-shaped segments touching at the bend.
+                        bool reversedX = ((a0.x < a1.x) || (a2.x < a1.x));
+                        bool reversedY = ((a0.y < a1.y) || (a2.y < a1.y));
+                        // XXX: Why do we need to invert the reversed values 
+                        //      here?  Are they wrong for orthogonal points
+                        //      in the other places?
+                        (*pointOrders)[b1].addPoints(0, 
+                                std::make_pair(&b1, polyConnRef), 
+                                std::make_pair(&a1, connConnRef), 
+                                !reversedX);
+                        (*pointOrders)[b1].addPoints(1, 
+                                std::make_pair(&b1, polyConnRef), 
+                                std::make_pair(&a1, connConnRef),
+                                !reversedY);
+                    }
+                    else
+                    {
+                        int turnDirA = vecDir(a0, a1, a2); 
+                        int turnDirB = vecDir(b0, b1, b2); 
+                        bool reversed = (side1 != -turnDirA); 
+                        if (side1 != side2) 
+                        { 
+                            // Interesting case where a connector routes round 
+                            // the edge of a shape and intersects a connector 
+                            // which is connected to a port on the edge of the 
+                            // shape. 
+                            if (turnDirA == 0) 
+                            { 
+                                // We'll make B the outer by preference,  
+                                // because the points of A are collinear. 
+                                reversed = false; 
+                            } 
+                            else if (turnDirB == 0) 
+                            { 
+                                reversed = true; 
+                            } 
+                            // TODO COLA_ASSERT((turnDirB != 0) || 
+                            //          (turnDirA != 0)); 
+                        }
+                        VertID vID(b1.id, true, b1.vn);
+                        //(*pointOrders)[b1].addPoints(&b1, &a1, reversed);
+                    }
+                }
+            }
+        }
+        else
+        {
+            if ( polyIsOrthogonal && connIsOrthogonal)
+            {
+                // All crossings in orthogonal connectors will be at a
+                // vertex in the visibility graph, so we need not bother
+                // doing normal line intersection.
+                continue;
+            }
+
+            // No endpoint is shared between these two line segments,
+            // so just calculate normal segment intersection.
+
+            Point cPt;
+            int intersectResult = Avoid::segmentIntersectPoint(
+                    a1, a2, b1, b2, &(cPt.x), &(cPt.y));
+
+            if (intersectResult == Avoid::DO_INTERSECT)
+            {
+                if (!polyIsConn && 
+                        ((a1 == cPt) || (a2 == cPt) || (b1 == cPt) || (b2 == cPt)))
+                {
+                    // XXX: This shouldn't actually happen, because these
+                    //      points should be added as bends to each line by
+                    //      splitBranchingSegments().  Thus, lets ignore them.
+                    COLA_ASSERT(a1 != cPt);
+                    COLA_ASSERT(a2 != cPt);
+                    COLA_ASSERT(b1 != cPt);
+                    COLA_ASSERT(b2 != cPt);
+                    continue;
+                }                
+                //db_printf("crossing lines:\n");
+                //db_printf("cPt: %g %g\n", cPt.x, cPt.y);
+                crossingCount += 1;
+                if (crossingPoints)
+                {
+                    crossingPoints->insert(cPt);
+                }
+            }
+        }
+    }
+    //db_printf("crossingcount %d\n", crossingCount);
+    return std::make_pair(crossingCount, crossingFlags);
+}
+
+
 //============================================================================
 
 }
diff --git a/src/libavoid/connector.h b/src/libavoid/connector.h
index 64afb4dda..8f7499a29 100644
--- a/src/libavoid/connector.h
+++ b/src/libavoid/connector.h
@@ -2,93 +2,336 @@
  * 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
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file    shape.h
+//! @brief   Contains the interface for the ConnRef class.
+
+
 #ifndef AVOID_CONNECTOR_H
 #define AVOID_CONNECTOR_H
 
-#include "libavoid/router.h"
+#include <list>
+#include <vector>
+
+#include "libavoid/vertices.h"
 #include "libavoid/geometry.h"
 #include "libavoid/shape.h"
-#include <list>
 
 
 namespace Avoid {
 
+class Router;
+class ConnRef;
+typedef std::list<ConnRef *> ConnRefList;
+
+
+//! @brief  Describes the type of routing that is performed for each 
+//!         connector.
+enum ConnType {
+    ConnType_None       = 0,
+    //! @brief  The connector path will be a shortest-path poly-line that
+    //!         routes around obstacles.
+    ConnType_PolyLine   = 1,
+    //! @brief  The connector path will be a shortest-path orthogonal 
+    //!         poly-line (only vertical and horizontal line segments) that
+    //!         routes around obstacles.
+    ConnType_Orthogonal = 2
+};
+
+//! @brief  Flags that can be passed to the ConnEnd constructor to specify
+//!         which sides of a shape this point should have visibility to if
+//!         it is located within the shape's area.
+//!
+//! Like SVG, libavoid considers the Y-axis to point downwards, that is, 
+//! like screen coordinates the coordinates increase from left-to-right and 
+//! also from top-to-bottom.
+//!
+enum ConnDirFlag {
+    ConnDirNone  = 0,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the top of the shape that it is located within.
+    ConnDirUp    = 1,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the bottom of the shape that it is located within.
+    ConnDirDown  = 2,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the left side of the shape that it is located within.
+    ConnDirLeft  = 4,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the right side of the shape that it is located within.
+    ConnDirRight = 8,
+    //! @brief  This option, provided for convenience, specifies the point 
+    //!         should be given visibility to all four sides of the shape 
+    //!         that it is located within.
+    ConnDirAll   = 15
+};
+//! @brief  One or more Avoid::ConnDirFlag options.
+//!
+typedef unsigned int ConnDirFlags;
+
+
+static const double ATTACH_POS_TOP = 0;
+static const double ATTACH_POS_CENTER = 0.5;
+static const double ATTACH_POS_BOTTOM = 1;
+static const double ATTACH_POS_LEFT = ATTACH_POS_TOP;
+static const double ATTACH_POS_RIGHT = ATTACH_POS_BOTTOM;
+
+
+//! @brief  The ConnEnd class represents different possible endpoints for 
+//!         connectors.
+//!
+//! Currently this class just allows free-floating endpoints, but in future
+//! will be capable of representing attachments to connection points on shapes.
+//! 
+class ConnEnd 
+{
+    public:
+        //! @brief Constructs a ConnEnd from a free-floating point.
+        //!
+        //! @param[in]  point  The position of the connector endpoint.
+        //!
+        ConnEnd(const Point& point);
+
+        //! @brief Constructs a ConnEnd from a free-floating point as well
+        //!        as a set of flags specifying visibility for this point 
+        //!        if it is located inside a shape.
+        //!
+        //! @param[in]  point    The position of the connector endpoint.
+        //! @param[in]  visDirs  One or more Avoid::ConnDirFlag options 
+        //!                      specifying the directions that this point 
+        //!                      should be given visibility if it is inside 
+        //!                      a shape.
+        //!
+        ConnEnd(const Point& point, const ConnDirFlags visDirs);
+
+        ConnEnd(ShapeRef *shapeRef, const double x_pos, const double y_pos,
+                const double insideOffset = 0.0,
+                const ConnDirFlags visDirs = ConnDirNone);
 
-static const int ConnType_PolyLine   = 1;
-static const int ConnType_Orthogonal = 2;
+        //! @brief Returns the position of this connector endpoint
+        //!
+        //! @return The position of this connector endpoint.
+        const Point point(void) const;
+
+        ConnDirFlags directions(void) const;
+    private:
+        Point _point;
+        ConnDirFlags _directions;
+        
+        // For referencing ConnEnds
+        ShapeRef *_shapeRef;
+        double _xPosition;
+        double _yPosition;
+        double _insideOffset;
+};
 
 
+//! @brief   The ConnRef class represents a connector object.
+//!
+//! Connectors are a (possible multi-segment) line between two points.
+//! They are routed intelligently so as not to overlap any of the shape
+//! objects in the Router scene.
+//! 
+//! Routing penalties can be applied, resulting in more aesthetically pleasing
+//! connector paths with fewer segments or less severe bend-points.
+//!
+//! You can set a function to be called when the connector has been rerouted
+//! and needs to be redrawn.  Alternatively, you can query the connector's
+//! needsRepaint() function to determine this manually.
+//!
+//! Usually, it is expected that you would create a ConnRef for each connector 
+//! in your diagram and keep that reference in your own connector class.
+//!
 class ConnRef
 {
     public:
-        ConnRef(Router *router, const unsigned int id);
-        ConnRef(Router *router, const unsigned int id,
-                const Point& src, const Point& dst);
-        virtual ~ConnRef();
+        //! @brief Constructs a connector with no endpoints specified.
+        //!
+        //! @param[in]  router  The router scene to place the connector into.
+        //! @param[in]  id      A unique positive integer ID for the connector.  
+        //!
+        //! If an ID is not specified, then one will be assigned to the shape.
+        //! If assigning an ID yourself, note that it should be a unique 
+        //! positive integer.  Also, IDs are given to all objects in a scene,
+        //! so the same ID cannot be given to a shape and a connector for 
+        //! example.
+        //!
+        ConnRef(Router *router, const unsigned int id = 0);
+        //! @brief Constructs a connector with endpoints specified.
+        //!
+        //! @param[in]  router  The router scene to place the connector into.
+        //! @param[in]  id      A unique positive integer ID for the connector.
+        //! @param[in]  src     The source endpoint of the connector.
+        //! @param[in]  dst     The destination endpoint of the connector.
+        //!
+        //! If an ID is not specified, then one will be assigned to the shape.
+        //! If assigning an ID yourself, note that it should be a unique 
+        //! positive integer.  Also, IDs are given to all objects in a scene,
+        //! so the same ID cannot be given to a shape and a connector for 
+        //! example.
+        //!
+        ConnRef(Router *router, const ConnEnd& src, const ConnEnd& dst,
+                const unsigned int id = 0);
+        //! @brief  Destuctor.
+        ~ConnRef();
         
-        void setType(unsigned int type);
-        PolyLine& route(void);
-        bool needsReroute(void);
-        void freeRoute(void);
+        //! @brief  Sets both new source and destination endpoints for this 
+        //!         connector.
+        //!
+        //! @param[in]  srcPoint  New source endpoint for the connector.
+        //! @param[in]  dstPoint  New destination endpoint for the connector.
+        void setEndpoints(const ConnEnd& srcPoint, const ConnEnd& dstPoint);
+        //! @brief  Sets just a new source endpoint for this connector.
+        //!
+        //! @param[in]  srcPoint  New source endpoint for the connector.
+        void setSourceEndpoint(const ConnEnd& srcPoint);
+        //! @brief  Sets just a new destination endpoint for this connector.
+        //!
+        //! @param[in]  dstPoint  New destination endpoint for the connector.
+        void setDestEndpoint(const ConnEnd& dstPoint);
+        //! @brief   Returns the ID of this connector.
+        //! @returns The ID of the connector. 
+        unsigned int id(void) const;
+        //! @brief   Returns a pointer to the router scene this connector is in.
+        //! @returns A pointer to the router scene for this connector.
+        Router *router(void) const;
+
+        //! @brief   Returns an indication of whether this connector has a 
+        //!          new route and thus needs to be repainted.
+        //!
+        //! If the connector has been rerouted and need repainting, the  
+        //! route() method can be called to get a reference to the new route.
+        //!
+        //! @returns Returns true if the connector requires repainting, or 
+        //!          false if it does not.
+        bool needsRepaint(void) const;
+        
+        //! @brief   Returns a reference to the current route for the connector.
+        //!
+        //! This is a "raw" version of the route, where each line segment in
+        //! the route may be made up of multiple collinear line segments.  It
+        //! will also not have post-processing (like curved corners) applied
+        //! to it.  The simplified route for display can be obtained by calling
+        //! displayRoute().
+        //!
+        //! @returns The PolyLine route for the connector.
+        //! @note    You can obtain a modified version of this poly-line 
+        //!          route with curved corners added by calling 
+        //!          PolyLine::curvedPolyline().
+        const PolyLine& route(void) const;
+        
+        //! @brief   Returns a reference to the current display version of the
+        //!          route for the connector.
+        //! 
+        //! The display version of a route has been simplified to collapse all
+        //! collinear line segments into single segments.  It may also have 
+        //! post-processing applied to the route, such as curved corners or
+        //! nudging.
+        //! 
+        //! @returns The PolyLine display route for the connector.
+        PolyLine& displayRoute(void);
+        
+        //! @brief   Sets a callback function that will called to indicate that
+        //!          the connector needs rerouting.
+        //!
+        //! The cb function will be called when shapes are added to, removed 
+        //! from or moved about on the page.  The pointer ptr will be passed 
+        //! as an argument to the callback function.
+        //!
+        //! @param[in]  cb   A pointer to the callback function.
+        //! @param[in]  ptr  A generic pointer that will be passed to the 
+        //!                  callback function.
+        void setCallback(void (*cb)(void *), void *ptr);
+        //! @brief   Returns the type of routing performed for this connector.
+        //! @return  The type of routing performed.
+        //!
+        ConnType routingType(void) const;
+        //! @brief       Sets the type of routing to be performed for this 
+        //!              connector.
+        //! 
+        //! If a call to this method changes the current type of routing 
+        //! being used for the connector, then it will get rerouted during
+        //! the next processTransaction() call, or immediately if 
+        //! transactions are not being used.
+        //!
+        //! @param type  The type of routing to be performed.
+        //!
+        void setRoutingType(ConnType type);
+
+       
+
+        // @brief   Returns the source endpoint vertex in the visibility graph.
+        // @returns The source endpoint vertex.
+        VertInf *src(void);
+        // @brief   Returns the destination endpoint vertex in the 
+        //          visibility graph.
+        // @returns The destination endpoint vertex.
+        VertInf *dst(void);
+        
+
+        void set_route(const PolyLine& route);
         void calcRouteDist(void);
-        void updateEndPoint(const unsigned int type, const Point& point);
         void setEndPointId(const unsigned int type, const unsigned int id);
         unsigned int getSrcShapeId(void);
         unsigned int getDstShapeId(void);
         void makeActive(void);
         void makeInactive(void);
-        void lateSetup(const Point& src, const Point& dst);
-        unsigned int id(void);
-        VertInf *src(void);
-        VertInf *dst(void);
+        VertInf *start(void);
         void removeFromGraph(void);
         bool isInitialised(void);
-        void unInitialise(void);
-        void setCallback(void (*cb)(void *), void *ptr);
-        void handleInvalid(void);
-        int generatePath(Point p0, Point p1);
         void makePathInvalid(void);
-        Router *router(void);
         void setHateCrossings(bool value);
         bool doesHateCrossings(void);
-        
-        friend void Router::attachedShapes(IntList &shapes,
-                const unsigned int shapeId, const unsigned int type);
-        friend void Router::attachedConns(IntList &conns,
-                const unsigned int shapeId, const unsigned int type);
-        friend void Router::markConnectors(ShapeRef *shape);
-        
+        void setEndpoint(const unsigned int type, const ConnEnd& connEnd);
+        bool setEndpoint(const unsigned int type, const VertID& pointID, 
+                Point *pointSuggestion = NULL);
+    
     private:
+        friend class Router;
+
+        PolyLine& routeRef(void);
+        void freeRoutes(void);
+        void performCallback(void);
+        bool generatePath(void);
+        bool generatePath(Point p0, Point p1);
+        void unInitialise(void);
+        void updateEndPoint(const unsigned int type, const ConnEnd& connEnd);
+        void common_updateEndPoint(const unsigned int type, const ConnEnd& connEnd);
         Router *_router;
         unsigned int _id;
-        unsigned int _type;
+        ConnType _type;
         unsigned int _srcId, _dstId;
+        bool _orthogonal;
         bool _needs_reroute_flag;
         bool _false_path;
+        bool _needs_repaint;
         bool _active;
         PolyLine _route;
+        Polygon _display_route;
         double _route_dist;
         ConnRefList::iterator _pos;
         VertInf *_srcVert;
         VertInf *_dstVert;
+        VertInf *_startVert;
         bool _initialised;
         void (*_callback)(void *);
         void *_connector;
@@ -96,6 +339,69 @@ class ConnRef
 };
 
 
+class PointRep;
+typedef std::set<PointRep *> PointRepSet;
+typedef std::list<PointRep *> PointRepList;
+
+class PointRep
+{
+    public:
+        PointRep(Point *p, const ConnRef *c)
+            : point(p),
+              conn(c)
+
+        {
+        }
+        bool follow_inner(PointRep *target);
+
+        Point *point;
+        const ConnRef *conn;
+        // inner_set: Set of pointers to the PointReps 'inner' of 
+        // this one, at this corner.
+        PointRepSet inner_set;
+};
+
+
+typedef std::pair<Point *, ConnRef *> PtConnPtrPair;
+
+class PtOrder
+{
+    public:
+        PtOrder()
+        {
+        }
+        ~PtOrder();
+        bool addPoints(const int dim, PtConnPtrPair innerArg, 
+                PtConnPtrPair outerArg, bool swapped);
+        void sort(const int dim);
+        int positionFor(const ConnRef *conn, const size_t dim) const;
+
+        // One for each dimension.
+        PointRepList connList[2];
+};
+
+typedef std::map<Avoid::Point,PtOrder> PtOrderMap;
+typedef std::set<Avoid::Point> PointSet;
+
+
+const unsigned int CROSSING_NONE = 0;
+const unsigned int CROSSING_TOUCHES = 1;
+const unsigned int CROSSING_SHARES_PATH = 2;
+const unsigned int CROSSING_SHARES_PATH_AT_END = 4;
+const unsigned int CROSSING_SHARES_FIXED_SEGMENT = 8;
+
+
+typedef std::pair<int, unsigned int> CrossingsInfoPair;
+
+extern CrossingsInfoPair countRealCrossings( Avoid::Polygon& poly, 
+        bool polyIsConn, Avoid::Polygon& conn, size_t cIndex, 
+        bool checkForBranchingSegments, const bool finalSegment = false, 
+        PointSet *crossingPoints = NULL, PtOrderMap *pointOrders = NULL, 
+        ConnRef *polyConnRef = NULL, ConnRef *connConnRef = NULL);
+extern void splitBranchingSegments(Avoid::Polygon& poly, bool polyIsConn,
+        Avoid::Polygon& conn, const double tolerance = 0);
+extern bool validateBendPoint(VertInf *aInf, VertInf *bInf, VertInf *cInf);
+
 }
 
 
diff --git a/src/libavoid/debug.h b/src/libavoid/debug.h
index 20e6f4705..443529ece 100644
--- a/src/libavoid/debug.h
+++ b/src/libavoid/debug.h
@@ -2,24 +2,27 @@
  * 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-2008  Monash University
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_DEBUG_H
 #define AVOID_DEBUG_H
 
@@ -42,7 +45,7 @@ inline void db_printf(const char *fmt, ...)
     va_end(ap);
 }
 #else
-inline void db_printf(const char */*fmt*/, ...)
+inline void db_printf(const char *, ...)
 {
 }
 #endif
diff --git a/src/libavoid/geometry.cpp b/src/libavoid/geometry.cpp
index 15840c381..2523375cf 100644
--- a/src/libavoid/geometry.cpp
+++ b/src/libavoid/geometry.cpp
@@ -2,7 +2,8 @@
  * 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
  *
  * --------------------------------------------------------------------
  * Much of the code in this module is based on code published with
@@ -18,70 +19,42 @@
  * 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 <cmath>
+
 #include "libavoid/graph.h"
 #include "libavoid/geometry.h"
-#include "libavoid/polyutil.h"
-
-#include <math.h>
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
 
-Point::Point()
-{
-}
-
-
-Point::Point(const double xv, const double yv)
-    : x(xv)
-    , y(yv)
-{
-}
-
-
-bool Point::operator==(const Point& rhs) const
-{
-    if ((x == rhs.x) && (y == rhs.y))
-    {
-        return true;
-    }
-    return false;
-}
-
-
-bool Point::operator!=(const Point& rhs) const
-{
-    if ((x != rhs.x) || (y != rhs.y))
-    {
-        return true;
-    }
-    return false;
-}
-
 
 // Returns true iff the point c lies on the closed segment ab.
+// To be used when the points are known to be collinear.
 //
 // Based on the code of 'Between'.
 //
-static const bool inBetween(const Point& a, const Point& b, const Point& c)
+bool inBetween(const Point& a, const Point& b, const Point& c)
 {
     // We only call this when we know the points are collinear,
     // otherwise we should be checking this here.
-    assert(vecDir(a, b, c) == 0);
+    COLA_ASSERT(vecDir(a, b, c, 0.0001) == 0);
 
-    if (a.x != b.x)
+    if ((fabs(a.x - b.x) > 1) && (a.x != b.x))
     {
         // not vertical
         return (((a.x < c.x) && (c.x < b.x)) ||
@@ -95,6 +68,15 @@ static const bool inBetween(const Point& a, const Point& b, const Point& c)
 }
 
 
+// Returns true iff the point c lies on the closed segment ab.
+//
+bool pointOnLine(const Point& a, const Point& b, const Point& c, 
+        const double tolerance)
+{
+    return (vecDir(a, b, c, tolerance) == 0) && inBetween(a, b, c);
+}
+
+
 // Returns true if the segment cd intersects the segment ab, blocking
 // visibility.
 //
@@ -104,15 +86,15 @@ bool segmentIntersect(const Point& a, const Point& b, const Point& c,
         const Point& d)
 {
     int ab_c = vecDir(a, b, c);
-    if ((ab_c == 0) && inBetween(a, b, c))
+    if (ab_c == 0)
     {
-        return true;
+        return false;
     }
 
     int ab_d = vecDir(a, b, d);
-    if ((ab_d == 0) && inBetween(a, b, d))
+    if (ab_d == 0)
     {
-        return true;
+        return false;
     }
 
     // It's ok for either of the points a or b to be on the line cd,
@@ -131,6 +113,37 @@ bool segmentIntersect(const Point& a, const Point& b, const Point& c,
 }
 
 
+// Returns true if the segment e1-e2 intersects the shape boundary 
+// segment s1-s2, blocking visibility.
+//
+bool segmentShapeIntersect(const Point& e1, const Point& e2, const Point& s1,
+        const Point& s2, bool& seenIntersectionAtEndpoint)
+{
+    if (segmentIntersect(e1, e2, s1, s2))
+    {
+        // Basic intersection of segments.
+        return true;
+    }
+    else if ( (((s2 == e1) || pointOnLine(s1, s2, e1)) && 
+               (vecDir(s1, s2, e2) != 0)) 
+              ||
+              (((s2 == e2) || pointOnLine(s1, s2, e2)) &&
+               (vecDir(s1, s2, e1) != 0)) )
+    {
+        // Segments intersect at the endpoint of one of the segments.  We
+        // allow this once, but the second one blocks visibility.  Otherwise
+        // shapes butted up against each other could have visibility through
+        // shapes.
+        if (seenIntersectionAtEndpoint)
+        {
+            return true;
+        }
+        seenIntersectionAtEndpoint = true;
+    }
+    return false;
+}
+
+
 // Returns true iff the point p in a valid region that can contain
 // shortest paths.  a0, a1, a2 are ordered vertices of a shape.
 //
@@ -205,20 +218,9 @@ int cornerSide(const Point &c1, const Point &c2, const Point &c3,
     int s12p = vecDir(c1, c2, p);
     int s23p = vecDir(c2, c3, p);
 
-    if (s12p == 0)
-    {
-        // Case of p being somewhere on c1-c2.
-        return s23p;
-    }
-    if (s23p == 0)
-    {
-        // Case of p being somewhere on c2-c3.
-        return s12p;
-    }
-
     if (s123 == 1)
     {
-        if ((s12p == 1) && (s23p == 1))
+        if ((s12p >= 0) && (s23p >= 0))
         {
             return 1;
         }
@@ -226,18 +228,37 @@ int cornerSide(const Point &c1, const Point &c2, const Point &c3,
     }
     else if (s123 == -1)
     {
-        if ((s12p == -1) && (s23p == -1))
+        if ((s12p <= 0) && (s23p <= 0))
         {
             return -1;
         }
         return 1;
     }
-    // Case of c3 being somewhere on c1-c2.
+
+    // c1-c2-c3 are collinear, so just return vecDir from c1-c2
     return s12p;
 }
 
 
-// Returns the distance between points a and b.
+// Returns the Euclidean distance between points a and b.
+//
+double euclideanDist(const Point& a, const Point& b)
+{
+    double xdiff = a.x - b.x;
+    double ydiff = a.y - b.y;
+
+    return sqrt((xdiff * xdiff) + (ydiff * ydiff));
+}
+
+// Returns the Manhattan distance between points a and b.
+//
+double manhattanDist(const Point& a, const Point& b)
+{
+    return fabs(a.x - b.x) + fabs(a.y - b.y);
+}
+
+
+// Returns the Euclidean distance between points a and b.
 //
 double dist(const Point& a, const Point& b)
 {
@@ -248,11 +269,12 @@ double dist(const Point& a, const Point& b)
 }
 
 // Returns the total length of all line segments in the polygon
-double totalLength(const Polygn& poly)
+double totalLength(const Polygon& poly)
 {
     double l = 0;
-    for (int i = 0; i < poly.pn-1; ++i) {
-        l += dist(poly.ps[i], poly.ps[i+1]);
+    for (size_t i = 1; i < poly.size(); ++i) 
+    {
+        l += dist(poly.ps[i-1], poly.ps[i]);
     }
     return l;
 }
@@ -277,18 +299,27 @@ double angle(const Point& a, const Point& b, const Point& c)
 // This is a fast version that only works for convex shapes.  The
 // other version (inPolyGen) is more general.
 //
-bool inPoly(const Polygn& poly, const Point& q)
+bool inPoly(const Polygon& poly, const Point& q, bool countBorder)
 {
-    int n = poly.pn;
-    Point *P = poly.ps;
-    for (int i = 0; i < n; i++)
+    size_t n = poly.size();
+    const std::vector<Point>& P = poly.ps;
+    bool onBorder = false;
+    for (size_t i = 0; i < n; i++)
     {
         // point index; i1 = i-1 mod n
-        int prev = (i + n - 1) % n;
-        if (vecDir(P[prev], P[i], q) == -1)
+        size_t prev = (i + n - 1) % n;
+        int dir = vecDir(P[prev], P[i], q);
+        if (dir == -1)
         {
+            // Point is outside
             return false;
         }
+        // Record if point was on a boundary.
+        onBorder |= (dir == 0);
+    }
+    if (!countBorder && onBorder)
+    {
+        return false;
     }
     return true;
 }
@@ -299,37 +330,36 @@ bool inPoly(const Polygn& poly, const Point& q)
 //
 // Based on the code of 'InPoly'.
 //
-bool inPolyGen(const Polygn& argpoly, const Point& q)
+bool inPolyGen(const PolygonInterface& argpoly, const Point& q)
 {
     // Numbers of right and left edge/ray crossings.
     int Rcross = 0;
     int Lcross = 0;
 
     // Copy the argument polygon
-    Polygn poly = copyPoly(argpoly);
-    Point *P = poly.ps;
-    int    n = poly.pn;
+    Polygon poly = argpoly;
+    std::vector<Point>& P = poly.ps;
+    size_t    n = poly.size();
 
     // Shift so that q is the origin. This is done for pedogical clarity.
-    for (int i = 0; i < n; ++i)
+    for (size_t i = 0; i < n; ++i)
     {
         P[i].x = P[i].x - q.x;
         P[i].y = P[i].y - q.y;
     }
 
     // For each edge e=(i-1,i), see if crosses ray.
-    for (int i = 0; i < n; ++i)
+    for (size_t i = 0; i < n; ++i)
     {
         // First see if q=(0,0) is a vertex.
         if ((P[i].x == 0) && (P[i].y == 0))
         {
             // We count a vertex as inside.
-            freePoly(poly);
             return true;
         }
 
         // point index; i1 = i-1 mod n
-        int i1 = ( i + n - 1 ) % n;
+        size_t i1 = ( i + n - 1 ) % n;
 
         // if e "straddles" the x-axis...
         // The commented-out statement is logically equivalent to the one
@@ -367,7 +397,6 @@ bool inPolyGen(const Polygn& argpoly, const Point& q)
             }
         }
     }
-    freePoly(poly);
 
     // q on the edge if left and right cross are not the same parity.
     if ( (Rcross % 2) != (Lcross % 2) )
@@ -400,8 +429,7 @@ bool inPolyGen(const Polygn& argpoly, const Point& q)
 int segmentIntersectPoint(const Point& a1, const Point& a2,
         const Point& b1, const Point& b2, double *x, double *y) 
 {
-
-    double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num,offset;
+    double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num;
     double x1lo,x1hi,y1lo,y1hi;
 
     Ax = a2.x - a1.x;
@@ -450,14 +478,13 @@ int segmentIntersectPoint(const Point& a1, const Point& a2,
         if (y1hi < b1.y || b2.y < y1lo) return DONT_INTERSECT;
     }
 
-
     Cx = a1.x - b1.x;
     Cy = a1.y - b1.y;
     // alpha numerator:
     d = By*Cx - Bx*Cy;
     // Both denominator:
     f = Ay*Bx - Ax*By;
-    // aplha tests:
+    // alpha tests:
     if (f > 0)
     {
         if (d < 0 || d > f) return DONT_INTERSECT;
@@ -485,15 +512,49 @@ int segmentIntersectPoint(const Point& a1, const Point& a2,
     
     // Numerator:
     num = d*Ax;
-    // Round direction:
-    offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
     // Intersection X:
-    *x = a1.x + (num+offset) / f;
+    *x = a1.x + (num) / f;
+
+    num = d*Ay;
+    // Intersection Y:
+    *y = a1.y + (num) / f;
+
+    return DO_INTERSECT;
+}
+
+
+// Line Segment Intersection
+// Original code by Franklin Antonio 
+//
+int rayIntersectPoint(const Point& a1, const Point& a2,
+        const Point& b1, const Point& b2, double *x, double *y) 
+{
+    double Ax,Bx,Cx,Ay,By,Cy,d,f,num;
+
+    Ay = a2.y - a1.y;
+    By = b1.y - b2.y;
+    Ax = a2.x - a1.x;
+    Bx = b1.x - b2.x;
+
+    Cx = a1.x - b1.x;
+    Cy = a1.y - b1.y;
+    // alpha numerator:
+    d = By*Cx - Bx*Cy;
+    // Both denominator:
+    f = Ay*Bx - Ax*By;
+
+    // compute intersection coordinates:
+
+    if (f == 0) return PARALLEL;
+    
+    // Numerator:
+    num = d*Ax;
+    // Intersection X:
+    *x = a1.x + (num) / f;
 
     num = d*Ay;
-    offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
     // Intersection Y:
-    *y = a1.y + (num+offset) / f;
+    *y = a1.y + (num) / f;
 
     return DO_INTERSECT;
 }
diff --git a/src/libavoid/geometry.h b/src/libavoid/geometry.h
index 1422be050..abd0d60e2 100644
--- a/src/libavoid/geometry.h
+++ b/src/libavoid/geometry.h
@@ -2,7 +2,8 @@
  * 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
  *
  * --------------------------------------------------------------------
  * Much of the code in this module is based on code published with
@@ -18,16 +19,17 @@
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
 
@@ -35,21 +37,30 @@
 #define _GEOMETRY_H
 
 #include "libavoid/geomtypes.h"
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
 
-extern double dist(const Point& a, const Point& b);
-extern double totalLength(const Polygn& poly);
+extern double euclideanDist(const Point& a, const Point& b);
+extern double manhattanDist(const Point& a, const Point& b);
+extern double totalLength(const Polygon& poly);
 extern double angle(const Point& a, const Point& b, const Point& c);
 extern bool segmentIntersect(const Point& a, const Point& b,
         const Point& c, const Point& d);
-extern bool inPoly(const Polygn& poly, const Point& q);
-extern bool inPolyGen(const Polygn& poly, const Point& q);
+extern bool segmentShapeIntersect(const Point& e1, const Point& e2, 
+        const Point& s1, const Point& s2, bool& seenIntersectionAtEndpoint);
+extern bool inPoly(const Polygon& poly, const Point& q, bool countBorder = true);
+extern bool inPolyGen(const PolygonInterface& poly, const Point& q);
 extern bool inValidRegion(bool IgnoreRegions, const Point& a0,
         const Point& a1, const Point& a2, const Point& b);
 extern int cornerSide(const Point &c1, const Point &c2, const Point &c3,
         const Point& p);
+extern bool pointOnLine(const Point& a, const Point& b, const Point& c,
+        const double tolerance = 0.0);
+
+// To be used only when the points are known to be colinear.
+extern bool inBetween(const Point& a, const Point& b, const Point& c);
 
 
 // Direction from vector.
@@ -61,16 +72,22 @@ extern int cornerSide(const Point &c1, const Point &c2, const Point &c3,
 //
 // Based on the code of 'AreaSign'.
 //
-static inline int vecDir(const Point& a, const Point& b, const Point& c)
+// The 'maybeZero' argument can be used to adjust the tolerance of the 
+// function.  It will be most accurate when 'maybeZero' == 0.0, the default.
+//
+static inline int vecDir(const Point& a, const Point& b, const Point& c, 
+        const double maybeZero = 0.0)
 {
+    COLA_ASSERT(maybeZero >= 0);
+
     double area2 = ((b.x - a.x) * (c.y - a.y)) -
                    ((c.x - a.x) * (b.y - a.y));
     
-    if (area2 < -0.001)
+    if (area2 < (-maybeZero))
     {
         return -1;
     }
-    else if (area2 > 0.001)
+    else if (area2 > maybeZero)
     {
         return 1;
     }
@@ -100,6 +117,8 @@ static const int DO_INTERSECT = 1;
 static const int PARALLEL = 3;
 extern int segmentIntersectPoint(const Point& a1, const Point& a2,
         const Point& b1, const Point& b2, double *x, double *y);
+extern int rayIntersectPoint(const Point& a1, const Point& a2,
+        const Point& b1, const Point& b2, double *x, double *y);
 
 
 }
diff --git a/src/libavoid/geomtypes.h b/src/libavoid/geomtypes.h
index dd9d26f2f..ced53e6b0 100644
--- a/src/libavoid/geomtypes.h
+++ b/src/libavoid/geomtypes.h
@@ -2,70 +2,314 @@
  * 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
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file  geomtypes.h
+//! @brief Contains the interface for various geometry types and classes.
+
 
 #ifndef AVOID_GEOMTYPES_H
 #define AVOID_GEOMTYPES_H
 
+#include <vector>
+#include <utility>
+
 
 namespace Avoid
 {
 
-    
+
+//! @brief  The Point class defines a point in the plane.
+//!
+//! Points consist of an x and y value.  They may also have an ID and vertex
+//! number associated with them.
+//!
 class Point
 {
     public:
+        //! @brief  Default constructor.
+        //!
         Point();
+        //! @brief  Standard constructor.
+        //!
+        //! @param[in]  xv  The x position of the point.
+        //! @param[in]  yv  The y position of the point.
+        //!
         Point(const double xv, const double yv);
+
+        //! @brief  Comparison operator. Returns true if at same position.
+        //!
+        //! @param[in]  rhs  The point to compare with this one.
+        //! @return          The result of the comparison.
+        //!
         bool operator==(const Point& rhs) const;
+        //! @brief  Comparison operator. Returns true if at different positions.
+        //!
+        //! @param[in]  rhs  The point to compare with this one.
+        //! @return          The result of the comparison.
+        //!
         bool operator!=(const Point& rhs) const;
+        //! @brief  Comparison operator. Returns true if less-then rhs point.
+        //!
+        //! @note  This operator is not particularly useful, but is defined 
+        //!        to allow std::set<Point>.
+        //!
+        //! @param[in]  rhs  The point to compare with this one.
+        //! @return          The result of the comparison.
+        //!
+        bool operator<(const Point& rhs) const;
 
+        //! @brief  Returns the x or y value of the point, given the dimension.
+        //!
+        //! @param[in]  dimension  The dimension:  0 for x, 1 for y.
+        //! @return                The component of the point in that dimension.
+        double& operator[](const unsigned int dimension);
+        const double& operator[](const unsigned int dimension) const;
+        
+        //! The x position.
         double x;
+        //! The y position.
         double y;
-        int id;
+        //! The ID associated with this point.
+        unsigned int id;
+        //! The vertex number associated with this point.
+        unsigned short vn;
 
 };
 
 
+//! Constant value representing an unassigned vertex number.
+//!
+static const unsigned short kUnassignedVertexNumber = 8;
+
+
+//! @brief  A vector, represented by the Point class.
+//!
 typedef Point Vector;
 
 
-typedef struct
+//! @brief  A common interface used by the Polygon classes.
+//!
+class PolygonInterface
 {
-    int id;
-    Point *ps;
-    int pn;
-} Polygn;
-
-typedef Polygn PolyLine;
+    public:
+        //! @brief  Constructor.
+        PolygonInterface() { }
+        //! @brief  Destructor.
+        virtual ~PolygonInterface() { }
+        //! @brief  Resets this to the empty polygon.
+        virtual void clear(void) = 0;
+        //! @brief  Returns true if this polygon is empty.
+        virtual bool empty(void) const = 0;
+        //! @brief  Returns the number of points in this polygon.
+        virtual size_t size(void) const = 0;
+        //! @brief  Returns the ID value associated with this polygon.
+        virtual int id(void) const = 0;
+        //! @brief  Returns a specific point in the polygon.
+        //! @param[in]  index  The array index of the point to be returned.
+        virtual const Point& at(size_t index) const = 0;
+        //! @brief  Returns the bounding rectangle that contains this polygon.
+        //!
+        //! If a NULL pointer is passed for any of the arguments, then that
+        //! value is ignored and not returned.
+        //!
+        //! @param[out]  minX  The left hand side of the bounding box.
+        //! @param[out]  minY  The top of the bounding box.
+        //! @param[out]  maxX  The right hand side of the bounding box.
+        //! @param[out]  maxY  The bottom of the bounding box.
+        void getBoundingRect(double *minX, double *minY,
+                double *maxX, double *maxY) const;
+};
 
 
-typedef struct
+//! @brief  A line between two points. 
+//!
+class Edge
 {
-    Point a;
-    Point b;
-} Edge;
+    public:
+        //! The first point.
+        Point a;
+        //! The second point.
+        Point b;
+};
+
 
+//! @brief  A bounding box, represented with an Edge between top-left and
+//!         bottom-right corners.
+//!
 typedef Edge BBox;
 
 
+class Router;
+class ReferencingPolygon;
+
+
+//! @brief  A dynamic Polygon, to which points can be easily added and removed.
+//!
+//! @note The Rectangle class can be used as an easy way of constructing a
+//!       square or rectangular polygon.
+//!
+class Polygon : public PolygonInterface
+{
+    public:
+        //! @brief  Constructs an empty polygon (with zero points). 
+        Polygon();
+        //! @brief  Constructs a new polygon with n points.
+        //! 
+        //! A rectangle would be comprised of four point.  An n segment 
+        //! PolyLine (represented as a Polygon) would be comprised of n+1
+        //! points.  Whether a particular Polygon is closed or not, depends
+        //! on whether it is a Polygon or Polyline.  Shape polygons are always
+        //! considered to be closed, meaning the last point joins back to the
+        //! first point.
+        //!
+        //! @param[in]  n  Number of points in the polygon.
+        //!
+        Polygon(const int n);
+        //! @brief  Constructs a new polygon from an existing Polygon.
+        //!
+        //! @param[in]  poly  An existing polygon to copy the new polygon from.
+        //!
+        Polygon(const PolygonInterface& poly);
+        //! @brief  Resets this to the empty polygon.
+        void clear(void);
+        //! @brief  Returns true if this polygon is empty.
+        bool empty(void) const;
+        //! @brief  Returns the number of points in this polygon.
+        size_t size(void) const;
+        //! @brief  Returns the ID value associated with this polygon.
+        int id(void) const;
+        //! @brief  Returns a specific point in the polygon.
+        //! @param[in]  index  The array index of the point to be returned.
+        const Point& at(size_t index) const;
+        //! @brief  Returns a simplified Polyline, where all collinear line
+        //!         segments have been collapsed down into single line 
+        //!         segments.
+        //!
+        //! @return A new polyline with a simplified representation.
+        //!
+        Polygon simplify(void) const;
+        //! @brief  Returns a curved approximation of this multi-segment 
+        //!         PolyLine, with the corners replaced by smooth Bezier 
+        //!         curves.
+        //!
+        //! This function does not do any further obstacle avoidance with the
+        //! curves produced.  Hence, you would usually specify a curve_amount
+        //! in similar size to the space buffer around obstacles in the scene.
+        //! This way the curves will cut the corners around shapes but still
+        //! run within this buffer space.
+        //!
+        //! @param  curve_amount  Describes the distance along the end of each 
+        //!                       line segment to turn into a curve.
+        //! @param  closed        Describes whether the Polygon should be 
+        //!                       treated as closed.  Defaults to false.
+        //! @return A new polyline (polygon) representing the curved path.
+        //!         Its points represent endpoints of line segments and 
+        //!         Bezier spline control points.  The Polygon::ts vector for
+        //!         this returned polygon is populated with a character for 
+        //!         each point describing its type.
+        //! @sa     ts
+        Polygon curvedPolyline(const double curve_amount, 
+                const bool closed = false) const;
+        //! @brief  Translates the polygon position by a relative amount.
+        //!
+        //! @param[in]  xDist  Distance to move polygon in the x dimension.
+        //! @param[in]  yDist  Distance to move polygon in the y dimension.
+        void translate(const double xDist, const double yDist);
+
+        //! @brief  An ID for the polygon.
+        int _id;
+        //! @brief  A vector of the points that make up the Polygon.
+        std::vector<Point> ps;
+        //! @brief  If used, denotes whether the corresponding point in ps is 
+        //!         a move-to operation or a Bezier curve-to.
+        //! 
+        //! Each character describes the drawing operation for the 
+        //! corresponding point in the ps vector.  Possible values are:
+        //!  -  'M': A moveto operation, marks the first point;
+        //!  -  'L': A lineto operation, is a line from the previous point to
+        //!     the current point; or
+        //!  -  'C': A curveto operation, three consecutive 'C' points 
+        //!     (along with the previous point) describe the control points 
+        //!     of a Bezier curve.
+        //!  -  'Z': Closes the path (used for cluster boundaries).
+        //!
+        //! @note   This vector will currently only be populated for polygons 
+        //!         returned by curvedPolyline().  
+        std::vector<char> ts;
+};
+
+
+//! @brief  A multi-segment line, represented with the Polygon class.
+//!
+typedef Polygon PolyLine;
+
+
+//! @brief  A Polygon which just references its points from other Polygons.
+//!          
+//! This type of Polygon is used to accurately represent cluster boundaries 
+//! made up from the corner points of shapes.
+//!
+class ReferencingPolygon : public PolygonInterface
+{
+    public:
+        ReferencingPolygon();
+        ReferencingPolygon(const Polygon& poly, const Router *router);
+        void clear(void);
+        bool empty(void) const;
+        size_t size(void) const;
+        int id(void) const;
+        const Point& at(size_t index) const;
+
+        int _id;
+        std::vector<std::pair<const Polygon *, unsigned short> > ps;
+};
+
+
+//! @brief  A Rectangle, a simpler way to define the polygon for square or
+//!         rectangular shapes.
+//!
+class Rectangle : public Polygon
+{
+    public:
+        //! @brief  Constructs a rectangular polygon given two opposing 
+        //!         corner points.
+        //!
+        //! @param[in]  topLeft      The first corner point of the rectangle.
+        //! @param[in]  bottomRight  The opposing corner point of the rectangle.
+        //!
+        Rectangle(const Point& topLeft, const Point& bottomRight);
+        
+        //! @brief  Constructs a rectangular polygon given the centre, width
+        //!         and height.
+        //!
+        //! @param[in]  centre  The centre of the rectangle, specified as 
+        //!                     a point.
+        //! @param[in]  width   The width of the rectangle.
+        //! @param[in]  height  The height of the rectangle.
+        //!
+        Rectangle(const Point& centre, const double width, const double height);
+};
+
+
 }
 
 #endif
diff --git a/src/libavoid/graph.cpp b/src/libavoid/graph.cpp
index 1970212df..728f8c085 100644
--- a/src/libavoid/graph.cpp
+++ b/src/libavoid/graph.cpp
@@ -2,56 +2,61 @@
  * 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
  *
  * 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 <cmath>
+
 #include "libavoid/debug.h"
 #include "libavoid/graph.h"
 #include "libavoid/connector.h"
 #include "libavoid/geometry.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/timer.h"
 #include "libavoid/vertices.h"
 #include "libavoid/router.h"
+#include "libavoid/assertions.h"
 
-#include <math.h>
 
 using std::pair;
 
 namespace Avoid {
 
 
-EdgeInf::EdgeInf(VertInf *v1, VertInf *v2)
-    : lstPrev(NULL)
-    , lstNext(NULL)
-    , _blocker(0)
-    , _router(NULL)
-    , _added(false)
-    , _visible(false)
-    , _v1(v1)
-    , _v2(v2)
-    , _dist(-1)
+EdgeInf::EdgeInf(VertInf *v1, VertInf *v2, const bool orthogonal)
+    : lstPrev(NULL),
+      lstNext(NULL),
+      _blocker(0),
+      _router(NULL),
+      _added(false),
+      _visible(false),
+      _orthogonal(orthogonal),
+      _v1(v1),
+      _v2(v2),
+      _dist(-1)
 {
     // Not passed NULL values.
-    assert(v1 && v2);
+    COLA_ASSERT(v1 && v2);
 
     // We are in the same instance
-    assert(_v1->_router == _v2->_router);
+    COLA_ASSERT(_v1->_router == _v2->_router);
     _router = _v1->_router;
 
     _conns.clear();
@@ -67,25 +72,142 @@ EdgeInf::~EdgeInf()
 }
 
 
+// Gives an order value between 0 and 3 for the point c, given the last
+// segment was from a to b.  Returns the following value:
+//    0 : Point c is directly backwards from point b.
+//    1 : Point c is a left-hand 90 degree turn.
+//    2 : Point c is a right-hand 90 degree turn.
+//    3 : Point c is straight ahead (collinear).
+//
+static inline int orthogTurnOrder(const Point& a, const Point& b, 
+        const Point& c)
+{
+    // We should only be calling this with orthogonal points, 
+    COLA_ASSERT((c.x == b.x) || (c.y == b.y));
+    COLA_ASSERT((a.x == b.x) || (a.y == b.y));
+
+    int direction = vecDir(a, b, c);
+
+    if (direction > 0)
+    {
+        // Counterclockwise := left
+        return 1;
+    }
+    else if (direction < 0)
+    {
+        // Clockwise := right
+        return 2;
+    }
+
+    if (b.x == c.x)
+    {
+        if ( ((a.y < b.y) && (c.y < b.y)) || 
+             ((a.y > b.y) && (c.y > b.y)) ) 
+        {
+            // Behind.
+            return 0;
+        }
+    }
+    else
+    {
+        if ( ((a.x < b.x) && (c.x < b.x)) || 
+             ((a.x > b.x) && (c.x > b.x)) ) 
+        {
+            // Behind.
+            return 0;
+        }
+    }
+
+    // Ahead.
+    return 3;
+}
+
+
+// Returns a less than operation for a set exploration order for orthogonal
+// searching.  Forward, then left, then right.  Or if there is no previous 
+// point, then the order is north, east, south, then west.
+// Note: This method assumes the two Edges that share a common point.
+bool EdgeInf::rotationLessThan(const VertInf *lastV, const EdgeInf *rhs) const
+{
+    if ((_v1 == rhs->_v1) && (_v2 == rhs->_v2))
+    {
+        // Effectively the same visibility edge, so they are equal.
+        return false;
+    }
+    VertInf *lhsV = NULL, *rhsV = NULL, *commonV = NULL;
+    
+    // Determine common Point and the comparison point on the left- and
+    // the right-hand-side.
+    if (_v1 == rhs->_v1)
+    {
+        commonV = _v1;
+        lhsV = _v2;
+        rhsV = rhs->_v2;
+    }
+    else if (_v1 == rhs->_v2)
+    {
+        commonV = _v1;
+        lhsV = _v2;
+        rhsV = rhs->_v1;
+    }
+    else if (_v2 == rhs->_v1)
+    {
+        commonV = _v2;
+        lhsV = _v1;
+        rhsV = rhs->_v2;
+    }
+    else if (_v2 == rhs->_v2)
+    {
+        commonV = _v2;
+        lhsV = _v1;
+        rhsV = rhs->_v1;
+    }
+
+    const Point& lhsPt = lhsV->point;
+    const Point& rhsPt = rhsV->point;
+    const Point& commonPt = commonV->point;
+    
+    // If no lastPt, use one directly to the left;
+    Point lastPt = (lastV) ? lastV->point : Point(commonPt.x - 10,  commonPt.y);
+
+    int lhsVal = orthogTurnOrder(lastPt, commonPt, lhsPt);
+    int rhsVal = orthogTurnOrder(lastPt, commonPt, rhsPt);
+
+    return lhsVal < rhsVal;
+}
+
+
 void EdgeInf::makeActive(void)
 {
-    assert(_added == false);
+    COLA_ASSERT(_added == false);
 
-    if (_visible)
+    if (_orthogonal)
     {
-        _router->visGraph.addEdge(this);
-        _pos1 = _v1->visList.insert(_v1->visList.begin(), this);
-        _v1->visListSize++;
-        _pos2 = _v2->visList.insert(_v2->visList.begin(), this);
-        _v2->visListSize++;
+        COLA_ASSERT(_visible);
+        _router->visOrthogGraph.addEdge(this);
+        _pos1 = _v1->orthogVisList.insert(_v1->orthogVisList.begin(), this);
+        _v1->orthogVisListSize++;
+        _pos2 = _v2->orthogVisList.insert(_v2->orthogVisList.begin(), this);
+        _v2->orthogVisListSize++;
     }
-    else // if (invisible)
+    else
     {
-        _router->invisGraph.addEdge(this);
-        _pos1 = _v1->invisList.insert(_v1->invisList.begin(), this);
-        _v1->invisListSize++;
-        _pos2 = _v2->invisList.insert(_v2->invisList.begin(), this);
-        _v2->invisListSize++;
+        if (_visible)
+        {
+            _router->visGraph.addEdge(this);
+            _pos1 = _v1->visList.insert(_v1->visList.begin(), this);
+            _v1->visListSize++;
+            _pos2 = _v2->visList.insert(_v2->visList.begin(), this);
+            _v2->visListSize++;
+        }
+        else // if (invisible)
+        {
+            _router->invisGraph.addEdge(this);
+            _pos1 = _v1->invisList.insert(_v1->invisList.begin(), this);
+            _v1->invisListSize++;
+            _pos2 = _v2->invisList.insert(_v2->invisList.begin(), this);
+            _v2->invisListSize++;
+        }
     }
     _added = true;
 }
@@ -93,23 +215,35 @@ void EdgeInf::makeActive(void)
 
 void EdgeInf::makeInactive(void)
 {
-    assert(_added == true);
+    COLA_ASSERT(_added == true);
 
-    if (_visible)
+    if (_orthogonal)
     {
-        _router->visGraph.removeEdge(this);
-        _v1->visList.erase(_pos1);
-        _v1->visListSize--;
-        _v2->visList.erase(_pos2);
-        _v2->visListSize--;
+        COLA_ASSERT(_visible);
+        _router->visOrthogGraph.removeEdge(this);
+        _v1->orthogVisList.erase(_pos1);
+        _v1->orthogVisListSize--;
+        _v2->orthogVisList.erase(_pos2);
+        _v2->orthogVisListSize--;
     }
-    else // if (invisible)
+    else
     {
-        _router->invisGraph.removeEdge(this);
-        _v1->invisList.erase(_pos1);
-        _v1->invisListSize--;
-        _v2->invisList.erase(_pos2);
-        _v2->invisListSize--;
+        if (_visible)
+        {
+            _router->visGraph.removeEdge(this);
+            _v1->visList.erase(_pos1);
+            _v1->visListSize--;
+            _v2->visList.erase(_pos2);
+            _v2->visListSize--;
+        }
+        else // if (invisible)
+        {
+            _router->invisGraph.removeEdge(this);
+            _v1->invisList.erase(_pos1);
+            _v1->invisListSize--;
+            _v2->invisList.erase(_pos2);
+            _v2->invisListSize--;
+        }
     }
     _blocker = 0;
     _conns.clear();
@@ -119,11 +253,12 @@ void EdgeInf::makeInactive(void)
 
 void EdgeInf::setDist(double dist)
 {
-    //assert(dist != 0);
+    //COLA_ASSERT(dist != 0);
 
     if (_added && !_visible)
     {
         makeInactive();
+        COLA_ASSERT(!_added);
     }
     if (!_added)
     {
@@ -135,6 +270,12 @@ void EdgeInf::setDist(double dist)
 }
 
 
+bool EdgeInf::added(void)
+{
+    return _added;
+}
+
+
 void EdgeInf::alertConns(void)
 {
     FlagList::iterator finish = _conns.end();
@@ -161,11 +302,12 @@ void EdgeInf::addCycleBlocker(void)
 
 void EdgeInf::addBlocker(int b)
 {
-    assert(_router->InvisibilityGrph);
+    COLA_ASSERT(_router->InvisibilityGrph);
 
     if (_added && _visible)
     {
         makeInactive();
+        COLA_ASSERT(!_added);
     }
     if (!_added)
     {
@@ -232,8 +374,11 @@ void EdgeInf::checkVis(void)
         cone1 = inValidRegion(_router->IgnoreRegions, i->shPrev->point,
                 iPoint, i->shNext->point, jPoint);
     }
-    else
+    else if (_router->IgnoreRegions == false)
     {
+        // If Ignoring regions then this case is already caught by 
+        // the invalid regions, so only check it when not ignoring
+        // regions.
         ShapeSet& ss = _router->contains[iID];
 
         if ((jID.isShape) && (ss.find(jID.objID) != ss.end()))
@@ -253,8 +398,11 @@ void EdgeInf::checkVis(void)
             cone2 = inValidRegion(_router->IgnoreRegions, j->shPrev->point,
                     jPoint, j->shNext->point, iPoint);
         }
-        else
+        else if (_router->IgnoreRegions == false)
         {
+            // If Ignoring regions then this case is already caught by 
+            // the invalid regions, so only check it when not ignoring
+            // regions.
             ShapeSet& ss = _router->contains[jID];
 
             if ((iID.isShape) && (ss.find(iID.objID) != ss.end()))
@@ -274,7 +422,7 @@ void EdgeInf::checkVis(void)
         db_printf("\tSetting visibility edge... \n\t\t");
         db_print();
 
-        double d = dist(iPoint, jPoint);
+        double d = euclideanDist(iPoint, jPoint);
 
         setDist(d);
 
@@ -316,26 +464,39 @@ int EdgeInf::firstBlocker(void)
     }
 
     VertInf *last = _router->vertices.end();
+    unsigned int lastId = 0;
+    bool seenIntersectionAtEndpoint = false;
     for (VertInf *k = _router->vertices.shapesBegin(); k != last; )
     {
         VertID kID = k->id;
-        if ((ss.find(kID.objID) != ss.end()))
+        if (k->id == dummyOrthogID)
+        {
+            // Don't include orthogonal dummy vertices.
+            k = k->lstNext;
+            continue;
+        }
+        if (kID.objID != lastId)
         {
-            unsigned int shapeID = kID.objID;
-            db_printf("Endpoint is inside shape %u so ignore shape edges.\n",
-                    kID.objID);
-            // One of the endpoints is inside this shape so ignore it.
-            while ((k != last) && (k->id.objID == shapeID))
+            if ((ss.find(kID.objID) != ss.end()))
             {
-                // And skip the other vertices from this shape.
-                k = k->lstNext;
+                unsigned int shapeID = kID.objID;
+                db_printf("Endpoint is inside shape %u so ignore shape "
+                        "edges.\n", kID.objID);
+                // 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;
             }
-            continue;
+            seenIntersectionAtEndpoint = false;
+            lastId = kID.objID;
         }
         Point& kPoint = k->point;
         Point& kPrevPoint = k->shPrev->point;
-
-        if (segmentIntersect(pti, ptj, kPrevPoint, kPoint))
+        if (segmentShapeIntersect(pti, ptj, kPrevPoint, kPoint, 
+                    seenIntersectionAtEndpoint))
         {
             ss.clear();
             return kID.objID;
@@ -358,9 +519,17 @@ bool EdgeInf::isBetween(VertInf *i, VertInf *j)
 }
 
 
+    // Returns true if this edge is a vertical or horizontal line segment.
+bool EdgeInf::isOrthogonal(void) const
+{
+    return ((_v1->point.x == _v2->point.x) || 
+            (_v1->point.y == _v2->point.y));
+}
+
+
 VertInf *EdgeInf::otherVert(VertInf *vert)
 {
-    assert((vert == _v1) || (vert == _v2));
+    COLA_ASSERT((vert == _v1) || (vert == _v2));
 
     if (vert == _v1)
     {
@@ -372,12 +541,17 @@ VertInf *EdgeInf::otherVert(VertInf *vert)
 
 EdgeInf *EdgeInf::checkEdgeVisibility(VertInf *i, VertInf *j, bool knownNew)
 {
+    // This is for polyline routing, so check we're not 
+    // considering orthogonal vertices.
+    COLA_ASSERT(i->id != dummyOrthogID);
+    COLA_ASSERT(j->id != dummyOrthogID);
+    
     Router *router = i->_router;
     EdgeInf *edge = NULL;
 
     if (knownNew)
     {
-        assert(existingEdge(i, j) == NULL);
+        COLA_ASSERT(existingEdge(i, j) == NULL);
         edge = new EdgeInf(i, j);
     }
     else
@@ -399,22 +573,17 @@ EdgeInf *EdgeInf::checkEdgeVisibility(VertInf *i, VertInf *j, bool knownNew)
 }
 
 
+    // XXX: This function is ineffecient, and shouldn't even really be
+    //      required.
 EdgeInf *EdgeInf::existingEdge(VertInf *i, VertInf *j)
 {
     VertInf *selected = NULL;
 
-    if (i->visListSize <= j->visListSize)
-    {
-        selected = i;
-    }
-    else
-    {
-        selected = j;
-    }
-
+    // Look through poly-line visibility edges.
+    selected = (i->visListSize <= j->visListSize) ? i : j;
     EdgeInfList& visList = selected->visList;
-    EdgeInfList::iterator finish = visList.end();
-    for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
+    EdgeInfList::const_iterator finish = visList.end();
+    for (EdgeInfList::const_iterator edge = visList.begin(); edge != finish;
             ++edge)
     {
         if ((*edge)->isBetween(i, j))
@@ -423,18 +592,24 @@ EdgeInf *EdgeInf::existingEdge(VertInf *i, VertInf *j)
         }
     }
 
-    if (i->invisListSize <= j->invisListSize)
-    {
-        selected = i;
-    }
-    else
+    // Look through orthogonal visbility edges.
+    selected = (i->orthogVisListSize <= j->orthogVisListSize) ? i : j;
+    EdgeInfList& orthogVisList = selected->orthogVisList;
+    finish = orthogVisList.end();
+    for (EdgeInfList::const_iterator edge = orthogVisList.begin(); 
+            edge != finish; ++edge)
     {
-        selected = j;
+        if ((*edge)->isBetween(i, j))
+        {
+            return (*edge);
+        }
     }
 
+    // Look through poly-line invisbility edges.
+    selected = (i->invisListSize <= j->invisListSize) ? i : j;
     EdgeInfList& invisList = selected->invisList;
     finish = invisList.end();
-    for (EdgeInfList::iterator edge = invisList.begin(); edge != finish;
+    for (EdgeInfList::const_iterator edge = invisList.begin(); edge != finish;
             ++edge)
     {
         if ((*edge)->isBetween(i, j))
@@ -450,19 +625,45 @@ EdgeInf *EdgeInf::existingEdge(VertInf *i, VertInf *j)
 //===========================================================================
 
 
-EdgeList::EdgeList()
-    : _firstEdge(NULL)
-    , _lastEdge(NULL)
-    , _count(0)
+EdgeList::EdgeList(bool orthogonal)
+    : _orthogonal(orthogonal),
+      _firstEdge(NULL),
+      _lastEdge(NULL),
+      _count(0)
+{
+}
+
+
+EdgeList::~EdgeList()
+{
+    clear();
+}
+
+
+void EdgeList::clear(void)
+{
+    while (_firstEdge)
+    {
+        delete _firstEdge;
+    }
+    COLA_ASSERT(_count == 0);
+    _lastEdge = NULL;
+}
+
+
+int EdgeList::size(void) const
 {
+    return _count;
 }
 
 
 void EdgeList::addEdge(EdgeInf *edge)
 {
+    COLA_ASSERT(!_orthogonal || edge->isOrthogonal());
+    
     if (_firstEdge == NULL)
     {
-        assert(_lastEdge == NULL);
+        COLA_ASSERT(_lastEdge == NULL);
 
         _lastEdge = edge;
         _firstEdge = edge;
@@ -472,7 +673,7 @@ void EdgeList::addEdge(EdgeInf *edge)
     }
     else
     {
-        assert(_lastEdge != NULL);
+        COLA_ASSERT(_lastEdge != NULL);
 
         _lastEdge->lstNext = edge;
         edge->lstPrev = _lastEdge;
diff --git a/src/libavoid/graph.h b/src/libavoid/graph.h
index 05f03a988..db776b80b 100644
--- a/src/libavoid/graph.h
+++ b/src/libavoid/graph.h
@@ -2,24 +2,27 @@
  * 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
  *
  * 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>
 */
 
+
 #ifndef AVOID_GRAPH_H
 #define AVOID_GRAPH_H
 
@@ -31,6 +34,7 @@
 
 namespace Avoid {
 
+
 class ConnRef;
 class Router;
 
@@ -42,8 +46,8 @@ typedef std::list<bool *> FlagList;
 class EdgeInf
 {
     public:
-        EdgeInf(VertInf *v1, VertInf *v2);
-        virtual ~EdgeInf();
+        EdgeInf(VertInf *v1, VertInf *v2, const bool orthogonal = false);
+        ~EdgeInf();
         inline double getDist(void)
         {
             return _dist;
@@ -53,6 +57,9 @@ class EdgeInf
         void addConn(bool *flag);
         void addCycleBlocker(void);
         void addBlocker(int b);
+        bool added(void);
+        bool isOrthogonal(void) const;
+        bool rotationLessThan(const VertInf* last, const EdgeInf *rhs) const;
 
         std::pair<VertID, VertID> ids(void);
         std::pair<Point, Point> points(void);
@@ -70,6 +77,7 @@ class EdgeInf
         Router *_router;
         bool _added;
         bool _visible;
+        bool _orthogonal;
         VertInf *_v1;
         VertInf *_v2;
         EdgeInfList::iterator _pos1;
@@ -87,12 +95,17 @@ class EdgeInf
 class EdgeList
 {
     public:
-        EdgeList();
-        void addEdge(EdgeInf *edge);
-        void removeEdge(EdgeInf *edge);
+        friend class EdgeInf;
+        EdgeList(bool orthogonal = false);
+        ~EdgeList();
+        void clear(void);
         EdgeInf *begin(void);
         EdgeInf *end(void);
+        int size(void) const;
     private:
+        void addEdge(EdgeInf *edge);
+        void removeEdge(EdgeInf *edge);
+        bool _orthogonal;
         EdgeInf *_firstEdge;
         EdgeInf *_lastEdge;
         unsigned int _count;
diff --git a/src/libavoid/libavoid.h b/src/libavoid/libavoid.h
index d598c6c74..1d4cd1fdc 100644
--- a/src/libavoid/libavoid.h
+++ b/src/libavoid/libavoid.h
@@ -2,29 +2,37 @@
  * 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
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file  libavoid.h
+//! @brief Standard libavoid include file which includes all libavoid 
+//!        header files.
+
+//! @namespace Avoid
+//! @brief The namespace used by code in the libavoid library.
+
 #ifndef AVOID_LIBAVOID_H
 #define AVOID_LIBAVOID_H
 
 #include "libavoid/geomtypes.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/connector.h"
 #include "libavoid/graph.h"
 #include "libavoid/debug.h"
@@ -32,7 +40,6 @@
 #include "libavoid/makepath.h"
 #include "libavoid/vertices.h"
 #include "libavoid/visibility.h"
-#include "libavoid/static.h"
 #include "libavoid/router.h"
 
 #endif
diff --git a/src/libavoid/makepath.cpp b/src/libavoid/makepath.cpp
index 3a57f8e4e..4e15dbca9 100644
--- a/src/libavoid/makepath.cpp
+++ b/src/libavoid/makepath.cpp
@@ -2,43 +2,105 @@
  * 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>
  *
- * --------------------------------------------------------------------
- * The dijkstraPath function is based on code published and described
- * in "Algorithms in C" (Second Edition), 1990, by Robert Sedgewick.
- * --------------------------------------------------------------------
+ * Copyright (C) 2004-2009  Monash University
  *
  * 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 <vector>
+#include <climits>
+#define _USE_MATH_DEFINES
+#include <cmath>
+
 #include "libavoid/vertices.h"
 #include "libavoid/makepath.h"
 #include "libavoid/geometry.h"
 #include "libavoid/connector.h"
 #include "libavoid/graph.h"
 #include "libavoid/router.h"
-#include <algorithm>
-#include <vector>
-#include <climits>
-#include <limits.h>
-#include <math.h>
+#include "libavoid/debug.h"
+#include "libavoid/assertions.h"
+#ifdef ASTAR_DEBUG
+  #include <SDL_gfxPrimitives.h>
+#endif
 
 namespace Avoid {
 
+class ANode
+{
+    public:
+        VertInf* inf;
+        double g;        // Gone
+        double h;        // Heuristic
+        double f;        // Formula f = g + h
+        
+        int prevIndex;   // Index into DONE for the previous ANode.
+        int timeStamp;   // Timestamp used to determine explaration order of
+                         // seemingly equal paths during orthogonal routing.
+
+        ANode(VertInf *vinf, int time)
+            : inf(vinf),
+              g(0),
+              h(0),
+              f(0),
+              prevIndex(-1),
+              timeStamp(time)
+        {
+        }
+        ANode()
+            : inf(NULL),
+              g(0),
+              h(0),
+              f(0),
+              prevIndex(-1),
+              timeStamp(-1)
+        {
+        }
+};
+
+
+// This returns the opposite result (>) so that when used with stl::make_heap, 
+// the head node of the heap will be the smallest value, rather than the 
+// largest.  This saves us from having to sort the heap (and then reorder
+// it back into a heap) when getting the next node to examine.  This way we
+// get better complexity -- logarithmic pushs and pops to the heap.
+//
+bool operator<(const ANode &a, const ANode &b)
+{
+    if (a.f != b.f)
+    {
+        return a.f > b.f;
+    }
+    if (a.timeStamp != b.timeStamp)
+    {
+        // Tiebreaker, if two paths have equal cost, then choose the one with
+        // the highest timeStamp.  This corresponds to the furthest point
+        // explored along the straight-line path.  When exploring we give the
+        // directions the following timeStamps; left:1, right:2 and forward:3,
+        // then we always try to explore forward first.
+        return a.timeStamp < b.timeStamp;
+    }
+    COLA_ASSERT(a.prevIndex != b.prevIndex);
+    return a.prevIndex > b.prevIndex;
+}
+
 
 static double Dot(const Point& l, const Point& r)
 {
@@ -56,6 +118,13 @@ static double CrossLength(const Point& l, const Point& r)
 //
 static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
 {
+    if ((p1.x == p2.x && p1.y == p2.y) || (p2.x == p3.x && p2.y == p3.y))
+    {
+        // If two of the points are the same, then we can't say anything
+        // about the angle between.  Treat them as being collinear.
+        return M_PI;
+    }
+
     Point v1(p1.x - p2.x, p1.y - p2.y);
     Point v2(p3.x - p2.x, p3.y - p2.y);
 
@@ -63,21 +132,45 @@ static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
 }
 
 
+// Construct a temporary Polygon path given several VertInf's for a connector.
+//
+static void constructPolygonPath(Polygon& connRoute, VertInf *inf2, 
+        VertInf *inf3, std::vector<ANode>& done, int inf1Index)
+{
+    int routeSize = 2;
+    for (int curr = inf1Index; curr >= 0; curr = done[curr].prevIndex)
+    {
+        routeSize += 1;
+    }
+    connRoute.ps.resize(routeSize);
+    connRoute.ps[routeSize - 1] = inf3->point;
+    connRoute.ps[routeSize - 2] = inf2->point;
+    routeSize -= 3;
+    for (int curr = inf1Index; curr >= 0; curr = done[curr].prevIndex)
+    {
+        connRoute.ps[routeSize] = done[curr].inf->point;
+        routeSize -= 1;
+    }
+}
+
+
 // Given the two points for a new segment of a path (inf2 & inf3)
 // as well as the distance between these points (dist), as well as
 // possibly the previous point (inf1) [from inf1--inf2], return a
 // cost associated with this route.
 //
-double cost(ConnRef *lineRef, const double dist, VertInf *inf1,
-        VertInf *inf2, VertInf *inf3)
+static double cost(ConnRef *lineRef, const double dist, VertInf *inf2, 
+        VertInf *inf3, std::vector<ANode>& done, int inf1Index)
 {
+    VertInf *inf1 = (inf1Index >= 0) ?  done[inf1Index].inf : NULL;
     double result = dist;
+    Polygon connRoute;
 
     Router *router = inf2->_router;
-    if (inf2->pathNext != NULL)
+    if (inf1 != NULL)
     {
-        double& angle_penalty = router->angle_penalty;
-        double& segmt_penalty = router->segmt_penalty;
+        const double angle_penalty = router->routingPenalty(anglePenalty);
+        const double segmt_penalty = router->routingPenalty(segmentPenalty);
 
         // This is not the first segment, so there is a bend
         // between it and the last one in the existing path.
@@ -89,29 +182,83 @@ double cost(ConnRef *lineRef, const double dist, VertInf *inf1,
 
             double rad = M_PI - angleBetween(p1, p2, p3);
 
-            // Make `xval' between 0--10 then take its log so small
-            // angles are not penalised as much as large ones.
-            //
-            double xval = rad * 10 / M_PI;
-            double yval = xval * log10(xval + 1) / 10.5;
-            result += (angle_penalty * yval);
-            //printf("deg from straight: %g\tpenalty: %g\n",
-            //        rad * 180 / M_PI, (angle_penalty * yval));
-
-            // Don't penalise as an extra segment if there is no turn.
-            if (rad > 0.0005)
+            if (rad > 0)
+            {
+                // Make `xval' between 0--10 then take its log so small
+                // angles are not penalised as much as large ones.
+                //
+                double xval = rad * 10 / M_PI;
+                double yval = xval * log10(xval + 1) / 10.5;
+                result += (angle_penalty * yval);
+                //db_printf("deg from straight: %g\tpenalty: %g\n",
+                //        rad * 180 / M_PI, (angle_penalty * yval));
+            }
+
+            if (rad == M_PI)
             {
+                // Needs to double back
+                result += (2 * segmt_penalty);
+            }
+            else if (rad > 0)
+            {
+                // Only penalise as an extra segment if the two 
+                // segments are not collinear.
                 result += segmt_penalty;
             }
         }
     }
 
-    if (lineRef->doesHateCrossings() && (router->crossing_penalty > 0))
+    if (!router->_inCrossingPenaltyReroutingStage)
     {
-        Point& a1 = inf2->point;
-        Point& a2 = inf3->point;
+        // Return here if we ar not in the postprocessing stage 
+        return result;
+    }
 
-        ConnRefList::iterator curr, finish = router->connRefs.end();
+    const double cluster_crossing_penalty = 
+            router->routingPenalty(clusterCrossingPenalty);
+    // XXX: Clustered routing doesn't yet work with orthogonal connectors.
+    if (router->ClusteredRouting && !router->clusterRefs.empty() &&
+            (cluster_crossing_penalty > 0) && 
+            (lineRef->routingType() != ConnType_Orthogonal))
+    {
+        if (connRoute.empty())
+        {
+            constructPolygonPath(connRoute, inf2, inf3, done, inf1Index);
+        }
+        // There are clusters so do cluster routing.
+        for (ClusterRefList::const_iterator cl = router->clusterRefs.begin(); 
+                cl != router->clusterRefs.end(); ++cl)
+        {
+            ReferencingPolygon& cBoundary = (*cl)->polygon();
+            COLA_ASSERT(cBoundary.ps[0] != cBoundary.ps[cBoundary.size() - 1]);
+            for (size_t j = 0; j < cBoundary.size(); ++j)
+            {
+                // Cluster boundary points should correspond to shape 
+                // vertices and hence already be in the list of vertices.
+                COLA_ASSERT(router->vertices.getVertexByPos(cBoundary.at(j))!=NULL);
+            }
+            
+            bool isConn = false;
+            Polygon dynamic_c_boundary(cBoundary);
+            Polygon dynamic_conn_route(connRoute);
+            const bool finalSegment = (inf3 == lineRef->dst());
+            CrossingsInfoPair crossings = countRealCrossings(
+                    dynamic_c_boundary, isConn, dynamic_conn_route, 
+                    connRoute.size() - 1, true, finalSegment);
+            result += (crossings.first * cluster_crossing_penalty);
+        }
+    }
+
+    const double shared_path_penalty = 
+            router->routingPenalty(fixedSharedPathPenalty);
+    if (shared_path_penalty > 0)
+    {
+        // Penalises shared paths, except if the connectors shared an endpoint.
+        if (connRoute.empty())
+        {
+            constructPolygonPath(connRoute, inf2, inf3, done, inf1Index);
+        }
+        ConnRefList::const_iterator curr, finish = router->connRefs.end();
         for (curr = router->connRefs.begin(); curr != finish; ++curr)
         {
             ConnRef *connRef = *curr;
@@ -120,306 +267,237 @@ double cost(ConnRef *lineRef, const double dist, VertInf *inf1,
             {
                 continue;
             }
-            Avoid::PolyLine& route2 = connRef->route();
-            for (int j = 1; j < route2.pn; ++j)
-            {
-                Avoid::Point& b1 = route2.ps[j - 1];
-                Avoid::Point& b2 = route2.ps[j];
+            const Avoid::PolyLine& route2 = connRef->route();
             
-                if (((a1 == b1) && (a2 == b2)) ||
-                    ((a2 == b1) && (a1 == b2)))
-                {
-                    // Route along same segment: no penalty.  We detect
-                    // crossovers when we see the segments diverge.
-                    continue;
-                }
-
-                if ((a2 == b2) || (a2 == b1) || (b2 == a1))
-                {
-                    // Each crossing that is at a vertex in the 
-                    // visibility graph gets noticed four times.
-                    // We ignore three of these cases.
-                    // This also catches the case of a shared path,
-                    // but this is one that terminates at a common
-                    // endpoint, so we don't care about it.
-                    continue;
-                }
-
-                if (a1 == b1)
-                {
-                    if (j == 1)
-                    {
-                        // common source point.
-                        continue;
-                    }
-                    Avoid::Point& b0 = route2.ps[j - 2];
-                    // The segments share an endpoint -- a1==b1.
-                    if (a2 == b0)
-                    {
-                        // a2 is not a split, continue.
-                        continue;
-                    }
-                    
-                    // If here, then we know that a2 != b2
-                    // And a2 and its pair in b are a split.
-                    assert(a2 != b2);
-
-                    if (inf2->pathNext == NULL)
-                    {
-                        continue;
-                    }
-                    Avoid::Point& a0 = inf1->point;
-
-                    if ((a0 == b0) || (a0 == b2))
-                    {
-                        //printf("Shared path... ");
-                        bool normal = (a0 == b0) ? true : false;
-                        // Determine direction we have to look through
-                        // the points of connector b.
-                        int dir = normal ? -1 : 1;
-                        
-                        int traceJ = j - 1 + dir;
-                        
-                        int endCornerSide = Avoid::cornerSide(
-                                a0, a1, a2, normal ? b2 : b0);
-
-                        
-                        VertInf *traceInf1 = inf2->pathNext;
-                        VertInf *traceInf2 = inf2;
-                        VertInf *traceInf3 = inf3;
-                        while (traceInf1 &&
-                                (traceJ >= 0) && (traceJ < route2.pn) &&
-                                (traceInf1->point == route2.ps[traceJ]))
-                        {
-                            traceInf3 = traceInf2;
-                            traceInf2 = traceInf1;
-                            traceInf1 = traceInf1->pathNext;
-                            traceJ += dir;
-                        }
-                        
-                        if (!traceInf1 ||
-                                (traceJ < 0) || (traceJ >= route2.pn))
-                        {
-                            //printf("common source or destination.\n");
-                            // The connectors have a shared path, but it
-                            // comes from a common source point.
-                            // XXX: There might be a better way to
-                            //      check this by asking the connectors
-                            //      for the IDs of the attached shapes.
-                            continue;
-                        }
-                        
-                        int startCornerSide = Avoid::cornerSide(
-                                traceInf1->point, traceInf2->point,
-                                traceInf3->point, route2.ps[traceJ]);
-                        
-                        if (endCornerSide != startCornerSide)
-                        {
-                            //printf("crosses.\n");
-                            result += router->crossing_penalty;
-                        }
-                        else
-                        {
-                            //printf("doesn't cross.\n");
-                        }
-                    }
-                    else
-                    {
-                        // The connectors cross or touch at this point.
-                        //printf("Cross or touch at point... ");
-                    
-                        int side1 = Avoid::cornerSide(a0, a1, a2, b0);
-                        int side2 = Avoid::cornerSide(a0, a1, a2, b2);
-
-                        if (side1 != side2)
-                        {
-                            //printf("cross.\n");
-                            // The connectors cross at this point.
-                            result += router->crossing_penalty;
-                        }
-                        else
-                        {
-                            //printf("touch.\n");
-                            // The connectors touch at this point.
-                        }
-                    }
-                    continue;
-                }
+            bool isConn = true;
+            Polygon dynamic_route2(route2);
+            Polygon dynamic_conn_route(connRoute);
+            CrossingsInfoPair crossings = countRealCrossings(
+                    dynamic_route2, isConn, dynamic_conn_route, 
+                    connRoute.size() - 1, false, false, NULL, NULL,
+                    connRef, lineRef);
+
+            if ((crossings.second & CROSSING_SHARES_PATH) &&
+                    (crossings.second & CROSSING_SHARES_FIXED_SEGMENT) &&
+                    !(crossings.second & CROSSING_SHARES_PATH_AT_END))
+            {
+                // Penalise unecessary shared paths in the middle of
+                // connectors.
+                result += shared_path_penalty;
+            }
+        }
+    }
 
-                double xc, yc;
-                int intersectResult = Avoid::segmentIntersectPoint(
-                        a1, a2, b1, b2, &xc, &yc);
+    const double crossing_penalty = router->routingPenalty(crossingPenalty);
+    if (lineRef->doesHateCrossings() && (crossing_penalty > 0))
+    {
+        if (connRoute.empty())
+        {
+            constructPolygonPath(connRoute, inf2, inf3, done, inf1Index);
+        }
+        ConnRefList::const_iterator curr, finish = router->connRefs.end();
+        for (curr = router->connRefs.begin(); curr != finish; ++curr)
+        {
+            ConnRef *connRef = *curr;
 
-                if (intersectResult == Avoid::DO_INTERSECT)
-                {
-                    result += router->crossing_penalty;
-                }
+            if (connRef->id() == lineRef->id())
+            {
+                continue;
             }
+            const Avoid::PolyLine& route2 = connRef->route();
+            
+            bool isConn = true;
+            Polygon dynamic_route2(route2);
+            Polygon dynamic_conn_route(connRoute);
+            CrossingsInfoPair crossings = countRealCrossings(
+                    dynamic_route2, isConn, dynamic_conn_route, 
+                    connRoute.size() - 1, true);
+            result += (crossings.first * crossing_penalty);
         }
     }
-    
+
     return result;
 }
 
 
-// Returns the best path from src to tar using the cost function.
-//
-// The path is worked out via Dijkstra's algorithm, and is encoded via
-// pathNext links in each of the VerInfs along the path.
-//
-// Based on the code of 'matrixpfs'.
-//
-static void dijkstraPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
+static double estimatedCost(ConnRef *lineRef, const Point *last, 
+        const Point& a, const Point& b)
 {
-    Router *router = src->_router;
-
-    double unseen = (double) __INT_MAX__;
-
-    // initialize arrays
-    VertInf *finish = router->vertices.end();
-    for (VertInf *t = router->vertices.connsBegin(); t != finish; t = t->lstNext)
+    if (lineRef->routingType() == ConnType_PolyLine)
     {
-        t->pathNext = NULL;
-        t->pathDist = -unseen;
+        return euclideanDist(a, b);
     }
-
-    VertInf *min = src;
-    while (min != tar)
+    else // Orthogonal
     {
-        VertInf *k = min;
-        min = NULL;
-
-        k->pathDist *= -1;
-        if (k->pathDist == unseen)
+        // XXX: This currently just takes into account the compulsory
+        //      bend but will have to be updated when port direction 
+        //      information is available.
+        int num_penalties = 0;
+        double xmove = b.x - a.x;
+        double ymove = b.y - a.y;
+        if (!last)
         {
-            k->pathDist = 0;
-        }
-
-        EdgeInfList& visList = k->visList;
-        EdgeInfList::iterator finish = visList.end();
-        for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
-                ++edge)
-        {
-            VertInf *t = (*edge)->otherVert(k);
-            VertID tID = t->id;
-
-            // Only check shape verticies, or endpoints.
-            if ((t->pathDist < 0) &&
-                    ((tID.objID == src->id.objID) || tID.isShape))
+            // Just two points.
+            if ((xmove != 0) && (ymove != 0))
             {
-                double kt_dist = (*edge)->getDist();
-                double priority = k->pathDist +
-                        cost(lineRef, kt_dist, k->pathNext, k, t);
-
-                if ((kt_dist != 0) && (t->pathDist < -priority))
-                {
-                    t->pathDist = -priority;
-                    t->pathNext = k;
-                }
-                if ((min == NULL) || (t->pathDist > min->pathDist))
-                {
-                    min = t;
-                }
+                num_penalties += 1;
             }
         }
-        EdgeInfList& invisList = k->invisList;
-        finish = invisList.end();
-        for (EdgeInfList::iterator edge = invisList.begin(); edge != finish;
-                ++edge)
+        else
         {
-            VertInf *t = (*edge)->otherVert(k);
-            VertID tID = t->id;
-
-            // Only check shape verticies, or endpoints.
-            if ((t->pathDist < 0) &&
-                    ((tID.objID == src->id.objID) || tID.isShape > 0))
+            // We have three points, so we know the direction of the 
+            // previous segment.
+            double rad = M_PI - angleBetween(*last, a, b);
+            if (rad > (M_PI / 2))            
             {
-                if ((min == NULL) || (t->pathDist > min->pathDist))
-                {
-                    min = t;
-                }
+                // Target point is back in the direction of the first point,
+                // so at least two bends are required.
+                num_penalties += 2;
+            }
+            else if (rad > 0)
+            {
+                // To the side, so at least one bend.
+                num_penalties += 1;
             }
         }
+        double penalty = num_penalties * 
+                lineRef->router()->routingPenalty(segmentPenalty);
+
+        return manhattanDist(a, b) + penalty;
     }
 }
 
 
-class ANode
+class CmpVisEdgeRotation 
 {
     public:
-        VertInf* inf;
-        double g;        // Gone
-        double h;        // Heuristic
-        double f;        // Formula f = g + h
-        VertInf *pp;
-
-        ANode(VertInf *vinf)
-            : inf(vinf)
-            , g(0)
-            , h(0)
-            , f(0)
-            , pp(NULL)
+        CmpVisEdgeRotation(const VertInf* lastPt)
+            : _lastPt(lastPt)
         {
         }
-        ANode()
-            : inf(NULL)
-            , g(0)
-            , h(0)
-            , f(0)
-            , pp(NULL)
+        bool operator() (const EdgeInf* u, const EdgeInf* v) const 
         {
+            return u->rotationLessThan(_lastPt, v);
         }
+    private:
+        const VertInf *_lastPt;
 };
 
-bool operator<(const ANode &a, const ANode &b)
-{
-    return a.f < b.f;
-}
-
-
-bool operator>(const ANode &a, const ANode &b)
-{
-    return a.f > b.f;
-}
-
 
 // Returns the best path from src to tar using the cost function.
 //
 // The path is worked out using the aStar algorithm, and is encoded via
-// pathNext links in each of the VerInfs along the path.
+// prevIndex values for each ANode which point back to the previous ANode's
+// position in the DONE vector.  At completion, this order is written into
+// the pathNext links in each of the VerInfs along the path.
 //
 // The aStar STL code is based on public domain code available on the
 // internet.
 //
-static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
+static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar, 
+        VertInf *start)
 {
+    bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);
+
+    double (*dist)(const Point& a, const Point& b) = 
+        (isOrthogonal) ? manhattanDist : euclideanDist;
+
     std::vector<ANode> PENDING;     // STL Vectors chosen because of rapid
     std::vector<ANode> DONE;        // insertions/deletions at back,
     ANode Node, BestNode;           // Temporary Node and BestNode
     bool bNodeFound = false;        // Flag if node is found in container
+    int timestamp = 1;
 
-    tar->pathNext = NULL;
+    if (start == NULL)
+    {
+        start = src;
+    }
+
+    Router *router = lineRef->router();
+    if (router->RubberBandRouting && (start != src))
+    {
+        COLA_ASSERT(router->IgnoreRegions == true);
+        
+        const PolyLine& currRoute = lineRef->route();
+        VertInf *last = NULL;
+        int rIndx = 0;
+        while (last != start)
+        {
+            const Point& pnt = currRoute.at(rIndx);
+            bool isShape = (rIndx > 0);
+            VertID vID(pnt.id, isShape, pnt.vn);
+
+#ifdef PATHDEBUG
+            db_printf("/// %d %d %d\n", pnt.id, (int) isShape, pnt.vn);
+#endif
+            VertInf *curr = router->vertices.getVertexByID(vID);
+            COLA_ASSERT(curr != NULL);
+
+            Node = ANode(curr, timestamp++);
+            if (!last)
+            {
+                Node.g = 0;
+                Node.h = estimatedCost(lineRef, NULL, Node.inf->point, 
+                        tar->point);
+                Node.f = Node.g + Node.h;
+            }
+            else
+            {
+                double edgeDist = dist(BestNode.inf->point, curr->point);
+
+                Node.g = BestNode.g + cost(lineRef, edgeDist, BestNode.inf, 
+                        Node.inf, DONE, BestNode.prevIndex);
+
+                // Calculate the Heuristic.
+                Node.h = estimatedCost(lineRef, &(BestNode.inf->point),
+                        Node.inf->point, tar->point);
+
+                // The A* formula
+                Node.f = Node.g + Node.h;
+                
+                // Point parent to last BestNode (pushed onto DONE)
+                Node.prevIndex = DONE.size() - 1;
+            }
+
+            if (curr != start)
+            {
+                BestNode = Node;
+
+                DONE.push_back(BestNode);
+            }
+            else
+            {
+                PENDING.push_back(Node);
+            }
 
-    // Create the start node
-    Node = ANode(src);
-    Node.g = 0;
-    Node.h = dist(Node.inf->point, tar->point);
-    Node.f = Node.g + Node.h;
-    // Set a null parent, so cost function knows this is the first segment.
-    Node.pp = NULL;
+            rIndx++;
+            last = curr;
+        }
+    }
+    else
+    {
+        // Create the start node
+        Node = ANode(src, timestamp++);
+        Node.g = 0;
+        Node.h = estimatedCost(lineRef, NULL, Node.inf->point, tar->point);
+        Node.f = Node.g + Node.h;
+        // Set a null parent, so cost function knows this is the first segment.
+
+        // Populate the PENDING container with the first location
+        PENDING.push_back(Node);
+    }
+
+    tar->pathNext = NULL;
 
-    // Populate the PENDING container with the first location
-    PENDING.push_back(Node);
     // Create a heap from PENDING for sorting
     using std::make_heap; using std::push_heap; using std::pop_heap;
     make_heap( PENDING.begin(), PENDING.end() );
 
     while (!PENDING.empty())
     {
-        // Ascending sort based on overloaded operators below
-        sort_heap(PENDING.begin(), PENDING.end());
-
-        // Set the Node with lowest f value to BESTNODE
+        // Set the Node with lowest f value to BESTNODE.
+        // Since the ANode operator< is reversed, the head of the
+        // heap is the node with the lowest f value.
         BestNode = PENDING.front();
 
         // Pop off the heap.  Actually this moves the
@@ -427,38 +505,118 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
         // is not actually removed since the pop is to
         // the heap and not the container.
         pop_heap(PENDING.begin(), PENDING.end());
-
-
         // Remove node from right (the value we pop_heap'd)
         PENDING.pop_back();
 
         // Push the BestNode onto DONE
-        BestNode.inf->pathNext = BestNode.pp;
         DONE.push_back(BestNode);
 
+        VertInf *prevInf = (BestNode.prevIndex >= 0) ?
+                DONE[BestNode.prevIndex].inf : NULL;
 #if 0
-        printf("Considering... ");
-        BestNode.ID->print(stdout);
-        printf(" - g: %3.1f h: %3.1f f: %3.1f back: ", BestNode.g, BestNode.h,
-                BestNode.f);
-        BestNode.pp.print(stdout);
-        printf("\n");
+        db_printf("Considering... ");
+        db_printf(" %g %g  ", BestNode.inf->point.x, BestNode.inf->point.y);
+        BestNode.inf->id.db_print();
+        db_printf(" - g: %3.1f h: %3.1f back: ", BestNode.g, BestNode.h);
+        if (prevInf)
+        {
+            db_printf(" %g %g", prevInf->point.x, prevInf->point.y);
+            //prevInf->id.db_print();
+        }
+        db_printf("\n");
+#endif
+
+#if defined(ASTAR_DEBUG)
+        if (router->avoid_screen)
+        {
+            int canx = 151;
+            int cany = 55;
+            int radius = 5;
+            ANode curr;
+            for (curr = BestNode; curr.prevIndex >= 0; 
+                    curr = DONE[curr.prevIndex])
+            {
+                filledCircleRGBA(router->avoid_screen, 
+                        (int) curr.inf->point.x + canx,
+                        (int) curr.inf->point.y + cany, 
+                        radius, 0, 0, 255, 128);
+            }
+            filledCircleRGBA(router->avoid_screen, 
+                    (int) BestNode.inf->point.x + canx,
+                    (int) BestNode.inf->point.y + cany, 
+                    radius, 255, 0, 0, 255);
+
+            SDL_Flip(router->avoid_screen);
+            //SDL_Delay(500);
+
+            filledCircleRGBA(router->avoid_screen, 
+                    (int) BestNode.inf->point.x + canx,
+                    (int) BestNode.inf->point.y + cany, 
+                    radius, 255, 255, 255, 255);
+            filledCircleRGBA(router->avoid_screen, 
+                    (int) BestNode.inf->point.x + canx,
+                    (int) BestNode.inf->point.y + cany, 
+                    radius, 0, 255, 0, 128);
+            for (curr = BestNode; curr.prevIndex >= 0; 
+                    curr = DONE[curr.prevIndex])
+            {
+                filledCircleRGBA(router->avoid_screen, 
+                        (int) curr.inf->point.x + canx,
+                        (int) curr.inf->point.y + cany, 
+                        radius, 255, 255, 255, 255);
+                filledCircleRGBA(router->avoid_screen, 
+                        (int) curr.inf->point.x + canx,
+                        (int) curr.inf->point.y + cany, 
+                        radius, 0, 255, 0, 128);
+            }
+        }
 #endif
 
         // If at destination, break and create path below
         if (BestNode.inf == tar)
         {
+#ifdef PATHDEBUG
+            db_printf("Cost: %g\n", BestNode.f);
+#endif
             //bPathFound = true; // arrived at destination...
+            
+            // Correct all the pathNext pointers.
+            ANode curr;
+            int currIndex = DONE.size() - 1;
+            for (curr = BestNode; curr.prevIndex > 0; 
+                    curr = DONE[curr.prevIndex])
+            {
+                COLA_ASSERT(curr.prevIndex < currIndex);   
+                curr.inf->pathNext = DONE[curr.prevIndex].inf;
+                currIndex = curr.prevIndex;
+            }
+            // Check that we've gone through the complete path.
+            COLA_ASSERT(curr.prevIndex == 0);
+            // Fill in the final pathNext pointer.
+            curr.inf->pathNext = DONE[curr.prevIndex].inf;
+
             break;
         }
 
         // Check adjacent points in graph
-        EdgeInfList& visList = BestNode.inf->visList;
-        EdgeInfList::iterator finish = visList.end();
-        for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
-                ++edge)
+        EdgeInfList& visList = (!isOrthogonal) ?
+                BestNode.inf->visList : BestNode.inf->orthogVisList;
+        if (isOrthogonal)
+        {
+            // We would like to explore in a structured way, 
+            // so sort the points in the visList...
+            CmpVisEdgeRotation compare(prevInf);
+            visList.sort(compare);
+        }
+        EdgeInfList::const_iterator finish = visList.end();
+        for (EdgeInfList::const_iterator edge = visList.begin(); 
+                edge != finish; ++edge)
         {
-            Node.inf = (*edge)->otherVert(BestNode.inf);
+            Node = ANode((*edge)->otherVert(BestNode.inf), timestamp++);
+
+            // Set the index to the previous ANode that we reached
+            // this ANode through (the last BestNode pushed onto DONE).
+            Node.prevIndex = DONE.size() - 1;
 
             // Only check shape verticies, or the tar endpoint.
             if (!(Node.inf->id.isShape) && (Node.inf != tar))
@@ -466,6 +624,15 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 continue;
             }
 
+            VertInf *prevInf = (BestNode.prevIndex >= 0) ?
+                    DONE[BestNode.prevIndex].inf : NULL;
+
+            // Don't bother looking at the segment we just arrived along.
+            if (prevInf && (prevInf == Node.inf))
+            {
+                continue;
+            }
+
             double edgeDist = (*edge)->getDist();
 
             if (edgeDist == 0)
@@ -473,31 +640,49 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 continue;
             }
 
-            VertInf *prevInf = BestNode.inf->pathNext;
+            if (!router->_orthogonalRouting &&
+                  (!router->RubberBandRouting || (start == src)) && 
+                  (validateBendPoint(prevInf, BestNode.inf, Node.inf) == false))
+            {
+                // The bendpoint is not valid, i.e., is a zigzag corner, so...
+                continue;
+                // For RubberBand routing we want to allow these routes and
+                // unwind them later, otherwise instead or unwinding, paths
+                // can go the *really* long way round.
+            }
 
-            Node.g = BestNode.g + cost(lineRef, edgeDist, prevInf,
-                    BestNode.inf, Node.inf);
+            Node.g = BestNode.g + cost(lineRef, edgeDist, BestNode.inf, 
+                    Node.inf, DONE, BestNode.prevIndex);
 
             // Calculate the Heuristic.
-            Node.h = dist(Node.inf->point, tar->point);
+            Node.h = estimatedCost(lineRef, &(BestNode.inf->point),
+                    Node.inf->point, tar->point);
 
             // The A* formula
             Node.f = Node.g + Node.h;
-            // Point parent to last BestNode (pushed onto DONE)
-            Node.pp = BestNode.inf;
+
+#if 0
+            db_printf("-- Adding: %g %g  ", Node.inf->point.x, 
+                    Node.inf->point.y);
+            Node.inf->id.db_print();
+            db_printf(" - g: %3.1f h: %3.1f \n", Node.g, Node.h);
+#endif
 
             bNodeFound = false;
 
             // Check to see if already on PENDING
             for (unsigned int i = 0; i < PENDING.size(); i++)
             {
-                if (Node.inf == PENDING.at(i).inf)
-                {   // If already on PENDING
-                    if (Node.g < PENDING.at(i).g)
+                ANode& ati = PENDING.at(i);
+                if ((Node.inf == ati.inf) &&
+                        (DONE[Node.prevIndex].inf == DONE[ati.prevIndex].inf))
+                {
+                    // If already on PENDING
+                    if (Node.g < ati.g)
                     {
-                        PENDING.at(i).g = Node.g;
-                        PENDING.at(i).f = Node.g + PENDING.at(i).h;
-                        PENDING.at(i).pp = Node.pp;
+                        PENDING[i] = Node;
+
+                        make_heap( PENDING.begin(), PENDING.end() );
                     }
                     bNodeFound = true;
                     break;
@@ -508,15 +693,14 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 // Check to see if already on DONE
                 for (unsigned int i = 0; i < DONE.size(); i++)
                 {
-                    if (Node.inf == DONE.at(i).inf)
+                    ANode& ati = DONE.at(i);
+                    if ((Node.inf == ati.inf) && 
+                            (DONE[Node.prevIndex].inf == DONE[ati.prevIndex].inf))
                     {
                         // If on DONE, Which has lower gone?
-                        if (Node.g < DONE.at(i).g)
+                        if (Node.g < ati.g)
                         {
-                            DONE.at(i).g = Node.g;
-                            DONE.at(i).f = Node.g + DONE.at(i).h;
-                            DONE.at(i).pp = Node.pp;
-                            DONE.at(i).inf->pathNext = Node.pp;
+                            DONE[i] = Node;
                         }
                         bNodeFound = true;
                         break;
@@ -530,26 +714,24 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 PENDING.push_back(Node);
                 // Push NewNode onto heap
                 push_heap( PENDING.begin(), PENDING.end() );
-                // Re-Assert heap, or will be short by one
-                make_heap( PENDING.begin(), PENDING.end() );
 
 #if 0
+                using std::cout; using std::endl;
                 // Display PENDING and DONE containers (For Debugging)
                 cout << "PENDING:   ";
-                for (int i = 0; i < PENDING.size(); i++)
+                for (unsigned int i = 0; i < PENDING.size(); i++)
                 {
-                    cout << PENDING.at(i).x << "," << PENDING.at(i).y << ",";
-                    cout << PENDING.at(i).g << "," << PENDING.at(i).h << "  ";
+                    cout << PENDING.at(i).g << "," << PENDING.at(i).h << ",";
+                    cout << PENDING.at(i).inf << "," << PENDING.at(i).pp << "  ";
                 }
                 cout << endl;
                 cout << "DONE:   ";
-                for (int i = 0; i < DONE.size(); i++)
+                for (unsigned int i = 0; i < DONE.size(); i++)
                 {
-                    cout << DONE.at(i).x << "," << DONE.at(i).y << ",";
-                    cout << DONE.at(i).g << "," << DONE.at(i).h << "  ";
+                    cout << DONE.at(i).g << "," << DONE.at(i).h << ",";
+                    cout << DONE.at(i).inf << "," << DONE.at(i).pp << "  ";
                 }
                 cout << endl << endl;
-                int ch = _getch();
 #endif
             }
         }
@@ -559,75 +741,52 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
 
 // Returns the best path for the connector referred to by lineRef.
 //
-// The path encoded in the pathNext links in each of the VerInfs
+// The path encoded in the pathNext links in each of the VertInfs
 // backwards along the path, from the tar back to the source.
 //
 void makePath(ConnRef *lineRef, bool *flag)
 {
+    bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);
     Router *router = lineRef->router();
     VertInf *src = lineRef->src();
     VertInf *tar = lineRef->dst();
+    VertInf *start = lineRef->start();
 
-    // If the connector hates crossings then we want to examine direct paths:
-    bool examineDirectPath = lineRef->doesHateCrossings();
-    
     // TODO: Could be more efficient here.
-    EdgeInf *directEdge = EdgeInf::existingEdge(src, tar);
-    if (!(router->IncludeEndpoints) && directVis(src, tar))
-    {
-        Point p = src->point;
-        Point q = tar->point;
-
-        assert(directEdge == NULL);
-
-        directEdge = new EdgeInf(src, tar);
-        tar->pathNext = src;
-        directEdge->setDist(dist(p, q));
-        directEdge->addConn(flag);
-
-        return;
-    }
-    else if (router->IncludeEndpoints && directEdge &&
-            (directEdge->getDist() > 0) && !examineDirectPath)
+    if (isOrthogonal)
     {
-        tar->pathNext = src;
-        directEdge->addConn(flag);
+        aStarPath(lineRef, src, tar, start);
     }
-    else
+    else // if (!isOrthogonal)
     {
-        // Mark the path endpoints as not being able to see
-        // each other.  This is true if we are here.
-        if (!(router->IncludeEndpoints) && router->InvisibilityGrph)
-        {
-            if (!directEdge)
-            {
-                directEdge = new EdgeInf(src, tar);
-            }
-            directEdge->addBlocker(0);
-        }
-
-        if (router->UseAStarSearch)
+        EdgeInf *directEdge = EdgeInf::existingEdge(src, tar);
+        // If the connector hates crossings or there are clusters present,
+        // then we want to examine direct paths:
+        bool examineDirectPath = lineRef->doesHateCrossings() || 
+                !(router->clusterRefs.empty());
+        
+        if ((start == src) && directEdge && (directEdge->getDist() > 0) && 
+                !examineDirectPath)
         {
-            aStarPath(lineRef, src, tar);
+            tar->pathNext = src;
+            directEdge->addConn(flag);
         }
         else
         {
-            dijkstraPath(lineRef, src, tar);
+            aStarPath(lineRef, src, tar, start);
         }
+    }
 
 #if 0
-        PointMap::iterator t;
-        for (VertInf *t = vertices.connsBegin(); t != vertices.end();
-                t = t->lstNext)
-        {
-
-            t->id.print();
-            printf(" -> ");
-            t->pathNext->id.print();
-            printf("\n");
-        }
-#endif
+    for (VertInf *t = vertices.connsBegin(); t != vertices.end();
+            t = t->lstNext)
+    {
+        t->id.db_print();
+        db_printf(" -> ");
+        t->pathNext->id.db_print();
+        db_printf("\n");
     }
+#endif
 }
 
 
diff --git a/src/libavoid/makepath.h b/src/libavoid/makepath.h
index 4d68a01e3..b40bfbc3d 100644
--- a/src/libavoid/makepath.h
+++ b/src/libavoid/makepath.h
@@ -2,24 +2,27 @@
  * 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-2008  Monash University
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_MAKEPATH_H
 #define AVOID_MAKEPATH_H
 
diff --git a/src/libavoid/polyutil.cpp b/src/libavoid/polyutil.cpp
deleted file mode 100644
index 1b4b0c619..000000000
--- a/src/libavoid/polyutil.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * 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>
- *
- * 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 <cassert>
-#include <cstdlib>
-
-#include "libavoid/polyutil.h"
-#include "libavoid/geomtypes.h"
-#include "libavoid/vertices.h"
-#include "libavoid/shape.h"
-
-namespace Avoid {
-
-
-Polygn newPoly(int size)
-{
-    Polygn newpoly;
-
-    newpoly.id = 0;
-    newpoly.pn = size;
-    newpoly.ps = (Point *) calloc(size, sizeof(Point));
-    if (!newpoly.ps)
-    {
-        fprintf(stderr,
-                "Error: Unable to allocate Point array in Avoid::newPoly\n");
-        abort();
-    }
-    return newpoly;
-}
-
-
-Polygn copyPoly(Polygn poly)
-{
-    Polygn newpoly = newPoly(poly.pn);
-
-    newpoly.id = poly.id;
-    for (int i = 0; i < poly.pn; i++)
-    {
-        newpoly.ps[i] = poly.ps[i];
-    }
-    return newpoly;
-}
-
-
-Polygn copyPoly(ShapeRef *shape)
-{
-    Polygn poly = shape->poly();
-    Polygn newpoly = newPoly(poly.pn);
-
-    newpoly.id = poly.id;
-    for (int i = 0; i < poly.pn; i++)
-    {
-        newpoly.ps[i] = poly.ps[i];
-    }
-    return newpoly;
-}
-
-
-void freePoly(Polygn& poly)
-{
-    std::free(poly.ps);
-}
-
-
-void freePtrPoly(Polygn *poly)
-{
-    std::free(poly->ps);
-    std::free(poly);
-}
-
-
-}
-
diff --git a/src/libavoid/polyutil.h b/src/libavoid/polyutil.h
deleted file mode 100644
index 9340df5f4..000000000
--- a/src/libavoid/polyutil.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * 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>
- *
- * 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 "libavoid/geometry.h"
-#include "libavoid/shape.h"
-
-#ifndef AVOID_POLYUTIL_H
-#define AVOID_POLYUTIL_H
-
-namespace Avoid {
-
-
-extern Polygn newPoly(int size);
-extern Polygn copyPoly(Polygn);
-extern Polygn copyPoly(ShapeRef *shape);
-extern void freePoly(Polygn&);
-extern void freePtrPoly(Polygn *argpoly);
-
-
-}
-
-#endif
diff --git a/src/libavoid/region.cpp b/src/libavoid/region.cpp
deleted file mode 100644
index 5a46d7cbb..000000000
--- a/src/libavoid/region.cpp
+++ /dev/null
@@ -1,858 +0,0 @@
-/*
- * 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>
- *
- * 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 <stdio.h>
-#include <cstdlib>
-#include <algorithm>
-#include <cassert>
-#include <cmath>
-
-#include "libavoid/shape.h"
-#include "libavoid/region.h"
-
-
-namespace Avoid {
-
-Region *centerRegion = NULL;
-
-static const BBox screenBBox =
-        {Point(-INFINITY, -INFINITY), Point(INFINITY, INFINITY)};
-
-
-Region::Region()
-    : _bbox(screenBBox)
-    , _left(NULL), _right(NULL), _up(NULL), _down(NULL)
-{
-    _blocks.clear();
-}
-
-
-Region::Region(double x1, double y1, double x2, double y2)
-    : _left(NULL), _right(NULL), _up(NULL), _down(NULL)
-{
-    _bbox.a.x = x1;
-    _bbox.a.y = y1;
-    _bbox.b.x = x2;
-    _bbox.b.y = y2;
-
-    _blocks.clear();
-}
-
-
-static const unsigned int R_INSIDE = 0;
-static const unsigned int R_LEFT   = 1;
-static const unsigned int R_LEDGE  = 2;
-static const unsigned int R_RIGHT  = 4;
-static const unsigned int R_REDGE  = 8;
-static const unsigned int R_ABOVE  = 16;
-static const unsigned int R_TEDGE  = 32;
-static const unsigned int R_BELOW  = 64;
-static const unsigned int R_BEDGE  = 128;
-
-static const unsigned int R_NONE   = R_INSIDE;
-static const unsigned int R_UP     = R_ABOVE;
-static const unsigned int R_DOWN   = R_BELOW;
-static const unsigned int R_HORI   = R_LEFT | R_RIGHT;
-static const unsigned int R_VERT   = R_UP | R_DOWN;
-
-
-static void printBBox(const char *label, const BBox &bbox)
-{
-    if (label)
-    {
-        printf("%s: ", label);
-    }
-    printf("(%.2f, %.2f)-(%.2f, %.2f)\n", bbox.a.x, bbox.a.y,
-            bbox.b.x, bbox.b.y);
-}
-
-
-bool Region::overlapCheck(BBox& bbox, unsigned int& p)
-{
-    p = R_INSIDE;
-    Point& a = bbox.a;
-    Point& b = bbox.b;
-    Point& r = _bbox.a;
-    Point& s = _bbox.b;
-    
-    if (s.x <= a.x)
-    {
-        // Wholly right.
-        p = R_RIGHT;
-        return false;
-    }
-    else if (r.x >= b.x)
-    {
-        // Wholly left.
-        p = R_LEFT;
-        return false;
-    }
-    
-    if (s.y <= a.y)
-    {
-        // Wholly below.
-        p = R_BELOW;
-        return false;
-    }
-    else if (r.y >= b.y)
-    {
-        // Wholly above.
-        p = R_ABOVE;
-        return false;
-    }
-    
-    if (a.y == r.y)
-    {
-        // Shared top edge.
-        p |= R_TEDGE;
-    }
-    else if (a.y < r.y)
-    {
-        // Need to split above.
-        p |= R_ABOVE;
-    }
-    
-    if (b.y == s.y)
-    {
-        // Shared bottom edge.
-        p |= R_BEDGE;
-    }
-    else if (b.y > s.y)
-    {
-        // Need to split below.
-        p |= R_BELOW;
-    }
-
-    if (a.x == r.x)
-    {
-        // Shared left edge.
-        p |= R_LEDGE;
-    }
-    else if (a.x < r.x)
-    {
-        // Need to split left.
-        p |= R_LEFT;
-    }
-
-    if (b.x == s.x)
-    {
-        // Shared right edge.
-        p |= R_REDGE;
-    }
-    else if (b.x > s.x)
-    {
-        // Need to split right.
-        p |= R_RIGHT;
-    }
-    
-    return true;
-}
-
-
-void Region::getBBox(BBox& bb)
-{
-    bb.a = _bbox.a;
-    bb.b = _bbox.b;
-}
-
-
-Region *Region::up(void)
-{
-    return _up;
-}
-
-
-Region *Region::down(void)
-{
-    return _down;
-}
-
-
-Region *Region::left(void)
-{
-    return _left;
-}
-
-
-Region *Region::right(void)
-{
-    return _right;
-}
-
-
-bool Region::isBlock(void)
-{
-    return !(_blocks.empty());
-}
-
-
-void Region::initialSplit(BBox& bbox, unsigned int pos, unsigned int& shapeId)
-{
-    Point& n1 = bbox.a;
-    Point& n2 = bbox.b;
-    Region *newR = this;
-    Region *left, *right, *top, *bottom;
-    
-    if (pos == R_INSIDE)
-    {
-        split(n2.y, R_HORI);
-        split(n2.x, R_VERT);
-        newR = split(n1.y, R_HORI);
-        newR = newR->split(n1.x, R_VERT);
-        
-        printf("%p - list %d add %d\n", newR,
-                (int) newR->_blocks.size(), shapeId);
-        newR->_blocks.push_back((int) shapeId);
-        newR->_blocks.sort();
-        newR->_blocks.unique();
-    }
-    else
-    {
-        Region *tar = NULL;
-        tar = newR->findRegion(n1.x, R_VERT);
-        if (pos & R_LEFT)
-        {
-            if (n1.x == tar->_bbox.a.x)
-            {
-                newR = tar->_right;
-            }
-            else if (n1.x == tar->_bbox.b.x)
-            {
-                newR = tar;
-            }
-            else
-            {
-                newR = tar->split(n1.x, R_VERT);
-            }
-        }
-        else if (!(pos & R_LEDGE))
-        {
-            newR = tar->split(n1.x, R_VERT);
-        }
-        left = newR;
-        
-        tar = left->findRegion(n1.y, R_HORI);
-        if (pos & R_ABOVE)
-        {
-            if (n1.y == tar->_bbox.a.y)
-            {
-                newR = tar->_down;
-            }
-            else if (n1.y == tar->_bbox.b.y)
-            {
-                newR = tar;
-            }
-            else
-            {
-                newR = tar->split(n1.y, R_HORI);
-            }
-        }
-        else if (!(pos & R_TEDGE))
-        {
-            newR = tar->split(n1.y, R_HORI);
-        }
-        top = newR;
-        
-        right = newR;
-        tar = newR->findRegion(n2.x, R_VERT);
-        if (pos & R_RIGHT)
-        {
-        
-            if (n2.x == tar->_bbox.a.x)
-            {
-                right = tar->_left;
-            }
-            else if (n2.x == tar->_bbox.b.x)
-            {
-                right = tar;
-            }
-            else
-            {
-                tar->split(n2.x, R_VERT);
-                right = tar;
-            }
-        }
-        else if (!(pos & R_REDGE))
-        {
-            tar->split(n2.x, R_VERT);
-            right = tar;
-        }
-
-        bottom = right;
-        tar = right->findRegion(n2.y, R_HORI);
-        if (pos & R_BELOW)
-        {
-            if (n2.y == tar->_bbox.a.y)
-            {
-                bottom = tar->_up;
-            }
-            else if (n2.y == tar->_bbox.b.y)
-            {
-                bottom = tar;
-            }
-            else
-            {
-                tar->split(n2.y, R_HORI);
-                bottom = tar;
-            }
-        }
-        else if (!(pos & R_BEDGE))
-        {
-            tar->split(n2.y, R_HORI);
-            bottom = tar;
-        } 
-
-        // top is actually top-left, and bottom is bottom-right.
-        Region *curr = top, *cptr = NULL;
-        while (curr->_bbox.b.y <= bottom->_bbox.b.y)
-        {
-            cptr = curr;
-            while (cptr->_bbox.b.x <= bottom->_bbox.b.x)
-            {
-                printf("%p - list %d add %d\n", cptr,
-                        (int) cptr->_blocks.size(), shapeId);
-                cptr->_blocks.push_back((int) shapeId);
-                cptr->_blocks.sort();
-                cptr->_blocks.unique();
-
-                cptr = cptr->_right;
-            }
-
-            curr = curr->_down;
-        }
-    }
-}
-
-
-// Returns the region containing the value 'pos' in the direction 'dir'.
-// Thus, if looking for the x value 55, you would pass R_VERT as 'dir'.
-// 'forMerge' specifies that the left or top block of a pair of regions
-// with the split value of 'pos' should be returned.
-Region *Region::findRegion(double pos, unsigned int dir, const bool forMerge)
-{
-    Region *curr = this;
-
-    if (dir & R_VERT)
-    {
-        while (pos > curr->_bbox.b.x)
-        {
-            curr = curr->_right;
-        }
-        while (pos < curr->_bbox.a.x)
-        {
-            curr = curr->_left;
-        }
-        if (forMerge)
-        {
-            if (pos == curr->_bbox.a.x)
-            {
-                curr = curr->_left;
-            }
-            if (pos != curr->_bbox.b.x)
-            {
-                // 'pos' is not on the boundary.
-                return NULL;
-            }
-        }
-    }
-    else if (dir & R_HORI)
-    {
-        while (pos > curr->_bbox.b.y)
-        {
-            curr = curr->_down;
-        }
-        while (pos < curr->_bbox.a.y)
-        {
-            curr = curr->_up;
-        }
-        if (forMerge)
-        {
-            if (pos == curr->_bbox.a.y)
-            {
-                curr = curr->_up;
-            }
-            if (pos != curr->_bbox.b.y)
-            {
-                // 'pos' is not on the boundary.
-                return NULL;
-            }
-        }
-    }
-    return curr;
-}
-
-
-Region *Region::split(double pos, unsigned int dir)
-{
-    Region *newR = NULL;
-    bool first = true;
-
-    if (dir & R_VERT)
-    {
-        newR = splitDir(pos, R_UP, first);
-        if (_down) _down->splitDir(pos, R_DOWN);
-    }
-    else if (dir & R_HORI)
-    {
-        newR = splitDir(pos, R_RIGHT, first);
-        if (_left)  _left->splitDir(pos, R_LEFT);
-    }
-    return newR;
-}
-
-
-void Region::merge(unsigned int dir)
-{
-    bool first = true;
-
-    if (dir & R_VERT)
-    {
-        mergeDir(R_UP, first);
-        if (_down) _down->mergeDir(R_DOWN);
-    }
-    else if (dir & R_HORI)
-    {
-        mergeDir(R_RIGHT, first);
-        if (_left)  _left->mergeDir(R_LEFT);
-    }
-}
-
-
-void Region::mergeRegion(Region *src)
-{
-    assert(src != NULL);
-
-    if (src == _left)
-    {
-        pairHor(src->_left, this);
-        _bbox.a.x = src->_bbox.a.x;
-    }
-    else if (src == _right)
-    {
-        pairHor(this, src->_right);
-        _bbox.b.x = src->_bbox.b.x;
-    }
-    else if (src == _up)
-    {
-        pairVer(src->_up, this);
-        _bbox.a.y = src->_bbox.a.y;
-    }
-    else if (src == _down)
-    {
-        pairVer(this, src->_down);
-        _bbox.b.y = src->_bbox.b.y;
-    }
-    else
-    {
-        fprintf(stderr, "Error: Avoid::Region::merge(): "
-                "Argument not adjoining region.\n");
-        abort();
-    }
-    mergeAttributes(src);
-    printf("DEL %p\n", src);
-    delete src;
-}
-
-
-Region *Region::splitDir(double pos, unsigned int dir, bool first)
-{
-    Point& o1 = _bbox.a;
-    Point& o2 = _bbox.b;
-
-    Region *newR = NULL;
-    
-    if (dir & R_VERT)
-    {
-        assert(pos > _bbox.a.x);
-        assert(pos < _bbox.b.x);
-        
-        // Vertical recursion:
-       
-        // Create new block.
-        Region *r  = new Region(pos, o1.y, o2.x, o2.y);
-        printf("NEW %p\n", r);
-        r->copyAttributes(this);
-
-        Region *o_up    = _up;
-        Region *o_down  = _down;
-
-        // Resize old block.
-        o2.x = pos;
-
-        pairHor(r, _right);
-        pairHor(this, r);
-        
-        if (dir & R_UP)
-        {
-            if (!first)  pairVer(r, _down->_right);
-        
-            if (o_up)    o_up->splitDir(pos, R_UP);
-        }
-        else if (dir & R_DOWN)
-        {
-            if (!first)  pairVer(_up->_right, r);
-        
-            if (o_down)  o_down->splitDir(pos, R_DOWN);
-        }
-        newR = r;
-    }
-    else if (dir & R_HORI)
-    {
-        // Vertical recursion:
-
-        // Create new block.
-        Region *b  = new Region(o1.x, pos, o2.x, o2.y);
-        printf("NEW %p\n", b);
-        b->copyAttributes(this);
-
-        Region *o_left  = _left;
-        Region *o_right = _right;
-    
-        // Resize old block.
-        o2.y = pos;
-        
-        pairVer(b, _down);
-        pairVer(this, b);
-
-        if (dir & R_LEFT)
-        {
-            if (!first)  pairHor(b, _right->_down);
-        
-            if (o_left)  o_left->splitDir(pos, R_LEFT);
-        }
-        else if (dir & R_RIGHT)
-        {
-            if (!first)  pairHor(_left->_down, b);
-        
-            if (o_right) o_right->splitDir(pos, R_RIGHT);
-        }
-        newR = b;
-    }
-    return newR;
-}
-
-
-void Region::mergeDir(unsigned int dir, bool first)
-{
-    if (dir & R_VERT)
-    {
-        assert(_right != NULL);
-
-        mergeRegion(_right);
-
-        if (dir & R_UP)
-        {
-            if (_up)    _up->mergeDir(dir, R_UP);
-        }
-        else if (dir & R_DOWN)
-        {
-            if (_down)  _down->mergeDir(dir, R_DOWN);
-        }
-    }
-    else if (dir & R_HORI)
-    {
-        assert(_down != NULL);
-
-        mergeRegion(_down);
-
-        if (dir & R_LEFT)
-        {
-            if (_left)  _left->mergeDir(dir, R_LEFT);
-        }
-        else if (dir & R_RIGHT)
-        {
-            if (_right) _right->mergeDir(dir, R_RIGHT);
-        }
-    }
-}
-
-
-void Region::copyAttributes(Region *src)
-{
-    _blocks = src->_blocks;
-}
-
-
-void Region::mergeAttributes(Region *src)
-{
-    _blocks.merge(src->_blocks);
-    _blocks.sort();
-    _blocks.unique();
-}
-
-
-//---------------------------
-// Static member functions:
-
-
-void Region::pairHor(Region *l, Region *r)
-{
-    if (l)  l->_right = r;
-    if (r)  r->_left  = l;
-}
-
-
-void Region::pairVer(Region *a, Region *b)
-{
-    if (a)  a->_down = b;
-    if (b)  b->_up   = a;
-}
-
-
-void Region::addShape(ShapeRef *shape)
-{
-    if (!centerRegion)
-    {
-        // Add new default region.
-        centerRegion = new Region();
-        printf("NEW %p\n", centerRegion);
-    }
-    BBox bbox;
-    // Get bounding box for added shape.
-    shape->boundingBox(bbox);
-    printBBox("Add", bbox);
-    
-    unsigned int shapeId = shape->id();
-
-    // Find starting point for overlap
-    Region *curr = centerRegion;
-    unsigned int pos = R_INSIDE;
-    while (!(curr->overlapCheck(bbox, pos)))
-    {
-        if (pos & R_LEFT)
-        {
-            curr = curr->_left;
-        }
-        else if (pos & R_RIGHT)
-        {
-            curr = curr->_right;
-        }
-        else if (pos & R_ABOVE)
-        {
-            curr = curr->_up;
-        }
-        else if (pos & R_BELOW)
-        {
-            curr = curr->_down;
-        }
-    }
-    
-    curr->initialSplit(bbox, pos, shapeId);
-    centerRegion = curr;
-}
-
-
-void Region::removeShape(ShapeRef *shape)
-{
-    const bool forMerge = true;
-
-    BBox bbox;
-    // Get bounding box for added shape.
-    shape->boundingBox(bbox);
-    printBBox("Remove", bbox);
-
-    unsigned int shapeId = shape->id();
-    
-    Region *aboveTop = centerRegion->findRegion(bbox.a.y, R_HORI, forMerge);
-    Region *aboveBottom = aboveTop->findRegion(bbox.b.y, R_HORI, forMerge);
-    Region *leftOfLeft = aboveBottom->findRegion(bbox.a.x, R_VERT, forMerge);
-    Region *leftOfRight = leftOfLeft->findRegion(bbox.b.x, R_VERT, forMerge);
-    
-    assert(aboveTop    != NULL);
-    assert(aboveBottom != NULL);
-    assert(leftOfLeft  != NULL);
-    assert(leftOfRight != NULL);
-
-    // Find Top Left.
-    Region *topLeft = leftOfLeft->_right;
-    while (topLeft->_bbox.a.y != aboveTop->_bbox.b.y)
-    {
-        topLeft = topLeft->_up;
-    }
-    
-    // Find Bottom Right.
-    Region *botRight = leftOfRight;
-    while (botRight->_bbox.b.y != aboveBottom->_bbox.b.y)
-    {
-        botRight = botRight->_down;
-    }
-    
-    // Clear blocking flag.
-    Region *curr = topLeft, *cptr = NULL;
-    while (curr->_bbox.b.y <= botRight->_bbox.b.y)
-    {
-        cptr = curr;
-        while (cptr->_bbox.b.x <= botRight->_bbox.b.x)
-        {
-            ShapeList& blocks = cptr->_blocks;
-            
-            assert(std::find(blocks.begin(), blocks.end(), (int) shapeId) !=
-                    blocks.end());
-            printf("%p - list %d remove %d\n", cptr,
-                    (int) blocks.size(), shapeId);
-            cptr->_blocks.remove((int) shapeId);
-
-            cptr = cptr->_right;
-        }
-
-        curr = curr->_down;
-    }
-    
-    // These two are safe since they don't invalidate the pointers
-    // to the regions that are inside the shape.
-    if (aboveBottom->safeToMerge(R_HORI))
-    {
-        aboveBottom->merge(R_HORI);
-    }
-    if (leftOfRight->safeToMerge(R_VERT))
-    {
-        leftOfRight->merge(R_VERT);
-    }
-    
-    // Be a bit more careful with these.
-    double leftX = leftOfLeft->_bbox.b.x;
-    if (aboveTop->safeToMerge(R_HORI))
-    {
-        aboveTop->merge(R_HORI);
-    }
-    // leftOfLeft may have been freed, so look for the new block at
-    // that position.
-    leftOfLeft = aboveTop->findRegion(leftX, R_VERT, forMerge);
-    assert(leftOfLeft);
-    if (leftOfLeft->safeToMerge(R_VERT))
-    {
-        leftOfLeft->merge(R_VERT);
-    }
-}
-
-
-bool Region::safeToMerge(unsigned int dir)
-{
-    bool unsafe = false;
-
-    if (dir == R_HORI)
-    {
-        printf("safeToMerge? y = %.3f... ", _bbox.b.y); 
-        unsafe |= unsafeToMerge(R_RIGHT);
-        if (_left)  unsafe |= _left->unsafeToMerge(R_LEFT);
-    }
-    else if (dir == R_VERT)
-    {
-        printf("safeToMerge? x = %.3f... ", _bbox.b.x); 
-        unsafe |= unsafeToMerge(R_DOWN);
-        if (_up)  unsafe |= _up->unsafeToMerge(R_UP);
-    }
-    printf("%s.\n", (unsafe) ? "no" : "yes");
-        
-    return !unsafe;
-}
-
-
-bool Region::unsafeToMerge(unsigned int dir)
-{
-    bool unsafe = false;
-    
-    if (dir & R_HORI)
-    {
-        // If they are not the same on both sides then we can merge.
-        if (_blocks != _down->_blocks)
-        {
-            printf("\n  _blocks:\n    ");
-            for (ShapeList::iterator i = _blocks.begin(); i != _blocks.end();
-                    ++i)
-            {
-                printf("%d ", *i);
-            }
-            printf("\n  _down->_blocks:\n    ");
-            for (ShapeList::iterator i = _down->_blocks.begin();
-                    i != _down->_blocks.end(); ++i)
-            {
-                printf("%d ", *i);
-            }
-            unsafe |= true;
-            printf("\n");
-        }
-
-        if ((dir == R_LEFT) && _left)
-        {
-            unsafe |= _left->unsafeToMerge(dir);
-        }
-        else if ((dir == R_RIGHT) && _right)
-        {
-            unsafe |= _right->unsafeToMerge(dir);
-        }
-    }
-    else if (dir & R_VERT)
-    {
-        if (_blocks !=  _right->_blocks)
-        {
-            printf("\n  _blocks:\n    ");
-            for (ShapeList::iterator i = _blocks.begin(); i != _blocks.end();
-                    ++i)
-            {
-                printf("%d ", *i);
-            }
-            printf("\n  _right->_blocks:\n    ");
-            for (ShapeList::iterator i = _right->_blocks.begin();
-                    i != _right->_blocks.end(); ++i)
-            {
-                printf("%d ", *i);
-            }
-            unsafe |= true;
-            printf("\n");
-        }
-        
-        if ((dir == R_UP) && _up)
-        {
-            unsafe |= _up->unsafeToMerge(dir);
-        }
-        else if ((dir == R_DOWN) && _down)
-        {
-            unsafe |= _down->unsafeToMerge(dir);
-        }
-    }
-    return unsafe;
-        
-}
-
-
-Region *Region::getTopLeftRegion(void)
-{
-    Region *curr = centerRegion;
-
-    while (curr && (curr->_up || curr->_left))
-    {
-        if (curr->_up)
-        {
-            curr = curr->_up;
-        }
-        else
-        {
-            curr = curr->_left;
-        }
-    }
-    return curr;
-}
-
-
-}
-
diff --git a/src/libavoid/region.h b/src/libavoid/region.h
deleted file mode 100644
index 6a44ccb08..000000000
--- a/src/libavoid/region.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * 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>
- *
- * 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
- * 
-*/
-
-#ifndef AVOID_REGION_H
-#define AVOID_REGION_H
-
-#include <list>
-#include "libavoid/geomtypes.h"
-
-namespace Avoid {
-
-class ShapeRef;
-
-typedef std::list<int> ShapeList;
-
-
-class Region
-{
-    public:
-        Region();
-        Region(double x1, double y1, double x2, double y2);
-
-        bool overlapCheck(BBox& bbox, unsigned int& p);
-        void getBBox(BBox& bb);
-        Region *up(void);
-        Region *down(void);
-        Region *left(void);
-        Region *right(void);
-        void initialSplit(BBox& bbox, unsigned int pos, unsigned int& shapeId);
-        bool isBlock(void);
-        
-        static void pairHor(Region *l, Region *r);
-        static void pairVer(Region *a, Region *b);
-        static void addShape(ShapeRef *shape);
-        static void removeShape(ShapeRef *shape);
-        static Region *getTopLeftRegion(void);
-
-    private:
-        BBox _bbox;
-        Region *_left;
-        Region *_right;
-        Region *_up;
-        Region *_down;
-        ShapeList _blocks;
-        
-        Region *split(double pos, unsigned int dir); 
-        void merge(unsigned int dir);
-        void mergeRegion(Region *src); 
-        Region *findRegion(double pos, unsigned int dir,
-                const bool forMerge = false);
-        Region *splitDir(double pos, unsigned int dir, bool first = false); 
-        void mergeDir(unsigned int dir, bool first = false); 
-        void copyAttributes(Region *src);
-        void mergeAttributes(Region *src);
-        bool safeToMerge(unsigned int dir);
-        bool unsafeToMerge(unsigned int dir);
-};
-
-
-}
-
-#endif
diff --git a/src/libavoid/router.cpp b/src/libavoid/router.cpp
index df0bacd02..754c464b4 100644
--- a/src/libavoid/router.cpp
+++ b/src/libavoid/router.cpp
@@ -2,302 +2,667 @@
  * 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
  *
  * 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 <cstdlib>
+
+#include <algorithm>
+#include <cmath>
+
 #include "libavoid/shape.h"
 #include "libavoid/router.h"
 #include "libavoid/visibility.h"
 #include "libavoid/connector.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/debug.h"
-#include "libavoid/region.h"
-#include "math.h"
-
-//#define ORTHOGONAL_ROUTING
+#include "libavoid/orthogonal.h"
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
 
-static const unsigned int infoAdd = 1;
-static const unsigned int infoDel = 2;
-static const unsigned int infoMov = 3;
+enum ActionType {
+    ShapeMove,
+    ShapeAdd,
+    ShapeRemove,
+    ConnChange
+};
 
+typedef std::list<std::pair<unsigned int, ConnEnd> > ConnUpdateList;
 
-class MoveInfo {
+class ActionInfo {
     public:
-        MoveInfo(ShapeRef *s, Polygn *p, bool fM)
-            : shape(s)
-            , newPoly(copyPoly(*p))
-            , firstMove(fM)
-        { }
-        ~MoveInfo()
+        ActionInfo(ActionType t, ShapeRef *s, const Polygon& p, bool fM)
+            : type(t),
+              objPtr(s),
+              newPoly(p),
+              firstMove(fM)
+        {
+            COLA_ASSERT(type == ShapeMove);
+        }
+        ActionInfo(ActionType t, ShapeRef *s)
+            : type(t),
+              objPtr(s)
+        {
+            COLA_ASSERT(type != ConnChange);
+        }
+        ActionInfo(ActionType t, ConnRef *c)
+            : type(t),
+              objPtr(c)
+        {
+            COLA_ASSERT(type == ConnChange);
+        }
+        ~ActionInfo()
+        {
+        }
+        ShapeRef *shape(void) const
+        {
+            COLA_ASSERT((type == ShapeMove) || (type == ShapeAdd) || 
+                    (type == ShapeRemove));
+            return (static_cast<ShapeRef *> (objPtr));
+        }
+        ConnRef *conn(void) const
+        {
+            COLA_ASSERT(type == ConnChange);
+            return (static_cast<ConnRef *> (objPtr));
+        }
+        bool operator==(const ActionInfo& rhs) const
+        {
+            return (type == rhs.type) && (objPtr == rhs.objPtr);
+        }
+        bool operator<(const ActionInfo& rhs) const
         {
-            freePoly(newPoly);
+            if (type != rhs.type)
+            {
+                return type < rhs.type;
+            }
+            return objPtr < rhs.objPtr;
         }
-        ShapeRef *shape;
-        Polygn newPoly;
+        ActionType type;
+        void *objPtr;
+        Polygon newPoly;
         bool firstMove;
+        ConnUpdateList conns;
 };
 
 
-
-Router::Router()
-    : PartialTime(false)
-    , SimpleRouting(false)
-    , segmt_penalty(0)
-    , angle_penalty(0)
-    , crossing_penalty(200)
-    // Algorithm options:
-    , UseAStarSearch(true)
-    , IgnoreRegions(true)
-    , SelectiveReroute(true)
-    , IncludeEndpoints(true)
-    , UseLeesAlgorithm(false)
-    , InvisibilityGrph(true)
-    , ConsolidateMoves(true)
-    , PartialFeedback(false)
-    // Instrumentation:
-    , st_checked_edges(0)
-#ifdef LINEDEBUG
-    , avoid_screen(NULL)
+Router::Router(const unsigned int flags)
+    : visOrthogGraph(true),
+      PartialTime(false),
+      SimpleRouting(false),
+      ClusteredRouting(true),
+      // Poly-line algorithm options:
+      IgnoreRegions(true),
+      UseLeesAlgorithm(true),
+      InvisibilityGrph(true),
+      // General algorithm options:
+      SelectiveReroute(true),
+      PartialFeedback(false),
+      RubberBandRouting(false),
+      // Instrumentation:
+      st_checked_edges(0),
+#ifdef LIBAVOID_SDL
+      avoid_screen(NULL),
 #endif
-{ }
+      _largestAssignedId(0),
+      _consolidateActions(true),
+      _orthogonalNudgeDistance(4.0),
+      // Mode options:
+      _polyLineRouting(false),
+      _orthogonalRouting(false),
+      _staticGraphInvalidated(true),
+      _inCrossingPenaltyReroutingStage(false)
+{
+    // At least one of the Routing modes must be set.
+    COLA_ASSERT(flags & (PolyLineRouting | OrthogonalRouting));
 
+    if (flags & PolyLineRouting)
+    {
+        _polyLineRouting = true;
+    }
+    if (flags & OrthogonalRouting)
+    {
+        _orthogonalRouting = true;
+    }
 
+    for (size_t p = 0; p < lastPenaltyMarker; ++p)
+    {
+        _routingPenalties[p] = 0.0;
+    }
+    _routingPenalties[clusterCrossingPenalty] = 4000;
+}
 
 
-void Router::addShape(ShapeRef *shape)
+Router::~Router()
 {
-    unsigned int pid = shape->id();
-    Polygn poly = shape->poly();
+    // Delete remaining connectors.
+    ConnRefList::iterator conn = connRefs.begin();
+    while (conn != connRefs.end())
+    {
+        db_printf("Deleting connector %u in ~Router()\n", (*conn)->id());
+        delete *conn;
+        conn = connRefs.begin();
+    }
 
-    adjustContainsWithAdd(poly, pid);
-    
-    // o  Check all visibility edges to see if this one shape
-    //    blocks them.
-    newBlockingShape(&poly, pid);
+    // Remove remaining shapes.
+    ShapeRefList::iterator shape = shapeRefs.begin();
+    while (shape != shapeRefs.end())
+    {
+        ShapeRef *shapePtr = *shape;
+        db_printf("Deleting shape %u in ~Router()\n", shapePtr->id());
+        if (shapePtr->isActive())
+        {
+            shapePtr->removeFromGraph();
+            shapePtr->makeInactive();
+        }
+        delete shapePtr;
+        shape = shapeRefs.begin();
+    }
+
+    // Cleanup orphaned orthogonal graph vertices.
+    destroyOrthogonalVisGraph();
+
+    COLA_ASSERT(connRefs.size() == 0);
+    COLA_ASSERT(shapeRefs.size() == 0);
+    COLA_ASSERT(visGraph.size() == 0);
+    COLA_ASSERT(invisGraph.size() == 0);
+}
 
-#ifdef ORTHOGONAL_ROUTING
-    Region::addShape(shape);
-#endif
 
-    // o  Calculate visibility for the new vertices.
-    if (UseLeesAlgorithm)
+void Router::modifyConnector(ConnRef *conn, const unsigned int type,
+        const ConnEnd& connEnd)
+{
+    ActionInfo modInfo(ConnChange, conn);
+    
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found == actionList.end())
     {
-        shapeVisSweep(shape);
+        modInfo.conns.push_back(std::make_pair(type, connEnd));
+        actionList.push_back(modInfo);
     }
     else
     {
-        shapeVis(shape);
+        found->conns.push_back(std::make_pair(type, connEnd));
+    }
+
+    if (!_consolidateActions)
+    {
+        processTransaction();
     }
-    callbackAllInvalidConnectors();
 }
 
 
-void Router::delShape(ShapeRef *shape)
+void Router::modifyConnector(ConnRef *conn)
 {
-    unsigned int pid = shape->id();
+    ActionInfo modInfo(ConnChange, conn);
+    
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found == actionList.end())
+    {
+        actionList.push_back(modInfo);
+    }
 
-    // Delete items that are queued in the movList.
-    for (MoveInfoList::iterator it = moveList.begin(); it != moveList.end(); )
+    if (!_consolidateActions)
     {
-        if ((*it)->shape->id() == pid)
-        {
-            MoveInfoList::iterator doomed = it;
-            ++it;
-            moveList.erase(doomed);
-        }
-        else
-        {
-            ++it;
-        }
+        processTransaction();
     }
+}
 
-    // o  Remove entries related to this shape's vertices
-    shape->removeFromGraph();
-    
-    if (SelectiveReroute)
+
+void Router::removeQueuedConnectorActions(ConnRef *conn)
+{
+    ActionInfo modInfo(ConnChange, conn);
+
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found != actionList.end())
     {
-        markConnectors(shape);
+        actionList.erase(found);
     }
+}
 
-    adjustContainsWithDel(pid);
-    
-#ifdef ORTHOGONAL_ROUTING
-    Region::removeShape(shape);
-#endif
 
-    delete shape;
+void Router::addShape(ShapeRef *shape)
+{
+    // There shouldn't be remove events or move events for the same shape
+    // already in the action list.
+    // XXX: Possibly we could handle this by ordering them intelligently.
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeRemove, shape)) == actionList.end());
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeMove, shape)) == actionList.end());
+
+    ActionInfo addInfo(ShapeAdd, shape);
     
-    // o  Check all edges that were blocked by this shape.
-    if (InvisibilityGrph)
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), addInfo);
+    if (found == actionList.end())
     {
-        checkAllBlockedEdges(pid);
+        actionList.push_back(addInfo);
     }
-    else
+
+    if (!_consolidateActions)
     {
-        // check all edges not in graph
-        checkAllMissingEdges();
+        processTransaction();
     }
-    callbackAllInvalidConnectors();
 }
 
 
-void Router::moveShape(ShapeRef *shape, Polygn *newPoly, const bool first_move)
+void Router::removeShape(ShapeRef *shape)
 {
+    // There shouldn't be add events events for the same shape already 
+    // in the action list.
+    // XXX: Possibly we could handle this by ordering them intelligently.
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeAdd, shape)) == actionList.end());
+
+    // Delete any ShapeMove entries for this shape in the action list.
+    ActionInfoList::iterator found = find(actionList.begin(), 
+            actionList.end(), ActionInfo(ShapeMove, shape));
+    if (found != actionList.end())
+    {
+        actionList.erase(found);
+    }
+
+    // Add the ShapeRemove entry.
+    ActionInfo remInfo(ShapeRemove, shape);
+    found = find(actionList.begin(), actionList.end(), remInfo);
+    if (found == actionList.end())
+    {
+        actionList.push_back(remInfo);
+    }
+
+    if (!_consolidateActions)
+    {
+        processTransaction();
+    }
+}
+
+
+void Router::moveShape(ShapeRef *shape, const double xDiff, const double yDiff)
+{
+    Polygon newPoly = shape->polygon();
+    newPoly.translate(xDiff, yDiff);
+
+    moveShape(shape, newPoly);
+}
+
+
+void Router::moveShape(ShapeRef *shape, const Polygon& newPoly, 
+        const bool first_move)
+{
+    // There shouldn't be remove events or add events for the same shape
+    // already in the action list.
+    // XXX: Possibly we could handle this by ordering them intelligently.
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeRemove, shape)) == actionList.end());
+    
+    if (find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeAdd, shape)) != actionList.end())
+    {
+        // The Add is enough, no need for the Move action too.
+        return;
+    }
+
+    ActionInfo moveInfo(ShapeMove, shape, newPoly, first_move);
     // Sanely cope with the case where the user requests moving the same
     // shape multiple times before rerouting connectors.
-    bool alreadyThere = false;
-    unsigned int id = shape->id();
-    MoveInfoList::iterator finish = moveList.end();
-    for (MoveInfoList::iterator it = moveList.begin(); it != finish; ++it)
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), moveInfo);
+
+    if (found != actionList.end())
     {
-        if ((*it)->shape->id() == id)
+        if (!SimpleRouting)
         {
-            if (!SimpleRouting)
-            {
-                fprintf(stderr,
-                        "warning: multiple moves requested for shape %d.\n",
-                        (int) id);
-            }
-            // Just update the MoveInfo with the second polygon, but
-            // leave the firstMove setting alone.
-            (*it)->newPoly = copyPoly(*newPoly);
-            alreadyThere = true;
+            db_printf("warning: multiple moves requested for shape %d "
+                    "within a single transaction.\n", (int) shape->id());
         }
+        // Just update the ActionInfo with the second polygon, but
+        // leave the firstMove setting alone.
+        found->newPoly = newPoly;
+    }
+    else 
+    {
+        actionList.push_back(moveInfo);
     }
 
-    if (!alreadyThere)
+    if (!_consolidateActions)
     {
-        MoveInfo *moveInfo = new MoveInfo(shape, newPoly, first_move);
-        moveList.push_back(moveInfo);
+        processTransaction();
     }
+}
+
+
+void Router::setStaticGraphInvalidated(const bool invalidated)
+{
+    _staticGraphInvalidated = invalidated;
+}
+
+
+void Router::destroyOrthogonalVisGraph(void)
+{
+    // Remove orthogonal visibility graph edges.
+    visOrthogGraph.clear();
 
-    if (!ConsolidateMoves)
+    // Remove the now orphaned vertices.
+    VertInf *curr = vertices.shapesBegin();
+    while (curr)
     {
-        processMoves();
+        if (curr->orphaned() && (curr->id == dummyOrthogID))
+        {
+            VertInf *following = vertices.removeVertex(curr);
+            delete curr;
+            curr = following;
+            continue;
+        }
+        curr = curr->lstNext;
     }
 }
 
 
-void Router::processMoves(void)
+void Router::regenerateStaticBuiltGraph(void)
 {
-    // If SimpleRouting, then don't update yet.
-    if (moveList.empty() || SimpleRouting)
+    // Here we do talks involved in updating the static-built visibility 
+    // graph (if necessary) before we do any routing.
+    if (_staticGraphInvalidated)
     {
-        return;
+        if (_orthogonalRouting)
+        {
+            destroyOrthogonalVisGraph();
+
+            timers.Register(tmOrthogGraph, timerStart);
+            // Regenerate a new visibility graph.
+            generateStaticOrthogonalVisGraph(this);
+            
+            timers.Stop();
+        }
+        _staticGraphInvalidated = false;
     }
+}
+
 
-    MoveInfoList::iterator curr;
-    MoveInfoList::iterator finish = moveList.end();
-    for (curr = moveList.begin(); curr != finish; ++curr)
+bool Router::shapeInQueuedActionList(ShapeRef *shape) const
+{
+    bool foundAdd = find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeAdd, shape)) != actionList.end();
+    bool foundRem = find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeRemove, shape)) != actionList.end();
+    bool foundMove = find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeMove, shape)) != actionList.end();
+
+    return (foundAdd || foundRem || foundMove);
+}
+
+
+bool Router::transactionUse(void) const
+{
+    return _consolidateActions;
+}
+
+
+void Router::setTransactionUse(const bool transactions)
+{
+    _consolidateActions = transactions;
+}
+
+
+bool Router::processTransaction(void)
+{
+    bool notPartialTime = !(PartialFeedback && PartialTime);
+    bool seenShapeMovesOrDeletes = false;
+
+    // If SimpleRouting, then don't update here.
+    if (actionList.empty() || SimpleRouting)
+    {
+        return false;
+    }
+
+    actionList.sort();
+    ActionInfoList::iterator curr;
+    ActionInfoList::iterator finish = actionList.end();
+    for (curr = actionList.begin(); curr != finish; ++curr)
     {
-        MoveInfo *moveInf = *curr;
-        ShapeRef *shape = moveInf->shape;
-        Polygn *newPoly = &(moveInf->newPoly);
-        bool first_move = moveInf->firstMove;
+        ActionInfo& actInf = *curr;
+        if (!((actInf.type == ShapeRemove) || (actInf.type == ShapeMove)))
+        {
+            // Not a move or remove action, so don't do anything.
+            continue;
+        }
+        seenShapeMovesOrDeletes = true;
+
+        ShapeRef *shape = actInf.shape();
+        bool isMove = (actInf.type == ShapeMove);
+        bool first_move = actInf.firstMove;
 
         unsigned int pid = shape->id();
-        bool notPartialTime = !(PartialFeedback && PartialTime);
 
         // o  Remove entries related to this shape's vertices
         shape->removeFromGraph();
         
-        if (SelectiveReroute && (notPartialTime || first_move))
+        if (SelectiveReroute && (!isMove || notPartialTime || first_move))
         {
             markConnectors(shape);
         }
 
         adjustContainsWithDel(pid);
         
-#ifdef ORTHOGONAL_ROUTING
-        Region::removeShape(shape);
-#endif
-
-        shape->setNewPoly(*newPoly);
-
-        adjustContainsWithAdd(*newPoly, pid);
-
         // Ignore this shape for visibility.
         // XXX: We don't really need to do this if we're not using Partial
         //      Feedback.  Without this the blocked edges still route
         //      around the shape until it leaves the connector.
         shape->makeInactive();
-        
     }
     
-    if (InvisibilityGrph)
+    if (seenShapeMovesOrDeletes && _polyLineRouting)
     {
-        for (curr = moveList.begin(); curr != finish; ++curr)
+        if (InvisibilityGrph)
         {
-            MoveInfo *moveInf = *curr;
-            ShapeRef *shape = moveInf->shape;
-            unsigned int pid = shape->id();
-            
-            // o  Check all edges that were blocked by this shape.
-            checkAllBlockedEdges(pid);
+            for (curr = actionList.begin(); curr != finish; ++curr)
+            {
+                ActionInfo& actInf = *curr;
+                if (!((actInf.type == ShapeRemove) || 
+                            (actInf.type == ShapeMove)))
+                {
+                    // Not a move or remove action, so don't do anything.
+                    continue;
+                }
+
+                // o  Check all edges that were blocked by this shape.
+                checkAllBlockedEdges(actInf.shape()->id());
+            }
+        }
+        else
+        {
+            // check all edges not in graph
+            checkAllMissingEdges();
         }
-    }
-    else
-    {
-        // check all edges not in graph
-        checkAllMissingEdges();
     }
 
-    while ( ! moveList.empty() )
+    for (curr = actionList.begin(); curr != finish; ++curr)
     {
-        MoveInfo *moveInf = moveList.front();
-        ShapeRef *shape = moveInf->shape;
-        Polygn *newPoly = &(moveInf->newPoly);
+        ActionInfo& actInf = *curr;
+        if (!((actInf.type == ShapeAdd) || (actInf.type == ShapeMove)))
+        {
+            // Not a move or add action, so don't do anything.
+            continue;
+        }
+
+        ShapeRef *shape = actInf.shape();
+        Polygon& newPoly = actInf.newPoly;
+        bool isMove = (actInf.type == ShapeMove);
 
         unsigned int pid = shape->id();
-        bool notPartialTime = !(PartialFeedback && PartialTime);
 
         // Restore this shape for visibility.
         shape->makeActive();
+        
+        if (isMove)
+        {
+            shape->setNewPoly(newPoly);
+        }
+        const Polygon& shapePoly = shape->polygon();
 
-#ifdef ORTHOGONAL_ROUTING
-        Region::addShape(shape);
-#endif
+        adjustContainsWithAdd(shapePoly, pid);
 
-        // o  Check all visibility edges to see if this one shape
-        //    blocks them.
-        if (notPartialTime)
+        if (_polyLineRouting)
         {
-            newBlockingShape(newPoly, pid);
+            // o  Check all visibility edges to see if this one shape
+            //    blocks them.
+            if (!isMove || notPartialTime)
+            {
+                newBlockingShape(shapePoly, pid);
+            }
+
+            // o  Calculate visibility for the new vertices.
+            if (UseLeesAlgorithm)
+            {
+                shapeVisSweep(shape);
+            }
+            else
+            {
+                shapeVis(shape);
+            }
         }
+    }
 
-        // o  Calculate visibility for the new vertices.
-        if (UseLeesAlgorithm)
+    // Update connector endpoints.
+    for (curr = actionList.begin(); curr != finish; ++curr)
+    {
+        ActionInfo& actInf = *curr;
+        if (actInf.type != ConnChange)
         {
-            shapeVisSweep(shape);
+            continue;
         }
-        else
+        for (ConnUpdateList::iterator conn = actInf.conns.begin();
+                conn != actInf.conns.end(); ++conn)
         {
-            shapeVis(shape);
+            actInf.conn()->updateEndPoint(conn->first, conn->second);
         }
-        
-        moveList.pop_front();
-        delete moveInf;
     }
-    callbackAllInvalidConnectors();
+    // Clear the actionList.
+    actionList.clear();
+    
+    _staticGraphInvalidated = true;
+    rerouteAndCallbackConnectors();
+
+    return true;
+}
+
+
+void Router::addCluster(ClusterRef *cluster)
+{
+    cluster->makeActive();
+    
+    unsigned int pid = cluster->id();
+    ReferencingPolygon& poly = cluster->polygon();
+
+    adjustClustersWithAdd(poly, pid);
+}
+
+
+void Router::delCluster(ClusterRef *cluster)
+{
+    cluster->makeInactive();
+    
+    unsigned int pid = cluster->id();
+    
+    adjustClustersWithDel(pid);
+}
+
+
+void Router::setOrthogonalNudgeDistance(const double dist)
+{
+    COLA_ASSERT(dist >= 0);
+    _orthogonalNudgeDistance = dist;
+}
+
+
+double Router::orthogonalNudgeDistance(void) const
+{
+    return _orthogonalNudgeDistance;
+}
+
+
+unsigned int Router::assignId(const unsigned int suggestedId)
+{
+    // If the suggestedId is zero, then we assign the object the next
+    // smallest unassigned ID, otherwise we trust the ID given is unique.
+    unsigned int assignedId = (suggestedId == 0) ? 
+            (_largestAssignedId + 1) : suggestedId;
+    
+    // Have the router record if this ID is larger than the _largestAssignedId.
+    _largestAssignedId = std::max(_largestAssignedId, assignedId);
+
+    // If assertions are enabled, then we check that this ID really is unique.
+    COLA_ASSERT(idIsUnique(assignedId));
+
+    return assignedId;
+}
+
+
+    // Returns whether the given ID is unique among all objects known by the
+    // router.  Outputs a warning if the ID is found ore than once. 
+    // It is expected this is only going to be called from assertions while
+    // debugging, so efficiency is not an issue and we just iterate over all
+    // objects.
+bool Router::idIsUnique(const unsigned int id) const 
+{
+    unsigned int count = 0;
+
+    // Examine shapes.
+    for (ShapeRefList::const_iterator i = shapeRefs.begin(); 
+            i != shapeRefs.end(); ++i) 
+    {
+        if ((*i)->id() == id)
+        {
+            count++;
+        }
+    }
+
+    // Examine connectors.
+    for (ConnRefList::const_iterator i = connRefs.begin(); 
+            i != connRefs.end(); ++i) 
+    {
+        if ((*i)->id() == id)
+        {
+            count++;
+        }
+    }
+
+    // Examine clusters.
+    for (ClusterRefList::const_iterator i = clusterRefs.begin(); 
+            i != clusterRefs.end(); ++i) 
+    {
+        if ((*i)->id() == id)
+        {
+            count++;
+        }
+    }
+
+    if (count > 1)
+    {
+        db_printf("Warning:\tlibavoid object ID %d not unique.\n", id);
+        return false;
+    }
+    return true;
 }
 
 
@@ -313,13 +678,15 @@ void Router::processMoves(void)
 void Router::attachedConns(IntList &conns, const unsigned int shapeId,
         const unsigned int type)
 {
-    ConnRefList::iterator fin = connRefs.end();
-    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) {
-
-        if ((type & runningTo) && ((*i)->_dstId == shapeId)) {
+    ConnRefList::const_iterator fin = connRefs.end();
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        if ((type & runningTo) && ((*i)->_dstId == shapeId)) 
+        {
             conns.push_back((*i)->_id);
         }
-        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) {
+        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) 
+        {
             conns.push_back((*i)->_id);
         }
     }
@@ -331,16 +698,19 @@ void Router::attachedConns(IntList &conns, const unsigned int shapeId,
 void Router::attachedShapes(IntList &shapes, const unsigned int shapeId,
         const unsigned int type)
 {
-    ConnRefList::iterator fin = connRefs.end();
-    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) {
-        if ((type & runningTo) && ((*i)->_dstId == shapeId)) {
+    ConnRefList::const_iterator fin = connRefs.end();
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        if ((type & runningTo) && ((*i)->_dstId == shapeId)) 
+        {
             if ((*i)->_srcId != 0)
             {
                 // Only if there is a shape attached to the other end.
                 shapes.push_back((*i)->_srcId);
             }
         }
-        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) {
+        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) 
+        {
             if ((*i)->_dstId != 0)
             {
                 // Only if there is a shape attached to the other end.
@@ -351,18 +721,127 @@ void Router::attachedShapes(IntList &shapes, const unsigned int shapeId,
 }
 
 
-    // It's intended this function is called after shape movement has 
-    // happened to alert connectors that they need to be rerouted.
-void Router::callbackAllInvalidConnectors(void)
+    // It's intended this function is called after visibility changes 
+    // resulting from shape movement have happened.  It will alert 
+    // rerouted connectors (via a callback) that they need to be redrawn.
+void Router::rerouteAndCallbackConnectors(void)
+{
+    std::set<ConnRef *> reroutedConns;
+    ConnRefList::const_iterator fin = connRefs.end();
+    
+    // Updating the orthogonal visibility graph if necessary. 
+    regenerateStaticBuiltGraph();
+
+    timers.Register(tmOrthogRoute, timerStart);
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        (*i)->_needs_repaint = false;
+        bool rerouted = (*i)->generatePath();
+        if (rerouted)
+        {
+            reroutedConns.insert(*i);
+        }
+    }
+    timers.Stop();
+
+    // Find and reroute crossing connectors if crossing penalties are set.
+    improveCrossings();
+
+    // Perform centring and nudging for othogonal routes.
+    improveOrthogonalRoutes(this);
+
+    // Alert connectors that they need redrawing.
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        (*i)->_needs_repaint = true;
+        (*i)->performCallback();
+    }
+}
+
+
+typedef std::set<ConnRef *> ConnRefSet;
+
+void Router::improveCrossings(void)
 {
+    const double crossing_penalty = routingPenalty(crossingPenalty);
+    const double shared_path_penalty = routingPenalty(fixedSharedPathPenalty);
+    if ((crossing_penalty == 0) && (shared_path_penalty == 0))
+    {
+        // No penalties, return.
+        return;
+    }
+    
+    // Find crossings and reroute connectors.
+    _inCrossingPenaltyReroutingStage = true;
+    ConnRefSet crossingConns;
     ConnRefList::iterator fin = connRefs.end();
-    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) {
-        (*i)->handleInvalid();
+    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        Avoid::Polygon& iRoute = (*i)->routeRef();
+        ConnRefList::iterator j = i;
+        for (++j; j != fin; ++j) 
+        {
+            if ((crossingConns.find(*i) != crossingConns.end()) && 
+                    (crossingConns.find(*j) != crossingConns.end()))
+            {
+                // We already know both these have crossings.
+                continue;
+            }
+            // Determine if this pair cross.
+            Avoid::Polygon& jRoute = (*j)->routeRef();
+            CrossingsInfoPair crossingInfo = std::make_pair(0, 0);
+            bool meetsPenaltyCriteria = false;
+            for (size_t jInd = 1; jInd < jRoute.size(); ++jInd)
+            {
+                const bool finalSegment = ((jInd + 1) == jRoute.size());
+                CrossingsInfoPair crossingInfo = countRealCrossings(
+                        iRoute, true, jRoute, jInd, false, 
+                        finalSegment, NULL, NULL, *i, *j);
+                
+                if ((shared_path_penalty > 0) && 
+                    (crossingInfo.second & CROSSING_SHARES_PATH) && 
+                    (crossingInfo.second & CROSSING_SHARES_FIXED_SEGMENT) && 
+                    !(crossingInfo.second & CROSSING_SHARES_PATH_AT_END)) 
+                {
+                    // We are penalising fixedSharedPaths and there is a
+                    // fixedSharedPath.
+                    meetsPenaltyCriteria = true;
+                    break;
+                }
+                else if ((crossing_penalty > 0) && (crossingInfo.first > 0))
+                {
+                    // We are penalising crossings and this is a crossing.
+                    meetsPenaltyCriteria = true;
+                    break;
+                }
+            }
+            if (meetsPenaltyCriteria)
+            {
+                crossingConns.insert(*i);
+                crossingConns.insert(*j);
+            }
+        }
+    }
+
+    for (ConnRefSet::iterator i = crossingConns.begin(); 
+            i != crossingConns.end(); ++i)
+    {
+        ConnRef *conn = *i;
+        conn->makePathInvalid();
+        // XXX: Could we free these routes here for extra savings?
+        // conn->freeRoutes();
+    }
+    for (ConnRefSet::iterator i = crossingConns.begin(); 
+            i != crossingConns.end(); ++i)
+    {
+        ConnRef *conn = *i;
+        conn->generatePath();
     }
+    _inCrossingPenaltyReroutingStage = false;
 }
 
 
-void Router::newBlockingShape(Polygn *poly, int pid)
+void Router::newBlockingShape(const Polygon& poly, int pid)
 {
     // o  Check all visibility edges to see if this one shape
     //    blocks them.
@@ -382,8 +861,11 @@ void Router::newBlockingShape(Polygn *poly, int pid)
             Point e2 = points.second;
             bool blocked = false;
 
-            bool ep_in_poly1 = !(eID1.isShape) ? inPoly(*poly, e1) : false;
-            bool ep_in_poly2 = !(eID2.isShape) ? inPoly(*poly, e2) : false;
+            bool countBorder = false;
+            bool ep_in_poly1 = !(eID1.isShape) ? 
+                    inPoly(poly, e1, countBorder) : false;
+            bool ep_in_poly2 = !(eID2.isShape) ? 
+                    inPoly(poly, e2, countBorder) : false;
             if (ep_in_poly1 || ep_in_poly2)
             {
                 // Don't check edges that have a connector endpoint
@@ -391,10 +873,14 @@ void Router::newBlockingShape(Polygn *poly, int pid)
                 continue;
             }
 
-            for (int pt_i = 0; pt_i < poly->pn; pt_i++)
+            bool seenIntersectionAtEndpoint = false;
+            for (size_t pt_i = 0; pt_i < poly.size(); ++pt_i)
             {
-                int pt_n = (pt_i == (poly->pn - 1)) ? 0 : pt_i + 1;
-                if (segmentIntersect(e1, e2, poly->ps[pt_i], poly->ps[pt_n]))
+                size_t pt_n = (pt_i == (poly.size() - 1)) ? 0 : pt_i + 1;
+                const Point& pi = poly.ps[pt_i];
+                const Point& pn = poly.ps[pt_n];
+                if (segmentShapeIntersect(e1, e2, pi, pn, 
+                        seenIntersectionAtEndpoint))
                 {
                     blocked = true;
                     break;
@@ -422,7 +908,7 @@ void Router::newBlockingShape(Polygn *poly, int pid)
 
 void Router::checkAllBlockedEdges(int pid)
 {
-    assert(InvisibilityGrph);
+    COLA_ASSERT(InvisibilityGrph);
 
     for (EdgeInf *iter = invisGraph.begin(); iter != invisGraph.end() ; )
     {
@@ -444,18 +930,9 @@ void Router::checkAllBlockedEdges(int pid)
 
 void Router::checkAllMissingEdges(void)
 {
-    assert(!InvisibilityGrph);
+    COLA_ASSERT(!InvisibilityGrph);
 
-    VertInf *first = NULL;
-
-    if (IncludeEndpoints)
-    {
-        first = vertices.connsBegin();
-    }
-    else
-    {
-        first = vertices.shapesBegin();
-    }
+    VertInf *first = vertices.connsBegin();
 
     VertInf *pend = vertices.end();
     for (VertInf *i = first; i != pend; i = i->lstNext)
@@ -489,39 +966,79 @@ void Router::checkAllMissingEdges(void)
 void Router::generateContains(VertInf *pt)
 {
     contains[pt->id].clear();
+    enclosingClusters[pt->id].clear();
 
-    ShapeRefList::iterator finish = shapeRefs.end();
-    for (ShapeRefList::iterator i = shapeRefs.begin(); i != finish; ++i)
+    // Don't count points on the border as being inside.
+    bool countBorder = false;
+
+    // Compute enclosing shapes.
+    ShapeRefList::const_iterator finish = shapeRefs.end();
+    for (ShapeRefList::const_iterator i = shapeRefs.begin(); i != finish; ++i)
     {
-        Polygn poly = copyPoly(*i);
-        if (inPoly(poly, pt->point))
+        if (inPoly((*i)->polygon(), pt->point, countBorder))
         {
             contains[pt->id].insert((*i)->id());
         }
-        freePoly(poly);
+    }
+
+    // Computer enclosing Clusters
+    ClusterRefList::const_iterator clFinish = clusterRefs.end();
+    for (ClusterRefList::const_iterator i = clusterRefs.begin(); 
+            i != clFinish; ++i)
+    {
+        if (inPolyGen((*i)->polygon(), pt->point))
+        {
+            enclosingClusters[pt->id].insert((*i)->id());
+        }
     }
 }
 
 
-void Router::adjustContainsWithAdd(const Polygn& poly, const int p_shape)
+void Router::adjustClustersWithAdd(const PolygonInterface& poly, 
+        const int p_cluster)
 {
     for (VertInf *k = vertices.connsBegin(); k != vertices.shapesBegin();
             k = k->lstNext)
     {
-        if (inPoly(poly, k->point))
+        if (inPolyGen(poly, k->point))
         {
-            contains[k->id].insert(p_shape);
+            enclosingClusters[k->id].insert(p_cluster);
         }
     }
 }
 
 
-void Router::adjustContainsWithDel(const int p_shape)
+void Router::adjustClustersWithDel(const int p_cluster)
+{
+    for (ContainsMap::iterator k = enclosingClusters.begin();
+            k != enclosingClusters.end(); ++k)
+    {
+        (*k).second.erase(p_cluster);
+    }
+}
+
+
+void Router::adjustContainsWithAdd(const Polygon& poly, const int p_shape)
 {
+    // Don't count points on the border as being inside.
+    bool countBorder = false;
+
     for (VertInf *k = vertices.connsBegin(); k != vertices.shapesBegin();
             k = k->lstNext)
     {
-        contains[k->id].erase(p_shape);
+        if (inPoly(poly, k->point, countBorder))
+        {
+            contains[k->id].insert(p_shape);
+        }
+    }
+}
+
+
+void Router::adjustContainsWithDel(const int p_shape)
+{
+    for (ContainsMap::iterator k = contains.begin(); k != contains.end(); ++k)
+    {
+        (*k).second.erase(p_shape);
     }
 }
 
@@ -537,14 +1054,21 @@ static double AngleAFromThreeSides(const double a, const double b,
 
 void Router::markConnectors(ShapeRef *shape)
 {
-    assert(SelectiveReroute);
+    if (RubberBandRouting)
+    {
+        // When rubber-band routing, we do no reroute connectors that
+        // may have a better route, only invalid connectors.
+        return;
+    }
+
+    COLA_ASSERT(SelectiveReroute);
 
-    ConnRefList::iterator end = connRefs.end();
-    for (ConnRefList::iterator it = connRefs.begin(); it != end; ++it)
+    ConnRefList::const_iterator end = connRefs.end();
+    for (ConnRefList::const_iterator it = connRefs.begin(); it != end; ++it)
     {
         ConnRef *conn = (*it);
 
-        if (conn->_route.pn == 0)
+        if (conn->_route.empty())
         {
             // Ignore uninitialised connectors.
             continue;
@@ -556,7 +1080,7 @@ void Router::markConnectors(ShapeRef *shape)
         }
 
         Point start = conn->_route.ps[0];
-        Point end = conn->_route.ps[conn->_route.pn - 1];
+        Point end = conn->_route.ps[conn->_route.size() - 1];
 
         double conndist = conn->_route_dist;
 
@@ -609,12 +1133,12 @@ void Router::markConnectors(ShapeRef *shape)
                 Point n_p2(p2.x - p1.x, p2.y - p1.y);
                 Point n_start(start.x - p1.x, start.y - p1.y);
                 Point n_end(end.x - p1.x, end.y - p1.y);
-                //printf("n_p2:    (%.1f, %.1f)\n", n_p2.x, n_p2.y);
-                //printf("n_start: (%.1f, %.1f)\n", n_start.x, n_start.y);
-                //printf("n_end:   (%.1f, %.1f)\n", n_end.x, n_end.y);
+                //db_printf("n_p2:    (%.1f, %.1f)\n", n_p2.x, n_p2.y);
+                //db_printf("n_start: (%.1f, %.1f)\n", n_start.x, n_start.y);
+                //db_printf("n_end:   (%.1f, %.1f)\n", n_end.x, n_end.y);
 
                 double theta = 0 - atan2(n_p2.y, n_p2.x);
-                //printf("theta = %.2f\n", theta * (180 / PI));
+                //db_printf("theta = %.2f\n", theta * (180 / PI));
 
                 Point r_p1(0, 0);
                 Point r_p2 = n_p2;
@@ -630,16 +1154,15 @@ void Router::markConnectors(ShapeRef *shape)
                 start.y = cosv * n_start.y + sinv * n_start.x;
                 end.x = cosv * n_end.x - sinv * n_end.y;
                 end.y = cosv * n_end.y + sinv * n_end.x;
-                //printf("r_p2:    (%.1f, %.1f)\n", r_p2.x, r_p2.y);
-                //printf("r_start: (%.1f, %.1f)\n", start.x, start.y);
-                //printf("r_end:   (%.1f, %.1f)\n", end.x, end.y);
+                //db_printf("r_p2:    (%.1f, %.1f)\n", r_p2.x, r_p2.y);
+                //db_printf("r_start: (%.1f, %.1f)\n", start.x, start.y);
+                //db_printf("r_end:   (%.1f, %.1f)\n", end.x, end.y);
 
-                if (((int) r_p2.y) != 0)
+                // This might be slightly off.
+                if (fabs(r_p2.y) > 0.0001)
                 {
-                    printf("r_p2.y: %f != 0\n", r_p2.y);
-                    std::abort();
+                    db_printf("r_p2.y: %f != 0\n", r_p2.y);
                 }
-                // This might be slightly off.
                 r_p2.y = 0;
 
                 offy = r_p1.y;
@@ -679,13 +1202,13 @@ void Router::markConnectors(ShapeRef *shape)
                 x = ((b*c) + (a*d)) / (b + d);
             }
 
-            //printf("%.1f, %.1f, %.1f, %.1f\n", a, b, c, d);
-            //printf("x = %.1f\n", x);
+            //db_printf("%.1f, %.1f, %.1f, %.1f\n", a, b, c, d);
+            //db_printf("x = %.1f\n", x);
 
             x = std::max(min, x);
             x = std::min(max, x);
 
-            //printf("x = %.1f\n", x);
+            //db_printf("x = %.1f\n", x);
 
             Point xp;
             if (p1.x == p2.x)
@@ -698,20 +1221,20 @@ void Router::markConnectors(ShapeRef *shape)
                 xp.x = x;
                 xp.y = offy;
             }
-            //printf("(%.1f, %.1f)\n", xp.x, xp.y);
+            //db_printf("(%.1f, %.1f)\n", xp.x, xp.y);
 
-            e1 = dist(start, xp);
-            e2 = dist(xp, end);
+            e1 = euclideanDist(start, xp);
+            e2 = euclideanDist(xp, end);
             estdist = e1 + e2;
 
 
-            //printf("is %.1f < %.1f\n", estdist, conndist);
+            //db_printf("is %.1f < %.1f\n", estdist, conndist);
             if (estdist < conndist)
             {
 #ifdef SELECTIVE_DEBUG
                 //double angle = AngleAFromThreeSides(dist(start, end),
                 //        e1, e2);
-                printf("[%3d] - Possible better path found (%.1f < %.1f)\n",
+                db_printf("[%3d] - Possible better path found (%.1f < %.1f)\n",
                         conn->_id, estdist, conndist);
 #endif
                 conn->_needs_reroute_flag = true;
@@ -723,6 +1246,81 @@ void Router::markConnectors(ShapeRef *shape)
 }
 
 
+ConnType Router::validConnType(const ConnType select) const
+{
+    if (select != ConnType_None)
+    {
+        if ((select == ConnType_Orthogonal) && _orthogonalRouting)
+        {
+            return ConnType_Orthogonal;
+        }
+        else if ((select == ConnType_PolyLine) && _polyLineRouting)
+        {
+            return ConnType_PolyLine;
+        }
+    }
+
+    if (_polyLineRouting)
+    {
+        return ConnType_PolyLine;
+    }
+    else if (_orthogonalRouting)
+    {
+        return ConnType_Orthogonal;
+    }
+    return ConnType_None;
+}
+
+
+void Router::setRoutingPenalty(const PenaltyType penType, const double penVal)
+{
+    COLA_ASSERT(penType < lastPenaltyMarker);
+    if (penVal < 0)
+    {
+        // Set some sensible penalty.
+        switch (penType)
+        {
+            case segmentPenalty:
+                _routingPenalties[penType] = 50;
+                break;
+            case fixedSharedPathPenalty:
+                _routingPenalties[penType] = 110;
+                break;
+            case anglePenalty:
+                _routingPenalties[penType] = 50;
+                break;
+            case crossingPenalty:
+                _routingPenalties[penType] = 200;
+                break;
+            case clusterCrossingPenalty:
+                _routingPenalties[penType] = 4000;
+                break;
+            default:
+                _routingPenalties[penType] = 50;
+                break;
+        }
+    }
+    else
+    {
+        _routingPenalties[penType] = penVal;
+    }
+}
+
+
+double Router::routingPenalty(const PenaltyType penType) const
+{
+    COLA_ASSERT(penType < lastPenaltyMarker);
+    return _routingPenalties[penType];
+}
+
+
+double& Router::penaltyRef(const PenaltyType penType)
+{
+    COLA_ASSERT(penType < lastPenaltyMarker);
+    return _routingPenalties[penType];
+}
+
+
 void Router::printInfo(void)
 {
     FILE *fp = stdout;
@@ -735,6 +1333,7 @@ void Router::printInfo(void)
     int st_endpoints = 0;
     int st_valid_shape_visedges = 0;
     int st_valid_endpt_visedges = 0;
+    int st_orthogonal_visedges = 0;
     int st_invalid_visedges = 0;
     VertInf *finish = vertices.end();
     for (VertInf *t = vertices.connsBegin(); t != finish; t = t->lstNext)
@@ -775,9 +1374,15 @@ void Router::printInfo(void)
     {
         st_invalid_visedges++;
     }
+    for (EdgeInf *t = visOrthogGraph.begin(); t != visOrthogGraph.end();
+            t = t->lstNext)
+    {
+        st_orthogonal_visedges++;
+    }
     fprintf(fp, "Number of shapes: %d\n", st_shapes);
     fprintf(fp, "Number of vertices: %d (%d real, %d endpoints)\n",
             st_vertices + st_endpoints, st_vertices, st_endpoints);
+    fprintf(fp, "Number of orhtog_vis_edges: %d\n", st_orthogonal_visedges);
     fprintf(fp, "Number of vis_edges: %d (%d valid [%d normal, %d endpt], "
             "%d invalid)\n", st_valid_shape_visedges + st_invalid_visedges +
             st_valid_endpt_visedges, st_valid_shape_visedges +
@@ -787,12 +1392,462 @@ void Router::printInfo(void)
     fprintf(fp, "checkVisEdge tally: %d\n", st_checked_edges);
     fprintf(fp, "----------------------\n");
 
-    fprintf(fp, "ADDS:  "); timers.Print(tmAdd);
-    fprintf(fp, "DELS:  "); timers.Print(tmDel);
-    fprintf(fp, "MOVS:  "); timers.Print(tmMov);
-    fprintf(fp, "***S:  "); timers.Print(tmSev);
-    fprintf(fp, "PTHS:  "); timers.Print(tmPth);
+    fprintf(fp, "ADDS:  "); timers.Print(tmAdd, fp);
+    fprintf(fp, "DELS:  "); timers.Print(tmDel, fp);
+    fprintf(fp, "MOVS:  "); timers.Print(tmMov, fp);
+    fprintf(fp, "***S:  "); timers.Print(tmSev, fp);
+    fprintf(fp, "PTHS:  "); timers.Print(tmPth, fp);
+    fprintf(fp, "OrthogGraph:  "); timers.Print(tmOrthogGraph, fp);
+    fprintf(fp, "OrthogRoute:  "); timers.Print(tmOrthogRoute, fp);
+    fprintf(fp, "OrthogCentre:  "); timers.Print(tmOrthogCentre, fp);
+    fprintf(fp, "OrthogNudge:  "); timers.Print(tmOrthogNudge, fp);
     fprintf(fp, "\n");
+    timers.Reset();
+}
+
+
+static const double LIMIT = 100000000;
+
+static void reduceRange(double& val)
+{
+    val = std::min(val, LIMIT);
+    val = std::max(val, -LIMIT);
+}
+
+
+//=============================================================================
+// The following methods are for testing and debugging.
+
+
+bool Router::existsOrthogonalPathOverlap(void)
+{
+    ConnRefList::iterator fin = connRefs.end();
+    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        Avoid::Polygon iRoute = (*i)->displayRoute();
+        ConnRefList::iterator j = i;
+        for (++j; j != fin; ++j) 
+        {
+            // Determine if this pair overlap
+            Avoid::Polygon jRoute = (*j)->displayRoute();
+            CrossingsInfoPair crossingInfo = std::make_pair(0, 0);
+            for (size_t jInd = 1; jInd < jRoute.size(); ++jInd)
+            {
+                const bool finalSegment = ((jInd + 1) == jRoute.size());
+                CrossingsInfoPair crossingInfo = countRealCrossings(
+                        iRoute, true, jRoute, jInd, true, 
+                        finalSegment, NULL, NULL, *i, *j);
+                
+                if ((crossingInfo.second & CROSSING_SHARES_PATH) && 
+                    (crossingInfo.second & CROSSING_SHARES_FIXED_SEGMENT) && 
+                    !(crossingInfo.second & CROSSING_SHARES_PATH_AT_END)) 
+                {
+                    // We looking for fixedSharedPaths and there is a
+                    // fixedSharedPath.
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+
+
+bool Router::existsOrthogonalTouchingCorners(void)
+{
+    ConnRefList::iterator fin = connRefs.end();
+    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        Avoid::Polygon iRoute = (*i)->displayRoute();
+        ConnRefList::iterator j = i;
+        for (++j; j != fin; ++j) 
+        {
+            // Determine if this pair overlap
+            Avoid::Polygon jRoute = (*j)->displayRoute();
+            CrossingsInfoPair crossingInfo = std::make_pair(0, 0);
+            for (size_t jInd = 1; jInd < jRoute.size(); ++jInd)
+            {
+                const bool finalSegment = ((jInd + 1) == jRoute.size());
+                CrossingsInfoPair crossingInfo = countRealCrossings(
+                        iRoute, true, jRoute, jInd, true, 
+                        finalSegment, NULL, NULL, *i, *j);
+                
+                if (crossingInfo.second & CROSSING_TOUCHES) 
+                {
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+
+
+void Router::outputInstanceToSVG(std::string instanceName)
+{
+    std::string filename;
+    if (!instanceName.empty())
+    {
+        filename = instanceName;
+    }
+    else
+    {
+        filename = "libavoid-debug";
+    }
+    filename += ".svg";
+    FILE *fp = fopen(filename.c_str(), "w");
+
+    if (fp == NULL)
+    {
+        return;
+    }
+
+    double minX = LIMIT;
+    double minY = LIMIT;
+    double maxX = -LIMIT;
+    double maxY = -LIMIT;
+
+    VertInf *curr = vertices.connsBegin();
+    while (curr)
+    {
+        Point p = curr->point;
+
+        reduceRange(p.x);
+        reduceRange(p.y);
+        
+        if (p.x > -LIMIT)
+        {
+            minX = std::min(minX, p.x);
+        }
+        if (p.x < LIMIT)
+        {
+            maxX = std::max(maxX, p.x);
+        }
+        if (p.y > -LIMIT)
+        {
+            minY = std::min(minY, p.y);
+        }
+        if (p.y < LIMIT)
+        {
+            maxY = std::max(maxY, p.y);
+        }
+        curr = curr->lstNext;
+    }
+    minX -= 50;
+    minY -= 50;
+    maxX += 50;
+    maxY += 50;
+
+    fprintf(fp, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
+    fprintf(fp, "<svg xmlns:inkscape=\"http://www.inkscape.org/namespaces/inkscape\" xmlns=\"http://www.w3.org/2000/svg\" width=\"100%%\" height=\"100%%\" viewBox=\"%g %g %g %g\">\n", minX, minY, maxX - minX, maxY - minY);
+
+    // Output source code to generate this instance of the router.
+    fprintf(fp, "<!-- Source code to generate this instance:\n");
+    fprintf(fp, "#include \"libavoid/libavoid.h\"\n");
+    fprintf(fp, "using namespace Avoid;\n");
+    fprintf(fp, "int main(void) {\n");
+    fprintf(fp, "    Router *router = new Router(\n");
+    fprintf(fp, "            PolyLineRouting | OrthogonalRouting);\n");
+    for (size_t p = 0; p < lastPenaltyMarker; ++p)
+    {
+        fprintf(fp, "    router->setRoutingPenalty((PenaltyType)%lu, %g);\n", 
+                p, _routingPenalties[p]);
+    }
+    fprintf(fp, "    router->setOrthogonalNudgeDistance(%g);\n\n",
+            orthogonalNudgeDistance());
+    ShapeRefList::iterator shRefIt = shapeRefs.begin();
+    while (shRefIt != shapeRefs.end())
+    {
+        ShapeRef *shRef = *shRefIt;
+        fprintf(fp, "    Polygon poly%u(%lu);\n", 
+                shRef->id(), shRef->polygon().size());
+        for (size_t i = 0; i < shRef->polygon().size(); ++i)
+        {
+            fprintf(fp, "    poly%u.ps[%lu] = Point(%g, %g);\n", 
+                    shRef->id(), i, shRef->polygon().at(i).x,
+                    shRef->polygon().at(i).y);
+        }
+        fprintf(fp, "    ShapeRef *shapeRef%u = new ShapeRef(router, poly%u, "
+                "%u);\n", shRef->id(), shRef->id(), shRef->id());
+        fprintf(fp, "    router->addShape(shapeRef%u);\n\n", shRef->id());
+        ++shRefIt;
+    }
+    ConnRefList::reverse_iterator revConnRefIt = connRefs.rbegin();
+    while (revConnRefIt != connRefs.rend())
+    {
+        ConnRef *connRef = *revConnRefIt;
+        fprintf(fp, "    ConnRef *connRef%u = new ConnRef(router, %u);\n",
+                connRef->id(), connRef->id());
+        if (connRef->src())
+        {
+            fprintf(fp, "    ConnEnd srcPt%u(Point(%g, %g), %u);\n",
+                    connRef->id(), connRef->src()->point.x,
+                    connRef->src()->point.y, connRef->src()->visDirections);
+            fprintf(fp, "    connRef%u->setSourceEndpoint(srcPt%u);\n",
+                    connRef->id(), connRef->id());
+        }
+        if (connRef->dst())
+        {
+            fprintf(fp, "    ConnEnd dstPt%u(Point(%g, %g), %u);\n",
+                    connRef->id(), connRef->dst()->point.x,
+                    connRef->dst()->point.y, connRef->dst()->visDirections);
+            fprintf(fp, "    connRef%u->setDestEndpoint(dstPt%u);\n",
+                    connRef->id(), connRef->id());
+        }
+        fprintf(fp, "    connRef%u->setRoutingType((ConnType)%u);\n\n", 
+                connRef->id(), connRef->routingType());
+        ++revConnRefIt;
+    }
+    fprintf(fp, "    router->processTransaction();\n");
+    fprintf(fp, "    router->outputInstanceToSVG();\n");
+    fprintf(fp, "    delete router;\n");
+    fprintf(fp, "    return 0;\n");
+    fprintf(fp, "};\n");
+    fprintf(fp, "-->\n");
+
+    
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "inkscape:label=\"ShapesPoly\">\n");
+    shRefIt = shapeRefs.begin();
+    while (shRefIt != shapeRefs.end())
+    {
+        ShapeRef *shRef = *shRefIt;
+    
+        fprintf(fp, "<path id=\"poly-%u\" style=\"stroke-width: 1px; "
+                "stroke: black; fill: blue; fill-opacity: 0.3;\" d=\"", 
+                shRef->id());
+        for (size_t i = 0; i < shRef->polygon().size(); ++i)
+        {
+            fprintf(fp, "%c %g,%g ", ((i == 0) ? 'M' : 'L'), 
+                    shRef->polygon().at(i).x, shRef->polygon().at(i).y);
+        }
+        fprintf(fp, "Z\" />\n");
+        ++shRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"ShapesRect\">\n");
+    shRefIt = shapeRefs.begin();
+    while (shRefIt != shapeRefs.end())
+    {
+        ShapeRef *shRef = *shRefIt;
+        double minX, minY, maxX, maxY;
+        shRef->polygon().getBoundingRect(&minX, &minY, &maxX, &maxY);
+    
+        fprintf(fp, "<rect id=\"rect-%u\" x=\"%g\" y=\"%g\" width=\"%g\" height=\"%g\" "
+                "style=\"stroke-width: 1px; stroke: black; fill: blue; fill-opacity: 0.3;\" />\n",
+                shRef->id(), minX, minY, maxX - minX, maxY - minY);
+        ++shRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "inkscape:label=\"VisGraph\""
+            ">\n");
+    EdgeInf *finish = NULL;
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"VisGraph-shape\""
+            ">\n");
+    finish = visGraph.end();
+    for (EdgeInf *t = visGraph.begin(); t != finish; t = t->lstNext)
+    {
+        std::pair<VertID, VertID> ids = t->ids();
+        bool isShape = (ids.first.isShape) && (ids.second.isShape);
+        if (!isShape)
+        {
+            continue;
+        }
+        std::pair<Point, Point> ptpair = t->points();
+        Point p1 = ptpair.first;
+        Point p2 = ptpair.second;
+        
+        reduceRange(p1.x);
+        reduceRange(p1.y);
+        reduceRange(p2.x);
+        reduceRange(p2.y);
+        
+        fprintf(fp, "<path d=\"M %g,%g L %g,%g\" "
+                "style=\"fill: none; stroke: %s; stroke-width: 1px;\" />\n", 
+                p1.x, p1.y, p2.x, p2.y,
+                (!(ids.first.isShape) || !(ids.second.isShape)) ? "green" : 
+                "red");
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"VisGraph-conn\""
+            ">\n");
+    finish = visGraph.end();
+    for (EdgeInf *t = visGraph.begin(); t != finish; t = t->lstNext)
+    {
+        std::pair<VertID, VertID> ids = t->ids();
+        bool isShape = (ids.first.isShape) && (ids.second.isShape);
+        if (isShape)
+        {
+            continue;
+        }
+        std::pair<Point, Point> ptpair = t->points();
+        Point p1 = ptpair.first;
+        Point p2 = ptpair.second;
+        
+        reduceRange(p1.x);
+        reduceRange(p1.y);
+        reduceRange(p2.x);
+        reduceRange(p2.y);
+        
+        fprintf(fp, "<path d=\"M %g,%g L %g,%g\" "
+                "style=\"fill: none; stroke: %s; stroke-width: 1px;\" />\n", 
+                p1.x, p1.y, p2.x, p2.y,
+                (!(ids.first.isShape) || !(ids.second.isShape)) ? "green" : 
+                "red");
+    }
+    fprintf(fp, "</g>\n");
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"OrthogVisGraph\">\n");
+    finish = visOrthogGraph.end();
+    for (EdgeInf *t = visOrthogGraph.begin(); t != finish; t = t->lstNext)
+    {
+        std::pair<Point, Point> ptpair = t->points();
+        Point p1 = ptpair.first;
+        Point p2 = ptpair.second;
+        
+        reduceRange(p1.x);
+        reduceRange(p1.y);
+        reduceRange(p2.x);
+        reduceRange(p2.y);
+        
+        std::pair<VertID, VertID> ids = t->ids();
+
+        fprintf(fp, "<path d=\"M %g,%g L %g,%g\" "
+                "style=\"fill: none; stroke: %s; stroke-width: 1px;\" />\n", 
+                p1.x, p1.y, p2.x, p2.y,
+                (!(ids.first.isShape) || !(ids.second.isShape)) ? "green" : 
+                "red");
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"RawConnectors\""
+            ">\n");
+    ConnRefList::iterator connRefIt = connRefs.begin();
+    while (connRefIt != connRefs.end())
+    {
+        ConnRef *connRef = *connRefIt;
+    
+        PolyLine route = connRef->route();
+        if (!route.empty())
+        {
+            fprintf(fp, "<path id=\"raw-%u\" d=\"M %g,%g ", connRef->id(),
+                    route.ps[0].x, route.ps[0].y);
+            for (size_t i = 1; i < route.size(); ++i)
+            {
+                fprintf(fp, "L %g,%g ", route.ps[i].x, route.ps[i].y);
+            }
+            fprintf(fp, "\" ");
+            if (connRef->src() && connRef->dst())
+            {
+                fprintf(fp, "debug=\"src: %d dst: %d\" ",
+                        connRef->src()->visDirections,
+                        connRef->dst()->visDirections);
+            }
+            fprintf(fp, "style=\"fill: none; stroke: black; "
+                    "stroke-width: 1px;\" />\n");
+        }
+        
+        ++connRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"CurvedDisplayConnectors\""
+            ">\n");
+    connRefIt = connRefs.begin();
+    while (connRefIt != connRefs.end())
+    {
+        ConnRef *connRef = *connRefIt;
+    
+        PolyLine route = connRef->displayRoute().curvedPolyline(8);
+        if (!route.empty())
+        {
+            fprintf(fp, "<path id=\"curved-%u\" d=\"M %g,%g ", connRef->id(),
+                    route.ps[0].x, route.ps[0].y);
+            for (size_t i = 1; i < route.size(); ++i)
+            {
+                if (route.ts[i] == 'C')
+                {
+                    fprintf(fp, "%c %g,%g %g,%g %g,%g", route.ts[i], 
+                            route.ps[i].x, route.ps[i].y,
+                            route.ps[i+1].x, route.ps[i+1].y,
+                            route.ps[i+2].x, route.ps[i+2].y);
+                    i += 2;
+                }
+                else
+                {
+                    fprintf(fp, "%c %g,%g ", route.ts[i], 
+                            route.ps[i].x, route.ps[i].y);
+                }
+            }
+            fprintf(fp, "\" ");
+            if (connRef->src() && connRef->dst())
+            {
+                fprintf(fp, "debug=\"src: %d dst: %d\" ",
+                        connRef->src()->visDirections,
+                        connRef->dst()->visDirections);
+            }
+            fprintf(fp, "style=\"fill: none; stroke: black; "
+                    "stroke-width: 1px;\" />\n");
+        }
+        
+        ++connRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "inkscape:label=\"DisplayConnectors\""
+            ">\n");
+    connRefIt = connRefs.begin();
+    while (connRefIt != connRefs.end())
+    {
+        ConnRef *connRef = *connRefIt;
+    
+        PolyLine route = connRef->displayRoute();
+        if (!route.empty())
+        {
+            fprintf(fp, "<path id=\"disp-%u\" d=\"M %g,%g ", connRef->id(),
+                    route.ps[0].x, route.ps[0].y);
+            for (size_t i = 1; i < route.size(); ++i)
+            {
+                fprintf(fp, "L %g,%g ", route.ps[i].x, route.ps[i].y);
+            }
+            fprintf(fp, "\" ");
+            if (connRef->src() && connRef->dst())
+            {
+                fprintf(fp, "debug=\"src: %d dst: %d\" ",
+                        connRef->src()->visDirections,
+                        connRef->dst()->visDirections);
+            }
+            fprintf(fp, "style=\"fill: none; stroke: black; "
+                    "stroke-width: 1px;\" />\n");
+        }
+        
+        ++connRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+
+    fprintf(fp, "</svg>\n");
+    fclose(fp);
 }
 
 
diff --git a/src/libavoid/router.h b/src/libavoid/router.h
index 597f48c5e..03060b025 100644
--- a/src/libavoid/router.h
+++ b/src/libavoid/router.h
@@ -2,94 +2,325 @@
  * 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
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file    router.h
+//! @brief   Contains the interface for the Router class.
+
 
 #ifndef AVOID_ROUTER_H
 #define AVOID_ROUTER_H
 
-//#define LINEDEBUG
+#include <list>
+#include <utility>
+#include <string>
 
 #include "libavoid/shape.h"
+#include "libavoid/viscluster.h"
 #include "libavoid/graph.h"
 #include "libavoid/timer.h"
-#include <list>
-#include <utility>
-#ifdef LINEDEBUG       
+#include "libavoid/connector.h"
+
+#if defined(LINEDEBUG) || defined(ASTAR_DEBUG) || defined(LIBAVOID_SDL)
     #include <SDL.h>
+    #ifndef LIBAVOID_SDL
+        #define LIBAVOID_SDL
+    #endif
 #endif
 
 
 namespace Avoid {
 
-class ConnRef;
-typedef std::list<ConnRef *> ConnRefList;
 typedef std::list<unsigned int> IntList;
-class MoveInfo;
-typedef std::list<MoveInfo *> MoveInfoList;
+
+class ActionInfo;
+typedef std::list<ActionInfo> ActionInfoList;
+class ShapeRef;
+
+//! @brief  Flags that can be passed to the router during initialisation 
+//!         to specify options.
+enum RouterFlag
+{
+    //! @brief  This option specifies that the router should maintain the
+    //!         structures necessary to allow poly-line connector routing.
+    PolyLineRouting = 1,
+    //! @brief  This option specifies that the router should maintain the
+    //!         structures necessary to allow orthogonal connector routing.
+    OrthogonalRouting = 2
+};
 
 
 static const unsigned int runningTo = 1;
 static const unsigned int runningFrom = 2;
 static const unsigned int runningToAndFrom = runningTo | runningFrom;
 
+//! @brief  Types of penalty cases that can be used to improve the quality 
+//!         of the connector routes produced.
+enum PenaltyType
+{
+    //! @brief  This penalty is applied for each segment in the connector 
+    //!         path beyond the first.  This should always normally be set
+    //!         when doing orthogonal routing to prevent step-like connector
+    //!         paths.
+    segmentPenalty = 0,
+    //! @brief  This penalty is applied in its full amount to tight acute 
+    //!         bends in the connector path.  A smaller portion of the penalty
+    //!         is applied for slight bends, i.e., where the bend is close to
+    //!         180 degrees.  This is useful for polyline routing where there
+    //!         is some evidence that tighter corners are worse for 
+    //!         readability, but that slight bends might not be so bad, 
+    //!         especially when smoothed by curves.
+    anglePenalty,
+    //! @brief  This penalty is applied whenever a connector path crosses 
+    //!         another connector path.  It takes shared paths into 
+    //!         consideration and the penalty is only applied if there
+    //!         is an actual crossing.
+    //! @note   This penalty is still experimental!  It is not recommended
+    //!         for normal use.
+    crossingPenalty,
+    //! @brief  This penalty is applied whenever a connector path crosses 
+    //!         a cluster boundary.
+    //! @note   This penalty is still experimental!  It is not recommended
+    //!         for normal use.
+    clusterCrossingPenalty,
+    //! @brief  This penalty is applied whenever a connector path shares 
+    //!         some segments with an immovable portion of an existing 
+    //!         connector route (such as the first or last segment of a
+    //!         connector).
+    //! @note   This penalty is still experimental!  It is not recommended
+    //!         for normal use.
+    fixedSharedPathPenalty,
+    // Used for determining the size of the penalty array.  
+    // This should always we the last value in the enum.
+    lastPenaltyMarker
+};
+
+
+static const double noPenalty = 0;
+static const double chooseSensiblePenalty = -1;
+
 
+//! @brief   The Router class represents a libavoid router instance.
+//!
+//! Usually you would keep a separate Router instance for each diagram
+//! or layout you have open in your application.
+//
 class Router {
     public:
-        Router();
+        //! @brief  Constructor for router instance.
+        //!
+        //! @param[in]  flags  One or more Avoid::RouterFlag options to 
+        //!                    control the behaviour of the router.
+        Router(const unsigned int flags);
+
+        //! @brief  Destructor for router instance.
+        //!
+        //! @note   Destroying a router instance will delete all remaining
+        //!         shapes and connectors, thereby invalidating any existing
+        //!         pointers to them.
+        ~Router();
 
         ShapeRefList shapeRefs;
         ConnRefList connRefs;
+        ClusterRefList clusterRefs;
         EdgeList visGraph;
         EdgeList invisGraph;
+        EdgeList visOrthogGraph;
         ContainsMap contains;
         VertInfList vertices;
+        ContainsMap enclosingClusters;
         
         bool PartialTime;
         bool SimpleRouting;
-        double segmt_penalty;
-        double angle_penalty;
-        double crossing_penalty;
-
+        bool ClusteredRouting;
 
-        bool UseAStarSearch;
+        // Poly-line routing options:
         bool IgnoreRegions;
-        bool SelectiveReroute;
-        bool IncludeEndpoints;
         bool UseLeesAlgorithm;
         bool InvisibilityGrph;
-        bool ConsolidateMoves;
+       
+        // General routing options:
+        bool SelectiveReroute;
+        
         bool PartialFeedback;
+        bool RubberBandRouting;
+        
 
         // Instrumentation:
         Timer timers;
         int st_checked_edges;
-#ifdef LINEDEBUG
+#ifdef LIBAVOID_SDL
         SDL_Surface *avoid_screen;
 #endif
 
+        //! @brief Allows setting of the behaviour of the router in regard
+        //!        to transactions.  This controls whether transactions are
+        //!        used to queue changes and process them effeciently at once
+        //!        or they are instead processed immediately.
+        //!
+        //! It is more efficient to perform actions like shape movement,
+        //! addition or deletion as batch tasks, and reroute the necessary
+        //! connectors just once after these actions have been performed.
+        //! For this reason, libavoid allows you to group such actions into
+        //! "transactions" that are processed efficiently when the 
+        //! processTransaction() method is called.
+        //!
+        //! By default, the router will process all actions as tranactions.
+        //! If transactionUse() is set to false, then all actions will get 
+        //! processed immediately, and cause immediate routing callbacks to 
+        //! all affected connectors after each action.
+        //!
+        //! @param[in]  transactions  A boolean value specifying whether to
+        //!                           use transactions.
+        //!
+        void setTransactionUse(const bool transactions);
+
+        //! @brief Reports whether the router groups actions into transactions.
+        //!
+        //! @return A boolean value describing whether transactions are in use.
+        //!
+        //! @sa setTransactionUse
+        //! @sa processTransaction
+        //!
+        bool transactionUse(void) const;
+
+        //! @brief Finishes the current transaction and processes all the 
+        //!        queued object changes efficiently.
+        //!
+        //! This method will efficiently process all moves, additions and
+        //! deletions that have occurred since processTransaction() was 
+        //! last called.
+        //!
+        //! If transactionUse() is false, then all actions will have been 
+        //! processed immediately and this method will do nothing.
+        //!
+        //! @return A boolean value describing whether there were any actions
+        //!         to process.
+        //!
+        //! @sa setTransactionUse
+        //!
+        bool processTransaction(void);
+        
+        //! @brief Add a shape to the router scene.
+        //!
+        //! This shape will be considered to be an obstacle. Calling this 
+        //! method will cause connectors intersecting the added shape to
+        //! be marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape  Pointer reference to the shape being added.
+        //!
         void addShape(ShapeRef *shape);
-        void delShape(ShapeRef *shape);
-        void moveShape(ShapeRef *shape, Polygn *newPoly,
+
+        //! @brief Remove a shape from the router scene.
+        //!
+        //! Connectors that could have a better (usually shorter) path after
+        //! the removal of this shape will be marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape  Pointer reference to the shape being removed.
+        //!
+        void removeShape(ShapeRef *shape);
+
+        //! @brief Move or resize an existing shape within the router scene.
+        //!
+        //! A new polygon for the shape can be given to effectively move or 
+        //! resize the shape with the scene.  Connectors that intersect the 
+        //! new shape polygon, or that could have a better (usually shorter)
+        //! path after the change, will be marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape       Pointer reference to the shape being 
+        //!                         moved/resized.
+        //! @param[in]  newPoly     The new polygon boundary for the shape.
+        //! @param[in]  first_move  This option is used for some advanced 
+        //!                         (currently undocumented) behaviour and 
+        //!                         it should be ignored for the moment.
+        //!
+        void moveShape(ShapeRef *shape, const Polygon& newPoly,
                 const bool first_move = false);
-        void processMoves(void);
-        
+
+        //! @brief Move an existing shape within the router scene by a relative
+        //!        distance.
+        //!         
+        //! Connectors that intersect the shape's new position, or that could 
+        //! have a better (usually shorter) path after the change, will be 
+        //! marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape       Pointer reference to the shape being moved.
+        //! @param[in]  xDiff       The distance to move the shape in the 
+        //!                         x dimension.
+        //! @param[in]  yDiff       The distance to move the shape in the 
+        //!                         y dimension.
+        //!
+        void moveShape(ShapeRef *shape, const double xDiff, const double yDiff);
+
+        //! @brief Sets a spacing distance for overlapping orthogonal 
+        //!        connectors to be nudged apart.
+        //!         
+        //! By default, this distance is set to a value of 4.
+        //!
+        //! This method does not re-trigger post-processing of connectors.
+        //! The new distance will be used the next time rerouting is performed.
+        //!
+        //! @param[in]  dist  The distance to be used for orthogonal nudging.
+        //!
+        void setOrthogonalNudgeDistance(const double dist);
+
+        //! @brief   Returns the spacing distance that overlapping orthogonal
+        //!          connecotrs are nudged apart.
+        //!
+        //! @return  The current spacing distance used for orthogonal nudging.
+        //!
+        double orthogonalNudgeDistance(void) const;
+
+        //! @brief  Sets or removes penalty values that are applied during 
+        //!         connector routing.
+        //!
+        //! By default, libavoid will produce shortest path routes between
+        //! the source and destination points for each connector.  There are
+        //! several penalties that can be applied during this stage to 
+        //! improve the aesthetics of the routes generated.  These different
+        //! penalties are specified and explained by the PenaltyType enum.
+        //! 
+        //! If a value of zero or Avoid::noPenalty is given then the penalty 
+        //! for this case will be removed.  If no penalty argument (or a 
+        //! negative value) is specified when calling this method, then a 
+        //! sensible penalty value will be automatically chosen.
+        //!
+        //! @param[in] penType  The type of penalty, a PenaltyType.
+        //! @param[in] penVal   The value to be applied for each occurance
+        //!                     of the penalty case.  
+        //!
+        void setRoutingPenalty(const PenaltyType penType, 
+                const double penVal = chooseSensiblePenalty);
+
+        //! @brief  Returns the current penalty value for a particular 
+        //!         routing penalty case.
+        //!
+        //! @param[in] penType  The type of penalty, a PenaltyType.
+        //! @return  The penalty value for the specified penalty case.
+        //!
+        double routingPenalty(const PenaltyType penType) const;
+
+        void addCluster(ClusterRef *cluster);
+        void delCluster(ClusterRef *cluster);
+
         void attachedConns(IntList &conns, const unsigned int shapeId,
                 const unsigned int type);
         void attachedShapes(IntList &shapes, const unsigned int shapeId,
@@ -98,15 +329,49 @@ class Router {
         void markConnectors(ShapeRef *shape);
         void generateContains(VertInf *pt);
         void printInfo(void);
+        void outputInstanceToSVG(std::string filename = std::string());
+        unsigned int assignId(const unsigned int suggestedId);
+        void regenerateStaticBuiltGraph(void);
+        void destroyOrthogonalVisGraph(void);
+        void setStaticGraphInvalidated(const bool invalidated);
+        ConnType validConnType(const ConnType select = ConnType_None) const;
+        bool shapeInQueuedActionList(ShapeRef *shape) const;
+        double& penaltyRef(const PenaltyType penType);
+        bool existsOrthogonalPathOverlap(void);
+        bool existsOrthogonalTouchingCorners(void);
+
     private:
-        void newBlockingShape(Polygn *poly, int pid);
+        friend class ConnRef;
+
+        void modifyConnector(ConnRef *conn);
+        void modifyConnector(ConnRef *conn, unsigned int type,
+                const ConnEnd &connEnd);
+        void removeQueuedConnectorActions(ConnRef *conn);
+        void newBlockingShape(const Polygon& poly, int pid);
         void checkAllBlockedEdges(int pid);
         void checkAllMissingEdges(void);
-        void adjustContainsWithAdd(const Polygn& poly, const int p_shape);
+        void adjustContainsWithAdd(const Polygon& poly, const int p_shape);
         void adjustContainsWithDel(const int p_shape);
-        void callbackAllInvalidConnectors(void);
+        void adjustClustersWithAdd(const PolygonInterface& poly, 
+                const int p_cluster);
+        void adjustClustersWithDel(const int p_cluster);
+        void rerouteAndCallbackConnectors(void);
+        bool idIsUnique(const unsigned int id) const;
+        void improveCrossings(void);
+
+        ActionInfoList actionList;
+        unsigned int _largestAssignedId;
+        bool _consolidateActions;
+        double _orthogonalNudgeDistance;
+        double _routingPenalties[lastPenaltyMarker];
+
+    public:
+        // Overall modes:
+        bool _polyLineRouting;
+        bool _orthogonalRouting;
 
-        MoveInfoList moveList;
+        bool _staticGraphInvalidated;
+        bool _inCrossingPenaltyReroutingStage;
 };
 
 }
diff --git a/src/libavoid/shape.cpp b/src/libavoid/shape.cpp
index c0ff2f6e8..ac3f0c193 100644
--- a/src/libavoid/shape.cpp
+++ b/src/libavoid/shape.cpp
@@ -2,53 +2,57 @@
  * 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-2008  Monash University
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
-#include <cassert>
 
 #include "libavoid/shape.h"
 #include "libavoid/graph.h"  // For alertConns
 #include "libavoid/vertices.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/router.h"
+#include "libavoid/debug.h"
+#include "libavoid/assertions.h"
 
 
 namespace Avoid {
 
 
-ShapeRef::ShapeRef(Router *router, unsigned int id, Polygn& ply)
+ShapeRef::ShapeRef(Router *router, Polygon& ply, const unsigned int id)
     : _router(router)
-    , _id(id)
-    , _poly(copyPoly(ply))
+    , _poly(ply)
     , _active(false)
     , _inMoveList(false)
     , _firstVert(NULL)
     , _lastVert(NULL)
 {
+    _id = router->assignId(id);
+
     bool isShape = true;
-    VertID i = VertID(id, isShape, 0);
+    VertID i = VertID(_id, isShape, 0);
     
+    const bool addToRouterNow = false;
     VertInf *last = NULL;
     VertInf *node = NULL;
-    for (int pt_i = 0; pt_i < _poly.pn; pt_i++)
+    for (size_t pt_i = 0; pt_i < _poly.size(); ++pt_i)
     {
-        node = new VertInf(_router, i, _poly.ps[pt_i]);
+        node = new VertInf(_router, i, _poly.ps[pt_i], addToRouterNow);
 
         if (!_firstVert)
         {
@@ -69,17 +73,22 @@ ShapeRef::ShapeRef(Router *router, unsigned int id, Polygn& ply)
     
     _lastVert->shNext = _firstVert;
     _firstVert->shPrev = _lastVert;
-    
-    makeActive();
 }
 
 
 ShapeRef::~ShapeRef()
 {
-    assert(_firstVert != NULL);
-    
-    makeInactive();
+    COLA_ASSERT(!_router->shapeInQueuedActionList(this));
 
+    if (_active)
+    {
+        // Destroying a shape without calling removeShape(), so do it now.
+        _router->removeShape(this);
+        _router->processTransaction();
+    }
+
+    COLA_ASSERT(_firstVert != NULL);
+    
     VertInf *it = _firstVert;
     do
     {
@@ -90,41 +99,37 @@ ShapeRef::~ShapeRef()
     }
     while (it != _firstVert);
     _firstVert = _lastVert = NULL;
-
-    freePoly(_poly);
 }
 
 
-void ShapeRef::setNewPoly(Polygn& poly)
+void ShapeRef::setNewPoly(const Polygon& poly)
 {
-    assert(_firstVert != NULL);
-    assert(_poly.pn == poly.pn);
+    COLA_ASSERT(_firstVert != NULL);
+    COLA_ASSERT(_poly.size() == poly.size());
     
     VertInf *curr = _firstVert;
-    for (int pt_i = 0; pt_i < _poly.pn; pt_i++)
+    for (size_t pt_i = 0; pt_i < _poly.size(); ++pt_i)
     {
-        assert(curr->visListSize == 0);
-        assert(curr->invisListSize == 0);
+        COLA_ASSERT(curr->visListSize == 0);
+        COLA_ASSERT(curr->invisListSize == 0);
 
         // Reset with the new polygon point.
         curr->Reset(poly.ps[pt_i]);
         curr->pathNext = NULL;
-        curr->pathDist = 0;
         
         curr = curr->shNext;
     }
-    assert(curr == _firstVert);
+    COLA_ASSERT(curr == _firstVert);
         
-    freePoly(_poly);
-    _poly = copyPoly(poly);
+    _poly = poly;
 }
 
 
 void ShapeRef::makeActive(void)
 {
-    assert(!_active);
+    COLA_ASSERT(!_active);
     
-    // Add to connRefs list.
+    // Add to shapeRefs list.
     _pos = _router->shapeRefs.insert(_router->shapeRefs.begin(), this);
 
     // Add points to vertex list.
@@ -144,9 +149,9 @@ void ShapeRef::makeActive(void)
 
 void ShapeRef::makeInactive(void)
 {
-    assert(_active);
+    COLA_ASSERT(_active);
     
-    // Remove from connRefs list.
+    // Remove from shapeRefs list.
     _router->shapeRefs.erase(_pos);
 
     // Remove points from vertex list.
@@ -162,7 +167,13 @@ void ShapeRef::makeInactive(void)
     
     _active = false;
 }
-    
+
+
+bool ShapeRef::isActive(void) const
+{
+    return _active;
+}
+
 
 VertInf *ShapeRef::firstVert(void)
 {
@@ -176,19 +187,19 @@ VertInf *ShapeRef::lastVert(void)
 }
 
 
-unsigned int ShapeRef::id(void)
+unsigned int ShapeRef::id(void) const
 {
     return _id;
 }
 
 
-Polygn ShapeRef::poly(void)
+const Polygon& ShapeRef::polygon(void) const
 {
     return _poly;
 }
 
 
-Router *ShapeRef::router(void)
+Router *ShapeRef::router(void) const
 {
     return _router;
 }
@@ -196,13 +207,13 @@ Router *ShapeRef::router(void)
 
 void ShapeRef::boundingBox(BBox& bbox)
 {
-    assert(_poly.pn > 0);
+    COLA_ASSERT(!_poly.empty());
 
     bbox.a = bbox.b = _poly.ps[0];
     Point& a = bbox.a;
     Point& b = bbox.b;
 
-    for (int i = 1; i < _poly.pn; ++i)
+    for (size_t i = 1; i < _poly.size(); ++i)
     {
         const Point& p = _poly.ps[i];
 
@@ -223,8 +234,8 @@ void ShapeRef::removeFromGraph(void)
         
         // For each vertex.
         EdgeInfList& visList = tmp->visList;
-        EdgeInfList::iterator finish = visList.end();
-        EdgeInfList::iterator edge;
+        EdgeInfList::const_iterator finish = visList.end();
+        EdgeInfList::const_iterator edge;
         while ((edge = visList.begin()) != finish)
         {
             // Remove each visibility edge
@@ -239,6 +250,15 @@ void ShapeRef::removeFromGraph(void)
             // Remove each invisibility edge
             delete (*edge);
         }
+
+        EdgeInfList& orthogList = tmp->orthogVisList;
+        finish = orthogList.end();
+        while ((edge = orthogList.begin()) != finish)
+        {
+            // Remove each orthogonal visibility edge
+            (*edge)->alertConns();
+            delete (*edge);
+        }
     }
 }
 
@@ -251,8 +271,8 @@ void ShapeRef::markForMove(void)
     }
     else
     {
-        fprintf(stderr, "WARNING: two moves queued for same shape prior to "
-                "rerouting.\n         This is not safe.\n");
+        db_printf("WARNING: two moves queued for same shape prior to rerouting."
+                "\n         This is not safe.\n");
     }
 }
 
diff --git a/src/libavoid/shape.h b/src/libavoid/shape.h
index b654c6eea..df4c98df1 100644
--- a/src/libavoid/shape.h
+++ b/src/libavoid/shape.h
@@ -2,24 +2,30 @@
  * 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-2008  Monash University
  *
  * 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>
 */
 
+//! @file    shape.h
+//! @brief   Contains the interface for the ShapeRef class.
+
+
 #ifndef AVOID_SHAPE_H
 #define AVOID_SHAPE_H
 
@@ -35,21 +41,64 @@ class ShapeRef;
 typedef std::list<ShapeRef *> ShapeRefList;
 
 
+//! @brief   The ShapeRef class represents a shape object.
+//!
+//! Shapes are obstacles that connectors must be routed around.  They can be 
+//! placed into a Router scene and can be repositioned or resized (via
+//! Router::moveShape()).
+//! 
+//! Usually, it is expected that you would create a ShapeRef for each shape 
+//! in your diagram and keep that reference in your own shape class.
+//!
 class ShapeRef
 {
     public:
-        ShapeRef(Router *router, unsigned int id, Polygn& poly);
-        virtual ~ShapeRef();
-        void setNewPoly(Polygn& poly);
+        //! @brief  Shape reference constructor.
+        //!
+        //! Creates a shape obect reference, but does not yet place it into the
+        //! Router scene.
+        //!
+        //! The poly argument will usually be the boundary of the shape in your 
+        //! application with additional buffer of several pixels on each side.
+        //! Specifying such a buffer results in connectors leaving a small 
+        //! amount of space around shapes, rather than touching them on the 
+        //! corners or edges.
+        //!
+        //! If an ID is not specified, then one will be assigned to the shape.
+        //! If assigning an ID yourself, note that it should be a unique 
+        //! positive integer.  Also, IDs are given to all objects in a scene,
+        //! so the same ID cannot be given to a shape and a connector for 
+        //! example.
+        //!
+        //! @param[in]  router  The router scene to place the shape into.
+        //! @param[in]  poly    A Polygon representing the boundary of the 
+        //!                     shape.
+        //! @param[in]  id      A unique positive integer ID for the shape.  
+        ShapeRef(Router *router, Polygon& poly, const unsigned int id = 0);
+        //! @brief  Shape reference destructor.
+        //!
+        //! This will call Router::removeShape() for this shape, if this has
+        //! not already be called.
+        ~ShapeRef();
+        
+        //! @brief   Returns the ID of this shape.
+        //! @returns The ID of the shape. 
+        unsigned int id(void) const;
+        //! @brief   Returns a reference to the polygon boundary of this shape.
+        //! @returns A reference to the polygon boundary of the shape.
+        const Polygon& polygon(void) const;
+        //! @brief   Returns a pointer to the router scene this shape is in.
+        //! @returns A pointer to the router scene for this shape.
+        Router *router(void) const;
+        
+        void setNewPoly(const Polygon& poly);
         VertInf *firstVert(void);
         VertInf *lastVert(void);
-        unsigned int id(void);
-        Polygn poly(void);
-        Router *router(void);
         void boundingBox(BBox& bbox);
 
         void makeActive(void);
         void makeInactive(void);
+        bool isActive(void) const;
 
         void removeFromGraph(void);
         void markForMove(void);
@@ -60,7 +109,7 @@ class ShapeRef
     private:
         Router *_router;
         unsigned int _id;
-        Polygn _poly;
+        Polygon _poly;
         bool _active;
         bool _inMoveList;
         ShapeRefList::iterator _pos;
diff --git a/src/libavoid/static.cpp b/src/libavoid/static.cpp
deleted file mode 100644
index 740a4f9c0..000000000
--- a/src/libavoid/static.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * 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>
- *
- * 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 <cassert>
-#include <iostream>
-#include "libavoid/vertices.h"
-#include "libavoid/connector.h"
-#include "libavoid/graph.h"
-#include "libavoid/static.h"
-#include "libavoid/shape.h"
-#include "libavoid/visibility.h"
-#include "libavoid/debug.h"
-#include "libavoid/router.h"
-
-namespace Avoid {
-
-static void computeCompleteVis(Router *router);
-
-
-// This should only be used for the static algorithm.
-//
-// XXX: If to set up the vis graph for incremental it would need 
-//      the shapeRef pointers in obs.
-//
-void CreateVisGraph(Router *router, Polygn **obs, int n_obs)
-{
-    for (int poly_i = 0; poly_i < n_obs; poly_i++)
-    {
-        unsigned int id = obs[poly_i]->id;
-        
-        new ShapeRef(router, id, *(obs[poly_i]));
-    }
-    computeCompleteVis(router);
-}
-
-
-static void computeCompleteVis(Router *router)
-{
-    VertInf *beginVert = router->vertices.shapesBegin();
-    VertInf *endVert = router->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 DestroyVisGraph(Router *router)
-{
-    ShapeRefList::iterator sFinish = router->shapeRefs.end();
-    ShapeRefList::iterator sCurr;
-    
-    while ((sCurr = router->shapeRefs.begin()) != sFinish)
-    {
-        ShapeRef *shape = (*sCurr);
-
-        shape->removeFromGraph();
-        delete shape;
-    }
-    
-    ConnRefList::iterator cFinish = router->connRefs.end();
-    ConnRefList::iterator cCurr;
-    
-    while ((cCurr = router->connRefs.begin())!= cFinish)
-    {
-        ConnRef *conn = (*cCurr);
-
-        conn->removeFromGraph();
-        conn->unInitialise();
-    }
-    // Clear contains info.
-    router->contains.clear();
-
-    assert(router->vertices.connsBegin() == NULL);
-}
-
-
-}
-
diff --git a/src/libavoid/static.h b/src/libavoid/static.h
deleted file mode 100644
index 18e9ac278..000000000
--- a/src/libavoid/static.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * 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>
- *
- * 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
- * 
-*/
-
-
-#ifndef AVOID_STATIC_H
-#define AVOID_STATIC_H
-
-#include "libavoid/geomtypes.h"
-
-
-namespace Avoid {
-
-class Router;
-
-
-extern void CreateVisGraph(Router *router, Polygn **obstacles,
-        int n_obstacles);
-extern void DestroyVisGraph(Router *router);
-
-}
-
-
-#endif
diff --git a/src/libavoid/timer.cpp b/src/libavoid/timer.cpp
index e4349bea9..f8600acbe 100644
--- a/src/libavoid/timer.cpp
+++ b/src/libavoid/timer.cpp
@@ -2,31 +2,34 @@
  * 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-2008  Monash University
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #include <cstdio>
 #include <cstdlib>
-#include <cassert>
-using std::abort;
 #include <climits>
 
 #include "libavoid/timer.h"
+#include "libavoid/debug.h"
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
@@ -52,9 +55,9 @@ void Timer::Reset(void)
 }
 
 
-void Timer::Register(const int t, const bool start)
+void Timer::Register(const TimerIndex t, const bool start)
 {
-    assert(t != tmNon);
+    COLA_ASSERT(t != tmNon);
 
     if (type == tmNon)
     {
@@ -73,11 +76,7 @@ void Timer::Register(const int t, const bool start)
 
 void Timer::Start(void)
 {
-    if (running)
-    {
-        fprintf(stderr, "ERROR: Timer already running in Timer::Start()\n");
-        abort();
-    }
+    COLA_ASSERT(!running);
     cStart[type] = clock();  // CPU time
     running = true;
 }
@@ -85,11 +84,7 @@ void Timer::Start(void)
 
 void Timer::Stop(void)
 {
-    if (!running)
-    {
-        fprintf(stderr, "ERROR: Timer not running in Timer::Stop()\n");
-        abort();
-    }
+    COLA_ASSERT(running);
     clock_t cStop = clock();      // CPU time
     running = false;
 
@@ -106,11 +101,7 @@ void Timer::Stop(void)
         cDiff = cStop - cStart[type];
     }
     
-    if (cDiff > LONG_MAX)
-    {
-        fprintf(stderr, "Error: cDiff overflow in Timer:Stop()\n");
-        abort();
-    }
+    COLA_ASSERT(cDiff < LONG_MAX);
 
     if (type == tmPth)
     {
@@ -136,11 +127,11 @@ void Timer::Stop(void)
 }
 
 
-void Timer::PrintAll(void)
+void Timer::PrintAll(FILE *fp)
 {
-    for (int i = 0; i < tmCount; i++)
+    for (unsigned int i = 0; i < tmCount; i++)
     {
-        Print(i);
+        Print((TimerIndex) i, fp);
     }
 }
 
@@ -148,14 +139,14 @@ void Timer::PrintAll(void)
 #define toMsec(tot) ((bigclock_t) ((tot) / (((double) CLOCKS_PER_SEC) / 1000)))
 #define toAvg(tot, cnt) ((((cnt) > 0) ? ((long double) (tot)) / (cnt) : 0))
 
-void Timer::Print(const int t)
+void Timer::Print(const TimerIndex t, FILE *fp)
 {
    bigclock_t avg = toMsec(toAvg(cTotal[t], cTally[t]));
    bigclock_t pind = toMsec(toAvg(cPath[t], cPathTally[t]));
    bigclock_t pavg = toMsec(toAvg(cPath[t], cTally[t]));
    double max = toMsec(cMax[t]); 
    double pmax = toMsec(cPathMax[t]);
-   printf("\t%lld %d %lld %.0f %lld %d %lld %.0f %lld\n",
+   fprintf(fp, "\t%lld %d %lld %.0f %lld %d %lld %.0f %lld\n",
            cTotal[t], cTally[t], avg, max,
            cPath[t], cPathTally[t], pavg, pmax, pind);
 }
diff --git a/src/libavoid/timer.h b/src/libavoid/timer.h
index a7e6081fa..9cab6d7ff 100644
--- a/src/libavoid/timer.h
+++ b/src/libavoid/timer.h
@@ -2,24 +2,27 @@
  * 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-2008  Monash University
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef PROFILE_H
 #define PROFILE_H
 
@@ -46,14 +49,20 @@ namespace Avoid {
 
 typedef unsigned long long int bigclock_t;
 
-static const int tmCount = 5;
-
-static const int tmNon = -1;
-static const int tmAdd = 0;
-static const int tmDel = 1;
-static const int tmMov = 2;
-static const int tmPth = 3;
-static const int tmSev = 4;
+enum TimerIndex 
+{
+    tmNon = 0,
+    tmAdd,
+    tmDel,
+    tmMov,
+    tmPth,
+    tmSev,
+    tmOrthogGraph,
+    tmOrthogRoute,
+    tmOrthogCentre,
+    tmOrthogNudge,
+    tmCount
+};
 
 
 static const bool timerStart = true;
@@ -64,12 +73,12 @@ class Timer
 {
     public:
         Timer();
-        void Register(const int t, const bool start = timerDelay);
+        void Register(const TimerIndex t, const bool start = timerDelay);
         void Start(void);
         void Stop(void);
         void Reset(void);
-        void Print(const int t);
-        void PrintAll(void);
+        void Print(TimerIndex, FILE *fp);
+        void PrintAll(FILE *fp);
 
     private:
         clock_t cStart[tmCount];
@@ -82,7 +91,7 @@ class Timer
 
         bool running;
         long count;
-        int type, lasttype;
+        TimerIndex type, lasttype;
 };
 
 
diff --git a/src/libavoid/vertices.cpp b/src/libavoid/vertices.cpp
index c2be955ac..85226498a 100644
--- a/src/libavoid/vertices.cpp
+++ b/src/libavoid/vertices.cpp
@@ -2,33 +2,36 @@
  * 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
  *
  * 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 <iostream>
+#include <cstdlib>
+
 #include "libavoid/vertices.h"
 #include "libavoid/geometry.h"
 #include "libavoid/graph.h"  // For alertConns
 #include "libavoid/debug.h"
 #include "libavoid/router.h"
-
-#include <iostream>
-#include <cstdlib>
-#include <cassert>
+#include "libavoid/assertions.h"
 
 using std::ostream;
 
@@ -76,7 +79,8 @@ bool VertID::operator==(const VertID& rhs) const
     {
         return false;
     }
-    assert(isShape == rhs.isShape);
+    // XXX RubberBand search breaks this:
+    // COLA_ASSERT(isShape == rhs.isShape);
     return true;
 }
 
@@ -87,7 +91,7 @@ bool VertID::operator!=(const VertID& rhs) const
     {
         return true;
     }
-    assert(isShape == rhs.isShape);
+    COLA_ASSERT(isShape == rhs.isShape);
     return false;
 }
 
@@ -133,8 +137,8 @@ void VertID::db_print(void) const
 }
 
 
-const int VertID::src = 1;
-const int VertID::tar = 2;
+const unsigned short VertID::src = 1;
+const unsigned short VertID::tar = 2;
 
 
 ostream& operator<<(ostream& os, const VertID& vID)
@@ -144,25 +148,57 @@ ostream& operator<<(ostream& os, const VertID& vID)
 
 
 
-VertInf::VertInf(Router *router, const VertID& vid, const Point& vpoint)
-    : _router(router)
-    , id(vid)
-    , point(vpoint)
-    , lstPrev(NULL)
-    , lstNext(NULL)
-    , shPrev(NULL)
-    , shNext(NULL)
-    , visListSize(0)
-    , invisListSize(0)
-    , pathNext(NULL)
-    , pathDist(0)
+VertInf::VertInf(Router *router, const VertID& vid, const Point& vpoint, 
+        const bool addToRouter)
+    : _router(router),
+      id(vid),
+      point(vpoint),
+      lstPrev(NULL),
+      lstNext(NULL),
+      shPrev(NULL),
+      shNext(NULL),
+      visListSize(0),
+      orthogVisListSize(0),
+      invisListSize(0),
+      pathNext(NULL),
+      visDirections(ConnDirNone)
+{
+    point.id = vid.objID;
+    point.vn = vid.vn;
+
+    if (addToRouter)
+    {
+        _router->vertices.addVertex(this);
+    }
+}
+
+
+VertInf::~VertInf()
+{
+}
+
+
+void VertInf::Reset(const VertID& vid, const Point& vpoint)
 {
+    id = vid;
+    point = vpoint;
+    point.id = id.objID;
+    point.vn = id.vn;
 }
 
 
 void VertInf::Reset(const Point& vpoint)
 {
     point = vpoint;
+    point.id = id.objID;
+    point.vn = id.vn;
+}
+
+
+// Returns true if this vertex is not involved in any (in)visibility graphs.
+bool VertInf::orphaned(void)
+{
+    return (visList.empty() && invisList.empty() && orthogVisList.empty());
 }
 
 
@@ -170,15 +206,12 @@ void VertInf::removeFromGraph(const bool isConnVert)
 {
     if (isConnVert)
     {
-        assert(!(id.isShape));
+        COLA_ASSERT(!(id.isShape));
     }
 
-    VertInf *tmp = this;
-
     // For each vertex.
-    EdgeInfList& visList = tmp->visList;
-    EdgeInfList::iterator finish = visList.end();
-    EdgeInfList::iterator edge;
+    EdgeInfList::const_iterator finish = visList.end();
+    EdgeInfList::const_iterator edge;
     while ((edge = visList.begin()) != finish)
     {
         // Remove each visibility edge
@@ -186,7 +219,14 @@ void VertInf::removeFromGraph(const bool isConnVert)
         delete (*edge);
     }
 
-    EdgeInfList& invisList = tmp->invisList;
+    finish = orthogVisList.end();
+    while ((edge = orthogVisList.begin()) != finish)
+    {
+        // Remove each orthogonal visibility edge.
+        (*edge)->alertConns();
+        delete (*edge);
+    }
+
     finish = invisList.end();
     while ((edge = invisList.begin()) != finish)
     {
@@ -208,7 +248,7 @@ bool directVis(VertInf *src, VertInf *dst)
 
     // We better be part of the same instance of libavoid.
     Router *router = src->_router;
-    assert(router == dst->_router);
+    COLA_ASSERT(router == dst->_router);
 
     ContainsMap& contains = router->contains;
     if (!(pID.isShape))
@@ -239,40 +279,40 @@ bool directVis(VertInf *src, VertInf *dst)
 
 
 VertInfList::VertInfList()
-    : _firstShapeVert(NULL)
-    , _firstConnVert(NULL)
-    , _lastShapeVert(NULL)
-    , _lastConnVert(NULL)
-    , _shapeVertices(0)
-    , _connVertices(0)
+    : _firstShapeVert(NULL),
+      _firstConnVert(NULL),
+      _lastShapeVert(NULL),
+      _lastConnVert(NULL),
+      _shapeVertices(0),
+      _connVertices(0)
 {
 }
 
 
 #define checkVertInfListConditions() \
         do { \
-            assert((!_firstConnVert && (_connVertices == 0)) || \
+            COLA_ASSERT((!_firstConnVert && (_connVertices == 0)) || \
                     ((_firstConnVert->lstPrev == NULL) && (_connVertices > 0))); \
-            assert((!_firstShapeVert && (_shapeVertices == 0)) || \
+            COLA_ASSERT((!_firstShapeVert && (_shapeVertices == 0)) || \
                     ((_firstShapeVert->lstPrev == NULL) && (_shapeVertices > 0))); \
-            assert(!_lastShapeVert || (_lastShapeVert->lstNext == NULL)); \
-            assert(!_lastConnVert || (_lastConnVert->lstNext == _firstShapeVert)); \
-            assert((!_firstConnVert && !_lastConnVert) || \
+            COLA_ASSERT(!_lastShapeVert || (_lastShapeVert->lstNext == NULL)); \
+            COLA_ASSERT(!_lastConnVert || (_lastConnVert->lstNext == _firstShapeVert)); \
+            COLA_ASSERT((!_firstConnVert && !_lastConnVert) || \
                     (_firstConnVert &&  _lastConnVert) ); \
-            assert((!_firstShapeVert && !_lastShapeVert) || \
+            COLA_ASSERT((!_firstShapeVert && !_lastShapeVert) || \
                     (_firstShapeVert &&  _lastShapeVert) ); \
-            assert(!_firstShapeVert || _firstShapeVert->id.isShape); \
-            assert(!_lastShapeVert || _lastShapeVert->id.isShape); \
-            assert(!_firstConnVert || !(_firstConnVert->id.isShape)); \
-            assert(!_lastConnVert || !(_lastConnVert->id.isShape)); \
+            COLA_ASSERT(!_firstShapeVert || _firstShapeVert->id.isShape); \
+            COLA_ASSERT(!_lastShapeVert || _lastShapeVert->id.isShape); \
+            COLA_ASSERT(!_firstConnVert || !(_firstConnVert->id.isShape)); \
+            COLA_ASSERT(!_lastConnVert || !(_lastConnVert->id.isShape)); \
         } while(0)
 
 
 void VertInfList::addVertex(VertInf *vert)
 {
     checkVertInfListConditions();
-    assert(vert->lstPrev == NULL);
-    assert(vert->lstNext == NULL);
+    COLA_ASSERT(vert->lstPrev == NULL);
+    COLA_ASSERT(vert->lstNext == NULL);
 
     if (!(vert->id.isShape))
     {
@@ -329,10 +369,18 @@ void VertInfList::addVertex(VertInf *vert)
 }
 
 
-void VertInfList::removeVertex(VertInf *vert)
+// Removes a vertex from the list and returns a pointer to the vertex
+// following the removed one.
+VertInf *VertInfList::removeVertex(VertInf *vert)
 {
+    if (vert == NULL)
+    {
+        return NULL;
+    }
     // Conditions for correct data structure
     checkVertInfListConditions();
+    
+    VertInf *following = vert->lstNext;
 
     if (!(vert->id.isShape))
     {
@@ -421,6 +469,50 @@ void VertInfList::removeVertex(VertInf *vert)
     vert->lstNext = NULL;
 
     checkVertInfListConditions();
+
+    return following;
+}
+
+
+VertInf *VertInfList::getVertexByID(const VertID& id)
+{
+    VertID searchID = id;
+    if (searchID.vn == kUnassignedVertexNumber)
+    {
+        unsigned int topbit = ((unsigned int) 1) << 31;
+        if (searchID.objID & topbit)
+        {
+            searchID.objID = searchID.objID & ~topbit;
+            searchID.vn = VertID::src;
+        }
+        else
+        {
+            searchID.vn = VertID::tar;
+        }
+    }
+    VertInf *last = end();
+    for (VertInf *curr = connsBegin(); curr != last; curr = curr->lstNext)
+    {
+        if (curr->id == searchID)
+        {
+            return curr;
+        }
+    }
+    return NULL;
+}
+
+
+VertInf *VertInfList::getVertexByPos(const Point& p)
+{
+    VertInf *last = end();
+    for (VertInf *curr = shapesBegin(); curr != last; curr = curr->lstNext)
+    {
+        if (curr->point == p)
+        {
+            return curr;
+        }
+    }
+    return NULL;
 }
 
 
@@ -447,6 +539,18 @@ VertInf *VertInfList::end(void)
 }
 
 
+unsigned int VertInfList::connsSize(void) const
+{
+    return _connVertices;
+}
+
+
+unsigned int VertInfList::shapesSize(void) const
+{
+    return _shapeVertices;
+}
+
+
 }
 
 
diff --git a/src/libavoid/vertices.h b/src/libavoid/vertices.h
index 1b0dcf3aa..b07c87f95 100644
--- a/src/libavoid/vertices.h
+++ b/src/libavoid/vertices.h
@@ -2,24 +2,27 @@
  * 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
  *
  * 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>
 */
 
+
 #ifndef AVOID_VERTICES_H
 #define AVOID_VERTICES_H
 
@@ -28,6 +31,7 @@
 #include <map>
 #include <iostream>
 #include <cstdio>
+
 #include "libavoid/geomtypes.h"
 
 namespace Avoid {
@@ -37,16 +41,18 @@ class Router;
 
 typedef std::list<EdgeInf *> EdgeInfList;
 
+typedef unsigned int ConnDirFlags;
+
 
 class VertID
 {
     public:
         unsigned int objID;
         bool isShape;
-        int vn;
+        unsigned short vn;
 
-        static const int src;
-        static const int tar;
+        static const unsigned short src;
+        static const unsigned short tar;
 
         VertID();
         VertID(unsigned int id, bool s, int n);
@@ -64,12 +70,23 @@ class VertID
 };
 
 
+// An ID given to all dummy vertices inserted to allow creation of the
+// orthogonal visibility graph since the vertices in the orthogonal graph 
+// mostly do not correspond to shape corners or connector endpoints.
+//
+static const VertID dummyOrthogID(0, true, 0);
+
+
 class VertInf
 {
     public:
-        VertInf(Router *router, const VertID& vid, const Point& vpoint);
+        VertInf(Router *router, const VertID& vid, const Point& vpoint,
+                const bool addToRouter = true);
+        ~VertInf();
+        void Reset(const VertID& vid, const Point& vpoint);
         void Reset(const Point& vpoint);
         void removeFromGraph(const bool isConnVert = true);
+        bool orphaned(void);
 
         Router *_router;
         VertID id;
@@ -80,28 +97,40 @@ class VertInf
         VertInf *shNext;
         EdgeInfList visList;
         unsigned int visListSize;
+        EdgeInfList orthogVisList;
+        unsigned int orthogVisListSize;
         EdgeInfList invisList;
         unsigned int invisListSize;
         VertInf *pathNext;
-        double pathDist;
+        ConnDirFlags visDirections;
 };
 
 
 bool directVis(VertInf *src, VertInf *dst);
 
 
+// A linked list of all the vertices in the router instance.  All the 
+// connector endpoints are listed first, then all the shape vertices.
+// Dunnny vertices inserted for orthogonal routing are classed as shape
+// vertices but have VertID(0, 0).
+//
 class VertInfList
 {
     public:
         VertInfList();
         void addVertex(VertInf *vert);
-        void removeVertex(VertInf *vert);
+        VertInf *removeVertex(VertInf *vert);
+        VertInf *getVertexByID(const VertID& id);
+        VertInf *getVertexByPos(const Point& p);
         VertInf *shapesBegin(void);
         VertInf *connsBegin(void);
         VertInf *end(void);
-        void stats(void)
+        unsigned int connsSize(void) const;
+        unsigned int shapesSize(void) const;
+        void stats(FILE *fp = stderr)
         {
-            printf("Conns %d, shapes %d\n", _connVertices, _shapeVertices);
+            fprintf(fp, "Conns %d, shapes %d\n", _connVertices, 
+                    _shapeVertices);
         }
     private:
         VertInf *_firstShapeVert;
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
     }
 }
diff --git a/src/libavoid/visibility.h b/src/libavoid/visibility.h
index dd68ac692..92e56d922 100644
--- a/src/libavoid/visibility.h
+++ b/src/libavoid/visibility.h
@@ -2,24 +2,27 @@
  * 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-2008  Monash University
  *
  * 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.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * 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
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_VISIBILITY_H
 #define AVOID_VISIBILITY_H
 
@@ -28,7 +31,9 @@
 
 namespace Avoid {
 
-    
+class ShapeRef;
+class VertInf;
+
 extern void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
             const bool gen_contains = false);
 extern void vertexSweep(VertInf *point);