* Merge from trunk

* Update to new snapping API
* Modify the join action slightly

(bzr r8846.2.11)

38 files changed, 10265 insertions, 2757 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/assertions.h b/src/libavoid/assertions.h
new file mode 100644
index 000000000..0725c4482
--- /dev/null
+++ b/src/libavoid/assertions.h
@@ -0,0 +1,49 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * Copyright (C) 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.  
+ *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
+*/
+
+#ifndef AVOID_ASSERTIONS_H
+#define AVOID_ASSERTIONS_H
+
+#ifdef NDEBUG 
+
+  #define COLA_ASSERT(expr)  static_cast<void>(0)
+
+#else // Not NDEBUG
+
+  #if defined(USE_ASSERT_EXCEPTIONS)
+
+    #include "libvpsc/assertions.h"
+
+  #else
+
+    #include <cassert>
+    #define COLA_ASSERT(expr)  assert(expr)
+
+  #endif
+
+#endif
+
+
+#endif // AVOID_ASSERTIONS_H
+
diff --git a/src/libavoid/connector.cpp b/src/libavoid/connector.cpp
index 647303371..3dbd941a4 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.cpp b/src/libavoid/geomtypes.cpp
new file mode 100644
index 000000000..10bb95a7a
--- /dev/null
+++ b/src/libavoid/geomtypes.cpp
@@ -0,0 +1,548 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * 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.  
+ *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
+*/
+
+
+#include <cmath>
+#include <cfloat>
+#include <cstdlib>
+
+#include "libavoid/geomtypes.h"
+#include "libavoid/shape.h"
+#include "libavoid/router.h"
+#include "libavoid/assertions.h"
+
+
+namespace Avoid
+{
+
+    
+Point::Point() :
+    id(0),
+    vn(kUnassignedVertexNumber)
+{
+}
+
+
+Point::Point(const double xv, const double yv) :
+    x(xv),
+    y(yv),
+    id(0),
+    vn(kUnassignedVertexNumber)
+{
+}
+
+
+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;
+}
+
+
+// Just defined to allow std::set<Point>.  Not particularly meaningful!
+bool Point::operator<(const Point& rhs) const
+{
+    if (x == rhs.x)
+    {
+        return (y < rhs.y);
+    }
+    return (x < rhs.x);
+}
+
+
+double& Point::operator[](const unsigned int dimension)
+{
+    COLA_ASSERT((dimension == 0) || (dimension == 1));
+    return ((dimension == 0) ? x : y);
+}
+
+
+const double& Point::operator[](const unsigned int dimension) const
+{
+    COLA_ASSERT((dimension == 0) || (dimension == 1));
+    return ((dimension == 0) ? x : y);
+}
+
+
+ReferencingPolygon::ReferencingPolygon(const Polygon& poly, const Router *router)
+    : PolygonInterface(),
+      _id(poly._id),
+      ps(poly.size())
+{
+    COLA_ASSERT(router != NULL);
+    for (size_t i = 0; i < poly.size(); ++i)
+    {
+        const Polygon *polyPtr = NULL;
+        for (ShapeRefList::const_iterator sh = router->shapeRefs.begin();
+                sh != router->shapeRefs.end(); ++sh) 
+        {
+            if ((*sh)->id() == poly.ps[i].id)
+            {
+                const Polygon& poly = (*sh)->polygon();
+                polyPtr = &poly;
+                break;
+            }
+        }
+        COLA_ASSERT(polyPtr != NULL);
+        ps[i] = std::make_pair(polyPtr, poly.ps[i].vn);
+    }
+}
+
+
+ReferencingPolygon::ReferencingPolygon()
+    : PolygonInterface()
+{
+    clear();
+}
+
+
+void ReferencingPolygon::clear(void)
+{
+    ps.clear();
+}
+
+
+bool ReferencingPolygon::empty(void) const
+{
+    return ps.empty();
+}
+
+
+size_t ReferencingPolygon::size(void) const
+{
+    return ps.size();
+}
+
+
+int ReferencingPolygon::id(void) const
+{
+    return _id;
+}
+
+
+const Point& ReferencingPolygon::at(size_t index) const 
+{
+    COLA_ASSERT(index < size());
+    const Polygon& poly = *(ps[index].first);
+    unsigned short poly_index = ps[index].second;
+    COLA_ASSERT(poly_index < poly.size());
+
+    return poly.ps[poly_index];
+}
+
+
+void PolygonInterface::getBoundingRect(double *minX, double *minY,
+        double *maxX, double *maxY) const
+{
+    double progressiveMinX = DBL_MAX;
+    double progressiveMinY = DBL_MAX;
+    double progressiveMaxX = -DBL_MAX;
+    double progressiveMaxY = -DBL_MAX;
+
+    for (size_t i = 0; i < size(); ++i)
+    {
+        progressiveMinX = std::min(progressiveMinX, at(i).x);
+        progressiveMinY = std::min(progressiveMinY, at(i).y);
+        progressiveMaxX = std::max(progressiveMaxX, at(i).x);
+        progressiveMaxY = std::max(progressiveMaxY, at(i).y);
+    }
+
+    if (minX)
+    {
+        *minX = progressiveMinX;
+    }
+    if (maxX)
+    {
+        *maxX = progressiveMaxX;
+    }
+    if (minY)
+    {
+        *minY = progressiveMinY;
+    }
+    if (maxY)
+    {
+        *maxY = progressiveMaxY;
+    }
+}
+
+
+Polygon::Polygon()
+    : PolygonInterface()
+{
+    clear();
+}
+
+
+Polygon::Polygon(const int pn)
+    : PolygonInterface(),
+      ps(pn)
+{
+}
+
+
+Polygon::Polygon(const PolygonInterface& poly)
+    : PolygonInterface(),
+      _id(poly.id()),
+      ps(poly.size())
+{
+    for (size_t i = 0; i < poly.size(); ++i)
+    {
+        ps[i] = poly.at(i);
+    }
+}
+
+
+void Polygon::clear(void)
+{
+    ps.clear();
+    ts.clear();
+}
+
+
+bool Polygon::empty(void) const
+{
+    return ps.empty();
+}
+
+
+size_t Polygon::size(void) const
+{
+    return ps.size();
+}
+
+
+int Polygon::id(void) const
+{
+    return _id;
+}
+
+
+const Point& Polygon::at(size_t index) const
+{
+    COLA_ASSERT(index < size());
+
+    return ps[index];
+}
+
+
+static const unsigned int SHORTEN_NONE  = 0;
+static const unsigned int SHORTEN_START = 1;
+static const unsigned int SHORTEN_END   = 2;
+static const unsigned int SHORTEN_BOTH  = SHORTEN_START | SHORTEN_END;
+
+// shorten_line():
+//     Given the two endpoints of a line segment, this function adjusts the
+//     endpoints of the line to shorten the line by shorten_length at either
+//     or both ends.
+//
+static void shorten_line(double& x1, double& y1, double& x2, double& y2, 
+        const unsigned int mode, const double shorten_length)
+{
+    if (mode == SHORTEN_NONE)
+    {
+        return;
+    }
+    
+    double rise = y1 - y2;
+    double run = x1 - x2;
+    double disty = fabs(rise);
+    double distx = fabs(run);
+
+    // Handle case where shorten length is greater than the length of the
+    // line segment.
+    if ((mode == SHORTEN_BOTH) &&
+            (((distx > disty) && ((shorten_length * 2) > distx)) ||
+             ((disty >= distx) && ((shorten_length * 2) > disty))))
+    {
+        x1 = x2 = x1 - (run / 2); 
+        y1 = y2 = y1 - (rise / 2); 
+        return;
+    }
+    else if ((mode == SHORTEN_START) && 
+            (((distx > disty) && (shorten_length > distx)) ||
+             ((disty >= distx) && (shorten_length > disty))))
+    {
+        x1 = x2;
+        y1 = y2;
+        return;
+    }
+    else if ((mode == SHORTEN_END) && 
+            (((distx > disty) && (shorten_length > distx)) ||
+             ((disty >= distx) && (shorten_length > disty))))
+    {
+        x2 = x1;
+        y2 = y1;
+        return;
+    }
+
+    // Handle orthogonal line segments.
+    if (x1 == x2)
+    {
+        // Vertical
+        int sign = (y1 < y2) ? 1: -1;
+        
+        if (mode & SHORTEN_START)
+        {
+            y1 += (sign * shorten_length);
+        }
+        if (mode & SHORTEN_END)
+        {
+            y2 -= (sign * shorten_length);
+        }
+        return;
+    }
+    else if (y1 == y2)
+    {
+        // Horizontal
+        int sign = (x1 < x2) ? 1: -1;
+        
+        if (mode & SHORTEN_START)
+        {
+            x1 += (sign * shorten_length);
+        }
+        if (mode & SHORTEN_END)
+        {
+            x2 -= (sign * shorten_length);
+        }
+        return;
+    }
+    
+    int xpos = (x1 < x2) ? -1 : 1;
+    int ypos = (y1 < y2) ? -1 : 1;
+    
+    double tangent = rise / run;
+   
+    if (mode & SHORTEN_END)
+    {
+        if (disty > distx)
+        {
+            y2 += shorten_length * ypos;
+            x2 += shorten_length * ypos * (1 / tangent);
+        }
+        else if (disty < distx)
+        {
+            y2 += shorten_length * xpos * tangent;
+            x2 += shorten_length * xpos;
+        }
+    }
+
+    if (mode & SHORTEN_START)
+    {
+        if (disty > distx)
+        {
+            y1 -= shorten_length * ypos;
+            x1 -= shorten_length * ypos * (1 / tangent);
+        }
+        else if (disty < distx)
+        {
+            y1 -= shorten_length * xpos * tangent;
+            x1 -= shorten_length * xpos;
+        }
+    }
+}
+
+
+void Polygon::translate(const double xDist, const double yDist)
+{
+    for (size_t i = 0; i < size(); ++i)
+    {
+        ps[i].x += xDist;
+        ps[i].y += yDist;
+    }
+}
+
+
+Polygon Polygon::simplify(void) const
+{
+    Polygon simplified = *this;
+    std::vector<Point>::iterator it = simplified.ps.begin();
+    if (it != simplified.ps.end()) ++it;
+
+    // Combine collinear line segments into single segments:
+    for (size_t j = 2; j < simplified.size(); )
+    {
+        if (vecDir(simplified.ps[j - 2], simplified.ps[j - 1], 
+                simplified.ps[j]) == 0)
+        {
+            // These three points make up two collinear segments, so just
+            // compine them into a single segment.
+            it = simplified.ps.erase(it);
+        }
+        else
+        {
+            ++j;
+            ++it;
+        }
+    }
+
+    return simplified;
+}
+
+
+#define mid(a, b) ((a < b) ? a + ((b - a) / 2) : b + ((a - b) / 2))
+
+
+// curvedPolyline():
+//     Returns a curved approximation of this multi-segment PolyLine, with 
+//     the corners replaced by smooth Bezier curves.  curve_amount describes
+//     how large to make the curves.
+//     The ts value for each point in the returned Polygon describes the 
+//     drawing operation: 'M' (move) marks the first point, a line segment 
+//     is marked with an 'L' and three 'C's (along with the previous point) 
+//     describe the control points of a Bezier curve.
+//
+Polygon Polygon::curvedPolyline(const double curve_amount,
+        const bool closed) const
+{
+    Polygon simplified = this->simplify();
+
+    Polygon curved;
+    size_t num_of_points = size();
+    if (num_of_points <= 2)
+    {
+        // There is only a single segment, do nothing.
+        curved = *this;
+        curved.ts.push_back('M');
+        curved.ts.push_back('L');
+        return curved;
+    }
+
+    // Build the curved polyline:
+    curved._id = _id;
+    double last_x = 0;
+    double last_y = 0;
+    if (closed)
+    {
+        double x1 = simplified.ps[0].x;
+        double y1 = simplified.ps[0].y;
+        double x2 = simplified.ps[1].x;
+        double y2 = simplified.ps[1].y;
+        shorten_line(x1, y1, x2, y2, SHORTEN_START, curve_amount);
+        curved.ps.push_back(Point(x1, y1));
+        curved.ts.push_back('M');
+    }
+    else
+    {
+        curved.ps.push_back(ps[0]);
+        curved.ts.push_back('M');
+    }
+   
+    size_t simpSize = simplified.size();
+    size_t finish = (closed) ? simpSize + 2 : simpSize;
+    for (size_t j = 1; j < finish; ++j)
+    {
+        double x1 = simplified.ps[(simpSize + j - 1) % simpSize].x;
+        double y1 = simplified.ps[(simpSize + j - 1) % simpSize].y;
+        double x2 = simplified.ps[j % simpSize].x;
+        double y2 = simplified.ps[j % simpSize].y;
+
+        double old_x = x1;
+        double old_y = y1;
+        
+        unsigned int mode = SHORTEN_BOTH;
+        if (!closed)
+        {
+            if (j == 1)
+            {
+                mode = SHORTEN_END;
+            }
+            else if (j == (size() - 1))
+            {
+                mode = SHORTEN_START;
+            }
+        }
+        shorten_line(x1, y1, x2, y2, mode, curve_amount);
+
+        if (j > 1)
+        {
+            curved.ts.insert(curved.ts.end(), 3, 'C');
+            curved.ps.push_back(Point(mid(last_x, old_x), mid(last_y, old_y)));
+            curved.ps.push_back(Point(mid(x1, old_x), mid(y1, old_y)));
+            curved.ps.push_back(Point(x1, y1));
+        }
+        if (closed && (j == (finish - 1)))
+        {
+            // Close the path.
+            curved.ts.push_back('Z');
+            curved.ps.push_back(Point(x1, y1));
+            break;
+        }
+        curved.ts.push_back('L');
+        curved.ps.push_back(Point(x2, y2));
+            
+        last_x = x2;
+        last_y = y2;
+    }
+    
+    return curved;
+}
+
+
+Rectangle::Rectangle(const Point& topLeft, const Point& bottomRight)
+    : Polygon(4)
+{
+    double xMin = std::min(topLeft.x, bottomRight.x);
+    double xMax = std::max(topLeft.x, bottomRight.x);
+    double yMin = std::min(topLeft.y, bottomRight.y);
+    double yMax = std::max(topLeft.y, bottomRight.y);
+
+    ps[0] = Point(xMax, yMin);
+    ps[1] = Point(xMax, yMax);
+    ps[2] = Point(xMin, yMax);
+    ps[3] = Point(xMin, yMin);
+}
+
+
+Rectangle::Rectangle(const Point& centre, const double width, 
+        const double height)
+{
+    double halfWidth  = width / 2.0;
+    double halfHeight = height / 2.0;
+    double xMin = centre.x - halfWidth;
+    double xMax = centre.x + halfWidth;
+    double yMin = centre.y - halfHeight;
+    double yMax = centre.y + halfHeight;
+
+    ps[0] = Point(xMax, yMin);
+    ps[1] = Point(xMax, yMax);
+    ps[2] = Point(xMin, yMax);
+    ps[3] = Point(xMin, yMin);
+}
+
+
+}
+
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/makefile b/src/libavoid/makefile
new file mode 100644
index 000000000..e4f83a52d
--- /dev/null
+++ b/src/libavoid/makefile
@@ -0,0 +1,17 @@
+# Convenience stub makefile to call the real Makefile.
+
+
+
+OBJEXT = o
+
+# Explicit so that it's the default rule.
+all:
+	cd .. && $(MAKE) libavoid/all
+
+clean %.a %.$(OBJEXT):
+	cd .. && $(MAKE) libavoid/$@
+
+.PHONY: all clean
+
+.SUFFIXES:
+.SUFFIXES: .a .$(OBJEXT)
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/orthogonal.cpp b/src/libavoid/orthogonal.cpp
new file mode 100644
index 000000000..4a7b0af2d
--- /dev/null
+++ b/src/libavoid/orthogonal.cpp
@@ -0,0 +1,2354 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * Copyright (C) 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.
+ *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
+*/
+
+
+#include <cstdlib>
+#include <cfloat>
+#include <cmath>
+#include <set>
+#include <list>
+#include <algorithm>
+
+#include "libavoid/router.h"
+#include "libavoid/geomtypes.h"
+#include "libavoid/shape.h"
+#include "libavoid/orthogonal.h"
+#include "libavoid/connector.h"
+#include "libavoid/vpsc.h"
+#include "libavoid/assertions.h"
+
+#ifdef LIBAVOID_SDL
+  #include <SDL_gfxPrimitives.h>
+#endif
+
+
+namespace Avoid {
+
+
+static const double CHANNEL_MAX = 100000000;
+
+static const size_t XDIM = 0;
+static const size_t YDIM = 1;
+
+
+class ShiftSegment
+{
+    public:
+        // For shiftable segments.
+        ShiftSegment(ConnRef *conn, const size_t low, const size_t high,
+                bool isSBend, const size_t dim, double minLim, double maxLim)
+            : connRef(conn),
+              indexLow(low),
+              indexHigh(high),
+              sBend(isSBend),
+              fixed(false),
+              dimension(dim),
+              variable(NULL),
+              minSpaceLimit(minLim),
+              maxSpaceLimit(maxLim)
+        {
+        }
+
+        // For fixed segments.
+        ShiftSegment(ConnRef *conn, const size_t low, const size_t high,
+                const size_t dim)
+            : connRef(conn),
+              indexLow(low),
+              indexHigh(high),
+              sBend(false),
+              fixed(true),
+              dimension(dim),
+              variable(NULL)
+        {
+            // This has no space to shift.
+            minSpaceLimit = lowPoint()[dim];
+            maxSpaceLimit = lowPoint()[dim];
+        }
+
+        Point& lowPoint(void)
+        {
+            return connRef->displayRoute().ps[indexLow];
+        }
+
+        Point& highPoint(void)
+        {
+            return connRef->displayRoute().ps[indexHigh];
+        }
+
+        const Point& lowPoint(void) const
+        {
+            return connRef->displayRoute().ps[indexLow];
+        }
+
+        const Point& highPoint(void) const
+        {
+            return connRef->displayRoute().ps[indexHigh];
+        }
+
+        int fixedOrder(bool& isFixed) const
+        {
+            if (fixed)
+            {
+                isFixed = true;
+                return 0;
+            }
+            if (lowC())
+            {
+                return 1;
+            }
+            else if (highC())
+            {
+                return -1;
+            }
+            return 0;
+        }
+
+        int order(void) const
+        {
+            if (lowC())
+            {
+                return -1;
+            }
+            else if (highC())
+            {
+                return 1;
+            }
+            return 0;
+        }
+
+        bool operator<(const ShiftSegment& rhs) const
+        {
+            const Point& lowPt = lowPoint();
+            const Point& rhsLowPt = rhs.lowPoint();
+
+            if (lowPt[dimension] != rhsLowPt[dimension])
+            {
+                return lowPt[dimension] < rhsLowPt[dimension];
+            }
+            return this < &rhs;
+        }
+
+        // This counts segments that are colliear and share an endpoint as
+        // overlapping.  This allows them to be nudged apart where possible.
+        bool overlapsWith(const ShiftSegment& rhs, const size_t dim) const
+        {
+            size_t altDim = (dim + 1) % 2;
+            const Point& lowPt = lowPoint();
+            const Point& highPt = highPoint();
+            const Point& rhsLowPt = rhs.lowPoint();
+            const Point& rhsHighPt = rhs.highPoint();
+            if ( (lowPt[altDim] <= rhsHighPt[altDim]) &&
+                    (rhsLowPt[altDim] <= highPt[altDim]))
+            {
+                if ( (minSpaceLimit <= rhs.maxSpaceLimit) &&
+                        (rhs.minSpaceLimit <= maxSpaceLimit))
+                {
+                    return true;
+                }
+            }
+            return false;
+        }
+
+        ConnRef *connRef;
+        size_t indexLow;
+        size_t indexHigh;
+        bool sBend;
+        bool fixed;
+        size_t dimension;
+        Variable *variable;
+        double minSpaceLimit;
+        double maxSpaceLimit;
+    private:
+        bool lowC(void) const
+        {
+            // This is true if this is a cBend and its adjoining points
+            // are at lower positions.
+            if (!sBend && !fixed && (minSpaceLimit == lowPoint()[dimension]))
+            {
+                return true;
+            }
+            return false;
+        }
+
+        bool highC(void) const
+        {
+            // This is true if this is a cBend and its adjoining points
+            // are at higher positions.
+            if (!sBend && !fixed && (maxSpaceLimit == lowPoint()[dimension]))
+            {
+                return true;
+            }
+            return false;
+        }
+};
+typedef std::list<ShiftSegment> ShiftSegmentList;
+
+bool cmpShiftSegment(const ShiftSegment& u, const ShiftSegment& v)
+{
+    return u < v;
+}
+
+
+struct Node;
+struct CmpNodePos { bool operator()(const Node* u, const Node* v) const; };
+
+typedef std::set<Node*,CmpNodePos> NodeSet;
+struct Node
+{
+    ShapeRef *v;
+    VertInf *c;
+    ShiftSegment *ss;
+    double pos;
+    double min[2], max[2];
+    Node *firstAbove, *firstBelow;
+    NodeSet::iterator iter;
+
+    Node(ShapeRef *v, const double p)
+        : v(v),
+          c(NULL),
+          ss(NULL),
+          pos(p),
+          firstAbove(NULL),
+          firstBelow(NULL)
+    {
+        //COLA_ASSERT(r->width()<1e40);
+        v->polygon().getBoundingRect(&min[0], &min[1], &max[0], &max[1]);
+    }
+    Node(VertInf *c, const double p)
+        : v(NULL),
+          c(c),
+          ss(NULL),
+          pos(p),
+          firstAbove(NULL),
+          firstBelow(NULL)
+    {
+        min[0] = max[0] = c->point.x;
+        min[1] = max[1] = c->point.y;
+    }
+    Node(ShiftSegment *ss, const double p)
+        : v(NULL),
+          c(NULL),
+          ss(ss),
+          pos(p),
+          firstAbove(NULL),
+          firstBelow(NULL)
+    {
+        // These values shouldn't ever be used, so they don't matter.
+        min[0] = max[0] = min[1] = max[1] = 0;
+    }
+    ~Node()
+    {
+    }
+    // Find the first Node above in the scanline that is a shape edge,
+    // and does not have an open or close event at this position (meaning
+    // it is just about to be removed).
+    double firstObstacleAbove(size_t dim)
+    {
+        Node *curr = firstAbove;
+        while (curr && (curr->ss || (curr->max[dim] > pos)))
+        {
+            curr = curr->firstAbove;
+        }
+
+        if (curr)
+        {
+            return curr->max[dim];
+        }
+        return -DBL_MAX;
+    }
+    // Find the first Node below in the scanline that is a shape edge,
+    // and does not have an open or close event at this position (meaning
+    // it is just about to be removed).
+    double firstObstacleBelow(size_t dim)
+    {
+        Node *curr = firstBelow;
+        while (curr && (curr->ss || (curr->min[dim] < pos)))
+        {
+            curr = curr->firstBelow;
+        }
+
+        if (curr)
+        {
+            return curr->min[dim];
+        }
+        return DBL_MAX;
+    }
+    // Mark all connector segments above in the scanline as being able
+    // to see to this shape edge.
+    void markShiftSegmentsAbove(size_t dim)
+    {
+        Node *curr = firstAbove;
+        while (curr && (curr->ss || (curr->pos > min[dim])))
+        {
+            if (curr->ss && (curr->pos <= min[dim]))
+            {
+                curr->ss->maxSpaceLimit =
+                        std::min(min[dim], curr->ss->maxSpaceLimit);
+            }
+            curr = curr->firstAbove;
+        }
+    }
+    // Mark all connector segments below in the scanline as being able
+    // to see to this shape edge.
+    void markShiftSegmentsBelow(size_t dim)
+    {
+        Node *curr = firstBelow;
+        while (curr && (curr->ss || (curr->pos < max[dim])))
+        {
+            if (curr->ss && (curr->pos >= max[dim]))
+            {
+                curr->ss->minSpaceLimit =
+                        std::max(max[dim], curr->ss->minSpaceLimit);
+            }
+            curr = curr->firstBelow;
+        }
+    }
+    bool findFirstPointAboveAndBelow(const size_t dim, double& firstAbovePos,
+            double& firstBelowPos, double& lastAbovePos, double& lastBelowPos)
+    {
+        bool clearVisibility = true;
+        firstAbovePos = -DBL_MAX;
+        firstBelowPos = DBL_MAX;
+        // We start looking left from the right side of the shape,
+        // and vice versa.
+        lastAbovePos = max[dim];
+        lastBelowPos = min[dim];
+
+        // Find the first blocking edge above this point.  Don't count the
+        // edges as we are travelling out of shapes we are inside, but then
+        // mark clearVisibility as false.
+        Node *curr = firstAbove;
+        while (curr && (curr->max[dim] > min[dim]))
+        {
+            lastAbovePos = std::min(curr->min[dim], lastAbovePos);
+            if ((curr->max[dim] >= min[dim]) && (curr->max[dim] <= max[dim]))
+            {
+                lastAbovePos = std::min(curr->max[dim], lastAbovePos);
+            }
+            lastBelowPos = std::max(curr->max[dim], lastBelowPos);
+            clearVisibility = false;
+            curr = curr->firstAbove;
+        }
+        if (curr)
+        {
+            firstAbovePos = curr->max[dim];
+        }
+        while (curr)
+        {
+            // There might be a larger shape after this one in the ordering.
+            if (curr->max[dim] < min[dim])
+            {
+                firstAbovePos = std::max(curr->max[dim], firstAbovePos);
+            }
+            curr = curr->firstAbove;
+        }
+
+        // Find the first blocking edge below this point.  Don't count the
+        // edges as we are travelling out of shapes we are inside, but then
+        // mark clearVisibility as false.
+        curr = firstBelow;
+        while (curr && (curr->min[dim] < max[dim]))
+        {
+            lastBelowPos = std::max(curr->max[dim], lastBelowPos);
+            if ((curr->min[dim] >= min[dim]) && (curr->min[dim] <= max[dim]))
+            {
+                lastBelowPos = std::max(curr->min[dim], lastBelowPos);
+            }
+            lastAbovePos = std::min(curr->min[dim], lastAbovePos);
+            clearVisibility = false;
+            curr = curr->firstBelow;
+        }
+        if (curr)
+        {
+            firstBelowPos = curr->min[dim];
+        }
+        while (curr)
+        {
+            // There might be a larger shape after this one in the ordering.
+            if (curr->min[dim] > max[dim])
+            {
+                firstBelowPos = std::min(curr->min[dim], firstBelowPos);
+            }
+            curr = curr->firstBelow;
+        }
+
+        return clearVisibility;
+    }
+    double firstPointAbove(size_t dim)
+    {
+        Node *curr = firstAbove;
+        while (curr && (curr->max[dim] >= pos))
+        {
+            curr = curr->firstAbove;
+        }
+
+        if (curr)
+        {
+            return curr->max[dim];
+        }
+        return -DBL_MAX;
+    }
+    double firstPointBelow(size_t dim)
+    {
+        Node *curr = firstBelow;
+        while (curr && (curr->min[dim] <= pos))
+        {
+            curr = curr->firstBelow;
+        }
+
+        if (curr)
+        {
+            return curr->min[dim];
+        }
+        return DBL_MAX;
+    }
+    // This is a bit inefficient, but we won't need to do it once we have
+    // connection points.
+    bool isInsideShape(size_t dimension)
+    {
+        for (Node *curr = firstBelow; curr; curr = curr->firstBelow)
+        {
+            if ((curr->min[dimension] < pos) && (pos < curr->max[dimension]))
+            {
+                return true;
+            }
+        }
+        for (Node *curr = firstAbove; curr; curr = curr->firstAbove)
+        {
+            if ((curr->min[dimension] < pos) && (pos < curr->max[dimension]))
+            {
+                return true;
+            }
+        }
+        return false;
+    }
+};
+
+
+bool CmpNodePos::operator() (const Node* u, const Node* v) const
+{
+    if (u->pos != v->pos)
+    {
+        return u->pos < v->pos;
+    }
+
+    // Use the pointers to the base objects to differentiate them.
+    void *up = (u->v) ? (void *) u->v :
+            ((u->c) ? (void *) u->c : (void *) u->ss);
+    void *vp = (v->v) ? (void *) v->v :
+            ((v->c) ? (void *) v->c : (void *) v->ss);
+    return up < vp;
+}
+
+
+// Note: Open must come first.
+typedef enum {
+    Open = 1,
+    SegOpen = 2,
+    ConnPoint = 3,
+    SegClose = 4,
+    Close = 5
+} EventType;
+
+
+struct Event
+{
+    Event(EventType t, Node *v, double p)
+        : type(t),
+          v(v),
+          pos(p)
+    {};
+    EventType type;
+    Node *v;
+    double pos;
+};
+
+Event **events;
+
+
+// Used for quicksort.  Must return <0, 0, or >0.
+int compare_events(const void *a, const void *b)
+{
+	Event *ea = *(Event**) a;
+	Event *eb = *(Event**) b;
+    if (ea->pos != eb->pos)
+    {
+        return (ea->pos < eb->pos) ? -1 : 1;
+    }
+    if (ea->type != eb->type)
+    {
+        return ea->type - eb->type;
+    }
+    COLA_ASSERT(ea->v != eb->v);
+    return ea->v - eb->v;
+}
+
+
+// Returns a bitfield of the direction of visibility (in this dimension)
+// made up of ConnDirDown (for visibility towards lower position values)
+// and ConnDirUp (for visibility towards higher position values).
+//
+static ConnDirFlags getPosVertInfDirection(VertInf *v, size_t dim)
+{
+    if (dim == XDIM) // X-dimension
+    {
+        unsigned int dirs = v->visDirections & (ConnDirLeft | ConnDirRight);
+        if (dirs == (ConnDirLeft | ConnDirRight))
+        {
+            return (ConnDirDown | ConnDirUp);
+        }
+        else if (dirs == ConnDirLeft)
+        {
+            return ConnDirDown;
+        }
+        else if (dirs == ConnDirRight)
+        {
+            return ConnDirUp;
+        }
+    }
+    else if (dim == YDIM) // Y-dimension
+    {
+        unsigned int dirs = v->visDirections & (ConnDirDown | ConnDirUp);
+        if (dirs == (ConnDirDown | ConnDirUp))
+        {
+            return (ConnDirDown | ConnDirUp);
+        }
+        else if (dirs == ConnDirDown)
+        {
+            // For libavoid the Y-axis points downwards, so in terms of
+            // smaller or larger position values, Down is Up and vice versa.
+            return ConnDirUp;
+        }
+        else if (dirs == ConnDirUp)
+        {
+            // For libavoid the Y-axis points downwards, so in terms of
+            // smaller or larger position values, Down is Up and vice versa.
+            return ConnDirDown;
+        }
+    }
+
+    // Can occur for ConnDirNone visibility.
+    return ConnDirNone;
+}
+
+
+struct PosVertInf
+{
+    PosVertInf(double p, VertInf *vI, ConnDirFlags d = ConnDirNone)
+        : pos(p),
+          vert(vI),
+          dir(d)
+    {
+    }
+
+    bool operator<(const PosVertInf& rhs) const
+    {
+        if (pos != rhs.pos)
+        {
+            return pos < rhs.pos;
+        }
+        return vert < rhs.vert;
+    }
+
+    double pos;
+    VertInf *vert;
+
+    // A bitfield marking the direction of visibility (in this dimension)
+    // made up of ConnDirDown (for visibility towards lower position values)
+    // and ConnDirUp (for visibility towards higher position values).
+    //
+    ConnDirFlags dir;
+};
+
+
+struct CmpVertInf
+{
+    bool operator()(const VertInf* u, const VertInf* v) const
+    {
+        // Comparator for VertSet, an ordered set of VertInf pointers.
+        // It is assumed vertical sets of points will all have the same
+        // x position and horizontal sets all share a y position, so this
+        // method can be used to sort both these sets.
+        COLA_ASSERT((u->point.x == v->point.x) || (u->point.y == v->point.y));
+        if (u->point.x != v->point.x)
+        {
+            return u->point.x < v->point.x;
+        }
+        else if (u->point.y != v->point.y)
+        {
+            return u->point.y < v->point.y;
+        }
+        return u < v;
+    }
+};
+
+
+typedef std::set<VertInf *, CmpVertInf> VertSet;
+
+// A set of points to break the line segment,
+// along with vertices for these points.
+typedef std::set<PosVertInf> BreakpointSet;
+
+// Temporary structure used to store the possible horizontal visibility
+// lines arising from the vertical sweep.
+class LineSegment
+{
+public:
+    LineSegment(const double& b, const double& f, const double& p,
+                bool /*ss*/ = false, VertInf *bvi = NULL, VertInf *fvi = NULL)
+        : begin(b),
+          finish(f),
+          pos(p),
+          shapeSide(false)
+    {
+        COLA_ASSERT(begin < finish);
+
+        if (bvi)
+        {
+            vertInfs.insert(bvi);
+        }
+        if (fvi)
+        {
+            vertInfs.insert(fvi);
+        }
+    }
+
+    LineSegment(const double& bf, const double& p, VertInf *bfvi = NULL)
+        : begin(bf),
+          finish(bf),
+          pos(p),
+          shapeSide(false)
+    {
+        if (bfvi)
+        {
+            vertInfs.insert(bfvi);
+        }
+    }
+
+    // Order by begin, pos, finish.
+    bool operator<(const LineSegment& rhs) const
+    {
+        if (begin != rhs.begin)
+        {
+            return begin < rhs.begin;
+        }
+        if (pos != rhs.pos)
+        {
+            return pos < rhs.pos;
+        }
+        if (finish != rhs.finish)
+        {
+            return finish < rhs.finish;
+        }
+        COLA_ASSERT(shapeSide == rhs.shapeSide);
+        return false;
+    }
+
+    bool overlaps(const LineSegment& rhs) const
+    {
+        if ((begin == rhs.begin) && (pos == rhs.pos) &&
+                (finish == rhs.finish))
+        {
+            // Lines are exactly equal.
+            return true;
+        }
+
+        if (pos == rhs.pos)
+        {
+            if (((begin >= rhs.begin) && (begin <= rhs.finish)) ||
+                ((rhs.begin >= begin) && (rhs.begin <= finish)) )
+            {
+                // They are colinear and overlap by some amount.
+                return true;
+            }
+        }
+        return false;
+    }
+
+    void mergeVertInfs(const LineSegment& segment)
+    {
+        begin = std::min(begin, segment.begin);
+        finish = std::max(finish, segment.finish);
+        vertInfs.insert(segment.vertInfs.begin(), segment.vertInfs.end());
+    }
+
+    VertInf *beginVertInf(void) const
+    {
+        if (vertInfs.empty())
+        {
+            return NULL;
+        }
+        return *vertInfs.begin();
+    }
+    VertInf *finishVertInf(void) const
+    {
+        if (vertInfs.empty())
+        {
+            return NULL;
+        }
+        return *vertInfs.rbegin();
+    }
+
+    VertInf *commitPositionX(Router *router, double posX)
+    {
+        VertInf *found = NULL;
+        for (VertSet::iterator v = vertInfs.begin();
+                v != vertInfs.end(); ++v)
+        {
+            if ((*v)->point.x == posX)
+            {
+                found = *v;
+                break;
+            }
+        }
+        if (!found)
+        {
+            found = new VertInf(router, dummyOrthogID, Point(posX, pos));
+            vertInfs.insert(found);
+        }
+        return found;
+    }
+    // Set begin endpoint vertex if none has been assigned.
+    void commitBegin(Router *router, VertInf *vert = NULL)
+    {
+        if (vert)
+        {
+            vertInfs.insert(vert);
+        }
+
+        if (vertInfs.empty() ||
+                ((*vertInfs.begin())->point.x != begin))
+        {
+            vertInfs.insert(new
+                    VertInf(router, dummyOrthogID, Point(begin, pos)));
+        }
+    }
+
+    // Set begin endpoint vertex if none has been assigned.
+    void commitFinish(Router *router, VertInf *vert = NULL)
+    {
+        if (vert)
+        {
+            vertInfs.insert(vert);
+        }
+
+        if (vertInfs.empty() ||
+                ((*vertInfs.rbegin())->point.x != finish))
+        {
+            vertInfs.insert(new
+                    VertInf(router, dummyOrthogID, Point(finish, pos)));
+        }
+    }
+
+    // Converts a section of the points list to a set of breakPoints.
+    // Returns the first of the intersection points occuring at finishPos.
+    VertSet::iterator addSegmentsUpTo(Router */*router*/, double finishPos)
+    {
+        VertSet::iterator firstIntersectionPt = vertInfs.end();
+        for (VertSet::iterator vert = vertInfs.begin();
+                vert != vertInfs.end(); ++vert)
+        {
+            if ((*vert)->point.x > finishPos)
+            {
+                // We're done.
+                break;
+            }
+
+            breakPoints.insert(PosVertInf((*vert)->point.x, (*vert),
+                        getPosVertInfDirection(*vert, XDIM)));
+
+            if ((firstIntersectionPt == vertInfs.end()) &&
+                    ((*vert)->point.x == finishPos))
+            {
+                firstIntersectionPt = vert;
+            }
+        }
+        // Returns the first of the intersection points at finishPos.
+        return firstIntersectionPt;
+    }
+
+    // Add visibility edge(s) for this segment.  There may be multiple if
+    // one of the endpoints is shared by multiple connector endpoints.
+    void addEdgeHorizontal(Router *router)
+    {
+        commitBegin(router);
+        commitFinish(router);
+
+        addSegmentsUpTo(router, finish);
+    }
+
+    // Add visibility edge(s) for this segment up until an intersection.
+    // Then, move the segment beginning to the intersection point, so we
+    // later only consider the remainder of the segment.
+    // There may be multiple segments added to the graph if the beginning
+    // endpoint of the segment is shared by multiple connector endpoints.
+    VertSet addEdgeHorizontalTillIntersection(Router *router,
+            LineSegment& vertLine)
+    {
+        VertSet intersectionSet;
+
+        commitBegin(router);
+
+        // Does a vertex already exist for this point.
+        commitPositionX(router, vertLine.pos);
+
+        // Generate segments and set end iterator to the first point
+        // at the intersection position.
+        VertSet::iterator restBegin = addSegmentsUpTo(router, vertLine.pos);
+
+        // Add the intersections points to intersectionSet.
+        VertSet::iterator restEnd = restBegin;
+        while ((restEnd != vertInfs.end()) &&
+                (*restEnd)->point.x == vertLine.pos)
+        {
+            ++restEnd;
+        }
+        intersectionSet.insert(restBegin, restEnd);
+
+        // Adjust segment to remove processed portion.
+        begin = vertLine.pos;
+        vertInfs.erase(vertInfs.begin(), restBegin);
+
+        return intersectionSet;
+    }
+
+    // Insert vertical breakpoints.
+    void insertBreakpointsBegin(Router *router, LineSegment& vertLine)
+    {
+        VertInf *vert = NULL;
+        if (pos == vertLine.begin && vertLine.beginVertInf())
+        {
+            vert = vertLine.beginVertInf();
+        }
+        else if (pos == vertLine.finish && vertLine.finishVertInf())
+        {
+            vert = vertLine.finishVertInf();
+        }
+        commitBegin(router, vert);
+
+        for (VertSet::iterator v = vertInfs.begin();
+                v != vertInfs.end(); ++v)
+        {
+            if ((*v)->point.x == begin)
+            {
+                vertLine.breakPoints.insert(PosVertInf(pos, *v,
+                        getPosVertInfDirection(*v, YDIM)));
+            }
+        }
+    }
+
+    // Insert vertical breakpoints.
+    void insertBreakpointsFinish(Router *router, LineSegment& vertLine)
+    {
+        VertInf *vert = NULL;
+        if (pos == vertLine.begin && vertLine.beginVertInf())
+        {
+            vert = vertLine.beginVertInf();
+        }
+        else if (pos == vertLine.finish && vertLine.finishVertInf())
+        {
+            vert = vertLine.finishVertInf();
+        }
+        commitFinish(router, vert);
+
+        for (VertSet::iterator v = vertInfs.begin();
+                v != vertInfs.end(); ++v)
+        {
+            if ((*v)->point.x == finish)
+            {
+                vertLine.breakPoints.insert(PosVertInf(pos, *v,
+                        getPosVertInfDirection(*v, YDIM)));
+            }
+        }
+    }
+    void generateVisibilityEdgesFromBreakpointSet(Router *router, size_t dim)
+    {
+        if ((breakPoints.begin())->pos != begin)
+        {
+            if (!beginVertInf())
+            {
+                Point point(pos, pos);
+                point[dim] = begin;
+                // Add begin point if it didn't intersect another line.
+                VertInf *vert = new VertInf(router, dummyOrthogID, point);
+                breakPoints.insert(PosVertInf(begin, vert));
+            }
+        }
+        if ((breakPoints.rbegin())->pos != finish)
+        {
+            if (!finishVertInf())
+            {
+                Point point(pos, pos);
+                point[dim] = finish;
+                // Add finish point if it didn't intersect another line.
+                VertInf *vert = new VertInf(router, dummyOrthogID, point);
+                breakPoints.insert(PosVertInf(finish, vert));
+            }
+        }
+
+        const bool orthogonal = true;
+        BreakpointSet::iterator vert, last;
+        for (vert = last = breakPoints.begin(); vert != breakPoints.end();)
+        {
+            BreakpointSet::iterator firstPrev = last;
+            while (last->vert->point[dim] != vert->vert->point[dim])
+            {
+                COLA_ASSERT(vert != last);
+                // Assert points are not at the same position.
+                COLA_ASSERT(vert->vert->point != last->vert->point);
+
+                if ( !(vert->vert->id.isShape || last->vert->id.isShape))
+                {
+                    // Here we have a pair of two endpoints that are both
+                    // connector endpoints and both are inside a shape.
+
+                    // Give vert visibility back to the first non-connector
+                    // endpoint vertex (i.e., the side of the shape).
+                    BreakpointSet::iterator side = last;
+                    while (!side->vert->id.isShape)
+                    {
+                        if (side == breakPoints.begin())
+                        {
+                            break;
+                        }
+                        --side;
+                    }
+                    bool canSeeDown = (vert->dir & ConnDirDown);
+                    if (canSeeDown && side->vert->id.isShape)
+                    {
+                        EdgeInf *edge = new
+                                EdgeInf(side->vert, vert->vert, orthogonal);
+                        edge->setDist(vert->vert->point[dim] -
+                                side->vert->point[dim]);
+                    }
+
+                    // Give last visibility back to the first non-connector
+                    // endpoint vertex (i.e., the side of the shape).
+                    side = vert;
+                    while ((side != breakPoints.end()) &&
+                            !side->vert->id.isShape)
+                    {
+                        ++side;
+                    }
+                    bool canSeeUp = (last->dir & ConnDirUp);
+                    if (canSeeUp && (side != breakPoints.end()))
+                    {
+                        EdgeInf *edge = new
+                                EdgeInf(last->vert, side->vert, orthogonal);
+                        edge->setDist(side->vert->point[dim] -
+                                last->vert->point[dim]);
+                    }
+                }
+
+                // The normal case.
+                //
+                // Note: It's okay to give two connector endpoints visbility
+                // here since we only consider the partner endpoint as a
+                // candidate while searching if it is the other endpoint of
+                // the connector in question.
+                //
+                bool generateEdge = true;
+                if (!last->vert->id.isShape && !(last->dir & ConnDirUp))
+                {
+                    generateEdge = false;
+                }
+                else if (!vert->vert->id.isShape && !(vert->dir & ConnDirDown))
+                {
+                    generateEdge = false;
+                }
+                if (generateEdge)
+                {
+                    EdgeInf *edge =
+                            new EdgeInf(last->vert, vert->vert, orthogonal);
+                    edge->setDist(vert->vert->point[dim] -
+                            last->vert->point[dim]);
+                }
+
+                ++last;
+            }
+
+            ++vert;
+
+            if ((vert != breakPoints.end()) &&
+                    (last->vert->point[dim] == vert->vert->point[dim]))
+            {
+                // Still looking at same pair, just reset prev number pointer.
+                last = firstPrev;
+            }
+            else
+            {
+                // vert has moved to the beginning of a number number group.
+                // Last is now in the right place, so do nothing.
+            }
+        }
+    }
+
+    double begin;
+    double finish;
+    double pos;
+    bool shapeSide;
+
+    VertSet vertInfs;
+    BreakpointSet breakPoints;
+private:
+	// MSVC wants to generate the assignment operator and the default
+	// constructor, but fails.  Therefore we declare them private and
+	// don't implement them.
+    LineSegment & operator=(LineSegment const &);
+    LineSegment();
+};
+
+typedef std::list<LineSegment> SegmentList;
+
+class SegmentListWrapper
+{
+    public:
+        LineSegment *insert(LineSegment segment)
+        {
+            SegmentList::iterator found = _list.end();
+            for (SegmentList::iterator curr = _list.begin();
+                    curr != _list.end(); ++curr)
+            {
+                if (curr->overlaps(segment))
+                {
+                    if (found != _list.end())
+                    {
+                        // This is not the first segment that overlaps,
+                        // so we need to merge and then delete an existing
+                        // segment.
+                        curr->mergeVertInfs(*found);
+                        _list.erase(found);
+                        found = curr;
+                    }
+                    else
+                    {
+                        // This is the first overlapping segment, so just
+                        // merge the new segment with this one.
+                        curr->mergeVertInfs(segment);
+                        found = curr;
+                    }
+                }
+            }
+
+            if (found == _list.end())
+            {
+                // Add this line.
+                _list.push_back(segment);
+                return &(_list.back());
+            }
+
+            return &(*found);
+        }
+        SegmentList& list(void)
+        {
+            return _list;
+        }
+    private:
+        SegmentList _list;
+};
+
+
+// Given a router instance and a set of possible horizontal segments, and a
+// possible vertical visibility segment, compute and add edges to the
+// orthogonal visibility graph for all the visibility edges.
+static void intersectSegments(Router *router, SegmentList& segments,
+        LineSegment& vertLine)
+{
+    COLA_ASSERT(vertLine.beginVertInf() == NULL);
+    COLA_ASSERT(vertLine.finishVertInf() == NULL);
+    for (SegmentList::iterator it = segments.begin(); it != segments.end(); )
+    {
+        LineSegment& horiLine = *it;
+
+        bool inVertSegRegion = ((vertLine.begin <= horiLine.pos) &&
+                                (vertLine.finish >= horiLine.pos));
+
+        if (horiLine.finish < vertLine.pos)
+        {
+            // Add horizontal visibility segment.
+            horiLine.addEdgeHorizontal(router);
+
+            size_t dim = XDIM; // x-dimension
+            horiLine.generateVisibilityEdgesFromBreakpointSet(router, dim);
+
+            // We've now swept past this horizontal segment, so delete.
+            it = segments.erase(it);
+            continue;
+        }
+        else if (horiLine.begin > vertLine.pos)
+        {
+            // We've yet to reach this segment in the sweep, so ignore.
+            ++it;
+            continue;
+        }
+        else if (horiLine.begin == vertLine.pos)
+        {
+            if (inVertSegRegion)
+            {
+                horiLine.insertBreakpointsBegin(router, vertLine);
+            }
+        }
+        else if (horiLine.finish == vertLine.pos)
+        {
+            if (inVertSegRegion)
+            {
+                // Add horizontal visibility segment.
+                horiLine.addEdgeHorizontal(router);
+
+                horiLine.insertBreakpointsFinish(router, vertLine);
+
+                size_t dim = XDIM; // x-dimension
+                horiLine.generateVisibilityEdgesFromBreakpointSet(router, dim);
+
+                // And we've now finished with the segment, so delete.
+                it = segments.erase(it);
+                continue;
+            }
+        }
+        else
+        {
+            COLA_ASSERT(horiLine.begin < vertLine.pos);
+            COLA_ASSERT(horiLine.finish > vertLine.pos);
+
+            if (inVertSegRegion)
+            {
+                // Add horizontal visibility segment.
+                VertSet intersectionVerts =
+                        horiLine.addEdgeHorizontalTillIntersection(
+                            router, vertLine);
+
+                for (VertSet::iterator v = intersectionVerts.begin();
+                        v != intersectionVerts.end(); ++v)
+                {
+                    vertLine.breakPoints.insert(PosVertInf(horiLine.pos, *v,
+                            getPosVertInfDirection(*v, YDIM)));
+                }
+            }
+        }
+        ++it;
+    }
+
+    // Split breakPoints set into visibility segments.
+    size_t dimension = YDIM; // y-dimension
+    vertLine.generateVisibilityEdgesFromBreakpointSet(router, dimension);
+}
+
+
+// Processes an event for the vertical sweep used for computing the static
+// orthogonal visibility graph.  This adds possible visibility sgments to
+// the segments list.
+// The first pass is adding the event to the scanline, the second is for
+// processing the event and the third for removing it from the scanline.
+static void processEventVert(Router *router, NodeSet& scanline,
+        SegmentListWrapper& segments, Event *e, unsigned int pass)
+{
+    Node *v = e->v;
+
+    if ( ((pass == 1) && (e->type == Open)) ||
+         ((pass == 2) && (e->type == ConnPoint)) )
+    {
+        std::pair<NodeSet::iterator, bool> result = scanline.insert(v);
+        v->iter = result.first;
+        COLA_ASSERT(result.second);
+
+        NodeSet::iterator it = v->iter;
+        // Work out neighbours
+        if (it != scanline.begin())
+        {
+            Node *u = *(--it);
+            v->firstAbove = u;
+            u->firstBelow = v;
+        }
+        it = v->iter;
+        if (++it != scanline.end())
+        {
+            Node *u = *it;
+            v->firstBelow = u;
+            u->firstAbove = v;
+        }
+    }
+
+    if (pass == 2)
+    {
+        if ((e->type == Open) || (e->type == Close))
+        {
+            // Shape edge positions.
+            double minShape = v->min[0];
+            double maxShape = v->max[0];
+            // As far as we can see.
+            double minLimit, maxLimit;
+            double minLimitMax, maxLimitMin;
+            v->findFirstPointAboveAndBelow(0, minLimit, maxLimit,
+                    minLimitMax, maxLimitMin);
+
+            // Only difference between Open and Close is whether the line
+            // segments are at the top or bottom of the shape.  Decide here.
+            double lineY = (e->type == Open) ? v->min[1] : v->max[1];
+
+            if (minLimitMax >= maxLimitMin)
+            {
+                // Insert possible visibility segments.
+                VertInf *vI1 = new VertInf(router, dummyOrthogID,
+                            Point(minShape, lineY));
+                VertInf *vI2 = new VertInf(router, dummyOrthogID,
+                            Point(maxShape, lineY));
+
+                // There are no overlapping shapes, so give full visibility.
+                if (minLimit < minShape)
+                {
+                    segments.insert(LineSegment(minLimit, minShape, lineY,
+                                true, NULL, vI1));
+                }
+                segments.insert(LineSegment(minShape, maxShape, lineY,
+                            true, vI1, vI2));
+                if (maxShape < maxLimit)
+                {
+                    segments.insert(LineSegment(maxShape, maxLimit, lineY,
+                                true, vI2, NULL));
+                }
+            }
+            else
+            {
+                if ((minLimitMax > minLimit) && (minLimitMax >= minShape))
+                {
+                    segments.insert(LineSegment(minLimit, minLimitMax, lineY,
+                                true, NULL, NULL));
+                }
+                if ((maxLimitMin < maxLimit) && (maxLimitMin <= maxShape))
+                {
+                    segments.insert(LineSegment(maxLimitMin, maxLimit, lineY,
+                                true, NULL, NULL));
+                }
+            }
+        }
+        else if (e->type == ConnPoint)
+        {
+            // Connection point.
+            VertInf *centreVert = e->v->c;
+            Point& cp = centreVert->point;
+
+            // As far as we can see.
+            double minLimit = v->firstPointAbove(0);
+            double maxLimit = v->firstPointBelow(0);
+            bool inShape = v->isInsideShape(0);
+
+            LineSegment *line1 = NULL, *line2 = NULL;
+            if (!inShape || (centreVert->visDirections & ConnDirLeft))
+            {
+                line1 = segments.insert(LineSegment(minLimit, cp.x, e->pos,
+                        true, NULL, centreVert));
+            }
+            if (!inShape || (centreVert->visDirections & ConnDirRight))
+            {
+                line2 = segments.insert(LineSegment(cp.x, maxLimit, e->pos,
+                        true, centreVert, NULL));
+            }
+            if (!line1 && !line2)
+            {
+                // Add a point segment for the centre point.
+                segments.insert(LineSegment(cp.x, e->pos, centreVert));
+            }
+
+            if (!inShape)
+            {
+                // This is not contained within a shape so add a normal
+                // visibility graph point here too (since paths won't route
+                // *through* connector endpoint vertices).
+                if (line1 || line2)
+                {
+                    VertInf *cent = new VertInf(router, dummyOrthogID, cp);
+                    if (line1)
+                    {
+                        line1->vertInfs.insert(cent);
+                    }
+                    if (line2)
+                    {
+                        line2->vertInfs.insert(cent);
+                    }
+                }
+            }
+        }
+    }
+
+    if ( ((pass == 3) && (e->type == Close)) ||
+         ((pass == 2) && (e->type == ConnPoint)) )
+    {
+        // Clean up neighbour pointers.
+        Node *l = v->firstAbove, *r = v->firstBelow;
+        if (l != NULL)
+        {
+            l->firstBelow = v->firstBelow;
+        }
+        if (r != NULL)
+        {
+            r->firstAbove = v->firstAbove;
+        }
+
+        if (e->type == ConnPoint)
+        {
+            scanline.erase(v->iter);
+            delete v;
+        }
+        else  // if (e->type == Close)
+        {
+            size_t result;
+            result = scanline.erase(v);
+            COLA_ASSERT(result == 1);
+            delete v;
+        }
+    }
+}
+
+
+// Processes an event for the vertical sweep used for computing the static
+// orthogonal visibility graph.  This adds possible visibility sgments to
+// the segments list.
+// The first pass is adding the event to the scanline, the second is for
+// processing the event and the third for removing it from the scanline.
+static void processEventHori(Router */*router*/, NodeSet& scanline,
+        SegmentListWrapper& segments, Event *e, unsigned int pass)
+{
+    Node *v = e->v;
+
+    if ( ((pass == 1) && (e->type == Open)) ||
+         ((pass == 2) && (e->type == ConnPoint)) )
+    {
+        std::pair<NodeSet::iterator, bool> result = scanline.insert(v);
+        v->iter = result.first;
+        COLA_ASSERT(result.second);
+
+        NodeSet::iterator it = v->iter;
+        // Work out neighbours
+        if (it != scanline.begin())
+        {
+            Node *u = *(--it);
+            v->firstAbove = u;
+            u->firstBelow = v;
+        }
+        it = v->iter;
+        if (++it != scanline.end())
+        {
+            Node *u = *it;
+            v->firstBelow = u;
+            u->firstAbove = v;
+        }
+    }
+
+    if (pass == 2)
+    {
+        if ((e->type == Open) || (e->type == Close))
+        {
+            // Shape edge positions.
+            double minShape = v->min[1];
+            double maxShape = v->max[1];
+            // As far as we can see.
+            double minLimit, maxLimit;
+            double minLimitMax, maxLimitMin;
+            v->findFirstPointAboveAndBelow(1, minLimit, maxLimit,
+                    minLimitMax, maxLimitMin);
+
+            // Only difference between Open and Close is whether the line
+            // segments are at the left or right of the shape.  Decide here.
+            double lineX = (e->type == Open) ? v->min[0] : v->max[0];
+
+            if (minLimitMax >= maxLimitMin)
+            {
+                LineSegment vertSeg = LineSegment(minLimit, maxLimit, lineX);
+                segments.insert(vertSeg);
+            }
+            else
+            {
+                if ((minLimitMax > minLimit) && (minLimitMax >= minShape))
+                {
+                    LineSegment vertSeg =
+                            LineSegment(minLimit, minLimitMax, lineX);
+                    segments.insert(vertSeg);
+                }
+                if ((maxLimitMin < maxLimit) && (maxLimitMin <= maxShape))
+                {
+                    LineSegment vertSeg =
+                            LineSegment(maxLimitMin, maxLimit, lineX);
+                    segments.insert(vertSeg);
+                }
+            }
+        }
+        else if (e->type == ConnPoint)
+        {
+            // Connection point.
+            VertInf *centreVert = e->v->c;
+            Point& cp = centreVert->point;
+
+            // As far as we can see.
+            double minLimit = v->firstPointAbove(1);
+            double maxLimit = v->firstPointBelow(1);
+            bool inShape = v->isInsideShape(1);
+
+            if (!inShape || (centreVert->visDirections & ConnDirUp))
+            {
+                segments.insert(LineSegment(minLimit, cp.y, e->pos));
+            }
+            if (!inShape || (centreVert->visDirections & ConnDirDown))
+            {
+                segments.insert(LineSegment(cp.y, maxLimit, e->pos));
+            }
+        }
+    }
+
+    if ( ((pass == 3) && (e->type == Close)) ||
+         ((pass == 2) && (e->type == ConnPoint)) )
+    {
+        // Clean up neighbour pointers.
+        Node *l = v->firstAbove, *r = v->firstBelow;
+        if (l != NULL)
+        {
+            l->firstBelow = v->firstBelow;
+        }
+        if (r != NULL)
+        {
+            r->firstAbove = v->firstAbove;
+        }
+
+        if (e->type == ConnPoint)
+        {
+            scanline.erase(v->iter);
+            delete v;
+        }
+        else  // if (e->type == Close)
+        {
+            size_t result;
+            result = scanline.erase(v);
+            COLA_ASSERT(result == 1);
+            delete v;
+        }
+    }
+}
+
+
+extern void generateStaticOrthogonalVisGraph(Router *router)
+{
+    const size_t n = router->shapeRefs.size();
+    const unsigned cpn = router->vertices.connsSize();
+    // Set up the events for the vertical sweep.
+    size_t totalEvents = (2 * n) + cpn;
+    events = new Event*[totalEvents];
+    unsigned ctr = 0;
+    ShapeRefList::iterator shRefIt = router->shapeRefs.begin();
+    for (unsigned i = 0; i < n; i++)
+    {
+        ShapeRef *shRef = *shRefIt;
+        double minX, minY, maxX, maxY;
+        shRef->polygon().getBoundingRect(&minX, &minY, &maxX, &maxY);
+        double midX = minX + ((maxX - minX) / 2);
+        Node *v = new Node(shRef, midX);
+        events[ctr++] = new Event(Open, v, minY);
+        events[ctr++] = new Event(Close, v, maxY);
+
+        ++shRefIt;
+    }
+    for (VertInf *curr = router->vertices.connsBegin();
+            curr && (curr != router->vertices.shapesBegin());
+            curr = curr->lstNext)
+    {
+        Point& point = curr->point;
+
+        Node *v = new Node(curr, point.x);
+        events[ctr++] = new Event(ConnPoint, v, point.y);
+    }
+    qsort((Event*)events, (size_t) totalEvents, sizeof(Event*), compare_events);
+
+    // Process the vertical sweep.
+    // We do multiple passes over sections of the list so we can add relevant
+    // entries to the scanline that might follow, before process them.
+    SegmentListWrapper segments;
+    NodeSet scanline;
+    double thisPos = (totalEvents > 0) ? events[0]->pos : 0;
+    unsigned int posStartIndex = 0;
+    unsigned int posFinishIndex = 0;
+    for (unsigned i = 0; i <= totalEvents; ++i)
+    {
+        // If we have finished the current scanline or all events, then we
+        // process the events on the current scanline in a couple of passes.
+        if ((i == totalEvents) || (events[i]->pos != thisPos))
+        {
+            posFinishIndex = i;
+            for (int pass = 2; pass <= 3; ++pass)
+            {
+                for (unsigned j = posStartIndex; j < posFinishIndex; ++j)
+                {
+                    processEventVert(router, scanline, segments,
+                            events[j], pass);
+                }
+            }
+
+            if (i == totalEvents)
+            {
+                // We have cleaned up, so we can now break out of loop.
+                break;
+            }
+
+            thisPos = events[i]->pos;
+            posStartIndex = i;
+        }
+
+        // Do the first sweep event handling -- building the correct
+        // structure of the scanline.
+        const int pass = 1;
+        processEventVert(router, scanline, segments, events[i], pass);
+    }
+    COLA_ASSERT(scanline.size() == 0);
+    for (unsigned i = 0; i < totalEvents; ++i)
+    {
+        delete events[i];
+    }
+
+    segments.list().sort();
+
+    // Set up the events for the horizontal sweep.
+    SegmentListWrapper vertSegments;
+    ctr = 0;
+    shRefIt = router->shapeRefs.begin();
+    for (unsigned i = 0; i < n; i++)
+    {
+        ShapeRef *shRef = *shRefIt;
+        double minX, minY, maxX, maxY;
+        shRef->polygon().getBoundingRect(&minX, &minY, &maxX, &maxY);
+        double midY = minY + ((maxY - minY) / 2);
+        Node *v = new Node(shRef, midY);
+        events[ctr++] = new Event(Open, v, minX);
+        events[ctr++] = new Event(Close, v, maxX);
+
+        ++shRefIt;
+    }
+    for (VertInf *curr = router->vertices.connsBegin();
+            curr && (curr != router->vertices.shapesBegin());
+            curr = curr->lstNext)
+    {
+        Point& point = curr->point;
+
+        Node *v = new Node(curr, point.y);
+        events[ctr++] = new Event(ConnPoint, v, point.x);
+    }
+    qsort((Event*)events, (size_t) totalEvents, sizeof(Event*), compare_events);
+
+    // Process the horizontal sweep
+    thisPos = (totalEvents > 0) ? events[0]->pos : 0;
+    posStartIndex = 0;
+    posFinishIndex = 0;
+    for (unsigned i = 0; i <= totalEvents; ++i)
+    {
+        // If we have finished the current scanline or all events, then we
+        // process the events on the current scanline in a couple of passes.
+        if ((i == totalEvents) || (events[i]->pos != thisPos))
+        {
+            posFinishIndex = i;
+            for (int pass = 2; pass <= 3; ++pass)
+            {
+                for (unsigned j = posStartIndex; j < posFinishIndex; ++j)
+                {
+                    processEventHori(router, scanline, vertSegments,
+                            events[j], pass);
+                }
+            }
+
+            // Process the merged line segments.
+            vertSegments.list().sort();
+            for (SegmentList::iterator curr = vertSegments.list().begin();
+                    curr != vertSegments.list().end(); ++curr)
+            {
+                intersectSegments(router, segments.list(), *curr);
+            }
+            vertSegments.list().clear();
+
+            if (i == totalEvents)
+            {
+                // We have cleaned up, so we can now break out of loop.
+                break;
+            }
+
+            thisPos = events[i]->pos;
+            posStartIndex = i;
+        }
+
+        // Do the first sweep event handling -- building the correct
+        // structure of the scanline.
+        const int pass = 1;
+        processEventHori(router, scanline, vertSegments, events[i], pass);
+    }
+    COLA_ASSERT(scanline.size() == 0);
+    for (unsigned i = 0; i < totalEvents; ++i)
+    {
+        delete events[i];
+    }
+    delete [] events;
+
+    // Add portions of the horizontal line that are after the final vertical
+    // position we considered.
+    for (SegmentList::iterator it = segments.list().begin();
+            it != segments.list().end(); )
+    {
+        LineSegment& horiLine = *it;
+
+        horiLine.addEdgeHorizontal(router);
+
+        size_t dim = XDIM; // x-dimension
+        horiLine.generateVisibilityEdgesFromBreakpointSet(router, dim);
+
+        it = segments.list().erase(it);
+    }
+}
+
+
+//============================================================================
+//                           Path Adjustment code
+//============================================================================
+
+
+
+
+// Processes sweep events used to determine each horizontal and vertical
+// line segment in a connector's channel of visibility.
+// Four calls to this function are made at each position by the scanline:
+//   1) Handle all Close event processing.
+//   2) Remove Close event objects from the scanline.
+//   3) Add Open event objects to the scanline.
+//   4) Handle all Open event processing.
+//
+static void processShiftEvent(Router */*router*/, NodeSet& scanline,
+                              ShiftSegmentList& /*segments*/, Event *e, size_t dim,
+                              unsigned int pass)
+{
+    Node *v = e->v;
+
+    if ( ((pass == 3) && (e->type == Open)) ||
+         ((pass == 3) && (e->type == SegOpen)) )
+    {
+        std::pair<NodeSet::iterator, bool> result = scanline.insert(v);
+        v->iter = result.first;
+        COLA_ASSERT(result.second);
+
+        NodeSet::iterator it = v->iter;
+        // Work out neighbours
+        if (it != scanline.begin())
+        {
+            Node *u = *(--it);
+            v->firstAbove = u;
+            u->firstBelow = v;
+        }
+        it = v->iter;
+        if (++it != scanline.end())
+        {
+            Node *u = *it;
+            v->firstBelow = u;
+            u->firstAbove = v;
+        }
+    }
+
+    if ( ((pass == 4) && (e->type == Open)) ||
+         ((pass == 4) && (e->type == SegOpen)) ||
+         ((pass == 1) && (e->type == SegClose)) ||
+         ((pass == 1) && (e->type == Close)) )
+    {
+        if (v->ss)
+        {
+            // As far as we can see.
+            double minLimit = v->firstObstacleAbove(dim);
+            double maxLimit = v->firstObstacleBelow(dim);
+
+            v->ss->minSpaceLimit =
+                    std::max(minLimit, v->ss->minSpaceLimit);
+            v->ss->maxSpaceLimit =
+                    std::min(maxLimit, v->ss->maxSpaceLimit);
+        }
+        else
+        {
+            v->markShiftSegmentsAbove(dim);
+            v->markShiftSegmentsBelow(dim);
+        }
+    }
+
+    if ( ((pass == 2) && (e->type == SegClose)) ||
+         ((pass == 2) && (e->type == Close)) )
+    {
+        // Clean up neighbour pointers.
+        Node *l = v->firstAbove, *r = v->firstBelow;
+        if (l != NULL)
+        {
+            l->firstBelow = v->firstBelow;
+        }
+        if (r != NULL)
+        {
+            r->firstAbove = v->firstAbove;
+        }
+
+        size_t result;
+        result = scanline.erase(v);
+        COLA_ASSERT(result == 1);
+        delete v;
+    }
+}
+
+
+static void buildOrthogonalChannelInfo(Router *router,
+        const size_t dim, ShiftSegmentList& segmentList)
+{
+    if (router->routingPenalty(segmentPenalty) == 0)
+    {
+        // This code assumes the routes are pretty optimal, so we don't
+        // do this adjustment if the routes have no segment penalty.
+        return;
+    }
+
+    size_t altDim = (dim + 1) % 2;
+    // For each connector.
+    for (ConnRefList::const_iterator curr = router->connRefs.begin();
+            curr != router->connRefs.end(); ++curr)
+    {
+        if ((*curr)->routingType() != ConnType_Orthogonal)
+        {
+            continue;
+        }
+        Polygon& displayRoute = (*curr)->displayRoute();
+        // Determine all line segments that we are interested in shifting.
+        // We don't consider the first or last segment of a path.
+        for (size_t i = 1; i < displayRoute.size(); ++i)
+        {
+            if (displayRoute.ps[i - 1][dim] == displayRoute.ps[i][dim])
+            {
+                // It's a segment in the dimension we are processing,
+                size_t indexLow = i - 1;
+                size_t indexHigh = i;
+                if (displayRoute.ps[i - 1][altDim] > displayRoute.ps[i][altDim])
+                {
+                    indexLow = i;
+                    indexHigh = i - 1;
+                }
+                COLA_ASSERT(displayRoute.at(indexLow)[altDim] <
+                        displayRoute.at(indexHigh)[altDim]);
+
+                if ((i == 1) || ((i + 1) == displayRoute.size()))
+                {
+                    // The first and last segment of a connector can't be
+                    // shifted.  We call them fixed segments.  Note: this
+                    // will change if we later allow connection channels.
+                    segmentList.push_back(
+                            ShiftSegment(*curr, indexLow, indexHigh, dim));
+                    continue;
+                }
+
+                // The segment probably has space to be shifted.
+                double minLim = -CHANNEL_MAX;
+                double maxLim = CHANNEL_MAX;
+                bool isSBend = false;
+
+                double prevPos = displayRoute.ps[i - 2][dim];
+                double nextPos = displayRoute.ps[i + 1][dim];
+                if (((prevPos < displayRoute.ps[i][dim]) &&
+                     (nextPos > displayRoute.ps[i][dim]))
+                     ||
+                    ((prevPos > displayRoute.ps[i][dim]) &&
+                     (nextPos < displayRoute.ps[i][dim])) )
+                {
+                    isSBend = true;
+
+                    // Determine limits if the s-bend is not due to an
+                    // obstacle.  In this case we need to limit the channel
+                    // to the span of the adjoining segments to this one.
+                    if ((prevPos < displayRoute.ps[i][dim]) &&
+                        (nextPos > displayRoute.ps[i][dim]))
+                    {
+                        minLim = std::max(minLim, prevPos);
+                        maxLim = std::min(maxLim, nextPos);
+                    }
+                    else
+                    {
+                        minLim = std::max(minLim, nextPos);
+                        maxLim = std::min(maxLim, prevPos);
+                    }
+                }
+                else
+                {
+                    // isCBend: Both adjoining segments are in the same
+                    // direction.  We indicate this for later by setting
+                    // the maxLim or minLim to the segment position.
+                    if (prevPos < displayRoute.ps[i][dim])
+                    {
+                        minLim = displayRoute.ps[i][dim];
+                    }
+                    else
+                    {
+                        maxLim = displayRoute.ps[i][dim];
+                    }
+                }
+
+                segmentList.push_back(ShiftSegment(*curr, indexLow,
+                            indexHigh, isSBend, dim, minLim, maxLim));
+            }
+        }
+    }
+    if (segmentList.empty())
+    {
+        // There are no segments, so we can just return now.
+        return;
+    }
+
+    // Do a sweep and shift these segments.
+    const size_t n = router->shapeRefs.size();
+    const size_t cpn = segmentList.size();
+    // Set up the events for the sweep.
+    size_t totalEvents = 2 * (n + cpn);
+    events = new Event*[totalEvents];
+    unsigned ctr = 0;
+    ShapeRefList::iterator shRefIt = router->shapeRefs.begin();
+    for (unsigned i = 0; i < n; i++)
+    {
+        ShapeRef *shRef = *shRefIt;
+        Point min, max;
+        shRef->polygon().getBoundingRect(&min.x, &min.y, &max.x, &max.y);
+        double mid = min[dim] + ((max[dim] - min[dim]) / 2);
+        Node *v = new Node(shRef, mid);
+        events[ctr++] = new Event(Open, v, min[altDim]);
+        events[ctr++] = new Event(Close, v, max[altDim]);
+
+        ++shRefIt;
+    }
+    for (ShiftSegmentList::iterator curr = segmentList.begin();
+            curr != segmentList.end(); ++curr)
+    {
+        const Point& lowPt = curr->lowPoint();
+        const Point& highPt = curr->highPoint();
+
+        COLA_ASSERT(lowPt[dim] == highPt[dim]);
+        COLA_ASSERT(lowPt[altDim] < highPt[altDim]);
+        Node *v = new Node(&(*curr), lowPt[dim]);
+        events[ctr++] = new Event(SegOpen, v, lowPt[altDim]);
+        events[ctr++] = new Event(SegClose, v, highPt[altDim]);
+    }
+    qsort((Event*)events, (size_t) totalEvents, sizeof(Event*), compare_events);
+
+    // Process the sweep.
+    // We do multiple passes over sections of the list so we can add relevant
+    // entries to the scanline that might follow, before process them.
+    NodeSet scanline;
+    double thisPos = (totalEvents > 0) ? events[0]->pos : 0;
+    unsigned int posStartIndex = 0;
+    unsigned int posFinishIndex = 0;
+    for (unsigned i = 0; i <= totalEvents; ++i)
+    {
+        // If we have finished the current scanline or all events, then we
+        // process the events on the current scanline in a couple of passes.
+        if ((i == totalEvents) || (events[i]->pos != thisPos))
+        {
+            posFinishIndex = i;
+            for (int pass = 2; pass <= 4; ++pass)
+            {
+                for (unsigned j = posStartIndex; j < posFinishIndex; ++j)
+                {
+                    processShiftEvent(router, scanline, segmentList, events[j],
+                            dim, pass);
+                }
+            }
+
+            if (i == totalEvents)
+            {
+                // We have cleaned up, so we can now break out of loop.
+                break;
+            }
+
+            thisPos = events[i]->pos;
+            posStartIndex = i;
+        }
+
+        // Do the first sweep event handling -- building the correct
+        // structure of the scanline.
+        const int pass = 1;
+        processShiftEvent(router, scanline, segmentList, events[i],
+                dim, pass);
+    }
+    COLA_ASSERT(scanline.size() == 0);
+    for (unsigned i = 0; i < totalEvents; ++i)
+    {
+        delete events[i];
+    }
+    delete [] events;
+}
+
+
+static void simplifyOrthogonalRoutes(Router *router)
+{
+    // Simplify routes.
+    for (ConnRefList::const_iterator curr = router->connRefs.begin();
+            curr != router->connRefs.end(); ++curr)
+    {
+        if ((*curr)->routingType() != ConnType_Orthogonal)
+        {
+            continue;
+        }
+        (*curr)->set_route((*curr)->displayRoute().simplify());
+    }
+}
+
+
+static void buildOrthogonalNudgingOrderInfo(Router *router,
+        PtOrderMap& pointOrders)
+{
+    // Simplify routes.
+    simplifyOrthogonalRoutes(router);
+
+    int crossingsN = 0;
+
+    // Do segment splitting.
+    for (ConnRefList::const_iterator curr = router->connRefs.begin();
+            curr != router->connRefs.end(); ++curr)
+    {
+        if ((*curr)->routingType() != ConnType_Orthogonal)
+        {
+            continue;
+        }
+        ConnRef *conn = *curr;
+
+        for (ConnRefList::const_iterator curr2 = router->connRefs.begin();
+                curr2 != router->connRefs.end(); ++curr2)
+        {
+            if ((*curr2)->routingType() != ConnType_Orthogonal)
+            {
+                continue;
+            }
+            ConnRef *conn2 = *curr2;
+
+            if (conn == conn2)
+            {
+                continue;
+            }
+
+            Avoid::Polygon& route = conn->displayRoute();
+            Avoid::Polygon& route2 = conn2->displayRoute();
+            splitBranchingSegments(route2, true, route);
+        }
+    }
+
+    for (ConnRefList::const_iterator curr = router->connRefs.begin();
+            curr != router->connRefs.end(); ++curr)
+    {
+        if ((*curr)->routingType() != ConnType_Orthogonal)
+        {
+            continue;
+        }
+        ConnRef *conn = *curr;
+
+        for (ConnRefList::const_iterator curr2 = curr;
+                curr2 != router->connRefs.end(); ++curr2)
+        {
+            if ((*curr2)->routingType() != ConnType_Orthogonal)
+            {
+                continue;
+            }
+            ConnRef *conn2 = *curr2;
+
+            if (conn == conn2)
+            {
+                continue;
+            }
+
+            Avoid::Polygon& route = conn->displayRoute();
+            Avoid::Polygon& route2 = conn2->displayRoute();
+            bool checkForBranchingSegments = false;
+            int crossings = 0;
+            for (size_t i = 1; i < route.size(); ++i)
+            {
+                const bool finalSegment = ((i + 1) == route.size());
+                crossings += countRealCrossings(route2, true, route, i,
+                        checkForBranchingSegments, finalSegment, NULL,
+                        &pointOrders, conn2, conn).first;
+            }
+            if (crossings > 0)
+            {
+                crossingsN += crossings;
+            }
+        }
+    }
+
+    // Sort the point orders.
+    PtOrderMap::iterator finish = pointOrders.end();
+    for (PtOrderMap::iterator it = pointOrders.begin(); it != finish; ++it)
+    {
+        //const VertID& ptID = it->first;
+        PtOrder& order = it->second;
+
+        for (size_t dim = XDIM; dim <= YDIM; ++dim)
+        {
+            order.sort(dim);
+        }
+    }
+}
+
+
+class CmpLineOrder
+{
+    public:
+        CmpLineOrder(PtOrderMap& ord, const size_t dim)
+            : orders(ord),
+              dimension(dim)
+        {
+        }
+        bool operator()(const ShiftSegment& lhs, const ShiftSegment& rhs,
+                bool *comparable = NULL) const
+        {
+            if (comparable)
+            {
+                *comparable = true;
+            }
+            Point lhsLow  = lhs.lowPoint();
+            Point rhsLow  = rhs.lowPoint();
+#ifndef NDEBUG
+            const Point& lhsHigh = lhs.highPoint();
+            const Point& rhsHigh = rhs.highPoint();
+#endif
+            size_t altDim = (dimension + 1) % 2;
+
+            COLA_ASSERT(lhsLow[dimension] == lhsHigh[dimension]);
+            COLA_ASSERT(rhsLow[dimension] == rhsHigh[dimension]);
+
+            if (lhsLow[dimension] != rhsLow[dimension])
+            {
+                return lhsLow[dimension] < rhsLow[dimension];
+            }
+
+            // If one of these is fixed, then determine order based on
+            // fixed segment, that is, order so the fixed segment doesn't
+            // block movement.
+            bool oneIsFixed = false;
+            const int lhsFixedOrder = lhs.fixedOrder(oneIsFixed);
+            const int rhsFixedOrder = rhs.fixedOrder(oneIsFixed);
+            if (oneIsFixed && (lhsFixedOrder != rhsFixedOrder))
+            {
+                return lhsFixedOrder < rhsFixedOrder;
+            }
+
+            // C-bends that did not have a clear order with s-bends might
+            // not have a good ordering here, so compare their order in
+            // terms of C-bend direction and S-bends and use that if it
+            // differs for the two segments.
+            const int lhsOrder = lhs.order();
+            const int rhsOrder = rhs.order();
+            if (lhsOrder != rhsOrder)
+            {
+                return lhsOrder < rhsOrder;
+            }
+
+            // Need to index using the original point into the map, so find it.
+            Point& unchanged = (lhsLow[altDim] > rhsLow[altDim]) ?
+                    lhsLow : rhsLow;
+
+            PtOrder& lowOrder = orders[unchanged];
+            int lhsPos = lowOrder.positionFor(lhs.connRef, dimension);
+            int rhsPos = lowOrder.positionFor(rhs.connRef, dimension);
+            if ((lhsPos == -1) || (rhsPos == -1))
+            {
+                // A value for rhsPos or lhsPos mean the points are not directly
+                // comparable, meaning they are at the same position but cannot
+                // overlap (they are just collinear.  The relative order for
+                // these segments is not important since we do not constrain
+                // them against each other.
+                //COLA_ASSERT(lhs.overlapsWith(rhs, dimension) == false);
+                // We do need to be consistent though.
+                if (comparable)
+                {
+                    *comparable = false;
+                }
+                return lhsLow[altDim] < rhsLow[altDim];
+            }
+
+            return lhsPos < rhsPos;
+        }
+
+        PtOrderMap& orders;
+        const size_t dimension;
+};
+
+
+// We can use the normaal sort algorithm for lists since it is not possible
+// to comapre all elements, but there will be an ordering defined between
+// most of the elements.  Hence we order these, using insertion sort, and
+// the case of them not being able to be compared is handled by not setting
+// up any constraints between such segments when doing the nudging.
+//
+static ShiftSegmentList linesort(ShiftSegmentList origList,
+        CmpLineOrder& comparison)
+{
+    ShiftSegmentList resultList;
+
+    while (!origList.empty())
+    {
+        // Get and remove the first element from the origList.
+        ShiftSegment segment = origList.front();
+        origList.pop_front();
+
+        // Find the insertion point in the resultList.
+        ShiftSegmentList::iterator curr;
+        for (curr = resultList.begin(); curr != resultList.end(); ++curr)
+        {
+            bool comparable = false;
+            bool lessThan = comparison(segment, *curr, &comparable);
+
+            if (comparable && lessThan)
+            {
+                // If it is comparable and lessThan, then we have found the
+                // insertion point.
+                break;
+            }
+        }
+
+        // Insert the element into the reultList at the required point.
+        resultList.insert(curr, segment);
+    }
+
+    return resultList;
+}
+
+
+typedef std::list<ShiftSegment *> ShiftSegmentPtrList;
+
+
+static void nudgeOrthogonalRoutes(Router *router, size_t dimension,
+        PtOrderMap& pointOrders, ShiftSegmentList& segmentList)
+{
+    // Do the actual nudging.
+    ShiftSegmentList currentRegion;
+    while (!segmentList.empty())
+    {
+        // Take a reference segment
+        ShiftSegment& currentSegment = segmentList.front();
+        // Then, find the segments that overlap this one.
+        currentRegion.clear();
+        currentRegion.push_back(currentSegment);
+        segmentList.erase(segmentList.begin());
+        for (ShiftSegmentList::iterator curr = segmentList.begin();
+                curr != segmentList.end(); )
+        {
+            bool overlaps = false;
+            for (ShiftSegmentList::iterator curr2 = currentRegion.begin();
+                    curr2 != currentRegion.end(); ++curr2)
+            {
+                if (curr->overlapsWith(*curr2, dimension))
+                {
+                    overlaps = true;
+                    break;
+                }
+            }
+            if (overlaps)
+            {
+                currentRegion.push_back(*curr);
+                segmentList.erase(curr);
+                // Consider segments from the beginning, since we mave have
+                // since passed segments that overlap with the new set.
+                curr = segmentList.begin();
+            }
+            else
+            {
+                ++curr;
+            }
+        }
+        CmpLineOrder lineSortComp(pointOrders, dimension);
+        currentRegion = linesort(currentRegion, lineSortComp);
+
+        if (currentRegion.size() == 1)
+        {
+            // Save creating the solver instance if there is just one
+            // immovable segment.
+            if (!currentRegion.front().sBend)
+            {
+                continue;
+            }
+        }
+
+        // Process these segments.
+        Variables vs;
+        Constraints cs;
+        ShiftSegmentPtrList prevVars;
+        // IDs:
+        const int freeID    = 0;
+        const int fixedID   = 1;
+        // Weights:
+        double freeWeight   = 0.00001;
+        double strongWeight = 0.001;
+        double fixedWeight  = 100000;
+        //printf("-------------------------------------------------------\n");
+        //printf("Nudge -- size: %d\n", (int) currentRegion.size());
+        for (ShiftSegmentList::iterator currSegment = currentRegion.begin();
+                currSegment != currentRegion.end(); ++currSegment)
+        {
+            Point& lowPt = currSegment->lowPoint();
+
+            // Create a solver variable for the position of this segment.
+            int varID = freeID;
+            double idealPos = lowPt[dimension];
+            double weight = freeWeight;
+            if (currSegment->sBend)
+            {
+                COLA_ASSERT(currSegment->minSpaceLimit > -CHANNEL_MAX);
+                COLA_ASSERT(currSegment->maxSpaceLimit < CHANNEL_MAX);
+
+                // For s-bends, take the middle as ideal.
+                idealPos = currSegment->minSpaceLimit +
+                        ((currSegment->maxSpaceLimit -
+                          currSegment->minSpaceLimit) / 2);
+            }
+            else if (currSegment->fixed)
+            {
+                // Fixed segments shouldn't get moved.
+                weight = fixedWeight;
+                varID = fixedID;
+            }
+            else
+            {
+                // Set a higher weight for c-bends to stop them sometimes
+                // getting pushed out into channels by more-free connectors
+                // to the "inner" side of them.
+                weight = strongWeight;
+            }
+            currSegment->variable = new Variable(varID, idealPos, weight);
+            vs.push_back(currSegment->variable);
+            size_t index = vs.size() - 1;
+            //printf("line  %.15f  pos: %g   min: %g  max: %g\n",
+            //        lowPt[dimension], idealPos, currSegment->minSpaceLimit,
+            //        currSegment->maxSpaceLimit);
+
+            // Constrain position in relation to previously seen segments,
+            // if necessary (i.e. when they could overlap).
+            for (ShiftSegmentPtrList::iterator prevVarIt = prevVars.begin();
+                    prevVarIt != prevVars.end(); )
+            {
+                ShiftSegment *prevSeg = *prevVarIt;
+                Variable *prevVar = prevSeg->variable;
+
+                if (currSegment->overlapsWith(*prevSeg, dimension) &&
+                        (!(currSegment->fixed) || !(prevSeg->fixed)))
+                {
+                    // If there is a previous segment to the left that
+                    // could overlap this in the shift direction, then
+                    // constrain the two segments to be separated.
+                    // Though don't add the constraint if both the
+                    // segments are fixed in place.
+                    cs.push_back(new Constraint(prevVar, vs[index],
+                            router->orthogonalNudgeDistance()));
+                    prevVarIt = prevVars.erase(prevVarIt);
+                }
+                else
+                {
+                    ++prevVarIt;
+                }
+            }
+
+            if (!currSegment->fixed)
+            {
+                // If this segment sees a channel boundary to its left,
+                // then constrain its placement as such.
+                if (currSegment->minSpaceLimit > -CHANNEL_MAX)
+                {
+                    vs.push_back(new Variable(fixedID,
+                                currSegment->minSpaceLimit, fixedWeight));
+                    cs.push_back(new Constraint(vs[vs.size() - 1], vs[index],
+                                0.0));
+                }
+
+                // If this segment sees a channel boundary to its right,
+                // then constrain its placement as such.
+                if (currSegment->maxSpaceLimit < CHANNEL_MAX)
+                {
+                    vs.push_back(new Variable(fixedID,
+                                currSegment->maxSpaceLimit, fixedWeight));
+                    cs.push_back(new Constraint(vs[index], vs[vs.size() - 1],
+                                0.0));
+                }
+            }
+            prevVars.push_back(&(*currSegment));
+        }
+#if 0
+        for(unsigned i=0;i<vs.size();i++) {
+            printf("-vs[%d]=%f\n",i,vs[i]->desiredPosition);
+        }
+#endif
+        IncSolver f(vs,cs);
+        f.solve();
+        bool satisfied = true;
+        for (size_t i = 0; i < vs.size(); ++i)
+        {
+            if (vs[i]->id == fixedID)
+            {
+                if (fabs(vs[i]->finalPosition - vs[i]->desiredPosition) > 0.01)
+                {
+                    satisfied = false;
+                    break;
+                }
+            }
+        }
+        if (satisfied)
+        {
+            for (ShiftSegmentList::iterator currSegment = currentRegion.begin();
+                    currSegment != currentRegion.end(); ++currSegment)
+            {
+                Point& lowPt = currSegment->lowPoint();
+                Point& highPt = currSegment->highPoint();
+                double newPos = currSegment->variable->finalPosition;
+                //printf("Pos: %X, %g\n", (int) currSegment->connRef, newPos);
+                lowPt[dimension] = newPos;
+                highPt[dimension] = newPos;
+            }
+        }
+#if 0
+        for(unsigned i=0;i<vs.size();i++) {
+            printf("+vs[%d]=%f\n",i,vs[i]->finalPosition);
+        }
+#endif
+        for_each(vs.begin(),vs.end(),delete_object());
+        for_each(cs.begin(),cs.end(),delete_object());
+    }
+}
+
+
+extern void improveOrthogonalRoutes(Router *router)
+{
+    router->timers.Register(tmOrthogNudge, timerStart);
+    for (size_t dimension = 0; dimension < 2; ++dimension)
+    {
+        // Build nudging info.
+        // XXX: We need to build the point orders separately in each
+        //      dimension since things move.  There is probably a more
+        //      efficient way to do this.
+        PtOrderMap pointOrders;
+        buildOrthogonalNudgingOrderInfo(router, pointOrders);
+
+        // Simplify routes.
+        simplifyOrthogonalRoutes(router);
+
+        // Do the centring and nudging.
+        ShiftSegmentList segLists;
+        buildOrthogonalChannelInfo(router, dimension, segLists);
+        nudgeOrthogonalRoutes(router, dimension, pointOrders, segLists);
+    }
+    router->timers.Stop();
+}
+
+
+}
diff --git a/src/libavoid/orthogonal.h b/src/libavoid/orthogonal.h
new file mode 100644
index 000000000..4fb974099
--- /dev/null
+++ b/src/libavoid/orthogonal.h
@@ -0,0 +1,39 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * Copyright (C) 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.  
+ *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
+*/
+
+
+#ifndef AVOID_ORTHOGONAL_H
+#define AVOID_ORTHOGONAL_H
+
+namespace Avoid {
+
+
+extern void generateStaticOrthogonalVisGraph(Router *router);
+
+extern void improveOrthogonalRoutes(Router *router);
+
+
+}
+
+#endif
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..ab13a981b 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
         {
-            freePoly(newPoly);
+            COLA_ASSERT((type == ShapeMove) || (type == ShapeAdd) ||
+                    (type == ShapeRemove));
+            return (static_cast<ShapeRef *> (objPtr));
         }
-        ShapeRef *shape;
-        Polygn newPoly;
+        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
+        {
+            if (type != rhs.type)
+            {
+                return type < rhs.type;
+            }
+            return objPtr < rhs.objPtr;
+        }
+        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();
+    }
 
-#ifdef ORTHOGONAL_ROUTING
-    Region::addShape(shape);
-#endif
+    // 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);
+}
+
+
+void Router::modifyConnector(ConnRef *conn, const unsigned int type,
+        const ConnEnd& connEnd)
+{
+    ActionInfo modInfo(ConnChange, conn);
 
-    // o  Calculate visibility for the new vertices.
-    if (UseLeesAlgorithm)
+    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);
 
-    // Delete items that are queued in the movList.
-    for (MoveInfoList::iterator it = moveList.begin(); it != moveList.end(); )
+    ActionInfoList::iterator found =
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found == actionList.end())
     {
-        if ((*it)->shape->id() == pid)
-        {
-            MoveInfoList::iterator doomed = it;
-            ++it;
-            moveList.erase(doomed);
-        }
-        else
-        {
-            ++it;
-        }
+        actionList.push_back(modInfo);
     }
 
-    // o  Remove entries related to this shape's vertices
-    shape->removeFromGraph();
-    
-    if (SelectiveReroute)
+    if (!_consolidateActions)
     {
-        markConnectors(shape);
+        processTransaction();
     }
+}
 
-    adjustContainsWithDel(pid);
-    
-#ifdef ORTHOGONAL_ROUTING
-    Region::removeShape(shape);
-#endif
 
-    delete shape;
-    
-    // o  Check all edges that were blocked by this shape.
-    if (InvisibilityGrph)
+void Router::removeQueuedConnectorActions(ConnRef *conn)
+{
+    ActionInfo modInfo(ConnChange, conn);
+
+    ActionInfoList::iterator found =
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found != actionList.end())
     {
-        checkAllBlockedEdges(pid);
+        actionList.erase(found);
     }
-    else
+}
+
+
+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);
+
+    ActionInfoList::iterator found =
+            find(actionList.begin(), actionList.end(), addInfo);
+    if (found == actionList.end())
+    {
+        actionList.push_back(addInfo);
+    }
+
+    if (!_consolidateActions)
+    {
+        processTransaction();
+    }
+}
+
+
+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)
     {
-        // check all edges not in graph
-        checkAllMissingEdges();
+        processTransaction();
     }
-    callbackAllInvalidConnectors();
 }
 
 
-void Router::moveShape(ShapeRef *shape, Polygn *newPoly, const bool first_move)
+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();
     }
+}
+
 
-    if (!ConsolidateMoves)
+void Router::setStaticGraphInvalidated(const bool invalidated)
+{
+    _staticGraphInvalidated = invalidated;
+}
+
+
+void Router::destroyOrthogonalVisGraph(void)
+{
+    // Remove orthogonal visibility graph edges.
+    visOrthogGraph.clear();
+
+    // 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;
+    }
+}
+
+
+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;
     }
 
-    MoveInfoList::iterator curr;
-    MoveInfoList::iterator finish = moveList.end();
-    for (curr = moveList.begin(); curr != finish; ++curr)
+    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();
 
-#ifdef ORTHOGONAL_ROUTING
-        Region::addShape(shape);
-#endif
+        if (isMove)
+        {
+            shape->setNewPoly(newPoly);
+        }
+        const Polygon& shapePoly = shape->polygon();
 
-        // o  Check all visibility edges to see if this one shape
-        //    blocks them.
-        if (notPartialTime)
+        adjustContainsWithAdd(shapePoly, pid);
+
+        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();
+
+    // Don't count points on the border as being inside.
+    bool countBorder = false;
 
-    ShapeRefList::iterator finish = shapeRefs.end();
-    for (ShapeRefList::iterator i = shapeRefs.begin(); i != finish; ++i)
+    // 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",
+                static_cast<long unsigned int>(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(), static_cast<long unsigned int>(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(), static_cast<long unsigned int>(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/viscluster.cpp b/src/libavoid/viscluster.cpp
new file mode 100644
index 000000000..a127c5a17
--- /dev/null
+++ b/src/libavoid/viscluster.cpp
@@ -0,0 +1,96 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * 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.  
+ *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
+*/
+
+
+#include "libavoid/viscluster.h"
+#include "libavoid/router.h"
+#include "libavoid/assertions.h"
+
+
+namespace Avoid {
+
+
+ClusterRef::ClusterRef(Router *router, unsigned int id, Polygon& ply)
+    : _router(router)
+    , _poly(ply, router)
+    , _active(false)
+{
+    _id = router->assignId(id);
+}
+
+
+ClusterRef::~ClusterRef()
+{
+}
+
+
+void ClusterRef::makeActive(void)
+{
+    COLA_ASSERT(!_active);
+    
+    // Add to connRefs list.
+    _pos = _router->clusterRefs.insert(_router->clusterRefs.begin(), this);
+
+    _active = true;
+}
+
+
+void ClusterRef::makeInactive(void)
+{
+    COLA_ASSERT(_active);
+    
+    // Remove from connRefs list.
+    _router->clusterRefs.erase(_pos);
+
+    _active = false;
+}
+    
+
+void ClusterRef::setNewPoly(Polygon& poly)
+{
+    _poly = ReferencingPolygon(poly, _router);
+}
+
+
+unsigned int ClusterRef::id(void)
+{
+    return _id;
+}
+
+
+ReferencingPolygon& ClusterRef::polygon(void)
+{
+    return _poly;
+}
+
+
+Router *ClusterRef::router(void)
+{
+    return _router;
+}
+
+
+}
+
+
diff --git a/src/libavoid/viscluster.h b/src/libavoid/viscluster.h
new file mode 100644
index 000000000..5827e5070
--- /dev/null
+++ b/src/libavoid/viscluster.h
@@ -0,0 +1,67 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * 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.  
+ *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
+*/
+
+
+#ifndef AVOID_CLUSTER_H
+#define AVOID_CLUSTER_H
+
+#include <list>
+
+#include "libavoid/geometry.h"
+
+
+namespace Avoid {
+
+class Router;
+class ClusterRef;
+typedef std::list<ClusterRef *> ClusterRefList;
+
+
+class ClusterRef
+{
+    public:
+        ClusterRef(Router *router, unsigned int id, Polygon& poly);
+        ~ClusterRef();
+        void setNewPoly(Polygon& poly);
+        unsigned int id(void);
+        ReferencingPolygon& polygon(void);
+        Router *router(void);
+        void makeActive(void);
+        void makeInactive(void);
+
+    private:
+        Router *_router;
+        unsigned int _id;
+        ReferencingPolygon _poly;
+        bool _active;
+        ClusterRefList::iterator _pos;
+};
+
+
+}
+
+
+#endif
+
+
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);
diff --git a/src/libavoid/vpsc.cpp b/src/libavoid/vpsc.cpp
new file mode 100644
index 000000000..19d360375
--- /dev/null
+++ b/src/libavoid/vpsc.cpp
@@ -0,0 +1,1301 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * Copyright (C) 2005-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.
+ *
+ * Author(s):   Tim Dwyer  <Tim.Dwyer@csse.monash.edu.au>
+ *
+ * --------------
+ *
+ * This file contains a slightly modified version of Solver() from libvpsc:
+ * A solver for the problem of Variable Placement with Separation Constraints.
+ * It has the following changes from the Adaptagrams VPSC version:
+ *  -  The required VPSC code has been consolidated into a single file.
+ *  -  Unnecessary code (like Solver) has been removed.
+ *  -  The PairingHeap code has been replaced by a STL priority_queue.
+ *
+ * Modifications:  Michael Wybrow  <mjwybrow@users.sourceforge.net>
+ *
+*/
+
+#include <iostream>
+#include <math.h>
+#include <sstream>
+#include <map>
+#include <cfloat>
+#include <cstdio>
+
+#include "libavoid/vpsc.h"
+#include "libavoid/assertions.h"
+
+
+using namespace std;
+
+namespace Avoid {
+
+static const double ZERO_UPPERBOUND=-1e-10;
+static const double LAGRANGIAN_TOLERANCE=-1e-4;
+
+IncSolver::IncSolver(vector<Variable*> const &vs, vector<Constraint *> const &cs)
+    : m(cs.size()),
+      cs(cs),
+      n(vs.size()),
+      vs(vs)
+{
+    for(unsigned i=0;i<n;++i) {
+        vs[i]->in.clear();
+        vs[i]->out.clear();
+    }
+    for(unsigned i=0;i<m;++i) {
+        Constraint *c=cs[i];
+        c->left->out.push_back(c);
+        c->right->in.push_back(c);
+    }
+    bs=new Blocks(vs);
+#ifdef LIBVPSC_LOGGING
+    printBlocks();
+    //COLA_ASSERT(!constraintGraphIsCyclic(n,vs));
+#endif
+
+    inactive=cs;
+    for(Constraints::iterator i=inactive.begin();i!=inactive.end();++i) {
+        (*i)->active=false;
+    }
+}
+IncSolver::~IncSolver() {
+    delete bs;
+}
+
+// useful in debugging
+void IncSolver::printBlocks() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    for(set<Block*>::iterator i=bs->begin();i!=bs->end();++i) {
+        Block *b=*i;
+        f<<"  "<<*b<<endl;
+    }
+    for(unsigned i=0;i<m;i++) {
+        f<<"  "<<*cs[i]<<endl;
+    }
+#endif
+}
+
+/*
+ * Stores the relative positions of the variables in their finalPosition
+ * field.
+ */
+void IncSolver::copyResult() {
+    for(Variables::const_iterator i=vs.begin();i!=vs.end();++i) {
+        Variable* v=*i;
+        v->finalPosition=v->position();
+        COLA_ASSERT(v->finalPosition==v->finalPosition);
+    }
+}
+
+struct node {
+    set<node*> in;
+    set<node*> out;
+};
+// useful in debugging - cycles would be BAD
+bool IncSolver::constraintGraphIsCyclic(const unsigned n, Variable* const vs[]) {
+    map<Variable*, node*> varmap;
+    vector<node*> graph;
+    for(unsigned i=0;i<n;i++) {
+        node *u=new node;
+        graph.push_back(u);
+        varmap[vs[i]]=u;
+    }
+    for(unsigned i=0;i<n;i++) {
+        for(vector<Constraint*>::iterator c=vs[i]->in.begin();c!=vs[i]->in.end();++c) {
+            Variable *l=(*c)->left;
+            varmap[vs[i]]->in.insert(varmap[l]);
+        }
+
+        for(vector<Constraint*>::iterator c=vs[i]->out.begin();c!=vs[i]->out.end();++c) {
+            Variable *r=(*c)->right;
+            varmap[vs[i]]->out.insert(varmap[r]);
+        }
+    }
+    while(graph.size()>0) {
+        node *u=NULL;
+        vector<node*>::iterator i=graph.begin();
+        for(;i!=graph.end();++i) {
+            u=*i;
+            if(u->in.size()==0) {
+                break;
+            }
+        }
+        if(i==graph.end() && graph.size()>0) {
+            //cycle found!
+            return true;
+        } else {
+            graph.erase(i);
+            for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
+                node *v=*j;
+                v->in.erase(u);
+            }
+            delete u;
+        }
+    }
+    for(unsigned i=0; i<graph.size(); ++i) {
+        delete graph[i];
+    }
+    return false;
+}
+
+// useful in debugging - cycles would be BAD
+bool IncSolver::blockGraphIsCyclic() {
+    map<Block*, node*> bmap;
+    vector<node*> graph;
+    for(set<Block*>::const_iterator i=bs->begin();i!=bs->end();++i) {
+        Block *b=*i;
+        node *u=new node;
+        graph.push_back(u);
+        bmap[b]=u;
+    }
+    for(set<Block*>::const_iterator i=bs->begin();i!=bs->end();++i) {
+        Block *b=*i;
+        b->setUpInConstraints();
+        Constraint *c=b->findMinInConstraint();
+        while(c!=NULL) {
+            Block *l=c->left->block;
+            bmap[b]->in.insert(bmap[l]);
+            b->deleteMinInConstraint();
+            c=b->findMinInConstraint();
+        }
+
+        b->setUpOutConstraints();
+        c=b->findMinOutConstraint();
+        while(c!=NULL) {
+            Block *r=c->right->block;
+            bmap[b]->out.insert(bmap[r]);
+            b->deleteMinOutConstraint();
+            c=b->findMinOutConstraint();
+        }
+    }
+    while(graph.size()>0) {
+        node *u=NULL;
+        vector<node*>::iterator i=graph.begin();
+        for(;i!=graph.end();++i) {
+            u=*i;
+            if(u->in.size()==0) {
+                break;
+            }
+        }
+        if(i==graph.end() && graph.size()>0) {
+            //cycle found!
+            return true;
+        } else {
+            graph.erase(i);
+            for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
+                node *v=*j;
+                v->in.erase(u);
+            }
+            delete u;
+        }
+    }
+    for(unsigned i=0; i<graph.size(); i++) {
+        delete graph[i];
+    }
+    return false;
+}
+
+bool IncSolver::solve() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"solve_inc()..."<<endl;
+#endif
+    satisfy();
+    double lastcost = DBL_MAX, cost = bs->cost();
+    while(fabs(lastcost-cost)>0.0001) {
+        satisfy();
+        lastcost=cost;
+        cost = bs->cost();
+#ifdef LIBVPSC_LOGGING
+        f<<"  bs->size="<<bs->size()<<", cost="<<cost<<endl;
+#endif
+    }
+    copyResult();
+    return bs->size()!=n;
+}
+/*
+ * incremental version of satisfy that allows refinement after blocks are
+ * moved.
+ *
+ *  - move blocks to new positions
+ *  - repeatedly merge across most violated constraint until no more
+ *    violated constraints exist
+ *
+ * Note: there is a special case to handle when the most violated constraint
+ * is between two variables in the same block.  Then, we must split the block
+ * over an active constraint between the two variables.  We choose the
+ * constraint with the most negative lagrangian multiplier.
+ */
+bool IncSolver::satisfy() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"satisfy_inc()..."<<endl;
+#endif
+    splitBlocks();
+    //long splitCtr = 0;
+    Constraint* v = NULL;
+    //CBuffer buffer(inactive);
+    while((v = mostViolated(inactive))
+          && (v->equality || ((v->slack() < ZERO_UPPERBOUND) && !v->active)))
+    {
+        COLA_ASSERT(!v->active);
+        Block *lb = v->left->block, *rb = v->right->block;
+        if(lb != rb) {
+            lb->merge(rb,v);
+        } else {
+            if(lb->isActiveDirectedPathBetween(v->right,v->left)) {
+                // cycle found, relax the violated, cyclic constraint
+                v->unsatisfiable=true;
+                continue;
+                //UnsatisfiableException e;
+                //lb->getActiveDirectedPathBetween(e.path,v->right,v->left);
+                //e.path.push_back(v);
+                //throw e;
+            }
+            //if(splitCtr++>10000) {
+                //throw "Cycle Error!";
+            //}
+            // constraint is within block, need to split first
+            try {
+                Constraint* splitConstraint
+                    =lb->splitBetween(v->left,v->right,lb,rb);
+                if(splitConstraint!=NULL) {
+                    COLA_ASSERT(!splitConstraint->active);
+                    inactive.push_back(splitConstraint);
+                } else {
+                    v->unsatisfiable=true;
+                    continue;
+                }
+            } catch(UnsatisfiableException e) {
+                e.path.push_back(v);
+                std::cerr << "Unsatisfiable:" << std::endl;
+                for(std::vector<Constraint*>::iterator r=e.path.begin();
+                        r!=e.path.end();++r)
+                {
+                    std::cerr << **r <<std::endl;
+                }
+                v->unsatisfiable=true;
+                continue;
+            }
+            if(v->slack()>=0) {
+                COLA_ASSERT(!v->active);
+                // v was satisfied by the above split!
+                inactive.push_back(v);
+                bs->insert(lb);
+                bs->insert(rb);
+            } else {
+                bs->insert(lb->merge(rb,v));
+            }
+        }
+        bs->cleanup();
+#ifdef LIBVPSC_LOGGING
+        f<<"...remaining blocks="<<bs->size()<<", cost="<<bs->cost()<<endl;
+#endif
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  finished merges."<<endl;
+#endif
+    bs->cleanup();
+    bool activeConstraints=false;
+    for(unsigned i=0;i<m;i++) {
+        v=cs[i];
+        if(v->active) activeConstraints=true;
+        if(v->slack() < ZERO_UPPERBOUND) {
+            ostringstream s;
+            s<<"Unsatisfied constraint: "<<*v;
+#ifdef LIBVPSC_LOGGING
+            ofstream f(LOGFILE,ios::app);
+            f<<s.str()<<endl;
+#endif
+            throw s.str().c_str();
+        }
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  finished cleanup."<<endl;
+    printBlocks();
+#endif
+    copyResult();
+    return activeConstraints;
+}
+void IncSolver::moveBlocks() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"moveBlocks()..."<<endl;
+#endif
+    for(set<Block*>::const_iterator i(bs->begin());i!=bs->end();++i) {
+        Block *b = *i;
+        b->updateWeightedPosition();
+        //b->posn = b->wposn / b->weight;
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  moved blocks."<<endl;
+#endif
+}
+void IncSolver::splitBlocks() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+#endif
+    moveBlocks();
+    splitCnt=0;
+    // Split each block if necessary on min LM
+    for(set<Block*>::const_iterator i(bs->begin());i!=bs->end();++i) {
+        Block* b = *i;
+        Constraint* v=b->findMinLM();
+        if(v!=NULL && v->lm < LAGRANGIAN_TOLERANCE) {
+            COLA_ASSERT(!v->equality);
+#ifdef LIBVPSC_LOGGING
+            f<<"    found split point: "<<*v<<" lm="<<v->lm<<endl;
+#endif
+            splitCnt++;
+            Block *b = v->left->block, *l=NULL, *r=NULL;
+            COLA_ASSERT(v->left->block == v->right->block);
+            //double pos = b->posn;
+            b->split(l,r,v);
+            //l->posn=r->posn=pos;
+            //l->wposn = l->posn * l->weight;
+            //r->wposn = r->posn * r->weight;
+            l->updateWeightedPosition();
+            r->updateWeightedPosition();
+            bs->insert(l);
+            bs->insert(r);
+            b->deleted=true;
+            COLA_ASSERT(!v->active);
+            inactive.push_back(v);
+#ifdef LIBVPSC_LOGGING
+            f<<"  new blocks: "<<*l<<" and "<<*r<<endl;
+#endif
+        }
+    }
+    //if(splitCnt>0) { std::cout<<"  splits: "<<splitCnt<<endl; }
+#ifdef LIBVPSC_LOGGING
+    f<<"  finished splits."<<endl;
+#endif
+    bs->cleanup();
+}
+
+/*
+ * Scan constraint list for the most violated constraint, or the first equality
+ * constraint
+ */
+Constraint* IncSolver::mostViolated(Constraints &l) {
+    double minSlack = DBL_MAX;
+    Constraint* v=NULL;
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"Looking for most violated..."<<endl;
+#endif
+    Constraints::iterator end = l.end();
+    Constraints::iterator deletePoint = end;
+    for(Constraints::iterator i=l.begin();i!=end;++i) {
+        Constraint *c=*i;
+        double slack = c->slack();
+        if(c->equality || slack < minSlack) {
+            minSlack=slack;
+            v=c;
+            deletePoint=i;
+            if(c->equality) break;
+        }
+    }
+    // Because the constraint list is not order dependent we just
+    // move the last element over the deletePoint and resize
+    // downwards.  There is always at least 1 element in the
+    // vector because of search.
+    // TODO check this logic and add parens:
+    if((deletePoint != end) && ((minSlack < ZERO_UPPERBOUND) && !v->active || v->equality)) {
+        *deletePoint = l[l.size()-1];
+        l.resize(l.size()-1);
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  most violated is: "<<*v<<endl;
+#endif
+    return v;
+}
+
+
+using std::set;
+using std::vector;
+using std::iterator;
+using std::list;
+using std::copy;
+#define __NOTNAN(p) (p)==(p)
+
+long blockTimeCtr;
+
+Blocks::Blocks(vector<Variable*> const &vs) : vs(vs),nvs(vs.size()) {
+    blockTimeCtr=0;
+    for(int i=0;i<nvs;i++) {
+        insert(new Block(vs[i]));
+    }
+}
+Blocks::~Blocks(void)
+{
+    blockTimeCtr=0;
+    for(set<Block*>::iterator i=begin();i!=end();++i) {
+        delete *i;
+    }
+    clear();
+}
+
+/*
+ * returns a list of variables with total ordering determined by the constraint
+ * DAG
+ */
+list<Variable*> *Blocks::totalOrder() {
+    list<Variable*> *order = new list<Variable*>;
+    for(int i=0;i<nvs;i++) {
+        vs[i]->visited=false;
+    }
+    for(int i=0;i<nvs;i++) {
+        if(vs[i]->in.size()==0) {
+            dfsVisit(vs[i],order);
+        }
+    }
+    return order;
+}
+// Recursive depth first search giving total order by pushing nodes in the DAG
+// onto the front of the list when we finish searching them
+void Blocks::dfsVisit(Variable *v, list<Variable*> *order) {
+    v->visited=true;
+    vector<Constraint*>::iterator it=v->out.begin();
+    for(;it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(!c->right->visited) {
+            dfsVisit(c->right, order);
+        }
+    }
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  order="<<*v<<endl;
+#endif
+    order->push_front(v);
+}
+/*
+ * Processes incoming constraints, most violated to least, merging with the
+ * neighbouring (left) block until no more violated constraints are found
+ */
+void Blocks::mergeLeft(Block *r) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"mergeLeft called on "<<*r<<endl;
+#endif
+    r->timeStamp=++blockTimeCtr;
+    r->setUpInConstraints();
+    Constraint *c=r->findMinInConstraint();
+    while (c != NULL && c->slack()<0) {
+#ifdef LIBVPSC_LOGGING
+        f<<"mergeLeft on constraint: "<<*c<<endl;
+#endif
+        r->deleteMinInConstraint();
+        Block *l = c->left->block;
+        if (l->in==NULL) l->setUpInConstraints();
+        double dist = c->right->offset - c->left->offset - c->gap;
+        if (r->vars->size() < l->vars->size()) {
+            dist=-dist;
+            std::swap(l, r);
+        }
+        blockTimeCtr++;
+        r->merge(l, c, dist);
+        r->mergeIn(l);
+        r->timeStamp=blockTimeCtr;
+        removeBlock(l);
+        c=r->findMinInConstraint();
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"merged "<<*r<<endl;
+#endif
+}
+/*
+ * Symmetrical to mergeLeft
+ */
+void Blocks::mergeRight(Block *l) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"mergeRight called on "<<*l<<endl;
+#endif
+    l->setUpOutConstraints();
+    Constraint *c = l->findMinOutConstraint();
+    while (c != NULL && c->slack()<0) {
+#ifdef LIBVPSC_LOGGING
+        f<<"mergeRight on constraint: "<<*c<<endl;
+#endif
+        l->deleteMinOutConstraint();
+        Block *r = c->right->block;
+        r->setUpOutConstraints();
+        double dist = c->left->offset + c->gap - c->right->offset;
+        if (l->vars->size() > r->vars->size()) {
+            dist=-dist;
+            std::swap(l, r);
+        }
+        l->merge(r, c, dist);
+        l->mergeOut(r);
+        removeBlock(r);
+        c=l->findMinOutConstraint();
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"merged "<<*l<<endl;
+#endif
+}
+void Blocks::removeBlock(Block *doomed) {
+    doomed->deleted=true;
+    //erase(doomed);
+}
+void Blocks::cleanup() {
+    vector<Block*> bcopy(begin(),end());
+    for(vector<Block*>::iterator i=bcopy.begin();i!=bcopy.end();++i) {
+        Block *b=*i;
+        if(b->deleted) {
+            erase(b);
+            delete b;
+        }
+    }
+}
+/*
+ * Splits block b across constraint c into two new blocks, l and r (c's left
+ * and right sides respectively)
+ */
+void Blocks::split(Block *b, Block *&l, Block *&r, Constraint *c) {
+    b->split(l,r,c);
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"Split left: "<<*l<<endl;
+    f<<"Split right: "<<*r<<endl;
+#endif
+    r->posn = b->posn;
+    //COLA_ASSERT(r->weight!=0);
+    //r->wposn = r->posn * r->weight;
+    mergeLeft(l);
+    // r may have been merged!
+    r = c->right->block;
+    r->updateWeightedPosition();
+    //r->posn = r->wposn / r->weight;
+    mergeRight(r);
+    removeBlock(b);
+
+    insert(l);
+    insert(r);
+    COLA_ASSERT(__NOTNAN(l->posn));
+    COLA_ASSERT(__NOTNAN(r->posn));
+}
+/*
+ * returns the cost total squared distance of variables from their desired
+ * positions
+ */
+double Blocks::cost() {
+    double c = 0;
+    for(set<Block*>::iterator i=begin();i!=end();++i) {
+        c += (*i)->cost();
+    }
+    return c;
+}
+
+void PositionStats::addVariable(Variable* v) {
+    double ai=scale/v->scale;
+    double bi=v->offset/v->scale;
+    double wi=v->weight;
+    AB+=wi*ai*bi;
+    AD+=wi*ai*v->desiredPosition;
+    A2+=wi*ai*ai;
+    /*
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f << "adding v[" << v->id << "], blockscale=" << scale << ", despos="
+      << v->desiredPosition << ", ai=" << ai << ", bi=" << bi
+      << ", AB=" << AB << ", AD=" << AD << ", A2=" << A2;
+#endif
+*/
+}
+void Block::addVariable(Variable* v) {
+    v->block=this;
+    vars->push_back(v);
+    if(ps.A2==0) ps.scale=v->scale;
+    //weight+= v->weight;
+    //wposn += v->weight * (v->desiredPosition - v->offset);
+    //posn=wposn/weight;
+    ps.addVariable(v);
+    posn=(ps.AD - ps.AB) / ps.A2;
+    COLA_ASSERT(__NOTNAN(posn));
+    /*
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f << ", posn=" << posn << endl;
+#endif
+*/
+}
+Block::Block(Variable* const v)
+    : vars(new vector<Variable*>)
+    , posn(0)
+    //, weight(0)
+    //, wposn(0)
+    , deleted(false)
+    , timeStamp(0)
+    , in(NULL)
+    , out(NULL)
+{
+    if(v!=NULL) {
+        v->offset=0;
+        addVariable(v);
+    }
+}
+
+void Block::updateWeightedPosition() {
+    //wposn=0;
+    ps.AB=ps.AD=ps.A2=0;
+    for (Vit v=vars->begin();v!=vars->end();++v) {
+        //wposn += ((*v)->desiredPosition - (*v)->offset) * (*v)->weight;
+        ps.addVariable(*v);
+    }
+    posn=(ps.AD - ps.AB) / ps.A2;
+    COLA_ASSERT(__NOTNAN(posn));
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f << ", posn=" << posn << endl;
+#endif
+}
+Block::~Block(void)
+{
+    delete vars;
+    delete in;
+    delete out;
+}
+void Block::setUpInConstraints() {
+    setUpConstraintHeap(in,true);
+}
+void Block::setUpOutConstraints() {
+    setUpConstraintHeap(out,false);
+}
+void Block::setUpConstraintHeap(Heap* &h,bool in) {
+    delete h;
+    h = new Heap();
+    for (Vit i=vars->begin();i!=vars->end();++i) {
+        Variable *v=*i;
+        vector<Constraint*> *cs=in?&(v->in):&(v->out);
+        for (Cit j=cs->begin();j!=cs->end();++j) {
+            Constraint *c=*j;
+            c->timeStamp=blockTimeCtr;
+            if (((c->left->block != this) && in) || ((c->right->block != this) && !in)) {
+                h->push(c);
+            }
+        }
+    }
+}
+Block* Block::merge(Block* b, Constraint* c) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  merging on: "<<*c<<",c->left->offset="<<c->left->offset<<",c->right->offset="<<c->right->offset<<endl;
+#endif
+    double dist = c->right->offset - c->left->offset - c->gap;
+    Block *l=c->left->block;
+    Block *r=c->right->block;
+    if (l->vars->size() < r->vars->size()) {
+        r->merge(l,c,dist);
+    } else {
+               l->merge(r,c,-dist);
+    }
+    Block* mergeBlock=b->deleted?this:b;
+#ifdef LIBVPSC_LOGGING
+    f<<"  merged block="<<*mergeBlock<<endl;
+#endif
+    return mergeBlock;
+}
+/*
+ * Merges b into this block across c.  Can be either a
+ * right merge or a left merge
+ * @param b block to merge into this
+ * @param c constraint being merged
+ * @param distance separation required to satisfy c
+ */
+void Block::merge(Block *b, Constraint *c, double dist) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"    merging: "<<*b<<"dist="<<dist<<endl;
+#endif
+    c->active=true;
+    //wposn+=b->wposn-dist*b->weight;
+    //weight+=b->weight;
+    for(Vit i=b->vars->begin();i!=b->vars->end();++i) {
+        Variable *v=*i;
+        //v->block=this;
+        //vars->push_back(v);
+        v->offset+=dist;
+        addVariable(v);
+    }
+#ifdef LIBVPSC_LOGGING
+    for(Vit i=vars->begin();i!=vars->end();++i) {
+        Variable *v=*i;
+        f<<"    v["<<v->id<<"]: d="<<v->desiredPosition
+            <<" a="<<v->scale<<" o="<<v->offset
+            <<endl;
+    }
+    f<<"  AD="<<ps.AD<<" AB="<<ps.AB<<" A2="<<ps.A2<<endl;
+#endif
+    //posn=wposn/weight;
+    //COLA_ASSERT(wposn==ps.AD - ps.AB);
+    posn=(ps.AD - ps.AB) / ps.A2;
+    COLA_ASSERT(__NOTNAN(posn));
+    b->deleted=true;
+}
+
+void Block::mergeIn(Block *b) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  merging constraint heaps... "<<endl;
+#endif
+    // We check the top of the heaps to remove possible internal constraints
+    findMinInConstraint();
+    b->findMinInConstraint();
+    while (!b->in->empty())
+    {
+        in->push(b->in->top());
+        b->in->pop();
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  merged heap: "<<*in<<endl;
+#endif
+}
+void Block::mergeOut(Block *b) {
+    findMinOutConstraint();
+    b->findMinOutConstraint();
+    while (!b->out->empty())
+    {
+        out->push(b->out->top());
+        b->out->pop();
+    }
+}
+Constraint *Block::findMinInConstraint() {
+    Constraint *v = NULL;
+    vector<Constraint*> outOfDate;
+    while (!in->empty()) {
+        v = in->top();
+        Block *lb=v->left->block;
+        Block *rb=v->right->block;
+        // rb may not be this if called between merge and mergeIn
+#ifdef LIBVPSC_LOGGING
+        ofstream f(LOGFILE,ios::app);
+        f<<"  checking constraint ... "<<*v;
+        f<<"    timestamps: left="<<lb->timeStamp<<" right="<<rb->timeStamp<<" constraint="<<v->timeStamp<<endl;
+#endif
+        if(lb == rb) {
+            // constraint has been merged into the same block
+#ifdef LIBVPSC_LOGGING
+            if(v->slack()<0) {
+                f<<"  violated internal constraint found! "<<*v<<endl;
+                f<<"     lb="<<*lb<<endl;
+                f<<"     rb="<<*rb<<endl;
+            }
+#endif
+            in->pop();
+#ifdef LIBVPSC_LOGGING
+            f<<" ... skipping internal constraint"<<endl;
+#endif
+        } else if(v->timeStamp < lb->timeStamp) {
+            // block at other end of constraint has been moved since this
+            in->pop();
+            outOfDate.push_back(v);
+#ifdef LIBVPSC_LOGGING
+            f<<"    reinserting out of date (reinsert later)"<<endl;
+#endif
+        } else {
+            break;
+        }
+    }
+    for(Cit i=outOfDate.begin();i!=outOfDate.end();++i) {
+        v=*i;
+        v->timeStamp=blockTimeCtr;
+        in->push(v);
+    }
+    if(in->empty()) {
+        v=NULL;
+    } else {
+        v=in->top();
+    }
+    return v;
+}
+Constraint *Block::findMinOutConstraint() {
+    if(out->empty()) return NULL;
+    Constraint *v = out->top();
+    while (v->left->block == v->right->block) {
+        out->pop();
+        if(out->empty()) return NULL;
+        v = out->top();
+    }
+    return v;
+}
+void Block::deleteMinInConstraint() {
+    in->pop();
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"deleteMinInConstraint... "<<endl;
+    f<<"  result: "<<*in<<endl;
+#endif
+}
+void Block::deleteMinOutConstraint() {
+    out->pop();
+}
+inline bool Block::canFollowLeft(Constraint const* c, Variable const* last) const {
+    return c->left->block==this && c->active && last!=c->left;
+}
+inline bool Block::canFollowRight(Constraint const* c, Variable const* last) const {
+    return c->right->block==this && c->active && last!=c->right;
+}
+
+// computes the derivative of v and the lagrange multipliers
+// of v's out constraints (as the recursive sum of those below.
+// Does not backtrack over u.
+// also records the constraint with minimum lagrange multiplier
+// in min_lm
+double Block::compute_dfdv(Variable* const v, Variable* const u,
+               Constraint *&min_lm) {
+    double dfdv=v->dfdv();
+    for(Cit it=v->out.begin();it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+            c->lm=compute_dfdv(c->right,v,min_lm);
+            dfdv+=c->lm*c->left->scale;
+            if(!c->equality&&(min_lm==NULL||c->lm<min_lm->lm)) min_lm=c;
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+            c->lm=-compute_dfdv(c->left,v,min_lm);
+            dfdv-=c->lm*c->right->scale;
+            if(!c->equality&&(min_lm==NULL||c->lm<min_lm->lm)) min_lm=c;
+        }
+    }
+    return dfdv/v->scale;
+}
+double Block::compute_dfdv(Variable* const v, Variable* const u) {
+    double dfdv = v->dfdv();
+    for(Cit it = v->out.begin(); it != v->out.end(); ++it) {
+        Constraint *c = *it;
+        if(canFollowRight(c,u)) {
+            c->lm =   compute_dfdv(c->right,v);
+            dfdv += c->lm * c->left->scale;
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c = *it;
+        if(canFollowLeft(c,u)) {
+            c->lm = - compute_dfdv(c->left,v);
+            dfdv -= c->lm * c->right->scale;
+        }
+    }
+    return dfdv/v->scale;
+}
+
+// The top level v and r are variables between which we want to find the
+// constraint with the smallest lm.
+// Similarly, m is initially NULL and is only assigned a value if the next
+// variable to be visited is r or if a possible min constraint is returned from
+// a nested call (rather than NULL).
+// Then, the search for the m with minimum lm occurs as we return from
+// the recursion (checking only constraints traversed left-to-right
+// in order to avoid creating any new violations).
+// We also do not consider equality constraints as potential split points
+bool Block::split_path(
+    Variable* r,
+    Variable* const v,
+    Variable* const u,
+    Constraint* &m,
+    bool desperation=false
+    )
+{
+    for(Cit it(v->in.begin());it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  left split path: "<<*c<<endl;
+#endif
+            if(c->left==r) {
+                if(desperation&&!c->equality) m=c;
+                return true;
+            } else {
+                if(split_path(r,c->left,v,m)) {
+                    if(desperation && !c->equality && (!m||c->lm<m->lm)) {
+                               m=c;
+                    }
+                    return true;
+                }
+            }
+        }
+    }
+    for(Cit it(v->out.begin());it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  right split path: "<<*c<<endl;
+#endif
+            if(c->right==r) {
+                if(!c->equality) m=c;
+                return true;
+            } else {
+                if(split_path(r,c->right,v,m)) {
+                    if(!c->equality && (!m||c->lm<m->lm))
+                               m=c;
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+/*
+Block::Pair Block::compute_dfdv_between(
+        Variable* r, Variable* const v, Variable* const u,
+        const Direction dir = NONE, bool changedDirection = false) {
+    double dfdv=v->weight*(v->position() - v->desiredPosition);
+    Constraint *m=NULL;
+    for(Cit it(v->in.begin());it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+            if(dir==RIGHT) {
+                changedDirection = true;
+            }
+            if(c->left==r) {
+                       r=NULL;
+                    if(!c->equality) m=c;
+            }
+            Pair p=compute_dfdv_between(r,c->left,v,
+                    LEFT,changedDirection);
+            dfdv -= c->lm = -p.first;
+            if(r && p.second)
+                m = p.second;
+        }
+    }
+    for(Cit it(v->out.begin());it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+            if(dir==LEFT) {
+                changedDirection = true;
+            }
+            if(c->right==r) {
+                       r=NULL;
+                    if(!c->equality) m=c;
+            }
+            Pair p=compute_dfdv_between(r,c->right,v,
+                    RIGHT,changedDirection);
+            dfdv += c->lm = p.first;
+            if(r && p.second)
+                m = changedDirection && !c->equality && c->lm < p.second->lm
+                    ? c
+                    : p.second;
+        }
+    }
+    return Pair(dfdv,m);
+}
+*/
+
+// resets LMs for all active constraints to 0 by
+// traversing active constraint tree starting from v,
+// not back tracking over u
+void Block::reset_active_lm(Variable* const v, Variable* const u) {
+    for(Cit it=v->out.begin();it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+            c->lm=0;
+            reset_active_lm(c->right,v);
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+            c->lm=0;
+            reset_active_lm(c->left,v);
+        }
+    }
+}
+void Block::list_active(Variable* const v, Variable* const u) {
+    for(Cit it=v->out.begin();it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  "<<*c<<endl;
+#endif
+            list_active(c->right,v);
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  "<<*c<<endl;
+#endif
+            list_active(c->left,v);
+        }
+    }
+}
+/*
+ * finds the constraint with the minimum lagrange multiplier, that is, the constraint
+ * that most wants to split
+ */
+Constraint *Block::findMinLM() {
+    Constraint *min_lm=NULL;
+    reset_active_lm(vars->front(),NULL);
+    compute_dfdv(vars->front(),NULL,min_lm);
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  langrangians: "<<endl;
+    list_active(vars->front(),NULL);
+#endif
+    return min_lm;
+}
+Constraint *Block::findMinLMBetween(Variable* const lv, Variable* const rv) {
+    reset_active_lm(vars->front(),NULL);
+    compute_dfdv(vars->front(),NULL);
+    Constraint *min_lm=NULL;
+    split_path(rv,lv,NULL,min_lm);
+#if 0
+    if(min_lm==NULL) {
+        split_path(rv,lv,NULL,min_lm,true);
+    }
+#else
+    if(min_lm==NULL) {
+        fprintf(stderr,"Couldn't find split point!\n");
+        UnsatisfiableException e;
+        getActivePathBetween(e.path,lv,rv,NULL);
+        throw e;
+    }
+    COLA_ASSERT(min_lm!=NULL);
+#endif
+    return min_lm;
+}
+
+// populates block b by traversing the active constraint tree adding variables as they're
+// visited.  Starts from variable v and does not backtrack over variable u.
+void Block::populateSplitBlock(Block *b, Variable* v, Variable const* u) {
+    b->addVariable(v);
+    for (Cit c=v->in.begin();c!=v->in.end();++c) {
+        if (canFollowLeft(*c,u))
+            populateSplitBlock(b, (*c)->left, v);
+    }
+    for (Cit c=v->out.begin();c!=v->out.end();++c) {
+        if (canFollowRight(*c,u))
+            populateSplitBlock(b, (*c)->right, v);
+    }
+}
+/*
+ * Returns the active path between variables u and v... not back tracking over w
+ */
+bool Block::getActivePathBetween(Constraints& path, Variable const* u,
+               Variable const* v, Variable const *w) const {
+    if(u==v) return true;
+    for (Cit_const c=u->in.begin();c!=u->in.end();++c) {
+        if (canFollowLeft(*c,w)) {
+            if(getActivePathBetween(path, (*c)->left, v, u)) {
+                path.push_back(*c);
+                return true;
+            }
+        }
+    }
+    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
+        if (canFollowRight(*c,w)) {
+            if(getActivePathBetween(path, (*c)->right, v, u)) {
+                path.push_back(*c);
+                return true;
+            }
+        }
+    }
+    return false;
+}
+// Search active constraint tree from u to see if there is a directed path to v.
+// Returns true if path is found with all constraints in path having their visited flag
+// set true.
+bool Block::isActiveDirectedPathBetween(Variable const* u, Variable const* v) const {
+    if(u==v) return true;
+    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
+        if(canFollowRight(*c,NULL)) {
+            if(isActiveDirectedPathBetween((*c)->right,v)) {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+bool Block::getActiveDirectedPathBetween(
+        Constraints& path, Variable const* u, Variable const* v) const {
+    if(u==v) return true;
+    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
+        if(canFollowRight(*c,NULL)) {
+            if(getActiveDirectedPathBetween(path,(*c)->right,v)) {
+                path.push_back(*c);
+                return true;
+            }
+        }
+    }
+    return false;
+}
+/*
+ * Block needs to be split because of a violated constraint between vl and vr.
+ * We need to search the active constraint tree between l and r and find the constraint
+ * with min lagrangrian multiplier and split at that point.
+ * Returns the split constraint
+ */
+Constraint* Block::splitBetween(Variable* const vl, Variable* const vr,
+               Block* &lb, Block* &rb) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  need to split between: "<<*vl<<" and "<<*vr<<endl;
+#endif
+    Constraint *c=findMinLMBetween(vl, vr);
+#ifdef LIBVPSC_LOGGING
+    f<<"  going to split on: "<<*c<<endl;
+#endif
+    if(c!=NULL) {
+        split(lb,rb,c);
+        deleted = true;
+    }
+    return c;
+}
+
+/*
+ * Creates two new blocks, l and r, and splits this block across constraint c,
+ * placing the left subtree of constraints (and associated variables) into l
+ * and the right into r.
+ */
+void Block::split(Block* &l, Block* &r, Constraint* c) {
+    c->active=false;
+    l=new Block();
+    populateSplitBlock(l,c->left,c->right);
+    //COLA_ASSERT(l->weight>0);
+    r=new Block();
+    populateSplitBlock(r,c->right,c->left);
+    //COLA_ASSERT(r->weight>0);
+}
+
+/*
+ * Computes the cost (squared euclidean distance from desired positions) of the
+ * current positions for variables in this block
+ */
+double Block::cost() {
+    double c = 0;
+    for (Vit v=vars->begin();v!=vars->end();++v) {
+        double diff = (*v)->position() - (*v)->desiredPosition;
+        c += (*v)->weight * diff * diff;
+    }
+    return c;
+}
+ostream& operator <<(ostream &os, const Block& b)
+{
+    os<<"Block(posn="<<b.posn<<"):";
+    for(Block::Vit v=b.vars->begin();v!=b.vars->end();++v) {
+        os<<" "<<**v;
+    }
+    if(b.deleted) {
+        os<<" Deleted!";
+    }
+    return os;
+}
+
+Constraint::Constraint(Variable *left, Variable *right, double gap, bool equality)
+: left(left),
+  right(right),
+  gap(gap),
+  timeStamp(0),
+  active(false),
+  equality(equality),
+  unsatisfiable(false)
+{
+    // In hindsight I think it's probably better to build the constraint DAG
+    // (by creating variable in/out lists) when needed, rather than in advance
+    //left->out.push_back(this);
+    //right->in.push_back(this);
+}
+Constraint::~Constraint() {
+    // see constructor: the following is just way too slow.
+    // Better to create a
+    // new DAG on demand than maintain the lists dynamically.
+    //Constraints::iterator i;
+    //for(i=left->out.begin(); i!=left->out.end(); i++) {
+        //if(*i==this) break;
+    //}
+    //left->out.erase(i);
+    //for(i=right->in.begin(); i!=right->in.end(); i++) {
+        //if(*i==this) break;
+    //}
+    //right->in.erase(i);
+}
+double Constraint::slack() const {
+    return unsatisfiable ? DBL_MAX
+           : right->scale * right->position()
+         - gap - left->scale * left->position();
+}
+std::ostream& operator <<(std::ostream &os, const Constraint &c)
+{
+    if(&c==NULL) {
+        os<<"NULL";
+    } else {
+        const char *type=c.equality?"=":"<=";
+        std::ostringstream lscale, rscale;
+        if(c.left->scale!=1) {
+            lscale << c.left->scale << "*";
+        }
+        if(c.right->scale!=1) {
+            rscale << c.right->scale << "*";
+        }
+        os<<lscale.str()<<*c.left<<"+"<<c.gap<<type<<rscale.str()<<*c.right;
+        if(c.left->block&&c.right->block)
+            os<<"("<<c.slack()<<")"<<(c.active?"-active":"")
+                <<"(lm="<<c.lm<<")";
+        else
+            os<<"(vars have no position)";
+    }
+    return os;
+}
+
+bool CompareConstraints::operator() (
+    Constraint *const &l, Constraint *const &r
+) const {
+    double const sl =
+        l->left->block->timeStamp > l->timeStamp
+        ||l->left->block==l->right->block
+        ?-DBL_MAX:l->slack();
+    double const sr =
+        r->left->block->timeStamp > r->timeStamp
+        ||r->left->block==r->right->block
+        ?-DBL_MAX:r->slack();
+    if(sl==sr) {
+        // arbitrary choice based on id
+        if(l->left->id==r->left->id) {
+            if(l->right->id<r->right->id) return true;
+            return false;
+        }
+        if(l->left->id<r->left->id) return true;
+        return false;
+    }
+    return sl > sr;
+}
+
+std::ostream& operator <<(std::ostream &os, const Variable &v) {
+    if(v.block)
+        os << "(" << v.id << "=" << v.position() << ")";
+    else
+        os << "(" << v.id << "=" << v.desiredPosition << ")";
+    return os;
+}
+
+}
diff --git a/src/libavoid/vpsc.h b/src/libavoid/vpsc.h
new file mode 100644
index 000000000..4d6d8ce61
--- /dev/null
+++ b/src/libavoid/vpsc.h
@@ -0,0 +1,255 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * Copyright (C) 2005-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.  
+ *
+ * Author(s):   Tim Dwyer  <Tim.Dwyer@csse.monash.edu.au>
+ *
+ * --------------
+ *
+ * This file contains a slightly modified version of Solver() from libvpsc:
+ * A solver for the problem of Variable Placement with Separation Constraints.
+ * It has the following changes from the Adaptagrams VPSC version:
+ *  -  The required VPSC code has been consolidated into a single file.
+ *  -  Unnecessary code (like Solver) has been removed.
+ *  -  The PairingHeap code has been replaced by a STL priority_queue.
+ *
+ * Modifications:  Michael Wybrow  <mjwybrow@users.sourceforge.net>
+ *
+*/
+
+#ifndef LIBAVOID_VPSC_H
+#define LIBAVOID_VPSC_H
+
+#include <vector>
+#include <list>
+#include <set>
+#include <queue>
+
+namespace Avoid {
+
+class Variable;
+class Constraint;
+typedef std::vector<Variable*> Variables;
+typedef std::vector<Constraint*> Constraints;
+class CompareConstraints {
+public:
+    bool operator() (Constraint *const &l, Constraint *const &r) const;
+};
+struct PositionStats {
+    PositionStats() : scale(0), AB(0), AD(0), A2(0) {}
+    void addVariable(Variable* const v);
+    double scale;
+    double AB;
+    double AD;
+    double A2;
+};
+
+typedef std::priority_queue<Constraint*,std::vector<Constraint*>,
+        CompareConstraints> Heap;
+
+class Block
+{
+    typedef Variables::iterator Vit;
+    typedef Constraints::iterator Cit;
+    typedef Constraints::const_iterator Cit_const;
+
+    friend std::ostream& operator <<(std::ostream &os,const Block &b);
+public:
+    Variables *vars;
+    double posn;
+    //double weight;
+    //double wposn;
+    PositionStats ps;
+    Block(Variable* const v=NULL);
+    ~Block(void);
+    Constraint* findMinLM();
+    Constraint* findMinLMBetween(Variable* const lv, Variable* const rv);
+    Constraint* findMinInConstraint();
+    Constraint* findMinOutConstraint();
+    void deleteMinInConstraint();
+    void deleteMinOutConstraint();
+    void updateWeightedPosition();
+    void merge(Block *b, Constraint *c, double dist);
+    Block* merge(Block *b, Constraint *c);
+    void mergeIn(Block *b);
+    void mergeOut(Block *b);
+    void split(Block *&l, Block *&r, Constraint *c);
+    Constraint* splitBetween(Variable* vl, Variable* vr, Block* &lb, Block* &rb);
+    void setUpInConstraints();
+    void setUpOutConstraints();
+    double cost();
+    bool deleted;
+    long timeStamp;
+    Heap *in;
+    Heap *out;
+    bool getActivePathBetween(Constraints& path, Variable const* u,
+               Variable const* v, Variable const *w) const;
+    bool isActiveDirectedPathBetween(
+            Variable const* u, Variable const* v) const;
+    bool getActiveDirectedPathBetween(Constraints& path, Variable const * u, Variable const * v) const;
+private:
+    typedef enum {NONE, LEFT, RIGHT} Direction;
+    typedef std::pair<double, Constraint*> Pair;
+    void reset_active_lm(Variable* const v, Variable* const u);
+    void list_active(Variable* const v, Variable* const u);
+    double compute_dfdv(Variable* const v, Variable* const u);
+    double compute_dfdv(Variable* const v, Variable* const u, Constraint *&min_lm);
+    bool split_path(Variable*, Variable* const, Variable* const, 
+            Constraint* &min_lm, bool desperation);
+    bool canFollowLeft(Constraint const* c, Variable const* last) const;
+    bool canFollowRight(Constraint const* c, Variable const* last) const;
+    void populateSplitBlock(Block *b, Variable* v, Variable const* u);
+    void addVariable(Variable* v);
+    void setUpConstraintHeap(Heap* &h,bool in);
+};
+
+
+class Constraint;
+typedef std::vector<Constraint*> Constraints;
+class Variable
+{
+    friend std::ostream& operator <<(std::ostream &os, const Variable &v);
+    friend class Block;
+    friend class Constraint;
+    friend class IncSolver;
+public:
+    int id; // useful in log files
+    double desiredPosition;
+    double finalPosition;
+    double weight; // how much the variable wants to 
+                   // be at it's desired position
+    double scale; // translates variable to another space
+    double offset;
+    Block *block;
+    bool visited;
+    bool fixedDesiredPosition;
+    Constraints in;
+    Constraints out;
+    char *toString();
+    inline Variable(const int id, const double desiredPos=-1.0, 
+            const double weight=1.0, const double scale=1.0)
+        : id(id)
+        , desiredPosition(desiredPos)
+        , weight(weight)
+        , scale(scale)
+        , offset(0)
+        , block(NULL)
+        , visited(false)
+        , fixedDesiredPosition(false)
+    {
+    }
+    double dfdv() const {
+        return 2. * weight * ( position() - desiredPosition );
+    }
+private:
+    double position() const {
+        return (block->ps.scale*block->posn+offset)/scale;
+    }
+};
+
+
+class Constraint
+{
+    friend std::ostream& operator <<(std::ostream &os,const Constraint &c);
+public:
+    Variable *left;
+    Variable *right;
+    double gap;
+    double lm;
+    Constraint(Variable *left, Variable *right, double gap, bool equality=false);
+    ~Constraint();
+    double slack() const;
+    long timeStamp;
+    bool active;
+    const bool equality;
+    bool unsatisfiable;
+};
+/*
+ * A block structure defined over the variables such that each block contains
+ * 1 or more variables, with the invariant that all constraints inside a block
+ * are satisfied by keeping the variables fixed relative to one another
+ */
+class Blocks : public std::set<Block*>
+{
+public:
+    Blocks(Variables const &vs);
+    ~Blocks(void);
+    void mergeLeft(Block *r);
+    void mergeRight(Block *l);
+    void split(Block *b, Block *&l, Block *&r, Constraint *c);
+    std::list<Variable*> *totalOrder();
+    void cleanup();
+    double cost();
+private:
+    void dfsVisit(Variable *v, std::list<Variable*> *order);
+    void removeBlock(Block *doomed);
+    Variables const &vs;
+    int nvs;
+};
+
+extern long blockTimeCtr;
+
+struct UnsatisfiableException {
+    Constraints path;
+};
+struct UnsatisfiedConstraint {
+    UnsatisfiedConstraint(Constraint& c):c(c) {}
+    Constraint& c;
+};
+/*
+ * Variable Placement with Separation Constraints problem instance
+ */
+class IncSolver {
+public:
+    unsigned splitCnt;
+    bool satisfy();
+    bool solve();
+    void moveBlocks();
+    void splitBlocks();
+    IncSolver(Variables const &vs, Constraints const &cs);
+
+    ~IncSolver();
+    Variables const & getVariables() { return vs; }
+protected:
+    Blocks *bs;
+    unsigned m;
+    Constraints const &cs;
+    unsigned n;
+    Variables const &vs;
+    void printBlocks();
+    void copyResult();
+private:
+    bool constraintGraphIsCyclic(const unsigned n, Variable* const vs[]);
+    bool blockGraphIsCyclic();
+    Constraints inactive;
+    Constraints violated;
+    Constraint* mostViolated(Constraints &l);
+};
+
+struct delete_object
+{
+    template <typename T>
+    void operator()(T *ptr){ delete ptr;}
+};
+
+
+}
+
+#endif // AVOID_VPSC_H