Merge GSoC2009 Connectors into trunk

(bzr r8855)

49 files changed, 7622 insertions, 3314 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1885f56e5..46c2586d1 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -86,6 +86,7 @@ composite-undo-stack-observer.cpp
 common-context.cpp

 conditions.cpp

 conn-avoid-ref.cpp

+connection-points.cpp

 connector-context.cpp

 console-output-undo-observer.cpp

 context-fns.cpp

diff --git a/src/Makefile_insert b/src/Makefile_insert
index 18beaec37..da49187ba 100644
--- a/src/Makefile_insert
+++ b/src/Makefile_insert
@@ -20,6 +20,7 @@ ink_common_sources +=	\
 	composite-undo-stack-observer.cpp				\
 	composite-undo-stack-observer.h					\
 	conditions.cpp conditions.h					\
+	connection-points.cpp connection-points.h			\
 	conn-avoid-ref.cpp conn-avoid-ref.h				\
 	connection-pool.h						\
 	connector-context.cpp connector-context.h			\
diff --git a/src/attributes.cpp b/src/attributes.cpp
index 69b11fae9..d03c3be3e 100644
--- a/src/attributes.cpp
+++ b/src/attributes.cpp
@@ -37,6 +37,7 @@ static SPStyleProp const props[] = {
     {SP_ATTR_SODIPODI_INSENSITIVE, "sodipodi:insensitive"},
     {SP_ATTR_SODIPODI_NONPRINTABLE, "sodipodi:nonprintable"},
     {SP_ATTR_CONNECTOR_AVOID, "inkscape:connector-avoid"},
+    {SP_ATTR_CONNECTION_POINTS, "inkscape:connection-points"},
     {SP_ATTR_STYLE, "style"},
     {SP_ATTR_TRANSFORM_CENTER_X, "inkscape:transform-center-x"},
     {SP_ATTR_TRANSFORM_CENTER_Y, "inkscape:transform-center-y"},
@@ -124,6 +125,7 @@ static SPStyleProp const props[] = {
     {SP_ATTR_CONNECTOR_TYPE, "inkscape:connector-type"},
     {SP_ATTR_CONNECTION_START, "inkscape:connection-start"},
     {SP_ATTR_CONNECTION_END, "inkscape:connection-end"},
+    {SP_ATTR_CONNECTOR_CURVATURE, "inkscape:connector-curvature"},
     /* SPRect */
     {SP_ATTR_RX, "rx"},
     {SP_ATTR_RY, "ry"},
diff --git a/src/attributes.h b/src/attributes.h
index 52f71b203..af60b75be 100644
--- a/src/attributes.h
+++ b/src/attributes.h
@@ -36,6 +36,7 @@ enum SPAttributeEnum {
     SP_ATTR_SODIPODI_INSENSITIVE,
     SP_ATTR_SODIPODI_NONPRINTABLE,
     SP_ATTR_CONNECTOR_AVOID,
+    SP_ATTR_CONNECTION_POINTS,
     SP_ATTR_STYLE,
     SP_ATTR_TRANSFORM_CENTER_X,
     SP_ATTR_TRANSFORM_CENTER_Y,
@@ -124,6 +125,7 @@ enum SPAttributeEnum {
     SP_ATTR_CONNECTOR_TYPE,
     SP_ATTR_CONNECTION_START,
     SP_ATTR_CONNECTION_END,
+    SP_ATTR_CONNECTOR_CURVATURE,
     /* SPRect */
     SP_ATTR_RX,
     SP_ATTR_RY,
diff --git a/src/conn-avoid-ref.cpp b/src/conn-avoid-ref.cpp
index 43c9c0b66..c04ad9e49 100644
--- a/src/conn-avoid-ref.cpp
+++ b/src/conn-avoid-ref.cpp
@@ -12,23 +12,34 @@
 
 #include <cstring>
 #include <string>
+#include <iostream>
 
 #include "sp-item.h"
+#include "display/curve.h"
+#include "2geom/line.h"
+#include "2geom/crossing.h"
+#include "2geom/convex-cover.h"
+#include "svg/stringstream.h"
 #include "conn-avoid-ref.h"
-#include "libavoid/polyutil.h"
+#include "connection-points.h"
+#include "sp-conn-end.h"
+#include "sp-path.h"
 #include "libavoid/router.h"
 #include "libavoid/connector.h"
+#include "libavoid/geomtypes.h"
 #include "xml/node.h"
 #include "document.h"
 #include "desktop.h"
 #include "desktop-handles.h"
 #include "sp-namedview.h"
 #include "inkscape.h"
+#include <glibmm/i18n.h>
+
 
 
 using Avoid::Router;
 
-static Avoid::Polygn avoid_item_poly(SPItem const *item);
+static Avoid::Polygon avoid_item_poly(SPItem const *item);
 
 
 SPAvoidRef::SPAvoidRef(SPItem *spitem)
@@ -44,13 +55,17 @@ SPAvoidRef::SPAvoidRef(SPItem *spitem)
 SPAvoidRef::~SPAvoidRef()
 {
     _transformed_connection.disconnect();
-    if (shapeRef) {
+
+    // If the document is being destroyed then the router instance 
+    // and the ShapeRefs will have been destroyed with it.
+    const bool routerInstanceExists = (item->document->router != NULL);
+
+    if (shapeRef && routerInstanceExists) {
         Router *router = shapeRef->router();
-        // shapeRef is finalised by delShape,
-        // so no memory is lost here.
-        router->delShape(shapeRef);
-        shapeRef = NULL;
+        router->removeShape(shapeRef);
+        delete shapeRef;
     }
+    shapeRef = NULL;
 }
 
 
@@ -66,6 +81,205 @@ void SPAvoidRef::setAvoid(char const *value)
     }
 }
 
+void print_connection_points(std::map<int, ConnectionPoint>& cp)
+{
+    std::map<int, ConnectionPoint>::iterator i;
+    for (i=cp.begin(); i!=cp.end(); ++i)
+    {
+        const ConnectionPoint& p = i->second;
+        std::cout<<p.id<<" "<<p.type<<" "<<p.pos[Geom::X]<<" "<<p.pos[Geom::Y]<<std::endl;
+    }
+}
+
+void SPAvoidRef::setConnectionPoints(gchar const *value)
+{
+    std::set<int> updates;
+    std::set<int> deletes;
+    std::set<int> seen;
+
+    if (value)
+    {
+        /* Rebuild the connection points list.
+           Update the connectors for which
+           the endpoint has changed.
+        */
+        
+        gchar ** strarray = g_strsplit(value, "|", 0);
+        gchar ** iter = strarray;
+        
+        while (*iter != NULL) {
+            ConnectionPoint cp;
+            Inkscape::SVGIStringStream is(*iter);
+            is>>cp;
+            cp.type = ConnPointUserDefined;
+
+            /* Mark this connection point as seen, so we can delete
+               the other ones.
+            */
+            seen.insert(cp.id);
+            if ( connection_points.find(cp.id) != connection_points.end() )
+            {
+                /* An already existing connection point.
+                   Check to see if changed, and, if it is
+                   the case, trigger connector update for
+                   the connector attached to this connection
+                   point. This is done by adding the
+                   connection point to a list of connection
+                   points to be updated.
+                */
+                if ( connection_points[cp.id] != cp )
+                    // The connection point got updated.
+                    // Put it in the update list.
+                    updates.insert(cp.id);
+            }
+            connection_points[cp.id] = cp;
+            ++iter;
+        }
+        /* Delete the connection points that didn't appear
+           in the new connection point list.
+        */
+        std::map<int, ConnectionPoint>::iterator it;
+
+        for (it=connection_points.begin(); it!=connection_points.end(); ++it)
+            if ( seen.find(it->first) == seen.end())
+                deletes.insert(it->first);
+        g_strfreev(strarray);
+    }
+    else
+    {
+        /* Delete all the user-defined connection points
+           Actually we do this by adding them to the list
+           of connection points to be deleted.
+        */
+        std::map<int, ConnectionPoint>::iterator it;
+
+        for (it=connection_points.begin(); it!=connection_points.end(); ++it)
+            deletes.insert(it->first);
+    }
+    /* Act upon updates and deletes.
+    */
+    if (deletes.empty() && updates.empty())
+        // Nothing to do, just return.
+        return;
+    // Get a list of attached connectors.
+    GSList* conns = getAttachedConnectors(Avoid::runningToAndFrom);
+    for (GSList *i = conns; i != NULL; i = i->next)
+    {
+        SPPath* path = SP_PATH(i->data);
+        SPConnEnd** connEnds = path->connEndPair.getConnEnds();
+        for (int ix=0; ix<2; ++ix)
+            if (connEnds[ix]->type == ConnPointUserDefined)
+                if (updates.find(connEnds[ix]->id) != updates.end())
+                    if (path->connEndPair.isAutoRoutingConn())
+                        path->connEndPair.tellLibavoidNewEndpoints();
+                    else
+                    {
+                    }
+                else
+                    if (deletes.find(connEnds[ix]->id) != deletes.end())
+                        sp_conn_end_detach(path, ix);
+    }
+    g_slist_free(conns);
+    // Remove all deleted connection points
+    if (deletes.size())
+        for (std::set<int>::iterator it = deletes.begin(); it != deletes.end(); ++it)
+            connection_points.erase(*it);
+}
+
+void SPAvoidRef::setConnectionPointsAttrUndoable(const gchar* value, const gchar* action)
+{
+    SPDocument* doc = SP_OBJECT_DOCUMENT(item);
+    
+    sp_object_setAttribute( SP_OBJECT(item), "inkscape:connection-points", value, 0 );
+    item->updateRepr();
+    sp_document_ensure_up_to_date(doc);
+    sp_document_done(doc, SP_VERB_CONTEXT_CONNECTOR, action);
+}
+
+void SPAvoidRef::addConnectionPoint(ConnectionPoint &cp)
+{
+    Inkscape::SVGOStringStream ostr;
+    bool first = true;
+    int newId = 1;
+    if ( connection_points.size() )
+    {
+        for (IdConnectionPointMap::iterator it = connection_points.begin(); ; )
+        {
+            if ( first )
+            {
+                first = false;
+                ostr<<it->second;
+            }
+            else
+                ostr<<'|'<<it->second;
+            IdConnectionPointMap::iterator prev_it = it;
+            ++it;
+            if ( it == connection_points.end() || prev_it->first + 1 != it->first )
+            {
+                newId = prev_it->first + 1;
+                break;
+            }
+        }
+    }
+    cp.id = newId;
+    if ( first )
+    {
+        first = false;
+        ostr<<cp;
+    }
+    else
+        ostr<<'|'<<cp;
+        
+    this->setConnectionPointsAttrUndoable( ostr.str().c_str(), _("Added a new connection point") );
+}
+
+void SPAvoidRef::updateConnectionPoint(ConnectionPoint &cp)
+{
+    Inkscape::SVGOStringStream ostr;
+    IdConnectionPointMap::iterator cp_pos = connection_points.find( cp.id );
+    if ( cp_pos != connection_points.end() )
+    {
+        bool first = true;
+        for (IdConnectionPointMap::iterator it = connection_points.begin(); it != connection_points.end(); ++it)
+        {
+            ConnectionPoint* to_write;
+            if ( it != cp_pos )
+                to_write = &it->second;
+            else
+                to_write = &cp;
+            if ( first )
+            {
+                first = false;
+                ostr<<*to_write;
+            }
+            else
+                ostr<<'|'<<*to_write;
+        }
+        this->setConnectionPointsAttrUndoable( ostr.str().c_str(), _("Moved a connection point") );
+    }
+}
+
+void SPAvoidRef::deleteConnectionPoint(ConnectionPoint &cp)
+{
+    Inkscape::SVGOStringStream ostr;
+    IdConnectionPointMap::iterator cp_pos = connection_points.find( cp.id );
+    if ( cp_pos != connection_points.end() )
+    {
+        bool first = true;
+        for (IdConnectionPointMap::iterator it = connection_points.begin(); it != connection_points.end(); ++it)
+        {
+            if ( it != cp_pos )
+                if ( first )
+                {
+                    first = false;
+                    ostr<<it->second;
+                }
+                else
+                    ostr<<'|'<<it->second;
+        }
+        this->setConnectionPointsAttrUndoable( ostr.str().c_str(), _("Removed a connection point") );
+    }
+}
 
 void SPAvoidRef::handleSettingChange(void)
 {
@@ -93,8 +307,8 @@ void SPAvoidRef::handleSettingChange(void)
     
     _transformed_connection.disconnect();
     if (new_setting) {
-        Avoid::Polygn poly = avoid_item_poly(item);
-        if (poly.pn > 0) {
+        Avoid::Polygon poly = avoid_item_poly(item);
+        if (poly.size() > 0) {
             _transformed_connection = item->connectTransformed(
                     sigc::ptr_fun(&avoid_item_move));
 
@@ -104,8 +318,7 @@ void SPAvoidRef::handleSettingChange(void)
             // Get a unique ID for the item.
             GQuark itemID = g_quark_from_string(id);
 
-            shapeRef = new Avoid::ShapeRef(router, itemID, poly);
-            Avoid::freePoly(poly);
+            shapeRef = new Avoid::ShapeRef(router, poly, itemID);
         
             router->addShape(shapeRef);
         }
@@ -114,9 +327,8 @@ void SPAvoidRef::handleSettingChange(void)
     {
         g_assert(shapeRef);
         
-        // shapeRef is finalised by delShape,
-        // so no memory is lost here.
-        router->delShape(shapeRef);
+        router->removeShape(shapeRef);
+        delete shapeRef;
         shapeRef = NULL;
     }
 }
@@ -169,14 +381,53 @@ GSList *SPAvoidRef::getAttachedConnectors(const unsigned int type)
     return list;
 }
 
+Geom::Point SPAvoidRef::getConnectionPointPos(const int type, const int id)
+{
+    g_assert(item);
+    Geom::Point pos;
+    const Geom::Matrix& transform = sp_item_i2doc_affine(item);
+    SPDesktop *desktop = inkscape_active_desktop();
+
+    if ( type == ConnPointDefault )
+    {
+        // For now, just default to the centre of the item
+        Geom::OptRect bbox = item->getBounds(sp_item_i2doc_affine(item));
+        pos = (bbox) ? bbox->midpoint() : Geom::Point(0, 0);
+    }
+    else
+    {
+        // Get coordinates from the list of connection points
+        // that are attached to the item
+        pos = connection_points[id].pos * transform;
+    }
+
+    return pos;
+}
+
+bool SPAvoidRef::isValidConnPointId( const int type, const int id )
+{
+    if ( type < 0 || type > 1 )
+        return false;
+    else
+    {
+        if ( type == ConnPointDefault )
+            if ( id < 0 || id > 8 )
+                return false;
+            else
+            {
+            }
+        else
+            return connection_points.find( id ) != connection_points.end();
+    }
+    
+    return true;
+}
 
-static Avoid::Polygn avoid_item_poly(SPItem const *item)
+static Avoid::Polygon avoid_item_poly(SPItem const *item)
 {
     SPDesktop *desktop = inkscape_active_desktop();
     g_assert(desktop != NULL);
 
-    Avoid::Polygn poly;
-
     // TODO: The right way to do this is to return the convex hull of
     //       the object, or an approximation in the case of a rounded
     //       object.  Specific SPItems will need to have a new
@@ -185,24 +436,112 @@ static Avoid::Polygn avoid_item_poly(SPItem const *item)
     //       some convex hull code, though not NR::ConvexHull as this
     //       only keeps the bounding box of the convex hull currently.
 
-    // TODO: SPItem::getBounds gives the wrong result for some objects
-    //       that have internal representations that are updated later
-    //       by the sp_*_update functions, e.g., text.
-    sp_document_ensure_up_to_date(item->document);
+    double spacing = desktop->namedview->connector_spacing;
+
+    // [sommer] If item is a shape, use an approximation of its convex hull
+    {
+        // MJW: Disable this for the moment.  It still has some issues.
+        const bool convex_hull_approximation_enabled = false;
+
+        if ( convex_hull_approximation_enabled && SP_IS_SHAPE (item) ) {
+            // The number of points to use for approximation
+            const unsigned NUM_POINTS = 64; 
+
+//             printf("[sommer] is a shape\n");
+            SPCurve* curve = sp_shape_get_curve (SP_SHAPE (item));
+            if (curve) {
+//                 printf("[sommer] is a curve\n");
+
+                // apply all transformations
+                Geom::Matrix itd_mat = sp_item_i2doc_affine(item);
+                curve->transform(itd_mat);
+
+                // iterate over all paths
+                const Geom::PathVector& curve_pv = curve->get_pathvector();
+                std::vector<Geom::Point> hull_points;
+                for (Geom::PathVector::const_iterator i = curve_pv.begin(); i != curve_pv.end(); i++) {
+                    const Geom::Path& curve_pv_path = *i; 
+//                     printf("[sommer] tracing sub-path\n");
+
+                    // FIXME: enlarge path by "desktop->namedview->connector_spacing" (using sp_selected_path_do_offset)?
+
+                    // use appropriate fraction of points for this path (first one gets any remainder)
+                    unsigned num_points = NUM_POINTS / curve_pv.size();
+                    if (i == curve_pv.begin()) num_points += NUM_POINTS - (num_points * curve_pv.size());
+                    printf("[sommer] using %d points for this path\n", num_points);
+
+                    // sample points along the path for approximation of convex hull
+                    for (unsigned n = 0; n < num_points; n++) {
+                        double at = curve_pv_path.size() / static_cast<double>(num_points) * n;                        
+                        Geom::Point pt = curve_pv_path.pointAt(at);
+                        hull_points.push_back(pt);
+                    }
+                }
+
+                curve->unref();
+
+                // create convex hull from all sampled points
+                Geom::ConvexHull hull(hull_points);
+
+                // store expanded convex hull in Avoid::Polygn
+                unsigned n = 0;
+                Avoid::Polygon poly;
+                const Geom::Point& old_pt = *hull.boundary.begin();
+
+                Geom::Line hull_edge(*hull.boundary.begin(), *(hull.boundary.begin()+1));
+                Geom::Line parallel_hull_edge;
+                parallel_hull_edge.origin(hull_edge.origin()+hull_edge.versor().ccw()*spacing);
+                parallel_hull_edge.versor(hull_edge.versor());
+                Geom::Line bisector = Geom::make_angle_bisector_line( *(hull.boundary.end()), *hull.boundary.begin(),
+                                                                      *(hull.boundary.begin()+1));
+                Geom::OptCrossing int_pt = Geom::intersection(parallel_hull_edge, bisector);
+
+                if (int_pt)
+                {
+                    Avoid::Point avoid_pt((parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::X], 
+                                            (parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::Y]);
+//                     printf("[sommer] %f, %f\n", old_pt[Geom::X], old_pt[Geom::Y]);
+/*                    printf("[sommer] %f, %f\n", (parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::X], 
+                                                (parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::Y]);*/
+                    poly.ps.push_back(avoid_pt);
+                }
+                for (std::vector<Geom::Point>::const_iterator i = hull.boundary.begin() + 1; i != hull.boundary.end(); i++, n++) {
+                        const Geom::Point& old_pt = *i;
+                        Geom::Line hull_edge(*i, *(i+1));
+                        Geom::Line parallel_hull_edge;
+                        parallel_hull_edge.origin(hull_edge.origin()+hull_edge.versor().ccw()*spacing);
+                        parallel_hull_edge.versor(hull_edge.versor());
+                        Geom::Line bisector = Geom::make_angle_bisector_line( *(i-1), *i, *(i+1));
+                        Geom::OptCrossing intersect_pt = Geom::intersection(parallel_hull_edge, bisector);
+
+                        if (int_pt)
+                        {
+                            Avoid::Point avoid_pt((parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::X], 
+                                                  (parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::Y]);
+/*                            printf("[sommer] %f, %f\n", old_pt[Geom::X], old_pt[Geom::Y]);
+                            printf("[sommer] %f, %f\n", (parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::X], 
+                                                        (parallel_hull_edge.origin()+parallel_hull_edge.versor()*int_pt->ta)[Geom::Y]);*/
+                            poly.ps.push_back(avoid_pt);
+                        }
+                }
+                
+
+                return poly;
+            }// else printf("[sommer] is no curve\n");
+        }// else printf("[sommer] is no shape\n");
+    }
     
     Geom::OptRect rHull = item->getBounds(sp_item_i2doc_affine(item));
     if (!rHull) {
-        return Avoid::newPoly(0);
+        return Avoid::Polygon();
     }
 
-    double spacing = desktop->namedview->connector_spacing;
-
     // Add a little buffer around the edge of each object.
     Geom::Rect rExpandedHull = *rHull;
     rExpandedHull.expandBy(spacing); 
-    poly = Avoid::newPoly(4);
+    Avoid::Polygon poly(4);
 
-    for (unsigned n = 0; n < 4; ++n) {
+    for (size_t n = 0; n < 4; ++n) {
         Geom::Point hullPoint = rExpandedHull.corner(n);
         poly.ps[n].x = hullPoint[Geom::X];
         poly.ps[n].y = hullPoint[Geom::Y];
@@ -242,10 +581,9 @@ void avoid_item_move(Geom::Matrix const */*mp*/, SPItem *moved_item)
     g_assert(shapeRef);
 
     Router *router = moved_item->document->router;
-    Avoid::Polygn poly = avoid_item_poly(moved_item);
-    if (poly.pn > 0) {
-        router->moveShape(shapeRef, &poly);
-        Avoid::freePoly(poly);
+    Avoid::Polygon poly = avoid_item_poly(moved_item);
+    if (!poly.empty()) {
+        router->moveShape(shapeRef, poly);
     }
 }
 
diff --git a/src/conn-avoid-ref.h b/src/conn-avoid-ref.h
index d34d8ca2e..5dff8dd38 100644
--- a/src/conn-avoid-ref.h
+++ b/src/conn-avoid-ref.h
@@ -18,6 +18,8 @@
 
 struct SPDesktop;
 struct SPItem;
+struct ConnectionPoint;
+typedef std::map<int, ConnectionPoint> IdConnectionPointMap;
 namespace Avoid { class ShapeRef; }
 
 class SPAvoidRef {
@@ -28,9 +30,16 @@ public:
     // libavoid's internal representation of the item.
     Avoid::ShapeRef *shapeRef;
 
+    // Used for holding connection points for item
+    IdConnectionPointMap connection_points;
+
     void setAvoid(char const *value);
+    void setConnectionPoints(gchar const *value);
+    void addConnectionPoint(ConnectionPoint &cp);
+    void updateConnectionPoint(ConnectionPoint &cp);
+    void deleteConnectionPoint(ConnectionPoint &cp);
     void handleSettingChange(void);
-    
+
     // Returns a list of SPItems of all connectors/shapes attached to
     // this object.  Pass one of the following for 'type':
     //     Avoid::runningTo
@@ -38,6 +47,9 @@ public:
     //     Avoid::runningToAndFrom
     GSList *getAttachedShapes(const unsigned int type);
     GSList *getAttachedConnectors(const unsigned int type);
+    Geom::Point getConnectionPointPos(const int type, const int id);
+
+    bool isValidConnPointId( const int type, const int id );
 
 private:
     SPItem *item;
@@ -48,6 +60,7 @@ private:
 
     // A sigc connection for transformed signal.
     sigc::connection _transformed_connection;
+    void setConnectionPointsAttrUndoable(const gchar* value, const gchar* action);
 };
 
 extern GSList *get_avoided_items(GSList *list, SPObject *from,
@@ -56,6 +69,7 @@ extern void avoid_item_move(Geom::Matrix const *mp, SPItem *moved_item);
 extern void init_avoided_shape_geometry(SPDesktop *desktop);
 
 static const double defaultConnSpacing = 3.0;
+static const double defaultConnCurvature = 3.0;
 
 #endif /* !SEEN_CONN_AVOID_REF */
 
diff --git a/src/connector-context.cpp b/src/connector-context.cpp
index 2131bdced..228c81d29 100644
--- a/src/connector-context.cpp
+++ b/src/connector-context.cpp
@@ -4,31 +4,25 @@
  * Authors:
  *   Michael Wybrow <mjwybrow@users.sourceforge.net>
  *
- * Copyright (C) 2005 Michael Wybrow
+ * Copyright (C) 2005-2008  Michael Wybrow
+ * Copyright (C) 2009  Monash University
  *
  * Released under GNU GPL, read the file 'COPYING' for more information
  *
  * TODO:
- *  o  Have shapes avoid convex hulls of objects, rather than their
- *     bounding box.  Possibly implement the unfinished ConvexHull
- *     class in libnr.
- *     (HOWEVER, using the convex hull C of a shape S does the wrong thing if a
- *     connector starts outside of S but inside C, or if the best route around
- *     an object involves going inside C but without entering S.)
- *  o  Draw connectors to shape edges rather than bounding box.
  *  o  Show a visual indicator for objects with the 'avoid' property set.
  *  o  Allow user to change a object between a path and connector through
  *     the interface.
  *  o  Create an interface for setting markers (arrow heads).
  *  o  Better distinguish between paths and connectors to prevent problems
- *     in the node tool and paths accidently being turned into connectors
+ *     in the node tool and paths accidentally being turned into connectors
  *     in the connector tool.  Perhaps have a way to convert between.
  *  o  Only call libavoid's updateEndPoint as required.  Currently we do it
  *     for both endpoints, even if only one is moving.
  *  o  Allow user-placeable connection points.
  *  o  Deal sanely with connectors with both endpoints attached to the
  *     same connection point, and drawing of connectors attaching
- *     overlaping shapes (currently tries to adjust connector to be
+ *     overlapping shapes (currently tries to adjust connector to be
  *     outside both bounding boxes).
  *  o  Fix many special cases related to connectors updating,
  *     e.g., copying a couple of shapes and a connector that are
@@ -37,16 +31,133 @@
  *           one of the other contexts.
  *  o  Cope with shapes whose ids change when they have attached
  *     connectors.
- *  o  gobble_motion_events(GDK_BUTTON1_MASK)?;
+ *  o  During dragging motion, gobble up to and use the final motion event.
+ *     Gobbling away all duplicates after the current can occasionally result
+ *     in the path lagging behind the mouse cursor if it is no longer being
+ *     dragged.
+ *  o  Fix up libavoid's representation after undo actions.  It doesn't see 
+ *     any transform signals and hence doesn't know shapes have moved back to
+ *     there earlier positions.
+ *  o  Decide whether drawing/editing mode should be an Inkscape preference
+ *     or the connector tool should always start in drawing mode.
+ *  o  Correct the problem with switching to the select tool when pressing
+ *     space bar (there are moments when it refuses to do so).
+ *
+ * ----------------------------------------------------------------------------
+ *
+ * mjwybrow's observations on acracan's Summer of Code connector work:
+ * 
+ *  -  GUI comments:
+ * 
+ *      -  Buttons for adding and removing user-specified connection
+ * 	points should probably have "+" and "-" symbols on them so they
+ * 	are consistent with the similar buttons for the node tool.
+ *      -  Controls on the connector tool be should be reordered logically, 
+ * 	possibly as follows:
+ * 
+ * 	*Connector*: [Polyline-radio-button] [Orthgonal-radio-button] 
+ * 	  [Curvature-control] | *Shape*: [Avoid-button] [Dont-avoid-button]
+ * 	  [Spacing-control] | *Connection pts*: [Edit-mode] [Add-pt] [Rm-pt]
+ * 
+ * 	I think that the network layout controls be moved to the 
+ * 	Align and Distribute dialog (there is already the layout button 
+ * 	there, but no options are exposed).
+ * 
+ * 	I think that the style change between polyline and orthogonal
+ * 	would be much clearer with two buttons (radio behaviour -- just
+ * 	one is true).
+ * 
+ * 	The other tools show a label change from "New:" to "Change:" 
+ * 	depending on whether an object is selected.  We could consider
+ * 	this but there may not be space.
+ * 
+ * 	The Add-pt and Rm-pt buttons should be greyed out (inactive) if
+ * 	we are not in connection point editing mode.  And probably also
+ * 	if there is no shape selected, i.e. at the times they have no
+ * 	effect when clicked.
+ * 
+ * 	Likewise for the avoid/ignore shapes buttons.  These should be 
+ * 	inactive when a shape is not selected in the connector context.
+ * 
+ *  -  When creating/editing connection points:
+ * 
+ *      -  Strange things can happen if you have connectors selected, or
+ * 	try rerouting connectors by dragging their endpoints when in 
+ * 	connection point editing mode.
+ * 
+ *      -  Possibly the selected shape's connection points should always
+ * 	be shown (i.e., have knots) when in editing mode.
+ * 
+ *      -  It is a little strange to be able to place connection points
+ * 	competely outside shapes.  Especially when you later can't draw
+ * 	connectors to them since the knots are only visible when you
+ * 	are over the shape.  I think that you should only be able to
+ * 	place connection points inside or on the boundary of the shape 
+ * 	itself.
+ *  
+ *      -  The intended ability to place a new point at the current cursor 
+ * 	position by pressing RETURN does not seem to work.
+ * 
+ *      -  The Status bar tooltip should change to reflect editing mode
+ * 	and tell the user about RETURN and how to use the tool.
+ * 
+ *  -  Connection points general:
+ * 
+ *      -  Connection points that were inside the shape can end up outside 
+ * 	after a rotation is applied to the shape in the select tool.
+ * 	It doesn't seem like the correct transform is being applied to
+ * 	these, or it is being applied at the wrong time.  I'd expect
+ * 	connection points to rotate with the shape, and stay at the
+ * 	same position "on the shape"
+ * 
+ *      -  I was able to make the connectors attached to a shape fall off
+ * 	the shape after scaling it.  Not sure the exact cause, but may
+ * 	require more investigation/debugging.
+ * 
+ *      -  The user-defined connection points should be either absolute
+ * 	(as the current ones are) or defined as a percentage of the
+ * 	shape.  These would be based on a toggle setting on the
+ * 	toolbar, and they would be placed in exactly the same way by
+ * 	the user.  The only difference would be that they would be
+ * 	store as percentage positions in the SVG connection-points
+ * 	property and that they would update/move automatically if the 
+ * 	object was resized or scaled.
+ * 
+ *      -  Thinking more, I think you always want to store and think about
+ * 	the positions of connection points to be pre-transform, but
+ * 	obviously the shape transform is applied to them.  That way,
+ * 	they will rotate and scale automatically with the shape, when
+ * 	the shape transform is altered.  The Percentage version would
+ * 	compute their position from the pre-transform dimensions and
+ * 	then have the transform applied to them, for example.
+ * 
+ *      -  The connection points in the test_connection_points.svg file
+ * 	seem to follow the shape when it is moved, but connection
+ * 	points I add to new shapes, do not follow the shape, either
+ * 	when the shape is just moved or transformed.  There is
+ * 	something wrong here.  What exactly should the behaviour be 
+ * 	currently?
+ * 
+ *      -  I see that connection points are specified at absolute canvas
+ * 	positions.  I really think that they should be specified in
+ * 	shape coordinated relative to the shapes.  There may be
+ * 	transforms applied to layers and the canvas which would make
+ * 	specifying them quite difficult.  I'd expect a position of 0, 0
+ * 	to be on the shape in question or very close to it, for example.
  *
  */
 
+
+
 #include <gdk/gdkkeysyms.h>
 #include <string>
 #include <cstring>
 
 #include "connector-context.h"
 #include "pixmaps/cursor-connector.xpm"
+#include "pixmaps/cursor-node.xpm"
+#include "pixmaps/cursor-node-m.xpm"
+#include "pixmaps/cursor-node-d.xpm"
 #include "xml/node-event-vector.h"
 #include "xml/repr.h"
 #include "svg/svg.h"
@@ -63,11 +174,14 @@
 #include "display/canvas-bpath.h"
 #include "display/sodipodi-ctrl.h"
 #include <glibmm/i18n.h>
+#include <glibmm/stringutils.h>
 #include "snap.h"
 #include "knot.h"
 #include "sp-conn-end.h"
+#include "sp-conn-end-pair.h"
 #include "conn-avoid-ref.h"
 #include "libavoid/vertices.h"
+#include "libavoid/router.h"
 #include "context-fns.h"
 #include "sp-namedview.h"
 #include "sp-text.h"
@@ -79,6 +193,7 @@ static void sp_connector_context_init(SPConnectorContext *conn_context);
 static void sp_connector_context_dispose(GObject *object);
 
 static void sp_connector_context_setup(SPEventContext *ec);
+static void sp_connector_context_set(SPEventContext *ec, Inkscape::Preferences::Entry *val);
 static void sp_connector_context_finish(SPEventContext *ec);
 static gint sp_connector_context_root_handler(SPEventContext *ec, GdkEvent *event);
 static gint sp_connector_context_item_handler(SPEventContext *event_context, SPItem *item, GdkEvent *event);
@@ -98,11 +213,14 @@ static gint connector_handle_motion_notify(SPConnectorContext *const cc, GdkEven
 static gint connector_handle_button_release(SPConnectorContext *const cc, GdkEventButton const &revent);
 static gint connector_handle_key_press(SPConnectorContext *const cc, guint const keyval);
 
+static void cc_active_shape_add_knot(SPDesktop* desktop, SPItem* item, ConnectionPointMap &cphandles, ConnectionPoint& cp);
 static void cc_set_active_shape(SPConnectorContext *cc, SPItem *item);
 static void cc_clear_active_shape(SPConnectorContext *cc);
 static void cc_set_active_conn(SPConnectorContext *cc, SPItem *item);
 static void cc_clear_active_conn(SPConnectorContext *cc);
 static gchar *conn_pt_handle_test(SPConnectorContext *cc, Geom::Point& w);
+static void cc_select_handle(SPKnot* knot);
+static void cc_deselect_handle(SPKnot* knot);
 static bool cc_item_is_shape(SPItem *item);
 static void cc_selection_changed(Inkscape::Selection *selection, gpointer data);
 static void cc_connector_rerouting_finish(SPConnectorContext *const cc,
@@ -115,8 +233,8 @@ static void shape_event_attr_changed(Inkscape::XML::Node *repr, gchar const *nam
         gpointer data);
 
 
-static Geom::Point connector_drag_origin_w(0, 0);
-static bool connector_within_tolerance = false;
+/*static Geom::Point connector_drag_origin_w(0, 0);
+static bool connector_within_tolerance = false;*/
 static SPEventContextClass *parent_class;
 
 
@@ -171,6 +289,7 @@ sp_connector_context_class_init(SPConnectorContextClass *klass)
     object_class->dispose = sp_connector_context_dispose;
 
     event_context_class->setup = sp_connector_context_setup;
+    event_context_class->set = sp_connector_context_set;
     event_context_class->finish = sp_connector_context_finish;
     event_context_class->root_handler = sp_connector_context_root_handler;
     event_context_class->item_handler = sp_connector_context_item_handler;
@@ -188,10 +307,14 @@ sp_connector_context_init(SPConnectorContext *cc)
     ec->xp = 0;
     ec->yp = 0;
 
+    cc->mode = SP_CONNECTOR_CONTEXT_DRAWING_MODE;
+    cc->knot_tip = 0;
+
     cc->red_color = 0xff00007f;
 
     cc->newconn = NULL;
     cc->newConnRef = NULL;
+    cc->curvature = 0.0;
 
     cc->sel_changed_connection = sigc::connection();
 
@@ -204,10 +327,13 @@ sp_connector_context_init(SPConnectorContext *cc)
 
     cc->active_handle = NULL;
 
+    cc->selected_handle = NULL;
+
     cc->clickeditem = NULL;
     cc->clickedhandle = NULL;
 
-    cc->connpthandle = NULL;
+    new (&cc->connpthandles) ConnectionPointMap();
+    
     for (int i = 0; i < 2; ++i) {
         cc->endpt_handle[i] = NULL;
         cc->endpt_handler_id[i] = 0;
@@ -226,10 +352,14 @@ sp_connector_context_dispose(GObject *object)
 
     cc->sel_changed_connection.disconnect();
 
-    if (cc->connpthandle) {
-        g_object_unref(cc->connpthandle);
-        cc->connpthandle = NULL;
+    if (!cc->connpthandles.empty()) {
+        for (ConnectionPointMap::iterator it = cc->connpthandles.begin(); 
+                it != cc->connpthandles.end(); ++it) {
+            g_object_unref(it->first);
+        }
+        cc->connpthandles.clear();
     }
+    cc->connpthandles.~ConnectionPointMap();
     for (int i = 0; i < 2; ++i) {
         if (cc->endpt_handle[1]) {
             g_object_unref(cc->endpt_handle[i]);
@@ -282,6 +412,12 @@ sp_connector_context_setup(SPEventContext *ec)
     // Notice the initial selection.
     cc_selection_changed(cc->selection, (gpointer) cc);
 
+    cc->within_tolerance = false;
+    
+    sp_event_context_read(ec, "curvature");
+    sp_event_context_read(ec, "orthogonal");
+    sp_event_context_read(ec, "mode");
+    cc->knot_tip = cc->mode == SP_CONNECTOR_CONTEXT_DRAWING_MODE ? cc_knot_tips[0] : cc_knot_tips[1];
     Inkscape::Preferences *prefs = Inkscape::Preferences::get();
     if (prefs->getBool("/tools/connector/selcue", 0)) {
         ec->enableSelectionCue();
@@ -294,6 +430,64 @@ sp_connector_context_setup(SPEventContext *ec)
 
 
 static void
+sp_connector_context_set(SPEventContext *ec, Inkscape::Preferences::Entry *val)
+{
+    SPConnectorContext *cc = SP_CONNECTOR_CONTEXT(ec);
+
+    /* fixme: Proper error handling for non-numeric data.  Use a locale-independent function like
+     * g_ascii_strtod (or a thin wrapper that does the right thing for invalid values inf/nan). */
+    Glib::ustring name = val->getEntryName();
+    if ( name == "curvature" ) {
+        cc->curvature = val->getDoubleLimited(); // prevents NaN and +/-Inf from messing up
+    }
+    else if ( name == "orthogonal" ) {
+        cc->isOrthogonal = val->getBool();
+    }
+    else if ( name == "mode")
+    {
+        sp_connector_context_switch_mode(ec, val->getBool() ? SP_CONNECTOR_CONTEXT_EDITING_MODE : SP_CONNECTOR_CONTEXT_DRAWING_MODE);
+    }
+}
+
+void sp_connector_context_switch_mode(SPEventContext* ec, unsigned int newMode)
+{
+    SPConnectorContext *cc = SP_CONNECTOR_CONTEXT(ec);
+
+    cc->mode = newMode;
+    if ( cc->mode == SP_CONNECTOR_CONTEXT_DRAWING_MODE )
+    {
+        ec->cursor_shape = cursor_connector_xpm;
+        cc->knot_tip = cc_knot_tips[0];
+        if (cc->selected_handle)
+            cc_deselect_handle( cc->selected_handle );
+        cc->selected_handle = NULL;
+        // Show all default connection points
+
+    }
+    else if ( cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE )
+    {
+        ec->cursor_shape = cursor_node_xpm;
+        cc->knot_tip = cc_knot_tips[1];
+/*            if (cc->active_shape)
+        {
+            cc->selection->set( SP_OBJECT( cc->active_shape ) );
+        }
+        else 
+        {
+            SPItem* item = cc->selection->singleItem();
+            if ( item )
+            {
+                cc_set_active_shape(cc, item);
+                cc->selection->set( SP_OBJECT( item ) );
+            }
+        }*/
+    }
+    sp_event_context_update_cursor(ec);
+
+}
+
+
+static void
 sp_connector_context_finish(SPEventContext *ec)
 {
     SPConnectorContext *cc = SP_CONNECTOR_CONTEXT(ec);
@@ -341,9 +535,12 @@ cc_clear_active_shape(SPConnectorContext *cc)
         cc->active_shape_layer_repr = NULL;
     }
 
-    // Hide the center connection point if it exists.
-    if (cc->connpthandle) {
-        sp_knot_hide(cc->connpthandle);
+    // Hide the connection points if they exist.
+    if (cc->connpthandles.size()) {
+        for (ConnectionPointMap::iterator it = cc->connpthandles.begin(); 
+                it != cc->connpthandles.end(); ++it) {
+            sp_knot_hide(it->first);
+        }
     }
 }
 
@@ -379,16 +576,35 @@ conn_pt_handle_test(SPConnectorContext *cc, Geom::Point& p)
     // TODO: this will need to change when there are more connection
     //       points available for each shape.
 
-    SPKnot *centerpt = cc->connpthandle;
-    if (cc->active_handle && (cc->active_handle == centerpt))
+    if (cc->active_handle && (cc->connpthandles.find(cc->active_handle) != cc->connpthandles.end()))
     {
-        p = centerpt->pos;
-        return g_strdup_printf("#%s", SP_OBJECT_ID(cc->active_shape));
+        p = cc->active_handle->pos;
+        const ConnectionPoint& cp = cc->connpthandles[cc->active_handle];
+        return g_strdup_printf("#%s_%c_%d", SP_OBJECT_ID(cc->active_shape),
+                               cp.type == ConnPointDefault ? 'd' : 'u' , cp.id);
     }
     return NULL;
 }
 
+static void
+cc_select_handle(SPKnot* knot)
+{
+    knot->setShape(SP_KNOT_SHAPE_SQUARE);
+    knot->setSize(10);
+    knot->setAnchor(GTK_ANCHOR_CENTER);
+    knot->setFill(0x0000ffff, 0x0000ffff, 0x0000ffff);
+    sp_knot_update_ctrl(knot);
+}
 
+static void
+cc_deselect_handle(SPKnot* knot)
+{
+    knot->setShape(SP_KNOT_SHAPE_SQUARE);
+    knot->setSize(8);
+    knot->setAnchor(GTK_ANCHOR_CENTER);
+    knot->setFill(0xffffff00, 0xff0000ff, 0xff0000ff);
+    sp_knot_update_ctrl(knot);
+}
 
 static gint
 sp_connector_context_item_handler(SPEventContext *event_context, SPItem *item, GdkEvent *event)
@@ -400,29 +616,40 @@ sp_connector_context_item_handler(SPEventContext *event_context, SPItem *item, G
     SPConnectorContext *cc = SP_CONNECTOR_CONTEXT(event_context);
 
     Geom::Point p(event->button.x, event->button.y);
-
+    
     switch (event->type) {
         case GDK_BUTTON_RELEASE:
             if (event->button.button == 1 && !event_context->space_panning) {
                 if ((cc->state == SP_CONNECTOR_CONTEXT_DRAGGING) &&
-                        (connector_within_tolerance))
+                        (event_context->within_tolerance))
                 {
                     spcc_reset_colors(cc);
                     cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-                    sp_event_context_discard_delayed_snap_event(event_context);
+//                    sp_event_context_snap_window_closed(event_context);
                 }
                 if (cc->state != SP_CONNECTOR_CONTEXT_IDLE) {
-                    // Doing simething else like rerouting.
+                    // Doing something else like rerouting.
                     break;
                 }
-                // find out clicked item, disregarding groups, honoring Alt
-                SPItem *item_ungrouped = sp_event_context_find_item(desktop,
-                        p, event->button.state & GDK_MOD1_MASK, TRUE);
+                // find out clicked item, honoring Alt
+                SPItem *item = sp_event_context_find_item(desktop,
+                        p, event->button.state & GDK_MOD1_MASK, FALSE);
 
                 if (event->button.state & GDK_SHIFT_MASK) {
-                    cc->selection->toggle(item_ungrouped);
+                    cc->selection->toggle(item);
                 } else {
-                    cc->selection->set(item_ungrouped);
+                    cc->selection->set(item);
+                    if ( cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE && cc->selected_handle )
+                    {
+                        cc_deselect_handle( cc->selected_handle );
+                        cc->selected_handle = NULL;
+                    }
+                    /* When selecting a new item,
+                       do not allow showing connection points
+                       on connectors. (yet?)
+                    */
+                    if ( item != cc->active_shape && !cc_item_is_connector( item ) )
+                        cc_set_active_shape( cc, item );
                 }
                 ret = TRUE;
 
@@ -430,15 +657,24 @@ sp_connector_context_item_handler(SPEventContext *event_context, SPItem *item, G
             break;
         case GDK_ENTER_NOTIFY:
         {
-            if (cc_item_is_shape(item)) {
-                // This is a shape, so show connection point(s).
-                if (!(cc->active_shape) ||
-                        // Don't show handle for another handle.
-                        (item != ((SPItem *) cc->connpthandle))) {
+            if (cc->mode == SP_CONNECTOR_CONTEXT_DRAWING_MODE || (cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE && !cc->selected_handle))
+            {
+                if (cc_item_is_shape(item)) {
+                    
+                    // I don't really understand what the above does,
+                    // so I commented it.
+                    // This is a shape, so show connection point(s).
+    /*                if (!(cc->active_shape) 
+                            // Don't show handle for another handle.
+    //                         || (cc->connpthandles.find((SPKnot*) item) != cc->connpthandles.end())
+                        )
+                    {
+                        cc_set_active_shape(cc, item);
+                    }*/
                     cc_set_active_shape(cc, item);
                 }
+                ret = TRUE;
             }
-            ret = TRUE;
             break;
         }
         default:
@@ -462,7 +698,7 @@ sp_connector_context_root_handler(SPEventContext *ec, GdkEvent *event)
             break;
 
         case GDK_MOTION_NOTIFY:
-        	ret = connector_handle_motion_notify(cc, event->motion);
+            ret = connector_handle_motion_notify(cc, event->motion);
             break;
 
         case GDK_BUTTON_RELEASE:
@@ -497,96 +733,166 @@ connector_handle_button_press(SPConnectorContext *const cc, GdkEventButton const
     SPEventContext *event_context = SP_EVENT_CONTEXT(cc);
 
     gint ret = FALSE;
-    if ( bevent.button == 1 && !event_context->space_panning ) {
+    if ( cc->mode == SP_CONNECTOR_CONTEXT_DRAWING_MODE )
+    {
+        if ( bevent.button == 1 && !event_context->space_panning ) {
 
-        SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
+            SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
 
-        if (Inkscape::have_viable_layer(desktop, cc->_message_context) == false) {
-            return TRUE;
-        }
+            if (Inkscape::have_viable_layer(desktop, cc->_message_context) == false) {
+                return TRUE;
+            }
 
-        Geom::Point const event_w(bevent.x,
-                                bevent.y);
-        connector_drag_origin_w = event_w;
-        connector_within_tolerance = true;
+            Geom::Point const event_w(bevent.x,
+                                    bevent.y);
+//             connector_drag_origin_w = event_w;
+            cc->xp = bevent.x;
+            cc->yp = bevent.y;
+            cc->within_tolerance = true;
 
-        Geom::Point const event_dt = cc->desktop->w2d(event_w);
+            Geom::Point const event_dt = cc->desktop->w2d(event_w);
 
-        SnapManager &m = cc->desktop->namedview->snap_manager;
-		m.setup(cc->desktop);
+            SnapManager &m = cc->desktop->namedview->snap_manager;
+            m.setup(cc->desktop);
 
-        switch (cc->state) {
-            case SP_CONNECTOR_CONTEXT_STOP:
-                /* This is allowed, if we just cancelled curve */
-            case SP_CONNECTOR_CONTEXT_IDLE:
-            {
-                if ( cc->npoints == 0 ) {
-                    cc_clear_active_conn(cc);
+            switch (cc->state) {
+                case SP_CONNECTOR_CONTEXT_STOP:
+                    /* This is allowed, if we just cancelled curve */
+                case SP_CONNECTOR_CONTEXT_IDLE:
+                {
+                    if ( cc->npoints == 0 ) {
+                        cc_clear_active_conn(cc);
 
-                    SP_EVENT_CONTEXT_DESKTOP(cc)->messageStack()->flash(Inkscape::NORMAL_MESSAGE, _("Creating new connector"));
+                        SP_EVENT_CONTEXT_DESKTOP(cc)->messageStack()->flash(Inkscape::NORMAL_MESSAGE, _("Creating new connector"));
 
-                    /* Set start anchor */
-                    /* Create green anchor */
-                    Geom::Point p = event_dt;
+                        /* Set start anchor */
+                        /* Create green anchor */
+                        Geom::Point p = event_dt;
 
-                    // Test whether we clicked on a connection point
-                    cc->sid = conn_pt_handle_test(cc, p);
+                        // Test whether we clicked on a connection point
+                        cc->sid = conn_pt_handle_test(cc, p);
+
+//                        sp_event_context_snap_window_open(event_context);
+
+                        if (!cc->sid) {
+                            // This is the first point, so just snap it to the grid
+                            // as there's no other points to go off.
+                            m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+                        }
+                        spcc_connector_set_initial_point(cc, p);
 
-                    if (!cc->sid) {
-                        // This is the first point, so just snap it to the grid
-                        // as there's no other points to go off.
-                        m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
                     }
-                    spcc_connector_set_initial_point(cc, p);
+                    cc->state = SP_CONNECTOR_CONTEXT_DRAGGING;
+                    ret = TRUE;
+                    break;
+                }
+                case SP_CONNECTOR_CONTEXT_DRAGGING:
+                {
+                    // This is the second click of a connector creation.
+                    m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
 
+                    spcc_connector_set_subsequent_point(cc, p);
+                    spcc_connector_finish_segment(cc, p);
+                    // Test whether we clicked on a connection point
+                    cc->eid = conn_pt_handle_test(cc, p);
+                    if (cc->npoints != 0) {
+                        spcc_connector_finish(cc);
+                    }
+                    cc_set_active_conn(cc, cc->newconn);
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+//                    sp_event_context_snap_window_closed(event_context);
+                    ret = TRUE;
+                    break;
                 }
-                cc->state = SP_CONNECTOR_CONTEXT_DRAGGING;
-                ret = TRUE;
-                break;
+                case SP_CONNECTOR_CONTEXT_CLOSE:
+                {
+                    g_warning("Button down in CLOSE state");
+                    break;
+                }
+                default:
+                    break;
             }
-            case SP_CONNECTOR_CONTEXT_DRAGGING:
-            {
-                // This is the second click of a connector creation.
-            	m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+        } else if (bevent.button == 3) {
+            if (cc->state == SP_CONNECTOR_CONTEXT_REROUTING) {
+                // A context menu is going to be triggered here,
+                // so end the rerouting operation.
+                cc_connector_rerouting_finish(cc, &p);
 
-                spcc_connector_set_subsequent_point(cc, p);
-                spcc_connector_finish_segment(cc, p);
-                // Test whether we clicked on a connection point
-                cc->eid = conn_pt_handle_test(cc, p);
-                if (cc->npoints != 0) {
-                    spcc_connector_finish(cc);
-                }
-                cc_set_active_conn(cc, cc->newconn);
                 cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-                sp_event_context_discard_delayed_snap_event(event_context);
-                ret = TRUE;
-                break;
+//                sp_event_context_snap_window_closed(event_context);
+
+                // Don't set ret to TRUE, so we drop through to the
+                // parent handler which will open the context menu.
             }
-            case SP_CONNECTOR_CONTEXT_CLOSE:
-            {
-                g_warning("Button down in CLOSE state");
-                break;
+            else if (cc->npoints != 0) {
+                spcc_connector_finish(cc);
+                cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+//                sp_event_context_snap_window_closed(event_context);
+                ret = TRUE;
             }
-            default:
-                break;
         }
-    } else if (bevent.button == 3) {
-        if (cc->state == SP_CONNECTOR_CONTEXT_REROUTING) {
-            // A context menu is going to be triggered here,
-            // so end the rerouting operation.
-            cc_connector_rerouting_finish(cc, &p);
+    }
+    else if ( cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE )
+    {
+        if ( bevent.button == 1 && !event_context->space_panning )
+        {
+            // Initialize variables in case of dragging
 
-            cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-            sp_event_context_discard_delayed_snap_event(event_context);
+            SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
 
-            // Don't set ret to TRUE, so we drop through to the
-            // parent handler which will open the context menu.
-        }
-        else if (cc->npoints != 0) {
-            spcc_connector_finish(cc);
-            cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-            sp_event_context_discard_delayed_snap_event(event_context);
-            ret = TRUE;
+            if (Inkscape::have_viable_layer(desktop, cc->_message_context) == false) {
+                return TRUE;
+            }
+
+            cc->xp = bevent.x;
+            cc->yp = bevent.y;
+            cc->within_tolerance = true;
+            
+            ConnectionPointMap::iterator const& active_knot_it = cc->connpthandles.find( cc->active_handle );
+
+            switch (cc->state)
+            {
+                case SP_CONNECTOR_CONTEXT_IDLE:
+                    if ( active_knot_it != cc->connpthandles.end() )
+                    {
+                        // We do not allow selecting and, thereby, moving default knots
+                        if ( active_knot_it->second.type != ConnPointDefault)
+                        {
+                            if (cc->selected_handle != cc->active_handle)
+                            {
+                                if ( cc->selected_handle )
+                                    cc_deselect_handle( cc->selected_handle );
+                                cc->selected_handle = cc->active_handle;
+                                cc_select_handle( cc->selected_handle );
+                            }
+                        }
+                        else
+                            // Just ignore the default connection point
+                            return FALSE;
+                    }
+                    else
+                        if ( cc->selected_handle )
+                        {
+                            cc_deselect_handle( cc->selected_handle );
+                            cc->selected_handle = NULL;
+                        }
+
+                    if ( cc->selected_handle )
+                    {
+//                         sp_event_context_snap_window_open(event_context);
+                        cc->state = SP_CONNECTOR_CONTEXT_DRAGGING;
+                        cc->selection->set( SP_OBJECT( cc->active_shape ) );
+                    }
+
+                    ret = TRUE;
+                    break;
+                // Dragging valid because of the way we create
+                // new connection points.
+                case SP_CONNECTOR_CONTEXT_DRAGGING:
+                    // Do nothing.
+                    ret = TRUE;
+                    break;
+            }
         }
     }
     return ret;
@@ -607,71 +913,94 @@ connector_handle_motion_notify(SPConnectorContext *const cc, GdkEventMotion cons
 
     Geom::Point const event_w(mevent.x, mevent.y);
 
-    if (connector_within_tolerance) {
-        gint const tolerance = prefs->getIntLimited("/options/dragtolerance/value", 0, 0, 100);
-        if ( Geom::LInfty( event_w - connector_drag_origin_w ) < tolerance ) {
+    if (cc->within_tolerance) {
+        cc->tolerance = prefs->getIntLimited("/options/dragtolerance/value", 0, 0, 100);
+        if ( ( abs( (gint) mevent.x - cc->xp ) < cc->tolerance ) &&
+             ( abs( (gint) mevent.y - cc->yp ) < cc->tolerance ) ) {
             return FALSE;   // Do not drag if we're within tolerance from origin.
         }
     }
     // Once the user has moved farther than tolerance from the original location
     // (indicating they intend to move the object, not click), then always process
     // the motion notify coordinates as given (no snapping back to origin)
-    connector_within_tolerance = false;
+    cc->within_tolerance = false;
 
     SPDesktop *const dt = cc->desktop;
 
     /* Find desktop coordinates */
     Geom::Point p = dt->w2d(event_w);
 
-    SnapManager &m = dt->namedview->snap_manager;
-	m.setup(dt);
-
-    switch (cc->state) {
-        case SP_CONNECTOR_CONTEXT_DRAGGING:
-        {
-            // This is movement during a connector creation.
-        	if ( cc->npoints > 0 ) {
-        		m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
-        		cc->selection->clear();
-                spcc_connector_set_subsequent_point(cc, p);
-                ret = TRUE;
+    if ( cc->mode == SP_CONNECTOR_CONTEXT_DRAWING_MODE )
+    {
+        SnapManager &m = dt->namedview->snap_manager;
+        m.setup(dt);
+        
+        switch (cc->state) {
+            case SP_CONNECTOR_CONTEXT_DRAGGING:
+            {
+                gobble_motion_events(mevent.state);
+                // This is movement during a connector creation.
+                if ( cc->npoints > 0 ) {
+                    m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p,
+                            Inkscape::SNAPSOURCE_HANDLE);
+                    cc->selection->clear();
+                    spcc_connector_set_subsequent_point(cc, p);
+                    ret = TRUE;
+                }
+                break;
             }
-            break;
-        }
-        case SP_CONNECTOR_CONTEXT_REROUTING:
-        {
-            g_assert( SP_IS_PATH(cc->clickeditem));
+            case SP_CONNECTOR_CONTEXT_REROUTING:
+            {
+                gobble_motion_events(GDK_BUTTON1_MASK);
+                g_assert( SP_IS_PATH(cc->clickeditem));
 
-            m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+                m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p,
+                        Inkscape::SNAPSOURCE_HANDLE);
 
-            // Update the hidden path
-            Geom::Matrix i2d = sp_item_i2d_affine(cc->clickeditem);
-            Geom::Matrix d2i = i2d.inverse();
-            SPPath *path = SP_PATH(cc->clickeditem);
-            SPCurve *curve = (SP_SHAPE(path))->curve;
-            if (cc->clickedhandle == cc->endpt_handle[0]) {
-                Geom::Point o = cc->endpt_handle[1]->pos;
-                curve->stretch_endpoints(p * d2i, o * d2i);
-            }
-            else {
-                Geom::Point o = cc->endpt_handle[0]->pos;
-                curve->stretch_endpoints(o * d2i, p * d2i);
-            }
-            sp_conn_adjust_path(path);
+                // Update the hidden path
+                Geom::Matrix i2d = sp_item_i2d_affine(cc->clickeditem);
+                Geom::Matrix d2i = i2d.inverse();
+                SPPath *path = SP_PATH(cc->clickeditem);
+                SPCurve *curve = path->original_curve ? path->original_curve : path->curve;
+                if (cc->clickedhandle == cc->endpt_handle[0]) {
+                    Geom::Point o = cc->endpt_handle[1]->pos;
+                    curve->stretch_endpoints(p * d2i, o * d2i);
+                }
+                else {
+                    Geom::Point o = cc->endpt_handle[0]->pos;
+                    curve->stretch_endpoints(o * d2i, p * d2i);
+                }
+                sp_conn_reroute_path_immediate(path);
 
-            // Copy this to the temporary visible path
-            cc->red_curve = SP_SHAPE(path)->curve->copy();
-            cc->red_curve->transform(i2d);
+                // Copy this to the temporary visible path
+                cc->red_curve = path->original_curve ?
+                        path->original_curve->copy() : path->curve->copy();
+                cc->red_curve->transform(i2d);
 
-            sp_canvas_bpath_set_bpath(SP_CANVAS_BPATH(cc->red_bpath), cc->red_curve);
-            ret = TRUE;
-            break;
+                sp_canvas_bpath_set_bpath(SP_CANVAS_BPATH(cc->red_bpath), cc->red_curve);
+                ret = TRUE;
+                break;
+            }
+            case SP_CONNECTOR_CONTEXT_STOP:
+                /* This is perfectly valid */
+                break;
+            default:
+                break;
+        }
+    }
+    else if ( cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE )
+    {
+        switch ( cc->state )
+        {
+            case SP_CONNECTOR_CONTEXT_DRAGGING:
+                sp_knot_set_position(cc->selected_handle, p, 0);
+                ret = TRUE;
+                break;
+            case SP_CONNECTOR_CONTEXT_NEWCONNPOINT:
+                sp_knot_set_position(cc->selected_handle, p, 0);
+                ret = TRUE;
+                break;
         }
-        case SP_CONNECTOR_CONTEXT_STOP:
-            /* This is perfectly valid */
-            break;
-        default:
-            break;
     }
 
     return ret;
@@ -685,60 +1014,111 @@ connector_handle_button_release(SPConnectorContext *const cc, GdkEventButton con
     SPEventContext *event_context = SP_EVENT_CONTEXT(cc);
     if ( revent.button == 1 && !event_context->space_panning ) {
 
-    	SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
+        SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
         SPDocument *doc = sp_desktop_document(desktop);
 
         SnapManager &m = desktop->namedview->snap_manager;
-		m.setup(desktop);
+        m.setup(desktop);
 
         Geom::Point const event_w(revent.x, revent.y);
 
         /* Find desktop coordinates */
         Geom::Point p = cc->desktop->w2d(event_w);
-
-        switch (cc->state) {
-            //case SP_CONNECTOR_CONTEXT_POINT:
-            case SP_CONNECTOR_CONTEXT_DRAGGING:
-            {
-            	m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
-
-            	if (connector_within_tolerance)
+        if ( cc->mode == SP_CONNECTOR_CONTEXT_DRAWING_MODE )
+        {
+            switch (cc->state) {
+                //case SP_CONNECTOR_CONTEXT_POINT:
+                case SP_CONNECTOR_CONTEXT_DRAGGING:
                 {
+                    m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+
+                    if (cc->within_tolerance)
+                    {
+                        spcc_connector_finish_segment(cc, p);
+                        return TRUE;
+                    }
+                    // Connector has been created via a drag, end it now.
+                    spcc_connector_set_subsequent_point(cc, p);
                     spcc_connector_finish_segment(cc, p);
-                    return TRUE;
+                    // Test whether we clicked on a connection point
+                    cc->eid = conn_pt_handle_test(cc, p);
+                    if (cc->npoints != 0) {
+                        spcc_connector_finish(cc);
+                    }
+                    cc_set_active_conn(cc, cc->newconn);
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+//                    sp_event_context_snap_window_closed(event_context);
+                    break;
                 }
-                // Connector has been created via a drag, end it now.
-                spcc_connector_set_subsequent_point(cc, p);
-                spcc_connector_finish_segment(cc, p);
-                // Test whether we clicked on a connection point
-                cc->eid = conn_pt_handle_test(cc, p);
-                if (cc->npoints != 0) {
-                    spcc_connector_finish(cc);
+                case SP_CONNECTOR_CONTEXT_REROUTING:
+                {
+                    m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+                    cc_connector_rerouting_finish(cc, &p);
+
+                    sp_document_ensure_up_to_date(doc);
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+//                    sp_event_context_snap_window_closed(event_context);
+                    return TRUE;
+                    break;
                 }
-                cc_set_active_conn(cc, cc->newconn);
-                cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-                sp_event_context_discard_delayed_snap_event(event_context);
-                break;
+                case SP_CONNECTOR_CONTEXT_STOP:
+                    /* This is allowed, if we just cancelled curve */
+                    break;
+                default:
+                    break;
             }
-            case SP_CONNECTOR_CONTEXT_REROUTING:
+            ret = TRUE;
+        }
+        else if ( cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE )
+        {
+            switch ( cc->state )
             {
-            	m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
-            	cc_connector_rerouting_finish(cc, &p);
+                case SP_CONNECTOR_CONTEXT_DRAGGING:
+                    
+                    if (!cc->within_tolerance)
+                    {
+//                        sp_event_context_snap_window_open(event_context);
+                        m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+                        sp_knot_set_position(cc->selected_handle, p, 0);
+//                        sp_event_context_snap_window_closed(event_context);
+                        ConnectionPoint& cp = cc->connpthandles[cc->selected_handle];
+                        cp.pos = p * sp_item_dt2i_affine(cc->active_shape);
+                        cc->active_shape->avoidRef->updateConnectionPoint(cp);
+                    }
 
-                sp_document_ensure_up_to_date(doc);
-                cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-                sp_event_context_discard_delayed_snap_event(event_context);
-                return TRUE;
-                break;
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+                    ret = TRUE;
+                    break;
+
+
+                case SP_CONNECTOR_CONTEXT_NEWCONNPOINT:
+//                    sp_event_context_snap_window_open( event_context );
+                    m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+                    
+                    sp_knot_set_position(cc->selected_handle, p, 0);
+//                    sp_event_context_snap_window_closed(event_context);
+
+                    ConnectionPoint cp;
+                    cp.type = ConnPointUserDefined;
+                    cp.pos = p * sp_item_dt2i_affine(cc->active_shape);
+                    cp.dir = Avoid::ConnDirAll;
+                    g_object_unref(cc->selected_handle);
+                    cc->active_shape->avoidRef->addConnectionPoint(cp);
+                    sp_document_ensure_up_to_date(doc);
+                    for (ConnectionPointMap::iterator it = cc->connpthandles.begin(); it != cc->connpthandles.end(); ++it)
+                        if (it->second.type == ConnPointUserDefined && it->second.id == cp.id)
+                        {
+                            cc->selected_handle = it->first;
+                            break;
+                        }
+                    cc_select_handle( cc->selected_handle );
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+                    ret = TRUE;
+                    break;
             }
-            case SP_CONNECTOR_CONTEXT_STOP:
-                /* This is allowed, if we just cancelled curve */
-                break;
-            default:
-                break;
         }
-        ret = TRUE;
     }
+    
 
     return ret;
 }
@@ -749,43 +1129,145 @@ connector_handle_key_press(SPConnectorContext *const cc, guint const keyval)
 {
     gint ret = FALSE;
     /* fixme: */
-    switch (keyval) {
-        case GDK_Return:
-        case GDK_KP_Enter:
-            if (cc->npoints != 0) {
-                spcc_connector_finish(cc);
-                cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-                sp_event_context_discard_delayed_snap_event(SP_EVENT_CONTEXT(cc));
-                ret = TRUE;
-            }
-            break;
-        case GDK_Escape:
-            if (cc->state == SP_CONNECTOR_CONTEXT_REROUTING) {
+    if ( cc->mode == SP_CONNECTOR_CONTEXT_DRAWING_MODE )
+    {
+        switch (keyval) {
+            case GDK_Return:
+            case GDK_KP_Enter:
+                if (cc->npoints != 0) {
+                    spcc_connector_finish(cc);
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+//                    sp_event_context_snap_window_closed(SP_EVENT_CONTEXT(cc));
+                    ret = TRUE;
+                }
+                break;
+            case GDK_Escape:
+                if (cc->state == SP_CONNECTOR_CONTEXT_REROUTING) {
 
-                SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
-                SPDocument *doc = sp_desktop_document(desktop);
+                    SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
+                    SPDocument *doc = sp_desktop_document(desktop);
 
-                cc_connector_rerouting_finish(cc, NULL);
+                    cc_connector_rerouting_finish(cc, NULL);
 
-                sp_document_undo(doc);
+                    sp_document_undo(doc);
 
-                cc->state = SP_CONNECTOR_CONTEXT_IDLE;
-                sp_event_context_discard_delayed_snap_event(SP_EVENT_CONTEXT(cc));
-                desktop->messageStack()->flash( Inkscape::NORMAL_MESSAGE,
-                        _("Connector endpoint drag cancelled."));
-                ret = TRUE;
-            }
-            else if (cc->npoints != 0) {
-                // if drawing, cancel, otherwise pass it up for deselecting
-                cc->state = SP_CONNECTOR_CONTEXT_STOP;
-                sp_event_context_discard_delayed_snap_event(SP_EVENT_CONTEXT(cc));
-                spcc_reset_colors(cc);
-                ret = TRUE;
-            }
-            break;
-        default:
-            break;
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+//                    sp_event_context_snap_window_closed(SP_EVENT_CONTEXT(cc));
+                    desktop->messageStack()->flash( Inkscape::NORMAL_MESSAGE,
+                            _("Connector endpoint drag cancelled."));
+                    ret = TRUE;
+                }
+                else if (cc->npoints != 0) {
+                    // if drawing, cancel, otherwise pass it up for deselecting
+                    cc->state = SP_CONNECTOR_CONTEXT_STOP;
+//                    sp_event_context_snap_window_closed(SP_EVENT_CONTEXT(cc));
+                    spcc_reset_colors(cc);
+                    ret = TRUE;
+                }
+                break;
+            default:
+                break;
+        }
+    }
+    else if ( cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE )
+    {
+        switch ( cc->state )
+        {
+            case SP_CONNECTOR_CONTEXT_DRAGGING:
+                if ( keyval == GDK_Escape )
+                {
+                    // Cancel connection point dragging
+
+                    // Obtain original position
+                    ConnectionPoint const& cp = cc->connpthandles[cc->selected_handle];
+                    SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
+                    const Geom::Matrix& i2doc = sp_item_i2doc_affine(cc->active_shape);
+                    sp_knot_set_position(cc->selected_handle, cp.pos * i2doc * desktop->doc2dt(), 0);
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+                    desktop->messageStack()->flash( Inkscape::NORMAL_MESSAGE,
+                        _("Connection point drag cancelled."));
+                    ret = TRUE;
+                }
+                else if ( keyval == GDK_Return || keyval == GDK_KP_Enter )
+                {
+                    // Put connection point at current position
+
+                    SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
+                    SnapManager &m = desktop->namedview->snap_manager;
+                    m.setup(desktop);
+                    Geom::Point p = cc->selected_handle->pos;
+                    SPEventContext* event_context = SP_EVENT_CONTEXT( cc );
+                    
+                    if (!cc->within_tolerance)
+                    {
+//                        sp_event_context_snap_window_open(event_context);
+                        m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+                        sp_knot_set_position(cc->selected_handle, p, 0);
+//                        sp_event_context_snap_window_closed(event_context);
+                        ConnectionPoint& cp = cc->connpthandles[cc->selected_handle];
+                        cp.pos = p * sp_item_dt2i_affine(cc->active_shape);
+                        cc->active_shape->avoidRef->updateConnectionPoint(cp);
+                    }
+
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+                    ret = TRUE;
+                }
+                break;
+            case SP_CONNECTOR_CONTEXT_NEWCONNPOINT:
+                if ( keyval == GDK_Escape )
+                {
+                    // Just destroy the knot
+                    g_object_unref( cc->selected_handle );
+                    cc->selected_handle = NULL;
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+                    ret = TRUE;
+                }
+                else if ( keyval == GDK_Return || keyval == GDK_KP_Enter )
+                {
+                    SPDesktop *desktop = SP_EVENT_CONTEXT_DESKTOP(cc);
+                    SPDocument *doc = sp_desktop_document(desktop);
+                    SnapManager &m = desktop->namedview->snap_manager;
+                    m.setup(desktop);
+                    Geom::Point p = cc->selected_handle->pos;
+                    SPEventContext* event_context = SP_EVENT_CONTEXT( cc );
+                    
+//                    sp_event_context_snap_window_open( event_context );
+                    m.freeSnapReturnByRef(Inkscape::SnapPreferences::SNAPPOINT_NODE, p, Inkscape::SNAPSOURCE_HANDLE);
+                    
+                    sp_knot_set_position(cc->selected_handle, p, 0);
+//                    sp_event_context_snap_window_closed(event_context);
+
+                    ConnectionPoint cp;
+                    cp.type = ConnPointUserDefined;
+                    cp.pos = p * sp_item_dt2i_affine(cc->active_shape);
+                    cp.dir = Avoid::ConnDirAll;
+                    g_object_unref(cc->selected_handle);
+                    cc->active_shape->avoidRef->addConnectionPoint(cp);
+                    sp_document_ensure_up_to_date(doc);
+                    for (ConnectionPointMap::iterator it = cc->connpthandles.begin(); it != cc->connpthandles.end(); ++it)
+                        if (it->second.type == ConnPointUserDefined && it->second.id == cp.id)
+                        {
+                            cc->selected_handle = it->first;
+                            break;
+                        }
+                    cc_select_handle( cc->selected_handle );
+                    cc->state = SP_CONNECTOR_CONTEXT_IDLE;
+                    ret = TRUE;
+                }
+
+                break;
+            case SP_CONNECTOR_CONTEXT_IDLE:
+                if ( keyval == GDK_Delete && cc->selected_handle )
+                {
+                    cc->active_shape->avoidRef->deleteConnectionPoint(cc->connpthandles[cc->selected_handle]);
+                    cc->selected_handle = NULL;
+                    ret = TRUE;
+                }
+                
+                break;
+        }
     }
+
     return ret;
 }
 
@@ -818,7 +1300,7 @@ cc_connector_rerouting_finish(SPConnectorContext *const cc, Geom::Point *const p
         }
     }
     cc->clickeditem->setHidden(false);
-    sp_conn_adjust_path(SP_PATH(cc->clickeditem));
+    sp_conn_reroute_path_immediate(SP_PATH(cc->clickeditem));
     cc->clickeditem->updateRepr();
     sp_document_done(doc, SP_VERB_CONTEXT_CONNECTOR,
                      _("Reroute connector"));
@@ -863,25 +1345,20 @@ spcc_connector_set_subsequent_point(SPConnectorContext *const cc, Geom::Point co
 
     if (!cc->newConnRef) {
         Avoid::Router *router = sp_desktop_document(dt)->router;
-        cc->newConnRef = new Avoid::ConnRef(router, 0, src, dst);
-        cc->newConnRef->updateEndPoint(Avoid::VertID::src, src);
+        cc->newConnRef = new Avoid::ConnRef(router);
+        cc->newConnRef->setEndpoint(Avoid::VertID::src, src);
+        if (cc->isOrthogonal)
+            cc->newConnRef->setRoutingType(Avoid::ConnType_Orthogonal);
+        else
+            cc->newConnRef->setRoutingType(Avoid::ConnType_PolyLine);
     }
-    cc->newConnRef->updateEndPoint(Avoid::VertID::tar, dst);
-
+    // Set new endpoint.
+    cc->newConnRef->setEndpoint(Avoid::VertID::tar, dst);
+    // Immediately generate new routes for connector.
     cc->newConnRef->makePathInvalid();
-    cc->newConnRef->generatePath(src, dst);
-
-    Avoid::PolyLine route = cc->newConnRef->route();
-    cc->newConnRef->calcRouteDist();
-
-    cc->red_curve->reset();
-    Geom::Point pt(route.ps[0].x, route.ps[0].y);
-    cc->red_curve->moveto(pt);
-
-    for (int i = 1; i < route.pn; ++i) {
-        Geom::Point p(route.ps[i].x, route.ps[i].y);
-        cc->red_curve->lineto(p);
-    }
+    cc->newConnRef->router()->processTransaction();
+    // Recreate curve from libavoid route.
+    recreateCurve( cc->red_curve, cc->newConnRef, cc->curvature );
     cc->red_curve->transform(dt->doc2dt());
     sp_canvas_bpath_set_bpath(SP_CANVAS_BPATH(cc->red_bpath), cc->red_curve);
 }
@@ -953,14 +1430,13 @@ spcc_flush_white(SPConnectorContext *cc, SPCurve *gc)
 
         /* Attach repr */
         cc->newconn = SP_ITEM(desktop->currentLayer()->appendChildRepr(repr));
-        cc->selection->set(repr);
-        Inkscape::GC::release(repr);
         cc->newconn->transform = sp_item_i2doc_affine(SP_ITEM(desktop->currentLayer())).inverse();
-        cc->newconn->updateRepr();
 
         bool connection = false;
         sp_object_setAttribute(cc->newconn, "inkscape:connector-type",
-                "polyline", false);
+                cc->isOrthogonal ? "orthogonal" : "polyline", false);
+        sp_object_setAttribute(cc->newconn, "inkscape:connector-curvature",
+                Glib::Ascii::dtostr(cc->curvature).c_str(), false);
         if (cc->sid)
         {
             sp_object_setAttribute(cc->newconn, "inkscape:connection-start",
@@ -974,19 +1450,27 @@ spcc_flush_white(SPConnectorContext *cc, SPCurve *gc)
                     cc->eid, false);
             connection = true;
         }
+        // Process pending updates.
         cc->newconn->updateRepr();
+        sp_document_ensure_up_to_date(doc);
+        
         if (connection) {
             // Adjust endpoints to shape edge.
-            sp_conn_adjust_path(SP_PATH(cc->newconn));
+            sp_conn_reroute_path_immediate(SP_PATH(cc->newconn));
+            cc->newconn->updateRepr();
         }
-        cc->newconn->updateRepr();
+
+        // Only set the selection after we are finished with creating the attributes of
+        // the connector.  Otherwise, the selection change may alter the defaults for 
+        // values like curvature in the connector context, preventing subsequent lookup
+        // of their original values.
+        cc->selection->set(repr);
+        Inkscape::GC::release(repr);
     }
 
     c->unref();
 
-    /* Flush pending updates */
     sp_document_done(doc, SP_VERB_CONTEXT_CONNECTOR, _("Create connector"));
-    sp_document_ensure_up_to_date(doc);
 }
 
 
@@ -1036,16 +1520,16 @@ cc_generic_knot_handler(SPCanvasItem *, GdkEvent *event, SPKnot *knot)
 
     gboolean consumed = FALSE;
 
+    gchar* knot_tip = knot->tip ? knot->tip : cc->knot_tip;
     switch (event->type) {
         case GDK_ENTER_NOTIFY:
             sp_knot_set_flag(knot, SP_KNOT_MOUSEOVER, TRUE);
 
             cc->active_handle = knot;
-
-            if (knot->tip)
+            if (knot_tip)
             {
                 knot->desktop->event_context->defaultMessageContext()->set(
-                        Inkscape::NORMAL_MESSAGE, knot->tip);
+                        Inkscape::NORMAL_MESSAGE, knot_tip);
             }
 
             consumed = TRUE;
@@ -1055,7 +1539,7 @@ cc_generic_knot_handler(SPCanvasItem *, GdkEvent *event, SPKnot *knot)
 
             cc->active_handle = NULL;
 
-            if (knot->tip) {
+            if (knot_tip) {
                 knot->desktop->event_context->defaultMessageContext()->clear();
             }
 
@@ -1087,6 +1571,7 @@ endpt_handler(SPKnot */*knot*/, GdkEvent *event, SPConnectorContext *cc)
                 cc->clickedhandle = cc->active_handle;
                 cc_clear_active_conn(cc);
                 cc->state = SP_CONNECTOR_CONTEXT_REROUTING;
+//                sp_event_context_snap_window_open(SP_EVENT_CONTEXT(cc));
 
                 // Disconnect from attached shape
                 unsigned ind = (cc->active_handle == cc->endpt_handle[0]) ? 0 : 1;
@@ -1101,7 +1586,7 @@ endpt_handler(SPKnot */*knot*/, GdkEvent *event, SPConnectorContext *cc)
                 }
 
                 // Show the red path for dragging.
-                cc->red_curve = SP_PATH(cc->clickeditem)->curve->copy();
+                cc->red_curve = SP_PATH(cc->clickeditem)->original_curve ? SP_PATH(cc->clickeditem)->original_curve->copy() : SP_PATH(cc->clickeditem)->curve->copy();
                 Geom::Matrix i2d = sp_item_i2d_affine(cc->clickeditem);
                 cc->red_curve->transform(i2d);
                 sp_canvas_bpath_set_bpath(SP_CANVAS_BPATH(cc->red_bpath), cc->red_curve);
@@ -1120,38 +1605,9 @@ endpt_handler(SPKnot */*knot*/, GdkEvent *event, SPConnectorContext *cc)
     return consumed;
 }
 
-
-static void cc_set_active_shape(SPConnectorContext *cc, SPItem *item)
+static void cc_active_shape_add_knot(SPDesktop* desktop, SPItem* item, ConnectionPointMap &cphandles, ConnectionPoint& cp)
 {
-    g_assert(item != NULL );
-
-    cc->active_shape = item;
-
-    // Remove existing active shape listeners
-    if (cc->active_shape_repr) {
-        sp_repr_remove_listener_by_data(cc->active_shape_repr, cc);
-        Inkscape::GC::release(cc->active_shape_repr);
-
-        sp_repr_remove_listener_by_data(cc->active_shape_layer_repr, cc);
-        Inkscape::GC::release(cc->active_shape_layer_repr);
-    }
-
-    // Listen in case the active shape changes
-    cc->active_shape_repr = SP_OBJECT_REPR(item);
-    if (cc->active_shape_repr) {
-        Inkscape::GC::anchor(cc->active_shape_repr);
-        sp_repr_add_listener(cc->active_shape_repr, &shape_repr_events, cc);
-
-        cc->active_shape_layer_repr = cc->active_shape_repr->parent();
-        Inkscape::GC::anchor(cc->active_shape_layer_repr);
-        sp_repr_add_listener(cc->active_shape_layer_repr, &layer_repr_events, cc);
-    }
-
-
-    // Set center connection point.
-    if ( cc->connpthandle == NULL ) {
-        SPKnot *knot = sp_knot_new(cc->desktop,
-                _("<b>Connection point</b>: click or drag to create a new connector"));
+        SPKnot *knot = sp_knot_new(desktop, 0);
 
         knot->setShape(SP_KNOT_SHAPE_SQUARE);
         knot->setSize(8);
@@ -1159,26 +1615,127 @@ static void cc_set_active_shape(SPConnectorContext *cc, SPItem *item)
         knot->setFill(0xffffff00, 0xff0000ff, 0xff0000ff);
         sp_knot_update_ctrl(knot);
 
-        // We don't want to use the standard knot handler,
-        //since we don't want this knot to be draggable.
+        // We don't want to use the standard knot handler.
         g_signal_handler_disconnect(G_OBJECT(knot->item),
                 knot->_event_handler_id);
         knot->_event_handler_id = 0;
 
         gtk_signal_connect(GTK_OBJECT(knot->item), "event",
                 GTK_SIGNAL_FUNC(cc_generic_knot_handler), knot);
+        sp_knot_set_position(knot, item->avoidRef->getConnectionPointPos(cp.type, cp.id) * desktop->doc2dt(), 0);
+        sp_knot_show(knot);
+        cphandles[knot] = cp;
+}
+
+static void cc_set_active_shape(SPConnectorContext *cc, SPItem *item)
+{
+    g_assert(item != NULL );
+    
+    std::map<int, ConnectionPoint>* connpts = &item->avoidRef->connection_points;
+
+    if (cc->active_shape != item)
+    {
+        // The active shape has changed
+        // Rebuild everything
+        cc->active_shape = item;
+        // Remove existing active shape listeners
+        if (cc->active_shape_repr) {
+            sp_repr_remove_listener_by_data(cc->active_shape_repr, cc);
+            Inkscape::GC::release(cc->active_shape_repr);
+
+            sp_repr_remove_listener_by_data(cc->active_shape_layer_repr, cc);
+            Inkscape::GC::release(cc->active_shape_layer_repr);
+        }
+
+        // Listen in case the active shape changes
+        cc->active_shape_repr = SP_OBJECT_REPR(item);
+        if (cc->active_shape_repr) {
+            Inkscape::GC::anchor(cc->active_shape_repr);
+            sp_repr_add_listener(cc->active_shape_repr, &shape_repr_events, cc);
+
+            cc->active_shape_layer_repr = cc->active_shape_repr->parent();
+            Inkscape::GC::anchor(cc->active_shape_layer_repr);
+            sp_repr_add_listener(cc->active_shape_layer_repr, &layer_repr_events, cc);
+        }
 
-        cc->connpthandle = knot;
+
+        // Set the connection points.
+        if ( cc->connpthandles.size() )
+            // destroy the old list
+            while (! cc->connpthandles.empty() )
+            {
+                g_object_unref(cc->connpthandles.begin()->first);
+                cc->connpthandles.erase(cc->connpthandles.begin());
+            }
+        // build the new one
+        if ( connpts->size() )
+        for (std::map<int, ConnectionPoint>::iterator it = connpts->begin(); it != connpts->end(); ++it)
+            cc_active_shape_add_knot(cc->desktop, item, cc->connpthandles, it->second);
+        
+        // Also add default connection points
+        // For now, only centre default connection point will
+        // be available
+        ConnectionPoint centre;
+        centre.type = ConnPointDefault;
+        centre.id = ConnPointPosCC;
+        cc_active_shape_add_knot(cc->desktop, item, cc->connpthandles, centre);
     }
+    else
+    {
+        // The active shape didn't change
+        // Update only the connection point knots
 
+        // Ensure the item's connection_points map
+        // has been updated
+        sp_document_ensure_up_to_date(SP_OBJECT_DOCUMENT(item));
 
-    Geom::OptRect bbox = sp_item_bbox_desktop(cc->active_shape);
-    if (bbox) {
-        Geom::Point center = bbox->midpoint();
-        sp_knot_set_position(cc->connpthandle, center, 0);
-        sp_knot_show(cc->connpthandle);
-    } else {
-        sp_knot_hide(cc->connpthandle);
+        std::set<int> seen;
+        for  ( ConnectionPointMap::iterator it = cc->connpthandles.begin(); it != cc->connpthandles.end() ;)
+        {
+            bool removed = false;
+            if ( it->second.type == ConnPointUserDefined )
+            {
+                std::map<int, ConnectionPoint>::iterator p = connpts->find(it->second.id);
+                if (p != connpts->end())
+                {
+                    if ( it->second != p->second )
+                        // Connection point position has changed
+                        // Update knot position
+                        sp_knot_set_position(it->first,
+                                             item->avoidRef->getConnectionPointPos(it->second.type, it->second.id) * cc->desktop->doc2dt(), 0);
+                    seen.insert(it->second.id);
+                    sp_knot_show(it->first);
+                }
+                else
+                {
+                    // This connection point does no longer exist,
+                    // remove the knot
+                    ConnectionPointMap::iterator curr = it;
+                    ++it;
+                    g_object_unref( curr->first );
+                    cc->connpthandles.erase(curr);
+                    removed = true;
+                }
+            }
+            else
+            {
+                // It's a default connection point
+                // Just make sure it's position is correct
+                sp_knot_set_position(it->first,
+                                     item->avoidRef->getConnectionPointPos(it->second.type, it->second.id) * cc->desktop->doc2dt(), 0);
+                sp_knot_show(it->first);
+
+            }
+            if ( !removed )
+                ++it;
+        }
+        // Add knots for new connection points.
+        if (connpts->size())
+            for ( std::map<int, ConnectionPoint>::iterator it = connpts->begin(); it != connpts->end(); ++it )
+                if ( seen.find(it->first) == seen.end() )
+                    // A new connection point has been added
+                    // to the shape. Add a knot for it.
+                    cc_active_shape_add_knot(cc->desktop, item, cc->connpthandles, it->second);
     }
 }
 
@@ -1188,7 +1745,7 @@ cc_set_active_conn(SPConnectorContext *cc, SPItem *item)
 {
     g_assert( SP_IS_PATH(item) );
 
-    SPCurve *curve = SP_SHAPE(SP_PATH(item))->curve;
+    SPCurve *curve = SP_PATH(item)->original_curve ? SP_PATH(item)->original_curve : SP_PATH(item)->curve;
     Geom::Matrix i2d = sp_item_i2d_affine(item);
 
     if (cc->active_conn == item)
@@ -1234,7 +1791,7 @@ cc_set_active_conn(SPConnectorContext *cc, SPItem *item)
             sp_knot_update_ctrl(knot);
 
             // We don't want to use the standard knot handler,
-            //since we don't want this knot to be draggable.
+            // since we don't want this knot to be draggable.
             g_signal_handler_disconnect(G_OBJECT(knot->item),
                     knot->_event_handler_id);
             knot->_event_handler_id = 0;
@@ -1271,6 +1828,35 @@ cc_set_active_conn(SPConnectorContext *cc, SPItem *item)
     sp_knot_show(cc->endpt_handle[1]);
 }
 
+void cc_create_connection_point(SPConnectorContext* cc)
+{
+    if (cc->active_shape && cc->state == SP_CONNECTOR_CONTEXT_IDLE)
+    {
+        if (cc->selected_handle)
+        {
+            cc_deselect_handle( cc->selected_handle );
+        }
+        SPKnot *knot = sp_knot_new(cc->desktop, 0);
+        // We do not process events on this knot.
+        g_signal_handler_disconnect(G_OBJECT(knot->item),
+                                    knot->_event_handler_id);
+        knot->_event_handler_id = 0;
+
+        cc_select_handle( knot );
+        cc->selected_handle = knot;
+        sp_knot_show(cc->selected_handle);
+        cc->state = SP_CONNECTOR_CONTEXT_NEWCONNPOINT;
+    }
+}
+
+void cc_remove_connection_point(SPConnectorContext* cc)
+{
+    if (cc->selected_handle && cc->state == SP_CONNECTOR_CONTEXT_IDLE )
+    {
+        cc->active_shape->avoidRef->deleteConnectionPoint(cc->connpthandles[cc->selected_handle]);
+        cc->selected_handle = NULL;
+    }
+}
 
 static bool cc_item_is_shape(SPItem *item)
 {
@@ -1296,7 +1882,7 @@ bool cc_item_is_connector(SPItem *item)
 {
     if (SP_IS_PATH(item)) {
         if (SP_PATH(item)->connEndPair.isAutoRoutingConn()) {
-            g_assert( !(SP_SHAPE(item)->curve->is_closed()) );
+            g_assert( SP_PATH(item)->original_curve ? !(SP_PATH(item)->original_curve->is_closed()) : !(SP_PATH(item)->curve->is_closed()) );
             return true;
         }
     }
@@ -1394,7 +1980,7 @@ shape_event_attr_changed(Inkscape::XML::Node *repr, gchar const *name,
     g_assert(data);
     SPConnectorContext *cc = SP_CONNECTOR_CONTEXT(data);
 
-    // Look for changes than result in onscreen movement.
+    // Look for changes that result in onscreen movement.
     if (!strcmp(name, "d") || !strcmp(name, "x") || !strcmp(name, "y") ||
             !strcmp(name, "width") || !strcmp(name, "height") ||
             !strcmp(name, "transform"))
@@ -1409,6 +1995,12 @@ shape_event_attr_changed(Inkscape::XML::Node *repr, gchar const *name,
             cc_set_active_conn(cc, cc->active_conn);
         }
     }
+    else
+        if ( !strcmp(name, "inkscape:connection-points") )
+            if (repr == cc->active_shape_repr)
+                // The connection points of the active shape
+                // have changed. Update them.
+                cc_set_active_shape(cc, cc->active_shape);
 }
 
 
diff --git a/src/connector-context.h b/src/connector-context.h
index d67e12b81..640a03aae 100644
--- a/src/connector-context.h
+++ b/src/connector-context.h
@@ -19,7 +19,8 @@
 #include <display/display-forward.h>
 #include <2geom/point.h>
 #include "libavoid/connector.h"
-
+#include "connection-points.h"
+#include <glibmm/i18n.h>
 
 #define SP_TYPE_CONNECTOR_CONTEXT (sp_connector_context_get_type())
 #define SP_CONNECTOR_CONTEXT(o) (G_TYPE_CHECK_INSTANCE_CAST((o), SP_TYPE_CONNECTOR_CONTEXT, SPConnectorContext))
@@ -38,9 +39,18 @@ enum {
     SP_CONNECTOR_CONTEXT_DRAGGING,
     SP_CONNECTOR_CONTEXT_CLOSE,
     SP_CONNECTOR_CONTEXT_STOP,
-    SP_CONNECTOR_CONTEXT_REROUTING
+    SP_CONNECTOR_CONTEXT_REROUTING,
+    SP_CONNECTOR_CONTEXT_NEWCONNPOINT
+};
+
+enum {
+    SP_CONNECTOR_CONTEXT_DRAWING_MODE,
+    SP_CONNECTOR_CONTEXT_EDITING_MODE
 };
+static char* cc_knot_tips[] = { _("<b>Connection point</b>: click or drag to create a new connector"),
+                           _("<b>Connection point</b>: click to select, drag to move") };
 
+typedef std::map<SPKnot *, ConnectionPoint>  ConnectionPointMap;
 
 struct SPConnectorContext : public SPEventContext {
     Inkscape::Selection *selection;
@@ -48,10 +58,14 @@ struct SPConnectorContext : public SPEventContext {
 
     /** \invar npoints in {0, 2}. */
     gint npoints;
-
+    /* The tool mode can be connector drawing or
+       connection points editing.
+    */
     unsigned int mode : 1;
     unsigned int state : 4;
 
+    gchar* knot_tip;
+
     // Red curve
     SPCanvasItem *red_bpath;
     SPCurve *red_curve;
@@ -63,6 +77,8 @@ struct SPConnectorContext : public SPEventContext {
     // The new connector
     SPItem *newconn;
     Avoid::ConnRef *newConnRef;
+    gdouble curvature;
+    bool isOrthogonal;
     
     // The active shape
     SPItem *active_shape;
@@ -78,10 +94,13 @@ struct SPConnectorContext : public SPEventContext {
     // The activehandle
     SPKnot *active_handle;
 
+    // The selected handle, used in editing mode
+    SPKnot *selected_handle;
+
     SPItem *clickeditem;
     SPKnot *clickedhandle;
     
-    SPKnot *connpthandle;
+    ConnectionPointMap connpthandles;
     SPKnot *endpt_handle[2];
     guint  endpt_handler_id[2];
     gchar *sid;
@@ -93,7 +112,10 @@ struct SPConnectorContextClass : public SPEventContextClass { };
 
 GType sp_connector_context_get_type();
 
+void sp_connector_context_switch_mode(SPEventContext* ec, unsigned int newMode);
 void cc_selection_set_avoid(bool const set_ignore);
+void cc_create_connection_point(SPConnectorContext* cc);
+void cc_remove_connection_point(SPConnectorContext* cc);
 bool cc_item_is_connector(SPItem *item);
 
 
diff --git a/src/document.cpp b/src/document.cpp
index d406f3712..a3ad6f7be 100644
--- a/src/document.cpp
+++ b/src/document.cpp
@@ -63,10 +63,16 @@
 #include "xml/repr.h"
 #include "xml/rebase-hrefs.h"
 
-#define SP_DOCUMENT_UPDATE_PRIORITY (G_PRIORITY_HIGH_IDLE - 1)
+// Higher number means lower priority.
+#define SP_DOCUMENT_UPDATE_PRIORITY (G_PRIORITY_HIGH_IDLE - 2)
+
+// Should have a lower priority than SP_DOCUMENT_UPDATE_PRIORITY,
+// since we want it to happen when there are no more updates.
+#define SP_DOCUMENT_REROUTING_PRIORITY (G_PRIORITY_HIGH_IDLE - 1)
 
 
 static gint sp_document_idle_handler(gpointer data);
+static gint sp_document_rerouting_handler(gpointer data);
 
 gboolean sp_document_resource_list_free(gpointer key, gpointer value, gpointer data);
 
@@ -88,15 +94,17 @@ SPDocument::SPDocument() :
     priv(0), // reset in ctor
     actionkey(0),
     modified_id(0),
+    rerouting_handler_id(0),
     profileManager(0), // deferred until after other initialization
-    router(new Avoid::Router()),
+    router(new Avoid::Router(Avoid::PolyLineRouting|Avoid::OrthogonalRouting)),
     perspectives(0),
     current_persp3d(0),
     _collection_queue(0),
     oldSignalsConnected(false)
 {
-    // Don't use the Consolidate moves optimisation.
-    router->ConsolidateMoves = false;
+    // Penalise libavoid for choosing paths with needless extra segments.
+    // This results in much better looking orthogonal connector paths.
+    router->setRoutingPenalty(Avoid::segmentPenalty);
 
     SPDocumentPrivate *p = new SPDocumentPrivate();
 
@@ -132,6 +140,11 @@ SPDocument::~SPDocument() {
         profileManager = 0;
     }
 
+    if (router) {
+        delete router;
+        router = NULL;
+    }
+
     if (priv) {
         if (priv->partial) {
             sp_repr_free_log(priv->partial);
@@ -177,10 +190,15 @@ SPDocument::~SPDocument() {
     }
 
     if (modified_id) {
-        gtk_idle_remove(modified_id);
+        g_source_remove(modified_id);
         modified_id = 0;
     }
 
+    if (rerouting_handler_id) {
+        g_source_remove(rerouting_handler_id);
+        rerouting_handler_id = 0;
+    }
+
     if (oldSignalsConnected) {
         g_signal_handlers_disconnect_by_func(G_OBJECT(INKSCAPE),
                                              reinterpret_cast<gpointer>(sp_document_reset_key),
@@ -195,13 +213,7 @@ SPDocument::~SPDocument() {
         keepalive = FALSE;
     }
 
-    if (router) {
-        delete router;
-        router = NULL;
-    }
-
     //delete this->_whiteboard_session_manager;
-
 }
 
 void SPDocument::add_persp3d (Persp3D * const /*persp*/)
@@ -852,7 +864,12 @@ void
 sp_document_request_modified(SPDocument *doc)
 {
     if (!doc->modified_id) {
-        doc->modified_id = gtk_idle_add_priority(SP_DOCUMENT_UPDATE_PRIORITY, sp_document_idle_handler, doc);
+        doc->modified_id = g_idle_add_full(SP_DOCUMENT_UPDATE_PRIORITY, 
+                sp_document_idle_handler, doc, NULL);
+    }
+    if (!doc->rerouting_handler_id) {
+        doc->rerouting_handler_id = g_idle_add_full(SP_DOCUMENT_REROUTING_PRIORITY, 
+                sp_document_rerouting_handler, doc, NULL);
     }
 }
 
@@ -908,26 +925,49 @@ SPDocument::_updateDocument()
  * Repeatedly works on getting the document updated, since sometimes
  * it takes more than one pass to get the document updated.  But it
  * usually should not take more than a few loops, and certainly never
- * more than 64 iterations.  So we bail out if we hit 64 iterations,
+ * more than 32 iterations.  So we bail out if we hit 32 iterations,
  * since this typically indicates we're stuck in an update loop.
  */
 gint
 sp_document_ensure_up_to_date(SPDocument *doc)
 {
-    int counter = 64;
-    while (!doc->_updateDocument()) {
-        if (counter == 0) {
-            g_warning("More than 64 iteration while updating document '%s'", doc->uri? doc->uri:"<unknown URI, probably clipboard>");
+    // Bring the document up-to-date, specifically via the following:
+    //   1a) Process all document updates.
+    //   1b) When completed, process connector routing changes.
+    //   2a) Process any updates resulting from connector reroutings.
+    int counter = 32;
+    for (unsigned int pass = 1; pass <= 2; ++pass) {
+        // Process document updates.
+        while (!doc->_updateDocument()) {
+            if (counter == 0) {
+                g_warning("More than 32 iteration while updating document '%s'", doc->uri);
+                break;
+            }
+            counter--;
+        }
+        if (counter == 0)
+        {
             break;
         }
-        counter--;
-    }
 
+        // After updates on the first pass we get libavoid to process all the 
+        // changed objects and provide new routings.  This may cause some objects
+            // to be modified, hence the second update pass.
+        if (pass == 1) {
+            doc->router->processTransaction();
+        }
+    }
+    
     if (doc->modified_id) {
         /* Remove handler */
-        gtk_idle_remove(doc->modified_id);
+        g_source_remove(doc->modified_id);
         doc->modified_id = 0;
     }
+    if (doc->rerouting_handler_id) {
+        /* Remove handler */
+        g_source_remove(doc->rerouting_handler_id);
+        doc->rerouting_handler_id = 0;
+    }
     return counter>0;
 }
 
@@ -947,6 +987,24 @@ sp_document_idle_handler(gpointer data)
     }
 }
 
+/**
+ * An idle handler to reroute connectors in the document.  
+ */
+static gint
+sp_document_rerouting_handler(gpointer data)
+{
+    // Process any queued movement actions and determine new routings for 
+    // object-avoiding connectors.  Callbacks will be used to update and 
+    // redraw affected connectors.
+    SPDocument *doc = static_cast<SPDocument *>(data);
+    doc->router->processTransaction();
+    
+    // We don't need to handle rerouting again until there are further 
+    // diagram updates.
+    doc->rerouting_handler_id = 0;
+    return false;
+}
+
 static bool is_within(Geom::Rect const &area, Geom::Rect const &box)
 {
     return area.contains(box);
diff --git a/src/document.h b/src/document.h
index 789e3e2ed..06174c265 100644
--- a/src/document.h
+++ b/src/document.h
@@ -98,6 +98,9 @@ struct SPDocument : public Inkscape::GC::Managed<>,
     const gchar *actionkey;
     /// Handler ID
     guint modified_id;
+    
+    /// Connector rerouting handler ID
+    guint rerouting_handler_id;
 
     Inkscape::ProfileManager* profileManager;
 
diff --git a/src/graphlayout/graphlayout.cpp b/src/graphlayout/graphlayout.cpp
index cd1683c34..81ea59059 100644
--- a/src/graphlayout/graphlayout.cpp
+++ b/src/graphlayout/graphlayout.cpp
@@ -31,6 +31,7 @@
 #include "style.h"
 #include "conn-avoid-ref.h"
 #include "libavoid/connector.h"
+#include "libavoid/router.h"
 #include "libavoid/geomtypes.h"
 #include "libcola/cola.h"
 #include "libvpsc/generate-constraints.h"
diff --git a/src/libavoid/CMakeLists.txt b/src/libavoid/CMakeLists.txt
index df532c564..3f408074c 100644
--- a/src/libavoid/CMakeLists.txt
+++ b/src/libavoid/CMakeLists.txt
@@ -11,6 +11,8 @@ static.cpp
 timer.cpp
 vertices.cpp
 visibility.cpp
+orthogonal.cpp
+vpsc.cpp
 )
 ADD_LIBRARY(avoid STATIC ${libavoid_SRC})
 TARGET_LINK_LIBRARIES(avoid
diff --git a/src/libavoid/Makefile_insert b/src/libavoid/Makefile_insert
index f75470e26..77728499c 100644
--- a/src/libavoid/Makefile_insert
+++ b/src/libavoid/Makefile_insert
@@ -11,24 +11,26 @@ libavoid_libavoid_a_SOURCES =	\
 	libavoid/debug.h	\
 	libavoid/geometry.cpp	\
 	libavoid/geometry.h	\
+	libavoid/geomtypes.cpp	\
 	libavoid/geomtypes.h	\
 	libavoid/graph.cpp	\
 	libavoid/graph.h	\
 	libavoid/makepath.cpp	\
 	libavoid/makepath.h	\
-	libavoid/polyutil.cpp	\
-	libavoid/polyutil.h	\
+	libavoid/orthogonal.cpp	\
+	libavoid/orthogonal.h	\
+	libavoid/vpsc.cpp	\
+	libavoid/vpsc.h		\
 	libavoid/router.cpp	\
 	libavoid/router.h	\
 	libavoid/shape.cpp	\
 	libavoid/shape.h	\
-	libavoid/static.cpp	\
-	libavoid/static.h	\
 	libavoid/timer.cpp	\
 	libavoid/timer.h	\
 	libavoid/vertices.cpp	\
 	libavoid/vertices.h	\
 	libavoid/visibility.cpp	\
 	libavoid/visibility.h	\
-	libavoid/libavoid.h	\
-	libavoid/region.h
+	libavoid/viscluster.cpp \
+	libavoid/viscluster.h   \
+	libavoid/libavoid.h
diff --git a/src/libavoid/connector.cpp b/src/libavoid/connector.cpp
index 647303371..d9088dfe7 100644
--- a/src/libavoid/connector.cpp
+++ b/src/libavoid/connector.cpp
@@ -2,95 +2,236 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
+#include <cstring>
+#include <cfloat>
+#include <cmath>
 #include <cstdlib>
+
 #include "libavoid/graph.h"
 #include "libavoid/connector.h"
 #include "libavoid/makepath.h"
 #include "libavoid/visibility.h"
 #include "libavoid/debug.h"
 #include "libavoid/router.h"
+#include "libavoid/assertions.h"
 
 
 namespace Avoid {
 
+    
+ConnEnd::ConnEnd(const Point& point) 
+    : _point(point),
+      _directions(ConnDirAll),
+      _shapeRef(NULL)
+{
+}
+
+
+ConnEnd::ConnEnd(const Point& point, const ConnDirFlags visDirs) 
+    : _point(point),
+      _directions(visDirs),
+      _shapeRef(NULL)
+{
+}
+
+ConnEnd::ConnEnd(ShapeRef *shapeRef, const double x_pos, const double y_pos,
+        const double insideOffset, const ConnDirFlags visDirs) 
+    : _directions(visDirs),
+      _shapeRef(shapeRef),
+      _xPosition(x_pos),
+      _yPosition(y_pos),
+      _insideOffset(insideOffset)
+{
+}
+
+const Point ConnEnd::point(void) const
+{
+    if (_shapeRef)
+    {
+        const Polygon& poly = _shapeRef->polygon();
+
+        double x_min = DBL_MAX;
+        double x_max = -DBL_MAX;
+        double y_min = DBL_MAX;
+        double y_max = -DBL_MAX;
+        for (size_t i = 0; i < poly.size(); ++i)
+        {
+            x_min = std::min(x_min, poly.ps[i].x);
+            x_max = std::max(x_max, poly.ps[i].x);
+            y_min = std::min(y_min, poly.ps[i].y);
+            y_max = std::max(y_max, poly.ps[i].y);
+        }
+
+        Point point;
+
+        // We want to place connection points on the edges of shapes, 
+        // or possibly slightly inside them (if _insideOfset is set).
+
+        point.vn = kUnassignedVertexNumber;
+        if (_xPosition == ATTACH_POS_LEFT)
+        {
+            point.x = x_min + _insideOffset;
+            point.vn = 6;
+        }
+        else if (_xPosition == ATTACH_POS_RIGHT)
+        {
+            point.x = x_max - _insideOffset;
+            point.vn = 4;
+        }
+        else
+        {
+            point.x = x_min + (_xPosition * (x_max - x_min));
+        }
+
+        if (_yPosition == ATTACH_POS_TOP)
+        {
+            point.y = y_max - _insideOffset;
+            point.vn = 5;
+        }
+        else if (_yPosition == ATTACH_POS_BOTTOM)
+        {
+            point.y = y_min + _insideOffset;
+            point.vn = 7;
+        }
+        else
+        {
+            point.y = y_min + (_yPosition * (y_max - y_min));
+            point.vn = kUnassignedVertexNumber;
+        }
+
+        return point;
+    }
+    else
+    {
+        return _point;
+    }
+}
+
+
+ConnDirFlags ConnEnd::directions(void) const
+{
+    if (_shapeRef)
+    {
+        ConnDirFlags visDir = _directions;
+        if (_directions == ConnDirNone)
+        {
+            // None is set, use the defaults:
+            if (_xPosition == ATTACH_POS_LEFT)
+            {
+                visDir = ConnDirLeft;
+            }
+            else if (_xPosition == ATTACH_POS_RIGHT)
+            {
+                visDir = ConnDirRight;
+            }
+            if (_yPosition == ATTACH_POS_TOP)
+            {
+                visDir = ConnDirDown;
+            }
+            else if (_yPosition == ATTACH_POS_BOTTOM)
+            {
+                visDir = ConnDirUp;
+            }
+
+            if (visDir == ConnDirNone)
+            {
+                visDir = ConnDirAll;
+            }
+        }
+        return visDir;
+    }
+    else
+    {
+        return _directions;
+    }
+}
+
+
 ConnRef::ConnRef(Router *router, const unsigned int id)
-    : _router(router)
-    , _id(id)
-    , _type(ConnType_PolyLine)
-    , _srcId(0)
-    , _dstId(0)
-    , _needs_reroute_flag(true)
-    , _false_path(false)
-    , _active(false)
-    , _route_dist(0)
-    , _srcVert(NULL)
-    , _dstVert(NULL)
-    , _initialised(false)
-    , _callback(NULL)
-    , _connector(NULL)
-    , _hateCrossings(false)
+    : _router(router),
+      _type(router->validConnType()),
+      _srcId(0),
+      _dstId(0),
+      _needs_reroute_flag(true),
+      _false_path(false),
+      _needs_repaint(false),
+      _active(false),
+      _route_dist(0),
+      _srcVert(NULL),
+      _dstVert(NULL),
+      _startVert(NULL),
+      _initialised(false),
+      _callback(NULL),
+      _connector(NULL),
+      _hateCrossings(false)
 {
+    _id = router->assignId(id);
+
     // TODO: Store endpoints and details.
-    _route.pn = 0;
-    _route.ps = NULL;
-}
-
-
-ConnRef::ConnRef(Router *router, const unsigned int id,
-        const Point& src, const Point& dst)
-    : _router(router)
-    , _id(id)
-    , _type(ConnType_PolyLine)
-    , _srcId(0)
-    , _dstId(0)
-    , _needs_reroute_flag(true)
-    , _false_path(false)
-    , _active(false)
-    , _route_dist(0)
-    , _srcVert(NULL)
-    , _dstVert(NULL)
-    , _initialised(false)
-    , _callback(NULL)
-    , _connector(NULL)
-    , _hateCrossings(false)
-{
-    _route.pn = 0;
-    _route.ps = NULL;
-
-    if (_router->IncludeEndpoints)
-    {
-        bool isShape = false;
-        _srcVert = new VertInf(_router, VertID(id, isShape, 1), src);
-        _dstVert = new VertInf(_router, VertID(id, isShape, 2), dst);
-        _router->vertices.addVertex(_srcVert);
-        _router->vertices.addVertex(_dstVert);
-        makeActive();
-        _initialised = true;
-    }
+    _route.clear();
+}
+
+
+ConnRef::ConnRef(Router *router, const ConnEnd& src, const ConnEnd& dst,
+        const unsigned int id)
+    : _router(router),
+      _type(router->validConnType()),
+      _srcId(0),
+      _dstId(0),
+      _needs_reroute_flag(true),
+      _false_path(false),
+      _needs_repaint(false),
+      _active(false),
+      _route_dist(0),
+      _srcVert(NULL),
+      _dstVert(NULL),
+      _initialised(false),
+      _callback(NULL),
+      _connector(NULL),
+      _hateCrossings(false)
+{
+    _id = router->assignId(id);
+    _route.clear();
+
+    bool isShape = false;
+    _srcVert = new VertInf(_router, VertID(_id, isShape, 1), src.point());
+    _srcVert->visDirections = src.directions();
+    _dstVert = new VertInf(_router, VertID(_id, isShape, 2), dst.point());
+    _dstVert->visDirections = dst.directions();
+    makeActive();
+    _initialised = true;
+    
+    setEndpoints(src, dst);
 }
 
 
 ConnRef::~ConnRef()
 {
-    freeRoute();
+    _router->removeQueuedConnectorActions(this);
+    removeFromGraph();
+
+    freeRoutes();
 
     if (_srcVert)
     {
@@ -106,27 +247,38 @@ ConnRef::~ConnRef()
         _dstVert = NULL;
     }
 
-    if (_active)
-    {
-        makeInactive();
-    }
+    makeInactive();
 }
 
 
-void ConnRef::setType(unsigned int type)
+ConnType ConnRef::routingType(void) const
 {
-    _type = type;
+    return _type;
 }
 
 
-void ConnRef::updateEndPoint(const unsigned int type, const Point& point)
+void ConnRef::setRoutingType(ConnType type)
 {
-    assert((type == (unsigned int) VertID::src) ||
-           (type == (unsigned int) VertID::tar));
-    
-    // XXX: This was commented out.  Is there a case where it isn't true? 
-    assert(_router->IncludeEndpoints);
+    type = _router->validConnType(type);
+    if (_type != type)
+    {
+        _type = type;
+
+        makePathInvalid();
+
+        _router->modifyConnector(this);
+    }
+}
 
+
+void ConnRef::common_updateEndPoint(const unsigned int type, const ConnEnd& connEnd)
+{
+    const Point& point = connEnd.point();
+    //db_printf("common_updateEndPoint(%d,(pid=%d,vn=%d,(%f,%f)))\n",
+    //      type,point.id,point.vn,point.x,point.y);
+    COLA_ASSERT((type == (unsigned int) VertID::src) ||
+                (type == (unsigned int) VertID::tar));
+    
     if (!_initialised)
     {
         makeActive();
@@ -141,28 +293,28 @@ void ConnRef::updateEndPoint(const unsigned int type, const Point& point)
     {
         if (_srcVert)
         {
-            _srcVert->Reset(point);
+            _srcVert->Reset(VertID(_id, isShape, type), point);
         }
         else
         {
             _srcVert = new VertInf(_router, VertID(_id, isShape, type), point);
-            _router->vertices.addVertex(_srcVert);
         }
+        _srcVert->visDirections = connEnd.directions();
         
         altered = _srcVert;
         partner = _dstVert;
     }
-    else // if (type == (unsigned int) VertID::dst)
+    else // if (type == (unsigned int) VertID::tar)
     {
         if (_dstVert)
         {
-            _dstVert->Reset(point);
+            _dstVert->Reset(VertID(_id, isShape, type), point);
         }
         else
         {
             _dstVert = new VertInf(_router, VertID(_id, isShape, type), point);
-            _router->vertices.addVertex(_dstVert);
         }
+        _dstVert->visDirections = connEnd.directions();
         
         altered = _dstVert;
         partner = _srcVert;
@@ -171,8 +323,85 @@ void ConnRef::updateEndPoint(const unsigned int type, const Point& point)
     // XXX: Seems to be faster to just remove the edges and recreate
     bool isConn = true;
     altered->removeFromGraph(isConn);
-    bool knownNew = true;
-    vertexVisibility(altered, partner, knownNew, true);
+
+    makePathInvalid();
+    _router->setStaticGraphInvalidated(true);
+}
+
+
+void ConnRef::setEndpoints(const ConnEnd& srcPoint, const ConnEnd& dstPoint)
+{
+    _router->modifyConnector(this, VertID::src, srcPoint);
+    _router->modifyConnector(this, VertID::tar, dstPoint);
+}
+
+
+void ConnRef::setEndpoint(const unsigned int type, const ConnEnd& connEnd)
+{
+    _router->modifyConnector(this, type, connEnd);
+}
+
+
+void ConnRef::setSourceEndpoint(const ConnEnd& srcPoint)
+{
+    _router->modifyConnector(this, VertID::src, srcPoint);
+}
+
+
+void ConnRef::setDestEndpoint(const ConnEnd& dstPoint)
+{
+    _router->modifyConnector(this, VertID::tar, dstPoint);
+}
+
+
+void ConnRef::updateEndPoint(const unsigned int type, const ConnEnd& connEnd)
+{
+    common_updateEndPoint(type, connEnd);
+
+    if (_router->_polyLineRouting)
+    {
+        bool knownNew = true;
+        bool genContains = true;
+        if (type == (unsigned int) VertID::src)
+        {
+            vertexVisibility(_srcVert, _dstVert, knownNew, genContains);
+        }
+        else
+        {
+            vertexVisibility(_dstVert, _srcVert, knownNew, genContains);
+        }
+    }
+}
+
+
+bool ConnRef::setEndpoint(const unsigned int type, const VertID& pointID,
+        Point *pointSuggestion)
+{
+    VertInf *vInf = _router->vertices.getVertexByID(pointID);
+    if (vInf == NULL)
+    {
+        return false;
+    }
+    Point& point = vInf->point;
+    if (pointSuggestion)
+    {
+        if (euclideanDist(point, *pointSuggestion) > 0.5)
+        {
+            return false;
+        }
+    }
+
+    common_updateEndPoint(type, point);
+
+    // Give this visibility just to the point it is over.
+    EdgeInf *edge = new EdgeInf(
+            (type == VertID::src) ? _srcVert : _dstVert, vInf);
+    // XXX: We should be able to set this to zero, but can't due to 
+    //      assumptions elsewhere in the code.
+    edge->setDist(0.001);
+
+    _router->processTransaction();
+    return true;
 }
 
 
@@ -203,7 +432,7 @@ unsigned int ConnRef::getDstShapeId(void)
 
 void ConnRef::makeActive(void)
 {
-    assert(!_active);
+    COLA_ASSERT(!_active);
     
     // Add to connRefs list.
     _pos = _router->connRefs.insert(_router->connRefs.begin(), this);
@@ -213,7 +442,7 @@ void ConnRef::makeActive(void)
 
 void ConnRef::makeInactive(void)
 {
-    assert(_active);
+    COLA_ASSERT(_active);
     
     // Remove from connRefs list.
     _router->connRefs.erase(_pos);
@@ -221,54 +450,69 @@ void ConnRef::makeInactive(void)
 }
 
 
-void ConnRef::freeRoute(void)
+void ConnRef::freeRoutes(void)
 {
-    if (_route.ps)
-    {
-        _route.pn = 0;
-        std::free(_route.ps);
-        _route.ps = NULL;
-    }
+    _route.clear();
+    _display_route.clear();
 }
     
 
-PolyLine& ConnRef::route(void)
+const PolyLine& ConnRef::route(void) const
 {
     return _route;
 }
 
 
-void ConnRef::calcRouteDist(void)
+PolyLine& ConnRef::routeRef(void)
 {
-    _route_dist = 0;
-    for (int i = 1; i < _route.pn; i++)
+    return _route;
+}
+
+
+void ConnRef::set_route(const PolyLine& route)
+{
+    if (&_display_route == &route)
     {
-        _route_dist += dist(_route.ps[i], _route.ps[i - 1]);
+        db_printf("Error:\tTrying to update libavoid route with itself.\n");
+        return;
     }
+    _display_route.ps = route.ps;
+
+    //_display_route.clear();
 }
 
 
-bool ConnRef::needsReroute(void)
+Polygon& ConnRef::displayRoute(void)
 {
-    return (_false_path || _needs_reroute_flag);
+    if (_display_route.empty())
+    {
+        // No displayRoute is set.  Simplify the current route to get it.
+        _display_route = _route.simplify();
+    }
+    return _display_route;
 }
 
 
-void ConnRef::lateSetup(const Point& src, const Point& dst)
+void ConnRef::calcRouteDist(void)
 {
-    assert(!_initialised);
+    double (*dist)(const Point& a, const Point& b) = 
+            (_type == ConnType_PolyLine) ? euclideanDist : manhattanDist;
 
-    bool isShape = false;
-    _srcVert = new VertInf(_router, VertID(_id, isShape, 1), src);
-    _dstVert = new VertInf(_router, VertID(_id, isShape, 2), dst);
-    _router->vertices.addVertex(_srcVert);
-    _router->vertices.addVertex(_dstVert);
-    makeActive();
-    _initialised = true;
+    _route_dist = 0;
+    for (size_t i = 1; i < _route.size(); ++i)
+    {
+        _route_dist += dist(_route.at(i), _route.at(i - 1));
+    }
 }
 
 
-unsigned int ConnRef::id(void)
+bool ConnRef::needsRepaint(void) const
+{
+    return _needs_repaint;
+}
+
+
+unsigned int ConnRef::id(void) const
 {
     return _id;
 }
@@ -286,6 +530,12 @@ VertInf *ConnRef::dst(void)
 }
 
 
+VertInf *ConnRef::start(void)
+{
+    return _startVert;
+}
+
+
 bool ConnRef::isInitialised(void)
 {
     return _initialised;
@@ -303,29 +553,8 @@ void ConnRef::unInitialise(void)
 
 void ConnRef::removeFromGraph(void)
 {
-    for (VertInf *iter = _srcVert; iter != NULL; )
-    {
-        VertInf *tmp = iter;
-        iter = (iter == _srcVert) ? _dstVert : NULL;
-        
-        // For each vertex.
-        EdgeInfList& visList = tmp->visList;
-        EdgeInfList::iterator finish = visList.end();
-        EdgeInfList::iterator edge;
-        while ((edge = visList.begin()) != finish)
-        {
-            // Remove each visibility edge
-            delete (*edge);
-        }
-
-        EdgeInfList& invisList = tmp->invisList;
-        finish = invisList.end();
-        while ((edge = invisList.begin()) != finish)
-        {
-            // Remove each invisibility edge
-            delete (*edge);
-        }
-    }
+    _srcVert->removeFromGraph();
+    _dstVert->removeFromGraph();
 }
 
 
@@ -336,12 +565,11 @@ void ConnRef::setCallback(void (*cb)(void *), void *ptr)
 }
 
 
-void ConnRef::handleInvalid(void)
+void ConnRef::performCallback(void)
 {
-    if (_false_path || _needs_reroute_flag) {
-        if (_callback) {
-            _callback(_connector);
-        }
+    if (_callback) 
+    {
+        _callback(_connector);
     }
 }
 
@@ -352,79 +580,279 @@ void ConnRef::makePathInvalid(void)
 }
 
 
-Router *ConnRef::router(void)
+Router *ConnRef::router(void) const
 {
     return _router;
 }
 
 
-int ConnRef::generatePath(Point p0, Point p1)
+bool ConnRef::generatePath(Point p0, Point p1)
+{
+    // XXX Code to determine when connectors really need to be rerouted
+    //     does not yet work for orthogonal connectors.
+    if (_type != ConnType_Orthogonal)
+    {
+        if (!_false_path && !_needs_reroute_flag) 
+        {
+            // This connector is up to date.
+            return false;
+        }
+    }
+
+    bool result = generatePath();
+
+    return result;
+}
+
+
+// Validates a bend point on a path to check it does not form a zigzag corner.
+// a, b, c are consecutive points on the path.  d and e are b's neighbours,
+// forming the shape corner d-b-e.
+//
+bool validateBendPoint(VertInf *aInf, VertInf *bInf, VertInf *cInf)
 {
-    if (!_false_path && !_needs_reroute_flag) {
+    bool bendOkay = true;
+
+    if ((aInf == NULL) || (cInf == NULL))
+    {
+        // Not a bendpoint, i.e., the end of the connector, so don't test.
+        return bendOkay;
+    }
+
+    COLA_ASSERT(bInf != NULL);
+    VertInf *dInf = bInf->shPrev;
+    VertInf *eInf = bInf->shNext;
+    COLA_ASSERT(dInf != NULL);
+    COLA_ASSERT(eInf != NULL);
+
+    Point& a = aInf->point;
+    Point& b = bInf->point;
+    Point& c = cInf->point;
+    Point& d = dInf->point;
+    Point& e = eInf->point;
+
+    if ((a == b) || (b == c))
+    {
+        return bendOkay;
+    }
+
+#ifdef PATHDEBUG
+    db_printf("a=(%g, %g)\n", a.x, a.y);
+    db_printf("b=(%g, %g)\n", b.x, b.y);
+    db_printf("c=(%g, %g)\n", c.x, c.y);
+    db_printf("d=(%g, %g)\n", d.x, d.y);
+    db_printf("e=(%g, %g)\n", e.x, e.y);
+#endif
+    // Check angle:
+    int abc = vecDir(a, b, c);
+#ifdef PATHDEBUG
+    db_printf("(abc == %d) ", abc);
+#endif
+   
+    if (abc == 0)
+    {
+        // The three consecutive point on the path are in a line.
+        // Thus, there should always be an equally short path that
+        // skips this bend point.
+        bendOkay = false;
+    }
+    else // (abc != 0)
+    {
+        COLA_ASSERT(vecDir(d, b, e) > 0);
+        int abe = vecDir(a, b, e);
+        int abd = vecDir(a, b, d);
+        int bce = vecDir(b, c, e);
+        int bcd = vecDir(b, c, d);
+#ifdef PATHDEBUG
+        db_printf("&& (abe == %d) && (abd == %d) &&\n(bce == %d) && (bcd == %d)",
+                abe, abd, bce, bcd);
+#endif
+
+        bendOkay = false;
+        if (abe > 0)
+        {
+            if ((abc > 0) && (abd >= 0) && (bce >= 0))
+            {
+                bendOkay = true;
+            }
+        }
+        else if (abd < 0)
+        {
+            if ((abc < 0) && (abe <= 0) && (bcd <= 0))
+            {
+                bendOkay = true;
+            }
+        }
+    }
+#ifdef PATHDEBUG
+    db_printf("\n");
+#endif
+    return bendOkay;
+}
+
+
+bool ConnRef::generatePath(void)
+{
+    if (!_false_path && !_needs_reroute_flag) 
+    {
         // This connector is up to date.
-        return (int) false;
+        return false;
     }
 
+    if (!_dstVert || !_srcVert)
+    {
+        // Connector is not fully initialised..
+        return false;
+    }
+    
+    //COLA_ASSERT(_srcVert->point != _dstVert->point);
+
     _false_path = false;
     _needs_reroute_flag = false;
 
-    VertInf *src = _srcVert;
     VertInf *tar = _dstVert;
+    _startVert = _srcVert;
 
-    if ( !(_router->IncludeEndpoints) )
-    {
-        lateSetup(p0, p1);
-        
-        // Update as they have just been set by lateSetup.
-        src = _srcVert;
-        tar = _dstVert;
+    bool *flag = &(_needs_reroute_flag);
    
-        bool knownNew = true;
-        bool genContains = true;
-        vertexVisibility(src, tar, knownNew, genContains);
-        vertexVisibility(tar, src, knownNew, genContains);
+    size_t existingPathStart = 0;
+    const PolyLine& currRoute = route();
+    if (_router->RubberBandRouting)
+    {
+        COLA_ASSERT(_router->IgnoreRegions == true);
+
+#ifdef PATHDEBUG
+        db_printf("\n");
+        _srcVert->id.db_print();
+        db_printf(": %g, %g\n", _srcVert->point.x, _srcVert->point.y);
+        tar->id.db_print();
+        db_printf(": %g, %g\n", tar->point.x, tar->point.y);
+        for (size_t i = 0; i < currRoute.ps.size(); ++i)
+        {
+            db_printf("%g, %g  ", currRoute.ps[i].x, currRoute.ps[i].y);
+        }
+        db_printf("\n");
+#endif
+        if (currRoute.size() > 2)
+        {
+            if (_srcVert->point == currRoute.ps[0])
+            {
+                existingPathStart = currRoute.size() - 2;
+                COLA_ASSERT(existingPathStart != 0);
+                const Point& pnt = currRoute.at(existingPathStart);
+                bool isShape = true;
+                VertID vID(pnt.id, isShape, pnt.vn);
+
+                _startVert = _router->vertices.getVertexByID(vID);
+            }
+        }
+    }
+    //db_printf("GO\n");
+    //db_printf("src: %X strt: %X dst: %x\n", (int) _srcVert, (int) _startVert, (int) _dstVert);
+    bool found = false;
+    while (!found)
+    {
+        makePath(this, flag);
+        for (VertInf *i = tar; i != NULL; i = i->pathNext)
+        {
+            if (i == _srcVert)
+            {
+                found = true;
+                break;
+            }
+        }
+        if (!found)
+        {
+            if (existingPathStart == 0)
+            {
+                break;
+            }
+#ifdef PATHDEBUG
+            db_printf("BACK\n");
+#endif
+            existingPathStart--;
+            const Point& pnt = currRoute.at(existingPathStart);
+            bool isShape = (existingPathStart > 0);
+            VertID vID(pnt.id, isShape, pnt.vn);
+
+            _startVert = _router->vertices.getVertexByID(vID);
+            COLA_ASSERT(_startVert);
+        }
+        else if (_router->RubberBandRouting)
+        {
+            // found.
+            bool unwind = false;
+
+#ifdef PATHDEBUG
+            db_printf("\n\n\nSTART:\n\n");
+#endif
+            VertInf *prior = NULL;
+            for (VertInf *curr = tar; curr != _startVert->pathNext; 
+                    curr = curr->pathNext)
+            {
+                if (!validateBendPoint(curr->pathNext, curr, prior))
+                {
+                    unwind = true;
+                    break;
+                }
+                prior = curr;
+            }
+            if (unwind)
+            {
+#ifdef PATHDEBUG
+                db_printf("BACK II\n");
+#endif
+                if (existingPathStart == 0)
+                {
+                    break;
+                }
+                existingPathStart--;
+                const Point& pnt = currRoute.at(existingPathStart);
+                bool isShape = (existingPathStart > 0);
+                VertID vID(pnt.id, isShape, pnt.vn);
+
+                _startVert = _router->vertices.getVertexByID(vID);
+                COLA_ASSERT(_startVert);
+
+                found = false;
+            }
+        }
     }
 
-    bool *flag = &(_needs_reroute_flag);
-    
-    makePath(this, flag);
     
     bool result = true;
     
     int pathlen = 1;
-    for (VertInf *i = tar; i != src; i = i->pathNext)
+    for (VertInf *i = tar; i != _srcVert; i = i->pathNext)
     {
         pathlen++;
         if (i == NULL)
         {
             db_printf("Warning: Path not found...\n");
             pathlen = 2;
-            tar->pathNext = src;
-            if (_router->InvisibilityGrph)
+            tar->pathNext = _srcVert;
+            if ((_type == ConnType_PolyLine) && _router->InvisibilityGrph)
             {
                 // TODO:  Could we know this edge already?
-                EdgeInf *edge = EdgeInf::existingEdge(src, tar);
-                assert(edge != NULL);
+                EdgeInf *edge = EdgeInf::existingEdge(_srcVert, tar);
+                COLA_ASSERT(edge != NULL);
                 edge->addCycleBlocker();
             }
-            result = false;
             break;
         }
-        if (pathlen > 100)
-        {
-            fprintf(stderr, "ERROR: Should never be here...\n");
-            exit(1);
-        }
+        // Check we don't have an apparent infinite connector path.
+        COLA_ASSERT(pathlen < 200);
     }
-    Point *path = (Point *) malloc(pathlen * sizeof(Point));
+    std::vector<Point> path(pathlen);
 
     int j = pathlen - 1;
-    for (VertInf *i = tar; i != src; i = i->pathNext)
+    for (VertInf *i = tar; i != _srcVert; i = i->pathNext)
     {
-        if (_router->InvisibilityGrph)
+        if (_router->InvisibilityGrph && (_type == ConnType_PolyLine))
         {
             // TODO: Again, we could know this edge without searching.
             EdgeInf *edge = EdgeInf::existingEdge(i, i->pathNext);
+            COLA_ASSERT(edge != NULL);
             edge->addConn(flag);
         }
         else
@@ -432,25 +860,53 @@ int ConnRef::generatePath(Point p0, Point p1)
             _false_path = true;
         }
         path[j] = i->point;
-        path[j].id = i->id.objID;
+        if (i->id.isShape)
+        {
+            path[j].id = i->id.objID;
+            path[j].vn = i->id.vn;
+        }
+        else
+        {
+            path[j].id = _id;
+            path[j].vn = kUnassignedVertexNumber;
+        }
         j--;
-    }
-    path[0] = src->point;
 
+        if (i->pathNext && (i->pathNext->point == i->point))
+        {
+            if (i->pathNext->id.isShape && i->id.isShape)
+            {
+                // Check for consecutive points on opposite 
+                // corners of two touching shapes.
+                COLA_ASSERT(abs(i->pathNext->id.objID - i->id.objID) != 2);
+            }
+        }
+    }
+    path[0] = _srcVert->point;
+    // Use topbit to differentiate between start and end point of connector.
+    // They need unique IDs for nudging.
+    unsigned int topbit = ((unsigned int) 1) << 31;
+    path[0].id = _id | topbit; 
+    path[0].vn = kUnassignedVertexNumber;
 
     // Would clear visibility for endpoints here if required.
 
-    PolyLine& output_route = route();
-    output_route.pn = pathlen;
+    freeRoutes();
+    PolyLine& output_route = _route;
     output_route.ps = path;
  
-    if ( !(_router->IncludeEndpoints) )
+#ifdef PATHDEBUG
+    db_printf("Output route:\n");
+    for (size_t i = 0; i < output_route.ps.size(); ++i)
     {
-        assert(_initialised);
-        unInitialise();
+        db_printf("[%d,%d] %g, %g   ", output_route.ps[i].id, 
+                output_route.ps[i].vn, output_route.ps[i].x, 
+                output_route.ps[i].y);
     }
-   
-    return (int) result;
+    db_printf("\n\n");
+#endif
+
+    return result;
 }
 
 
@@ -466,6 +922,895 @@ bool ConnRef::doesHateCrossings(void)
 }
 
 
+PtOrder::~PtOrder()
+{
+    // Free the PointRep lists.
+    for (int dim = 0; dim < 2; ++dim)
+    {
+        PointRepList::iterator curr = connList[dim].begin();
+        while (curr != connList[dim].end())
+        {
+            PointRep *doomed = *curr;
+            curr = connList[dim].erase(curr);
+            delete doomed;
+        }
+    }
+}
+
+bool PointRep::follow_inner(PointRep *target)
+{
+    if (this == target)
+    {
+        return true;
+    }
+    else
+    {
+        for (PointRepSet::iterator curr = inner_set.begin(); 
+                curr != inner_set.end(); ++curr)
+        {
+            if ((*curr)->follow_inner(target))
+            {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+
+int PtOrder::positionFor(const ConnRef *conn, const size_t dim) const
+{
+    int position = 0;
+    for (PointRepList::const_iterator curr = connList[dim].begin(); 
+            curr != connList[dim].end(); ++curr)
+    {
+        if ((*curr)->conn == conn)
+        {
+            return position;
+        }
+        ++position;
+    }
+    // Not found.
+    return -1;
+}
+
+
+bool PtOrder::addPoints(const int dim, PtConnPtrPair innerArg, 
+        PtConnPtrPair outerArg, bool swapped)
+{
+    PtConnPtrPair inner = (swapped) ? outerArg : innerArg;
+    PtConnPtrPair outer = (swapped) ? innerArg : outerArg;
+    COLA_ASSERT(inner != outer);
+
+    //printf("addPoints(%d, [%g, %g]-%X, [%g, %g]-%X)\n", dim,
+    //        inner->x, inner->y, (int) inner, outer->x, outer->y, (int) outer);
+
+    PointRep *innerPtr = NULL;
+    PointRep *outerPtr = NULL;
+    for (PointRepList::iterator curr = connList[dim].begin(); 
+            curr != connList[dim].end(); ++curr)
+    {
+        if ((*curr)->point == inner.first)
+        {
+            innerPtr = *curr;
+        }
+        if ((*curr)->point == outer.first)
+        {
+            outerPtr = *curr;
+        }
+    }
+    
+    if (innerPtr == NULL)
+    {
+        innerPtr = new PointRep(inner.first, inner.second);
+        connList[dim].push_back(innerPtr);
+    }
+    
+    if (outerPtr == NULL)
+    {
+        outerPtr = new PointRep(outer.first, outer.second);
+        connList[dim].push_back(outerPtr);
+    }
+    // TODO COLA_ASSERT(innerPtr->inner_set.find(outerPtr) == innerPtr->inner_set.end());
+    bool cycle = innerPtr->follow_inner(outerPtr);
+    if (cycle)
+    {
+        // Must reverse to avoid a cycle.
+        innerPtr->inner_set.insert(outerPtr);
+    }
+    else
+    {
+        outerPtr->inner_set.insert(innerPtr);
+    }
+    return cycle;
+}
+
+
+// Assuming that addPoints has been called for each pair of points in the 
+// shared path at that corner, then the contents of inner_set can be used 
+// to determine the correct ordering.
+static bool pointRepLessThan(PointRep *r1, PointRep *r2)
+{
+    size_t r1less = r1->inner_set.size();
+    size_t r2less = r2->inner_set.size();
+    //COLA_ASSERT(r1less != r2less);
+    
+    return (r1less > r2less);
+}
+
+
+void PtOrder::sort(const int dim)
+{
+    connList[dim].sort(pointRepLessThan);
+}
+
+
+// Returns a vertex number representing a point on the line between 
+// two shape corners, represented by p0 and p1.
+//
+static int midVertexNumber(const Point& p0, const Point& p1, const Point& c)
+{
+    if (c.vn != kUnassignedVertexNumber)
+    {
+        // The split point is a shape corner, so doesn't need its 
+        // vertex number adjusting.
+        return c.vn;
+    }
+    if ((p0.vn >= 4) && (p0.vn < kUnassignedVertexNumber))
+    {
+        // The point next to this has the correct nudging direction,
+        // so use that.
+        return p0.vn;
+    }
+    if ((p1.vn >= 4) && (p1.vn < kUnassignedVertexNumber))
+    {
+        // The point next to this has the correct nudging direction,
+        // so use that.
+        return p1.vn;
+    }
+    if ((p0.vn < 4) && (p1.vn < 4))
+    {
+        if (p0.vn != p1.vn)
+        {
+            return p0.vn;
+        }
+        // Splitting between two ordinary shape corners.
+        int vn_mid = std::min(p0.vn, p1.vn);
+        if ((std::max(p0.vn, p1.vn) == 3) && (vn_mid == 0))
+        {
+            vn_mid = 3; // Next vn is effectively 4.
+        }
+        return vn_mid + 4;
+    }
+    COLA_ASSERT((p0.x == p1.x) || (p0.y == p1.y));
+    if (p0.vn != kUnassignedVertexNumber)
+    {
+        if (p0.x == p1.x)
+        {
+            if ((p0.vn == 2) || (p0.vn == 3))
+            {
+                return 6;
+            }
+            return 4;
+        }
+        else
+        {
+            if ((p0.vn == 0) || (p0.vn == 3))
+            {
+                return 7;
+            }
+            return 5;
+        }
+    }
+    else if (p1.vn != kUnassignedVertexNumber)
+    {
+        if (p0.x == p1.x)
+        {
+            if ((p1.vn == 2) || (p1.vn == 3))
+            {
+                return 6;
+            }
+            return 4;
+        }
+        else
+        {
+            if ((p1.vn == 0) || (p1.vn == 3))
+            {
+                return 7;
+            }
+            return 5;
+        }
+    }
+
+    // Shouldn't both be new (kUnassignedVertexNumber) points.
+    db_printf("midVertexNumber(): p0.vn and p1.vn both = "
+            "kUnassignedVertexNumber\n");
+    db_printf("p0.vn %d p1.vn %d\n", p0.vn, p1.vn);
+    return kUnassignedVertexNumber;
+}
+
+
+// Break up overlapping parallel segments that are not the same edge in 
+// the visibility graph, i.e., where one segment is a subsegment of another.
+void splitBranchingSegments(Avoid::Polygon& poly, bool polyIsConn,
+        Avoid::Polygon& conn, const double tolerance)
+{
+    for (std::vector<Avoid::Point>::iterator i = conn.ps.begin(); 
+            i != conn.ps.end(); ++i)
+    {
+        if (i == conn.ps.begin())
+        {
+            // Skip the first point.
+            // There are points-1 segments in a connector.
+            continue;
+        }
+
+        for (std::vector<Avoid::Point>::iterator j = poly.ps.begin(); 
+                j != poly.ps.end(); )
+        {
+            if (polyIsConn && (j == poly.ps.begin()))
+            {
+                // Skip the first point.
+                // There are points-1 segments in a connector.
+                ++j;
+                continue;
+            }
+            Point& c0 = *(i - 1);
+            Point& c1 = *i;
+
+            Point& p0 = (j == poly.ps.begin()) ? poly.ps.back() : *(j - 1);
+            Point& p1 = *j;
+
+            // Check the first point of the first segment.
+            if (((i - 1) == conn.ps.begin()) && 
+                    pointOnLine(p0, p1, c0, tolerance))
+            {
+                //db_printf("add to poly %g %g\n", c0.x, c0.y);
+                
+                c0.vn = midVertexNumber(p0, p1, c0);
+                j = poly.ps.insert(j, c0);
+                if (j != poly.ps.begin())
+                {
+                    --j;
+                }
+                continue;
+            }
+            // And the second point of every segment.
+            if (pointOnLine(p0, p1, c1, tolerance))
+            {
+                //db_printf("add to poly %g %g\n", c1.x, c1.y);
+                
+                c1.vn = midVertexNumber(p0, p1, c1);
+                j = poly.ps.insert(j, c1);
+                if (j != poly.ps.begin())
+                {
+                    --j;
+                }
+                continue;
+            }
+
+            // Check the first point of the first segment.
+            if (polyIsConn && ((j - 1) == poly.ps.begin()) && 
+                        pointOnLine(c0, c1, p0, tolerance))
+            {
+                //db_printf("add to conn %g %g\n", p0.x, p0.y);
+
+                p0.vn = midVertexNumber(c0, c1, p0);
+                i = conn.ps.insert(i, p0);
+                continue;
+            }
+            // And the second point of every segment.
+            if (pointOnLine(c0, c1, p1, tolerance))
+            {
+                //db_printf("add to conn %g %g\n", p1.x, p1.y);
+
+                p1.vn = midVertexNumber(c0, c1, p1);
+                i = conn.ps.insert(i, p1);
+            }
+            ++j;
+        }
+    }
+}
+
+
+static int segDir(const Point& p1, const Point& p2)
+{
+    int result = 1;
+    if (p1.x == p2.x)
+    {
+        if (p2.y > p1.y)
+        {
+            result = -1;
+        }
+    }
+    else if (p1.y == p2.y)
+    {
+        if (p2.x < p1.x)
+        {
+            result = -1;
+        }
+    }
+    return result;
+}
+
+
+// Works out if the segment conn[cIndex-1]--conn[cIndex] really crosses poly.
+// This does not not count non-crossing shared paths as crossings.
+// poly can be either a connector (polyIsConn = true) or a cluster
+// boundary (polyIsConn = false).
+//
+CrossingsInfoPair countRealCrossings(Avoid::Polygon& poly, 
+        bool polyIsConn, Avoid::Polygon& conn, size_t cIndex, 
+        bool checkForBranchingSegments, const bool finalSegment, 
+        PointSet *crossingPoints, PtOrderMap *pointOrders, 
+        ConnRef *polyConnRef, ConnRef *connConnRef)
+{
+    unsigned int crossingFlags = CROSSING_NONE;
+    if (checkForBranchingSegments)
+    {
+        size_t conn_pn = conn.size();
+        // XXX When doing the pointOnLine test we allow the points to be 
+        // slightly non-collinear.  This addresses a problem with clustered
+        // routing where connectors could otherwise route cheaply through
+        // shape corners that were not quite on the cluster boundary, but
+        // reported to be on there by the line segment intersection code,
+        // which I suspect is not numerically accurate enough.  This occured
+        // for points that only differed by about 10^-12 in the y-dimension.
+        double tolerance = (!polyIsConn) ? 0.00001 : 0.0;
+        splitBranchingSegments(poly, polyIsConn, conn, tolerance);
+        // cIndex is going to be the last, so take into account added points.
+        cIndex += (conn.size() - conn_pn);
+    }
+    COLA_ASSERT(cIndex >= 1);
+    COLA_ASSERT(cIndex < conn.size());
+
+    bool polyIsOrthogonal = (polyConnRef && 
+            (polyConnRef->routingType() == ConnType_Orthogonal));
+    bool connIsOrthogonal = (connConnRef &&
+            (connConnRef->routingType() == ConnType_Orthogonal));
+
+    size_t poly_size = poly.size();
+    int crossingCount = 0;
+    std::vector<Avoid::Point *> c_path;
+    std::vector<Avoid::Point *> p_path;
+
+    Avoid::Point& a1 = conn.ps[cIndex - 1];
+    Avoid::Point& a2 = conn.ps[cIndex];
+    //db_printf("a1: %g %g\n", a1.x, a1.y);
+    //db_printf("a2: %g %g\n", a2.x, a2.y);
+
+    for (size_t j = ((polyIsConn) ? 1 : 0); j < poly_size; ++j)
+    {
+        Avoid::Point& b1 = poly.ps[(j - 1 + poly_size) % poly_size];
+        Avoid::Point& b2 = poly.ps[j];
+        //db_printf("b1: %g %g\n", b1.x, b1.y);
+        //db_printf("b2: %g %g\n", b2.x, b2.y);
+
+        p_path.clear();
+        c_path.clear();
+        bool converging = false;
+
+        const bool a1_eq_b1 = (a1 == b1);
+        const bool a2_eq_b1 = (a2 == b1);
+        const bool a2_eq_b2 = (a2 == b2);
+        const bool a1_eq_b2 = (a1 == b2);
+
+        if ( (a1_eq_b1 && a2_eq_b2) ||
+             (a2_eq_b1 && a1_eq_b2) )
+        {
+            if (finalSegment)
+            {
+                converging = true;
+            }
+            else
+            {
+                // Route along same segment: no penalty.  We detect
+                // crossovers when we see the segments diverge.
+                continue;
+            }
+        }
+        else if (a2_eq_b1 || a2_eq_b2 || a1_eq_b2)
+        {
+            // Each crossing that is at a vertex in the 
+            // visibility graph gets noticed four times.
+            // We ignore three of these cases.
+            // This also catches the case of a shared path,
+            // but this is one that terminates at a common
+            // endpoint, so we don't care about it.
+            continue;
+        }
+    
+        if (a1_eq_b1 || converging)
+        {
+            if (!converging)
+            {
+                if (polyIsConn && (j == 1))
+                {
+                    // Can't be the end of a shared path or crossing path 
+                    // since the common point is the first point of the 
+                    // connector path.  This is not a shared path at all.
+                    continue;
+                }
+
+                Avoid::Point& b0 = poly.ps[(j - 2 + poly_size) % poly_size];
+                // The segments share an endpoint -- a1==b1.
+                if (a2 == b0)
+                {
+                    // a2 is not a split, continue.
+                    continue;
+                }
+            }
+            
+            // If here and not converging, then we know that a2 != b2
+            // And a2 and its pair in b are a split.
+            COLA_ASSERT(converging || !a2_eq_b2);
+
+            bool shared_path = false;
+            
+            // Initial values here don't matter. They are only used after 
+            // being set to sensible values, but we set them to stop a MSVC
+            // warning.
+            bool p_dir_back;
+            int p_dir = 0;
+            int trace_c = 0;
+            int trace_p = 0;
+            
+            if (converging)
+            {
+                // Determine direction we have to look through
+                // the points of connector b.
+                p_dir_back = a2_eq_b2 ? true : false;
+                p_dir = p_dir_back ? -1 : 1;
+                trace_c = (int) cIndex;
+                trace_p = (int) j;
+                if (!p_dir_back)
+                {
+                    if (finalSegment)
+                    {
+                        trace_p--;
+                    }
+                    else
+                    {   
+                        trace_c--;
+                    }
+                }
+
+                shared_path = true;
+            }
+            else if (cIndex >= 2)
+            {
+                Avoid::Point& b0 = poly.ps[(j - 2 + poly_size) % poly_size];
+                Avoid::Point& a0 = conn.ps[cIndex - 2];
+            
+                //db_printf("a0: %g %g\n", a0.x, a0.y);
+                //db_printf("b0: %g %g\n", b0.x, b0.y);
+
+                if ((a0 == b2) || (a0 == b0))
+                {
+                    // Determine direction we have to look through
+                    // the points of connector b.
+                    p_dir_back = (a0 == b0) ? true : false;
+                    p_dir = p_dir_back ? -1 : 1;
+                    trace_c = (int) cIndex;
+                    trace_p = (int) (p_dir_back ? j : j - 2);
+                    
+                    shared_path = true;
+                }
+            }    
+
+            if (shared_path)
+            {
+                crossingFlags |= CROSSING_SHARES_PATH;
+                // Shouldn't be here if p_dir is still equal to zero.
+                COLA_ASSERT(p_dir != 0);
+
+                // Build the shared path, including the diverging points at
+                // each end if the connector does not end at a common point.
+                while ( (trace_c >= 0) && (!polyIsConn || 
+                            ((trace_p >= 0) && (trace_p < (int) poly_size))) )
+                {
+                    // If poly is a cluster boundary, then it is a closed 
+                    // poly-line and so it wraps arounds.
+                    size_t index_p = (size_t)
+                            ((trace_p + (2 * poly_size)) % poly_size);
+                    size_t index_c = (size_t) trace_c;
+                    c_path.push_back(&conn.ps[index_c]);
+                    p_path.push_back(&poly.ps[index_p]);
+                    if ((c_path.size() > 1) && 
+                            (conn.ps[index_c] != poly.ps[index_p]))
+                    {
+                        // Points don't match, so break out of loop.
+                        break;
+                    }
+                    trace_c--;
+                    trace_p += p_dir;
+                }
+
+                // Are there diverging points at the ends of the shared path.
+                bool front_same = (*(c_path.front()) == *(p_path.front()));
+                bool back_same  = (*(c_path.back())  == *(p_path.back()));
+
+                size_t size = c_path.size();
+                
+                // Check to see if these share a fixed segment.
+                if (polyIsOrthogonal && connIsOrthogonal)
+                {
+                    size_t startPt = (front_same) ? 0 : 1;
+                    if (c_path[startPt]->x == c_path[startPt + 1]->x)
+                    {
+                        // Vertical
+                        double xPos = c_path[startPt]->x;
+                        // See if this is inline with either the start
+                        // or end point of both connectors.
+                        if ( ((xPos == poly.ps[0].x) || 
+                                (xPos == poly.ps[poly_size - 1].x)) &&
+                             ((xPos == conn.ps[0].x) || 
+                                (xPos == conn.ps[cIndex].x)) )
+                        {
+                            crossingFlags |= CROSSING_SHARES_FIXED_SEGMENT;
+                        }
+                    }
+                    else
+                    {
+                        // Horizontal
+                        double yPos = c_path[startPt]->y;
+                        // See if this is inline with either the start
+                        // or end point of both connectors.
+                        if ( ((yPos == poly.ps[0].y) || 
+                                (yPos == poly.ps[poly_size - 1].y)) &&
+                             ((yPos == conn.ps[0].y) || 
+                                (yPos == conn.ps[cIndex].y)) )
+                        {
+                            crossingFlags |= CROSSING_SHARES_FIXED_SEGMENT;
+                        }
+                    }
+                }
+
+                int prevTurnDir = -1;
+                int startCornerSide = 1;
+                int endCornerSide = 1;
+                bool reversed = false;
+                if (!front_same)
+                {
+                    // If there is a divergence at the beginning, 
+                    // then order the shared path based on this.
+                    prevTurnDir = vecDir(*c_path[0], *c_path[1], *c_path[2]);
+                    startCornerSide = Avoid::cornerSide(*c_path[0], *c_path[1], 
+                            *c_path[2], *p_path[0]) 
+                        * segDir(*c_path[1], *c_path[2]);
+                    reversed = (startCornerSide != -prevTurnDir);
+                }
+                if (!back_same)
+                {
+                    // If there is a divergence at the end of the path, 
+                    // then order the shared path based on this.
+                    prevTurnDir = vecDir(*c_path[size - 3],
+                            *c_path[size - 2], *c_path[size - 1]);
+                    endCornerSide = Avoid::cornerSide(*c_path[size - 3], 
+                            *c_path[size - 2], *c_path[size - 1], 
+                            *p_path[size - 1])
+                        * segDir(*c_path[size - 3], *c_path[size - 2]);
+                    reversed = (endCornerSide != -prevTurnDir);
+                }
+                else
+                {
+                    endCornerSide = startCornerSide;
+                }
+                if (front_same)
+                {
+                    startCornerSide = endCornerSide;
+                }
+                
+                if (front_same || back_same)
+                {
+                    crossingFlags |= CROSSING_SHARES_PATH_AT_END;
+                }
+                else if (polyIsOrthogonal && connIsOrthogonal)
+                {
+                    int cStartDir = vecDir(*c_path[0], *c_path[1], *c_path[2]);
+                    int pStartDir = vecDir(*p_path[0], *p_path[1], *p_path[2]);
+                    if ((cStartDir != 0) && (cStartDir == -pStartDir))
+                    {
+                        // The start segments diverge at 180 degrees to each 
+                        // other.  So order based on not introducing overlap
+                        // of the diverging segments when these are nudged
+                        // apart.
+                        startCornerSide = -cStartDir * 
+                                segDir(*c_path[1], *c_path[2]);
+                    }
+                    else 
+                    {
+                        int cEndDir = vecDir(*c_path[size - 3], 
+                                *c_path[size - 2], *c_path[size - 1]);
+                        int pEndDir = vecDir(*p_path[size - 3], 
+                                *p_path[size - 2], *p_path[size - 1]);
+                        if ((cEndDir != 0) && (cEndDir == -pEndDir))
+                        {
+                            // The end segments diverge at 180 degrees to 
+                            // each other.  So order based on not introducing 
+                            // overlap of the diverging segments when these 
+                            // are nudged apart.
+                            startCornerSide = -cEndDir * segDir(
+                                    *c_path[size - 3], *c_path[size - 2]);
+                        }
+                    }
+                }
+
+#if 0
+                prevTurnDir = 0;
+                if (pointOrders)
+                {
+                    // Return the ordering for the shared path.
+                    COLA_ASSERT(c_path.size() > 0 || back_same);
+                    size_t adj_size = (c_path.size() - ((back_same) ? 0 : 1));
+                    for (size_t i = (front_same) ? 0 : 1; i < adj_size; ++i)
+                    {
+                        Avoid::Point& an = *(c_path[i]);
+                        Avoid::Point& bn = *(p_path[i]);
+                        int currTurnDir = ((i > 0) && (i < (adj_size - 1))) ?  
+                                vecDir(*c_path[i - 1], an,
+                                       *c_path[i + 1]) : 0;
+                        VertID vID(an.id, true, an.vn);
+                        if ( (currTurnDir == (-1 * prevTurnDir)) &&
+                                (currTurnDir != 0) && (prevTurnDir != 0) )
+                        {
+                            // The connector turns the opposite way around 
+                            // this shape as the previous bend on the path,
+                            // so reverse the order so that the inner path
+                            // become the outer path and vice versa.
+                            reversed = !reversed;
+                        }
+                        bool orderSwapped = (*pointOrders)[an].addPoints(
+                                &bn, &an, reversed);
+                        if (orderSwapped)
+                        {
+                            // Reverse the order for later points.
+                            reversed = !reversed;
+                        }
+                        prevTurnDir = currTurnDir;
+                    }
+                }
+#endif
+                prevTurnDir = 0;
+                if (pointOrders)
+                {
+                    reversed = false;
+                    size_t startPt = (front_same) ? 0 : 1;
+                    
+                    // Orthogonal should always have at least one segment.
+                    COLA_ASSERT(c_path.size() > (startPt + 1));
+                    
+                    if (startCornerSide > 0)
+                    {
+                        reversed = !reversed;
+                    }
+
+                    int prevDir = 0;
+                    // Return the ordering for the shared path.
+                    COLA_ASSERT(c_path.size() > 0 || back_same);
+                    size_t adj_size = (c_path.size() - ((back_same) ? 0 : 1));
+                    for (size_t i = (front_same) ? 0 : 1; i < adj_size; ++i)
+                    {
+                        Avoid::Point& an = *(c_path[i]);
+                        Avoid::Point& bn = *(p_path[i]);
+                        COLA_ASSERT(an == bn);
+
+                        int thisDir = prevDir;
+                        if ((i > 0) && (*(c_path[i - 1]) == *(p_path[i - 1])))
+                        {
+                            thisDir = segDir(*c_path[i - 1], *c_path[i]);
+                        }
+
+                        if (thisDir != prevDir)
+                        {
+                            reversed = !reversed;
+                        }
+                        prevDir = thisDir;
+
+                        if (i > startPt)
+                        {
+                            Avoid::Point& ap = *(c_path[i - 1]);
+                            Avoid::Point& bp = *(p_path[i - 1]);
+                            int orientation = (ap.x == an.x) ? 0 : 1;
+                            //printf("prevOri %d\n", prevOrientation);
+                            //printf("1: %X, %X\n", (int) &(bn), (int) &(an));
+                            bool orderSwapped = (*pointOrders)[an].addPoints(
+                                    orientation, 
+                                    std::make_pair(&bn, polyConnRef), 
+                                    std::make_pair(&an, connConnRef), 
+                                    reversed);
+                            if (orderSwapped)
+                            {
+                                // Reverse the order for later points.
+                                reversed = !reversed;
+                            }
+                            COLA_ASSERT(ap == bp);
+                            //printf("2: %X, %X\n", (int) &bp, (int) &ap);
+                            orderSwapped = (*pointOrders)[ap].addPoints(
+                                    orientation, 
+                                    std::make_pair(&bp, polyConnRef), 
+                                    std::make_pair(&ap, connConnRef), 
+                                    reversed);
+                            COLA_ASSERT(!orderSwapped);
+                        }
+                    }
+                }
+#if 0
+                    int ymod = -1;
+                    if ((id.vn == 1) || (id.vn == 2))
+                    {
+                        // bottom.
+                        ymod = +1;
+                    }
+                    
+                    int xmod = -1;
+                    if ((id.vn == 0) || (id.vn == 1))
+                    {
+                        // right.
+                        xmod = +1;
+                    }
+                    if(id.vn > 3)
+                    {
+                        xmod = ymod = 0;
+                        if (id.vn == 4)
+                        {
+                            // right.
+                            xmod = +1;
+                        }
+                        else if (id.vn == 5)
+                        {
+                            // bottom.
+                            ymod = +1;
+                        }
+                        else if (id.vn == 6)
+                        {
+                            // left.
+                            xmod = -1;
+                        }
+                        else if (id.vn == 7)
+                        {
+                            // top.
+                            ymod = -1;
+                        }
+                    }
+#endif
+ 
+                if (endCornerSide != startCornerSide)
+                {
+                    // Mark that the shared path crosses.
+                    //db_printf("shared path crosses.\n");
+                    crossingCount += 1;
+                    if (crossingPoints)
+                    {
+                        crossingPoints->insert(*c_path[1]);
+                    }
+                }
+                crossingFlags |= CROSSING_TOUCHES;
+            }
+            else if (cIndex >= 2)
+            {
+                // The connectors cross or touch at this point.
+                //db_printf("Cross or touch at point... \n");
+                
+                // Crossing shouldn't be at an endpoint.
+                COLA_ASSERT(cIndex >= 2);
+                COLA_ASSERT(polyIsConn && (j >= 2));
+
+                Avoid::Point& b0 = poly.ps[(j - 2 + poly_size) % poly_size];
+                Avoid::Point& a0 = conn.ps[cIndex - 2];
+            
+                int side1 = Avoid::cornerSide(a0, a1, a2, b0);
+                int side2 = Avoid::cornerSide(a0, a1, a2, b2);
+                if (side1 != side2)
+                {
+                    // The connectors cross at this point.
+                    //db_printf("cross.\n");
+                    crossingCount += 1;
+                    if (crossingPoints)
+                    {
+                        crossingPoints->insert(a1);
+                    }
+                }
+
+                crossingFlags |= CROSSING_TOUCHES;
+                if (pointOrders)
+                {
+                    if (polyIsOrthogonal && connIsOrthogonal)
+                    {
+                        // Orthogonal case:
+                        // Just order based on which comes from the left and
+                        // top in each dimension because this can only be two
+                        // L-shaped segments touching at the bend.
+                        bool reversedX = ((a0.x < a1.x) || (a2.x < a1.x));
+                        bool reversedY = ((a0.y < a1.y) || (a2.y < a1.y));
+                        // XXX: Why do we need to invert the reversed values 
+                        //      here?  Are they wrong for orthogonal points
+                        //      in the other places?
+                        (*pointOrders)[b1].addPoints(0, 
+                                std::make_pair(&b1, polyConnRef), 
+                                std::make_pair(&a1, connConnRef), 
+                                !reversedX);
+                        (*pointOrders)[b1].addPoints(1, 
+                                std::make_pair(&b1, polyConnRef), 
+                                std::make_pair(&a1, connConnRef),
+                                !reversedY);
+                    }
+                    else
+                    {
+                        int turnDirA = vecDir(a0, a1, a2); 
+                        int turnDirB = vecDir(b0, b1, b2); 
+                        bool reversed = (side1 != -turnDirA); 
+                        if (side1 != side2) 
+                        { 
+                            // Interesting case where a connector routes round 
+                            // the edge of a shape and intersects a connector 
+                            // which is connected to a port on the edge of the 
+                            // shape. 
+                            if (turnDirA == 0) 
+                            { 
+                                // We'll make B the outer by preference,  
+                                // because the points of A are collinear. 
+                                reversed = false; 
+                            } 
+                            else if (turnDirB == 0) 
+                            { 
+                                reversed = true; 
+                            } 
+                            // TODO COLA_ASSERT((turnDirB != 0) || 
+                            //          (turnDirA != 0)); 
+                        }
+                        VertID vID(b1.id, true, b1.vn);
+                        //(*pointOrders)[b1].addPoints(&b1, &a1, reversed);
+                    }
+                }
+            }
+        }
+        else
+        {
+            if ( polyIsOrthogonal && connIsOrthogonal)
+            {
+                // All crossings in orthogonal connectors will be at a
+                // vertex in the visibility graph, so we need not bother
+                // doing normal line intersection.
+                continue;
+            }
+
+            // No endpoint is shared between these two line segments,
+            // so just calculate normal segment intersection.
+
+            Point cPt;
+            int intersectResult = Avoid::segmentIntersectPoint(
+                    a1, a2, b1, b2, &(cPt.x), &(cPt.y));
+
+            if (intersectResult == Avoid::DO_INTERSECT)
+            {
+                if (!polyIsConn && 
+                        ((a1 == cPt) || (a2 == cPt) || (b1 == cPt) || (b2 == cPt)))
+                {
+                    // XXX: This shouldn't actually happen, because these
+                    //      points should be added as bends to each line by
+                    //      splitBranchingSegments().  Thus, lets ignore them.
+                    COLA_ASSERT(a1 != cPt);
+                    COLA_ASSERT(a2 != cPt);
+                    COLA_ASSERT(b1 != cPt);
+                    COLA_ASSERT(b2 != cPt);
+                    continue;
+                }                
+                //db_printf("crossing lines:\n");
+                //db_printf("cPt: %g %g\n", cPt.x, cPt.y);
+                crossingCount += 1;
+                if (crossingPoints)
+                {
+                    crossingPoints->insert(cPt);
+                }
+            }
+        }
+    }
+    //db_printf("crossingcount %d\n", crossingCount);
+    return std::make_pair(crossingCount, crossingFlags);
+}
+
+
 //============================================================================
 
 }
diff --git a/src/libavoid/connector.h b/src/libavoid/connector.h
index 64afb4dda..8f7499a29 100644
--- a/src/libavoid/connector.h
+++ b/src/libavoid/connector.h
@@ -2,93 +2,336 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file    shape.h
+//! @brief   Contains the interface for the ConnRef class.
+
+
 #ifndef AVOID_CONNECTOR_H
 #define AVOID_CONNECTOR_H
 
-#include "libavoid/router.h"
+#include <list>
+#include <vector>
+
+#include "libavoid/vertices.h"
 #include "libavoid/geometry.h"
 #include "libavoid/shape.h"
-#include <list>
 
 
 namespace Avoid {
 
+class Router;
+class ConnRef;
+typedef std::list<ConnRef *> ConnRefList;
+
+
+//! @brief  Describes the type of routing that is performed for each 
+//!         connector.
+enum ConnType {
+    ConnType_None       = 0,
+    //! @brief  The connector path will be a shortest-path poly-line that
+    //!         routes around obstacles.
+    ConnType_PolyLine   = 1,
+    //! @brief  The connector path will be a shortest-path orthogonal 
+    //!         poly-line (only vertical and horizontal line segments) that
+    //!         routes around obstacles.
+    ConnType_Orthogonal = 2
+};
+
+//! @brief  Flags that can be passed to the ConnEnd constructor to specify
+//!         which sides of a shape this point should have visibility to if
+//!         it is located within the shape's area.
+//!
+//! Like SVG, libavoid considers the Y-axis to point downwards, that is, 
+//! like screen coordinates the coordinates increase from left-to-right and 
+//! also from top-to-bottom.
+//!
+enum ConnDirFlag {
+    ConnDirNone  = 0,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the top of the shape that it is located within.
+    ConnDirUp    = 1,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the bottom of the shape that it is located within.
+    ConnDirDown  = 2,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the left side of the shape that it is located within.
+    ConnDirLeft  = 4,
+    //! @brief  This option specifies the point should be given visibility 
+    //!         to the right side of the shape that it is located within.
+    ConnDirRight = 8,
+    //! @brief  This option, provided for convenience, specifies the point 
+    //!         should be given visibility to all four sides of the shape 
+    //!         that it is located within.
+    ConnDirAll   = 15
+};
+//! @brief  One or more Avoid::ConnDirFlag options.
+//!
+typedef unsigned int ConnDirFlags;
+
+
+static const double ATTACH_POS_TOP = 0;
+static const double ATTACH_POS_CENTER = 0.5;
+static const double ATTACH_POS_BOTTOM = 1;
+static const double ATTACH_POS_LEFT = ATTACH_POS_TOP;
+static const double ATTACH_POS_RIGHT = ATTACH_POS_BOTTOM;
+
+
+//! @brief  The ConnEnd class represents different possible endpoints for 
+//!         connectors.
+//!
+//! Currently this class just allows free-floating endpoints, but in future
+//! will be capable of representing attachments to connection points on shapes.
+//! 
+class ConnEnd 
+{
+    public:
+        //! @brief Constructs a ConnEnd from a free-floating point.
+        //!
+        //! @param[in]  point  The position of the connector endpoint.
+        //!
+        ConnEnd(const Point& point);
+
+        //! @brief Constructs a ConnEnd from a free-floating point as well
+        //!        as a set of flags specifying visibility for this point 
+        //!        if it is located inside a shape.
+        //!
+        //! @param[in]  point    The position of the connector endpoint.
+        //! @param[in]  visDirs  One or more Avoid::ConnDirFlag options 
+        //!                      specifying the directions that this point 
+        //!                      should be given visibility if it is inside 
+        //!                      a shape.
+        //!
+        ConnEnd(const Point& point, const ConnDirFlags visDirs);
+
+        ConnEnd(ShapeRef *shapeRef, const double x_pos, const double y_pos,
+                const double insideOffset = 0.0,
+                const ConnDirFlags visDirs = ConnDirNone);
 
-static const int ConnType_PolyLine   = 1;
-static const int ConnType_Orthogonal = 2;
+        //! @brief Returns the position of this connector endpoint
+        //!
+        //! @return The position of this connector endpoint.
+        const Point point(void) const;
+
+        ConnDirFlags directions(void) const;
+    private:
+        Point _point;
+        ConnDirFlags _directions;
+        
+        // For referencing ConnEnds
+        ShapeRef *_shapeRef;
+        double _xPosition;
+        double _yPosition;
+        double _insideOffset;
+};
 
 
+//! @brief   The ConnRef class represents a connector object.
+//!
+//! Connectors are a (possible multi-segment) line between two points.
+//! They are routed intelligently so as not to overlap any of the shape
+//! objects in the Router scene.
+//! 
+//! Routing penalties can be applied, resulting in more aesthetically pleasing
+//! connector paths with fewer segments or less severe bend-points.
+//!
+//! You can set a function to be called when the connector has been rerouted
+//! and needs to be redrawn.  Alternatively, you can query the connector's
+//! needsRepaint() function to determine this manually.
+//!
+//! Usually, it is expected that you would create a ConnRef for each connector 
+//! in your diagram and keep that reference in your own connector class.
+//!
 class ConnRef
 {
     public:
-        ConnRef(Router *router, const unsigned int id);
-        ConnRef(Router *router, const unsigned int id,
-                const Point& src, const Point& dst);
-        virtual ~ConnRef();
+        //! @brief Constructs a connector with no endpoints specified.
+        //!
+        //! @param[in]  router  The router scene to place the connector into.
+        //! @param[in]  id      A unique positive integer ID for the connector.  
+        //!
+        //! If an ID is not specified, then one will be assigned to the shape.
+        //! If assigning an ID yourself, note that it should be a unique 
+        //! positive integer.  Also, IDs are given to all objects in a scene,
+        //! so the same ID cannot be given to a shape and a connector for 
+        //! example.
+        //!
+        ConnRef(Router *router, const unsigned int id = 0);
+        //! @brief Constructs a connector with endpoints specified.
+        //!
+        //! @param[in]  router  The router scene to place the connector into.
+        //! @param[in]  id      A unique positive integer ID for the connector.
+        //! @param[in]  src     The source endpoint of the connector.
+        //! @param[in]  dst     The destination endpoint of the connector.
+        //!
+        //! If an ID is not specified, then one will be assigned to the shape.
+        //! If assigning an ID yourself, note that it should be a unique 
+        //! positive integer.  Also, IDs are given to all objects in a scene,
+        //! so the same ID cannot be given to a shape and a connector for 
+        //! example.
+        //!
+        ConnRef(Router *router, const ConnEnd& src, const ConnEnd& dst,
+                const unsigned int id = 0);
+        //! @brief  Destuctor.
+        ~ConnRef();
         
-        void setType(unsigned int type);
-        PolyLine& route(void);
-        bool needsReroute(void);
-        void freeRoute(void);
+        //! @brief  Sets both new source and destination endpoints for this 
+        //!         connector.
+        //!
+        //! @param[in]  srcPoint  New source endpoint for the connector.
+        //! @param[in]  dstPoint  New destination endpoint for the connector.
+        void setEndpoints(const ConnEnd& srcPoint, const ConnEnd& dstPoint);
+        //! @brief  Sets just a new source endpoint for this connector.
+        //!
+        //! @param[in]  srcPoint  New source endpoint for the connector.
+        void setSourceEndpoint(const ConnEnd& srcPoint);
+        //! @brief  Sets just a new destination endpoint for this connector.
+        //!
+        //! @param[in]  dstPoint  New destination endpoint for the connector.
+        void setDestEndpoint(const ConnEnd& dstPoint);
+        //! @brief   Returns the ID of this connector.
+        //! @returns The ID of the connector. 
+        unsigned int id(void) const;
+        //! @brief   Returns a pointer to the router scene this connector is in.
+        //! @returns A pointer to the router scene for this connector.
+        Router *router(void) const;
+
+        //! @brief   Returns an indication of whether this connector has a 
+        //!          new route and thus needs to be repainted.
+        //!
+        //! If the connector has been rerouted and need repainting, the  
+        //! route() method can be called to get a reference to the new route.
+        //!
+        //! @returns Returns true if the connector requires repainting, or 
+        //!          false if it does not.
+        bool needsRepaint(void) const;
+        
+        //! @brief   Returns a reference to the current route for the connector.
+        //!
+        //! This is a "raw" version of the route, where each line segment in
+        //! the route may be made up of multiple collinear line segments.  It
+        //! will also not have post-processing (like curved corners) applied
+        //! to it.  The simplified route for display can be obtained by calling
+        //! displayRoute().
+        //!
+        //! @returns The PolyLine route for the connector.
+        //! @note    You can obtain a modified version of this poly-line 
+        //!          route with curved corners added by calling 
+        //!          PolyLine::curvedPolyline().
+        const PolyLine& route(void) const;
+        
+        //! @brief   Returns a reference to the current display version of the
+        //!          route for the connector.
+        //! 
+        //! The display version of a route has been simplified to collapse all
+        //! collinear line segments into single segments.  It may also have 
+        //! post-processing applied to the route, such as curved corners or
+        //! nudging.
+        //! 
+        //! @returns The PolyLine display route for the connector.
+        PolyLine& displayRoute(void);
+        
+        //! @brief   Sets a callback function that will called to indicate that
+        //!          the connector needs rerouting.
+        //!
+        //! The cb function will be called when shapes are added to, removed 
+        //! from or moved about on the page.  The pointer ptr will be passed 
+        //! as an argument to the callback function.
+        //!
+        //! @param[in]  cb   A pointer to the callback function.
+        //! @param[in]  ptr  A generic pointer that will be passed to the 
+        //!                  callback function.
+        void setCallback(void (*cb)(void *), void *ptr);
+        //! @brief   Returns the type of routing performed for this connector.
+        //! @return  The type of routing performed.
+        //!
+        ConnType routingType(void) const;
+        //! @brief       Sets the type of routing to be performed for this 
+        //!              connector.
+        //! 
+        //! If a call to this method changes the current type of routing 
+        //! being used for the connector, then it will get rerouted during
+        //! the next processTransaction() call, or immediately if 
+        //! transactions are not being used.
+        //!
+        //! @param type  The type of routing to be performed.
+        //!
+        void setRoutingType(ConnType type);
+
+       
+
+        // @brief   Returns the source endpoint vertex in the visibility graph.
+        // @returns The source endpoint vertex.
+        VertInf *src(void);
+        // @brief   Returns the destination endpoint vertex in the 
+        //          visibility graph.
+        // @returns The destination endpoint vertex.
+        VertInf *dst(void);
+        
+
+        void set_route(const PolyLine& route);
         void calcRouteDist(void);
-        void updateEndPoint(const unsigned int type, const Point& point);
         void setEndPointId(const unsigned int type, const unsigned int id);
         unsigned int getSrcShapeId(void);
         unsigned int getDstShapeId(void);
         void makeActive(void);
         void makeInactive(void);
-        void lateSetup(const Point& src, const Point& dst);
-        unsigned int id(void);
-        VertInf *src(void);
-        VertInf *dst(void);
+        VertInf *start(void);
         void removeFromGraph(void);
         bool isInitialised(void);
-        void unInitialise(void);
-        void setCallback(void (*cb)(void *), void *ptr);
-        void handleInvalid(void);
-        int generatePath(Point p0, Point p1);
         void makePathInvalid(void);
-        Router *router(void);
         void setHateCrossings(bool value);
         bool doesHateCrossings(void);
-        
-        friend void Router::attachedShapes(IntList &shapes,
-                const unsigned int shapeId, const unsigned int type);
-        friend void Router::attachedConns(IntList &conns,
-                const unsigned int shapeId, const unsigned int type);
-        friend void Router::markConnectors(ShapeRef *shape);
-        
+        void setEndpoint(const unsigned int type, const ConnEnd& connEnd);
+        bool setEndpoint(const unsigned int type, const VertID& pointID, 
+                Point *pointSuggestion = NULL);
+    
     private:
+        friend class Router;
+
+        PolyLine& routeRef(void);
+        void freeRoutes(void);
+        void performCallback(void);
+        bool generatePath(void);
+        bool generatePath(Point p0, Point p1);
+        void unInitialise(void);
+        void updateEndPoint(const unsigned int type, const ConnEnd& connEnd);
+        void common_updateEndPoint(const unsigned int type, const ConnEnd& connEnd);
         Router *_router;
         unsigned int _id;
-        unsigned int _type;
+        ConnType _type;
         unsigned int _srcId, _dstId;
+        bool _orthogonal;
         bool _needs_reroute_flag;
         bool _false_path;
+        bool _needs_repaint;
         bool _active;
         PolyLine _route;
+        Polygon _display_route;
         double _route_dist;
         ConnRefList::iterator _pos;
         VertInf *_srcVert;
         VertInf *_dstVert;
+        VertInf *_startVert;
         bool _initialised;
         void (*_callback)(void *);
         void *_connector;
@@ -96,6 +339,69 @@ class ConnRef
 };
 
 
+class PointRep;
+typedef std::set<PointRep *> PointRepSet;
+typedef std::list<PointRep *> PointRepList;
+
+class PointRep
+{
+    public:
+        PointRep(Point *p, const ConnRef *c)
+            : point(p),
+              conn(c)
+
+        {
+        }
+        bool follow_inner(PointRep *target);
+
+        Point *point;
+        const ConnRef *conn;
+        // inner_set: Set of pointers to the PointReps 'inner' of 
+        // this one, at this corner.
+        PointRepSet inner_set;
+};
+
+
+typedef std::pair<Point *, ConnRef *> PtConnPtrPair;
+
+class PtOrder
+{
+    public:
+        PtOrder()
+        {
+        }
+        ~PtOrder();
+        bool addPoints(const int dim, PtConnPtrPair innerArg, 
+                PtConnPtrPair outerArg, bool swapped);
+        void sort(const int dim);
+        int positionFor(const ConnRef *conn, const size_t dim) const;
+
+        // One for each dimension.
+        PointRepList connList[2];
+};
+
+typedef std::map<Avoid::Point,PtOrder> PtOrderMap;
+typedef std::set<Avoid::Point> PointSet;
+
+
+const unsigned int CROSSING_NONE = 0;
+const unsigned int CROSSING_TOUCHES = 1;
+const unsigned int CROSSING_SHARES_PATH = 2;
+const unsigned int CROSSING_SHARES_PATH_AT_END = 4;
+const unsigned int CROSSING_SHARES_FIXED_SEGMENT = 8;
+
+
+typedef std::pair<int, unsigned int> CrossingsInfoPair;
+
+extern CrossingsInfoPair countRealCrossings( Avoid::Polygon& poly, 
+        bool polyIsConn, Avoid::Polygon& conn, size_t cIndex, 
+        bool checkForBranchingSegments, const bool finalSegment = false, 
+        PointSet *crossingPoints = NULL, PtOrderMap *pointOrders = NULL, 
+        ConnRef *polyConnRef = NULL, ConnRef *connConnRef = NULL);
+extern void splitBranchingSegments(Avoid::Polygon& poly, bool polyIsConn,
+        Avoid::Polygon& conn, const double tolerance = 0);
+extern bool validateBendPoint(VertInf *aInf, VertInf *bInf, VertInf *cInf);
+
 }
 
 
diff --git a/src/libavoid/debug.h b/src/libavoid/debug.h
index 20e6f4705..443529ece 100644
--- a/src/libavoid/debug.h
+++ b/src/libavoid/debug.h
@@ -2,24 +2,27 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2008  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_DEBUG_H
 #define AVOID_DEBUG_H
 
@@ -42,7 +45,7 @@ inline void db_printf(const char *fmt, ...)
     va_end(ap);
 }
 #else
-inline void db_printf(const char */*fmt*/, ...)
+inline void db_printf(const char *, ...)
 {
 }
 #endif
diff --git a/src/libavoid/geometry.cpp b/src/libavoid/geometry.cpp
index 15840c381..2523375cf 100644
--- a/src/libavoid/geometry.cpp
+++ b/src/libavoid/geometry.cpp
@@ -2,7 +2,8 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * --------------------------------------------------------------------
  * Much of the code in this module is based on code published with
@@ -18,70 +19,42 @@
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
+#include <cmath>
+
 #include "libavoid/graph.h"
 #include "libavoid/geometry.h"
-#include "libavoid/polyutil.h"
-
-#include <math.h>
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
 
-Point::Point()
-{
-}
-
-
-Point::Point(const double xv, const double yv)
-    : x(xv)
-    , y(yv)
-{
-}
-
-
-bool Point::operator==(const Point& rhs) const
-{
-    if ((x == rhs.x) && (y == rhs.y))
-    {
-        return true;
-    }
-    return false;
-}
-
-
-bool Point::operator!=(const Point& rhs) const
-{
-    if ((x != rhs.x) || (y != rhs.y))
-    {
-        return true;
-    }
-    return false;
-}
-
 
 // Returns true iff the point c lies on the closed segment ab.
+// To be used when the points are known to be collinear.
 //
 // Based on the code of 'Between'.
 //
-static const bool inBetween(const Point& a, const Point& b, const Point& c)
+bool inBetween(const Point& a, const Point& b, const Point& c)
 {
     // We only call this when we know the points are collinear,
     // otherwise we should be checking this here.
-    assert(vecDir(a, b, c) == 0);
+    COLA_ASSERT(vecDir(a, b, c, 0.0001) == 0);
 
-    if (a.x != b.x)
+    if ((fabs(a.x - b.x) > 1) && (a.x != b.x))
     {
         // not vertical
         return (((a.x < c.x) && (c.x < b.x)) ||
@@ -95,6 +68,15 @@ static const bool inBetween(const Point& a, const Point& b, const Point& c)
 }
 
 
+// Returns true iff the point c lies on the closed segment ab.
+//
+bool pointOnLine(const Point& a, const Point& b, const Point& c, 
+        const double tolerance)
+{
+    return (vecDir(a, b, c, tolerance) == 0) && inBetween(a, b, c);
+}
+
+
 // Returns true if the segment cd intersects the segment ab, blocking
 // visibility.
 //
@@ -104,15 +86,15 @@ bool segmentIntersect(const Point& a, const Point& b, const Point& c,
         const Point& d)
 {
     int ab_c = vecDir(a, b, c);
-    if ((ab_c == 0) && inBetween(a, b, c))
+    if (ab_c == 0)
     {
-        return true;
+        return false;
     }
 
     int ab_d = vecDir(a, b, d);
-    if ((ab_d == 0) && inBetween(a, b, d))
+    if (ab_d == 0)
     {
-        return true;
+        return false;
     }
 
     // It's ok for either of the points a or b to be on the line cd,
@@ -131,6 +113,37 @@ bool segmentIntersect(const Point& a, const Point& b, const Point& c,
 }
 
 
+// Returns true if the segment e1-e2 intersects the shape boundary 
+// segment s1-s2, blocking visibility.
+//
+bool segmentShapeIntersect(const Point& e1, const Point& e2, const Point& s1,
+        const Point& s2, bool& seenIntersectionAtEndpoint)
+{
+    if (segmentIntersect(e1, e2, s1, s2))
+    {
+        // Basic intersection of segments.
+        return true;
+    }
+    else if ( (((s2 == e1) || pointOnLine(s1, s2, e1)) && 
+               (vecDir(s1, s2, e2) != 0)) 
+              ||
+              (((s2 == e2) || pointOnLine(s1, s2, e2)) &&
+               (vecDir(s1, s2, e1) != 0)) )
+    {
+        // Segments intersect at the endpoint of one of the segments.  We
+        // allow this once, but the second one blocks visibility.  Otherwise
+        // shapes butted up against each other could have visibility through
+        // shapes.
+        if (seenIntersectionAtEndpoint)
+        {
+            return true;
+        }
+        seenIntersectionAtEndpoint = true;
+    }
+    return false;
+}
+
+
 // Returns true iff the point p in a valid region that can contain
 // shortest paths.  a0, a1, a2 are ordered vertices of a shape.
 //
@@ -205,20 +218,9 @@ int cornerSide(const Point &c1, const Point &c2, const Point &c3,
     int s12p = vecDir(c1, c2, p);
     int s23p = vecDir(c2, c3, p);
 
-    if (s12p == 0)
-    {
-        // Case of p being somewhere on c1-c2.
-        return s23p;
-    }
-    if (s23p == 0)
-    {
-        // Case of p being somewhere on c2-c3.
-        return s12p;
-    }
-
     if (s123 == 1)
     {
-        if ((s12p == 1) && (s23p == 1))
+        if ((s12p >= 0) && (s23p >= 0))
         {
             return 1;
         }
@@ -226,18 +228,37 @@ int cornerSide(const Point &c1, const Point &c2, const Point &c3,
     }
     else if (s123 == -1)
     {
-        if ((s12p == -1) && (s23p == -1))
+        if ((s12p <= 0) && (s23p <= 0))
         {
             return -1;
         }
         return 1;
     }
-    // Case of c3 being somewhere on c1-c2.
+
+    // c1-c2-c3 are collinear, so just return vecDir from c1-c2
     return s12p;
 }
 
 
-// Returns the distance between points a and b.
+// Returns the Euclidean distance between points a and b.
+//
+double euclideanDist(const Point& a, const Point& b)
+{
+    double xdiff = a.x - b.x;
+    double ydiff = a.y - b.y;
+
+    return sqrt((xdiff * xdiff) + (ydiff * ydiff));
+}
+
+// Returns the Manhattan distance between points a and b.
+//
+double manhattanDist(const Point& a, const Point& b)
+{
+    return fabs(a.x - b.x) + fabs(a.y - b.y);
+}
+
+
+// Returns the Euclidean distance between points a and b.
 //
 double dist(const Point& a, const Point& b)
 {
@@ -248,11 +269,12 @@ double dist(const Point& a, const Point& b)
 }
 
 // Returns the total length of all line segments in the polygon
-double totalLength(const Polygn& poly)
+double totalLength(const Polygon& poly)
 {
     double l = 0;
-    for (int i = 0; i < poly.pn-1; ++i) {
-        l += dist(poly.ps[i], poly.ps[i+1]);
+    for (size_t i = 1; i < poly.size(); ++i) 
+    {
+        l += dist(poly.ps[i-1], poly.ps[i]);
     }
     return l;
 }
@@ -277,18 +299,27 @@ double angle(const Point& a, const Point& b, const Point& c)
 // This is a fast version that only works for convex shapes.  The
 // other version (inPolyGen) is more general.
 //
-bool inPoly(const Polygn& poly, const Point& q)
+bool inPoly(const Polygon& poly, const Point& q, bool countBorder)
 {
-    int n = poly.pn;
-    Point *P = poly.ps;
-    for (int i = 0; i < n; i++)
+    size_t n = poly.size();
+    const std::vector<Point>& P = poly.ps;
+    bool onBorder = false;
+    for (size_t i = 0; i < n; i++)
     {
         // point index; i1 = i-1 mod n
-        int prev = (i + n - 1) % n;
-        if (vecDir(P[prev], P[i], q) == -1)
+        size_t prev = (i + n - 1) % n;
+        int dir = vecDir(P[prev], P[i], q);
+        if (dir == -1)
         {
+            // Point is outside
             return false;
         }
+        // Record if point was on a boundary.
+        onBorder |= (dir == 0);
+    }
+    if (!countBorder && onBorder)
+    {
+        return false;
     }
     return true;
 }
@@ -299,37 +330,36 @@ bool inPoly(const Polygn& poly, const Point& q)
 //
 // Based on the code of 'InPoly'.
 //
-bool inPolyGen(const Polygn& argpoly, const Point& q)
+bool inPolyGen(const PolygonInterface& argpoly, const Point& q)
 {
     // Numbers of right and left edge/ray crossings.
     int Rcross = 0;
     int Lcross = 0;
 
     // Copy the argument polygon
-    Polygn poly = copyPoly(argpoly);
-    Point *P = poly.ps;
-    int    n = poly.pn;
+    Polygon poly = argpoly;
+    std::vector<Point>& P = poly.ps;
+    size_t    n = poly.size();
 
     // Shift so that q is the origin. This is done for pedogical clarity.
-    for (int i = 0; i < n; ++i)
+    for (size_t i = 0; i < n; ++i)
     {
         P[i].x = P[i].x - q.x;
         P[i].y = P[i].y - q.y;
     }
 
     // For each edge e=(i-1,i), see if crosses ray.
-    for (int i = 0; i < n; ++i)
+    for (size_t i = 0; i < n; ++i)
     {
         // First see if q=(0,0) is a vertex.
         if ((P[i].x == 0) && (P[i].y == 0))
         {
             // We count a vertex as inside.
-            freePoly(poly);
             return true;
         }
 
         // point index; i1 = i-1 mod n
-        int i1 = ( i + n - 1 ) % n;
+        size_t i1 = ( i + n - 1 ) % n;
 
         // if e "straddles" the x-axis...
         // The commented-out statement is logically equivalent to the one
@@ -367,7 +397,6 @@ bool inPolyGen(const Polygn& argpoly, const Point& q)
             }
         }
     }
-    freePoly(poly);
 
     // q on the edge if left and right cross are not the same parity.
     if ( (Rcross % 2) != (Lcross % 2) )
@@ -400,8 +429,7 @@ bool inPolyGen(const Polygn& argpoly, const Point& q)
 int segmentIntersectPoint(const Point& a1, const Point& a2,
         const Point& b1, const Point& b2, double *x, double *y) 
 {
-
-    double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num,offset;
+    double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num;
     double x1lo,x1hi,y1lo,y1hi;
 
     Ax = a2.x - a1.x;
@@ -450,14 +478,13 @@ int segmentIntersectPoint(const Point& a1, const Point& a2,
         if (y1hi < b1.y || b2.y < y1lo) return DONT_INTERSECT;
     }
 
-
     Cx = a1.x - b1.x;
     Cy = a1.y - b1.y;
     // alpha numerator:
     d = By*Cx - Bx*Cy;
     // Both denominator:
     f = Ay*Bx - Ax*By;
-    // aplha tests:
+    // alpha tests:
     if (f > 0)
     {
         if (d < 0 || d > f) return DONT_INTERSECT;
@@ -485,15 +512,49 @@ int segmentIntersectPoint(const Point& a1, const Point& a2,
     
     // Numerator:
     num = d*Ax;
-    // Round direction:
-    offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
     // Intersection X:
-    *x = a1.x + (num+offset) / f;
+    *x = a1.x + (num) / f;
+
+    num = d*Ay;
+    // Intersection Y:
+    *y = a1.y + (num) / f;
+
+    return DO_INTERSECT;
+}
+
+
+// Line Segment Intersection
+// Original code by Franklin Antonio 
+//
+int rayIntersectPoint(const Point& a1, const Point& a2,
+        const Point& b1, const Point& b2, double *x, double *y) 
+{
+    double Ax,Bx,Cx,Ay,By,Cy,d,f,num;
+
+    Ay = a2.y - a1.y;
+    By = b1.y - b2.y;
+    Ax = a2.x - a1.x;
+    Bx = b1.x - b2.x;
+
+    Cx = a1.x - b1.x;
+    Cy = a1.y - b1.y;
+    // alpha numerator:
+    d = By*Cx - Bx*Cy;
+    // Both denominator:
+    f = Ay*Bx - Ax*By;
+
+    // compute intersection coordinates:
+
+    if (f == 0) return PARALLEL;
+    
+    // Numerator:
+    num = d*Ax;
+    // Intersection X:
+    *x = a1.x + (num) / f;
 
     num = d*Ay;
-    offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
     // Intersection Y:
-    *y = a1.y + (num+offset) / f;
+    *y = a1.y + (num) / f;
 
     return DO_INTERSECT;
 }
diff --git a/src/libavoid/geometry.h b/src/libavoid/geometry.h
index 1422be050..abd0d60e2 100644
--- a/src/libavoid/geometry.h
+++ b/src/libavoid/geometry.h
@@ -2,7 +2,8 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * --------------------------------------------------------------------
  * Much of the code in this module is based on code published with
@@ -18,16 +19,17 @@
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
 
@@ -35,21 +37,30 @@
 #define _GEOMETRY_H
 
 #include "libavoid/geomtypes.h"
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
 
-extern double dist(const Point& a, const Point& b);
-extern double totalLength(const Polygn& poly);
+extern double euclideanDist(const Point& a, const Point& b);
+extern double manhattanDist(const Point& a, const Point& b);
+extern double totalLength(const Polygon& poly);
 extern double angle(const Point& a, const Point& b, const Point& c);
 extern bool segmentIntersect(const Point& a, const Point& b,
         const Point& c, const Point& d);
-extern bool inPoly(const Polygn& poly, const Point& q);
-extern bool inPolyGen(const Polygn& poly, const Point& q);
+extern bool segmentShapeIntersect(const Point& e1, const Point& e2, 
+        const Point& s1, const Point& s2, bool& seenIntersectionAtEndpoint);
+extern bool inPoly(const Polygon& poly, const Point& q, bool countBorder = true);
+extern bool inPolyGen(const PolygonInterface& poly, const Point& q);
 extern bool inValidRegion(bool IgnoreRegions, const Point& a0,
         const Point& a1, const Point& a2, const Point& b);
 extern int cornerSide(const Point &c1, const Point &c2, const Point &c3,
         const Point& p);
+extern bool pointOnLine(const Point& a, const Point& b, const Point& c,
+        const double tolerance = 0.0);
+
+// To be used only when the points are known to be colinear.
+extern bool inBetween(const Point& a, const Point& b, const Point& c);
 
 
 // Direction from vector.
@@ -61,16 +72,22 @@ extern int cornerSide(const Point &c1, const Point &c2, const Point &c3,
 //
 // Based on the code of 'AreaSign'.
 //
-static inline int vecDir(const Point& a, const Point& b, const Point& c)
+// The 'maybeZero' argument can be used to adjust the tolerance of the 
+// function.  It will be most accurate when 'maybeZero' == 0.0, the default.
+//
+static inline int vecDir(const Point& a, const Point& b, const Point& c, 
+        const double maybeZero = 0.0)
 {
+    COLA_ASSERT(maybeZero >= 0);
+
     double area2 = ((b.x - a.x) * (c.y - a.y)) -
                    ((c.x - a.x) * (b.y - a.y));
     
-    if (area2 < -0.001)
+    if (area2 < (-maybeZero))
     {
         return -1;
     }
-    else if (area2 > 0.001)
+    else if (area2 > maybeZero)
     {
         return 1;
     }
@@ -100,6 +117,8 @@ static const int DO_INTERSECT = 1;
 static const int PARALLEL = 3;
 extern int segmentIntersectPoint(const Point& a1, const Point& a2,
         const Point& b1, const Point& b2, double *x, double *y);
+extern int rayIntersectPoint(const Point& a1, const Point& a2,
+        const Point& b1, const Point& b2, double *x, double *y);
 
 
 }
diff --git a/src/libavoid/geomtypes.h b/src/libavoid/geomtypes.h
index dd9d26f2f..ced53e6b0 100644
--- a/src/libavoid/geomtypes.h
+++ b/src/libavoid/geomtypes.h
@@ -2,70 +2,314 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file  geomtypes.h
+//! @brief Contains the interface for various geometry types and classes.
+
 
 #ifndef AVOID_GEOMTYPES_H
 #define AVOID_GEOMTYPES_H
 
+#include <vector>
+#include <utility>
+
 
 namespace Avoid
 {
 
-    
+
+//! @brief  The Point class defines a point in the plane.
+//!
+//! Points consist of an x and y value.  They may also have an ID and vertex
+//! number associated with them.
+//!
 class Point
 {
     public:
+        //! @brief  Default constructor.
+        //!
         Point();
+        //! @brief  Standard constructor.
+        //!
+        //! @param[in]  xv  The x position of the point.
+        //! @param[in]  yv  The y position of the point.
+        //!
         Point(const double xv, const double yv);
+
+        //! @brief  Comparison operator. Returns true if at same position.
+        //!
+        //! @param[in]  rhs  The point to compare with this one.
+        //! @return          The result of the comparison.
+        //!
         bool operator==(const Point& rhs) const;
+        //! @brief  Comparison operator. Returns true if at different positions.
+        //!
+        //! @param[in]  rhs  The point to compare with this one.
+        //! @return          The result of the comparison.
+        //!
         bool operator!=(const Point& rhs) const;
+        //! @brief  Comparison operator. Returns true if less-then rhs point.
+        //!
+        //! @note  This operator is not particularly useful, but is defined 
+        //!        to allow std::set<Point>.
+        //!
+        //! @param[in]  rhs  The point to compare with this one.
+        //! @return          The result of the comparison.
+        //!
+        bool operator<(const Point& rhs) const;
 
+        //! @brief  Returns the x or y value of the point, given the dimension.
+        //!
+        //! @param[in]  dimension  The dimension:  0 for x, 1 for y.
+        //! @return                The component of the point in that dimension.
+        double& operator[](const unsigned int dimension);
+        const double& operator[](const unsigned int dimension) const;
+        
+        //! The x position.
         double x;
+        //! The y position.
         double y;
-        int id;
+        //! The ID associated with this point.
+        unsigned int id;
+        //! The vertex number associated with this point.
+        unsigned short vn;
 
 };
 
 
+//! Constant value representing an unassigned vertex number.
+//!
+static const unsigned short kUnassignedVertexNumber = 8;
+
+
+//! @brief  A vector, represented by the Point class.
+//!
 typedef Point Vector;
 
 
-typedef struct
+//! @brief  A common interface used by the Polygon classes.
+//!
+class PolygonInterface
 {
-    int id;
-    Point *ps;
-    int pn;
-} Polygn;
-
-typedef Polygn PolyLine;
+    public:
+        //! @brief  Constructor.
+        PolygonInterface() { }
+        //! @brief  Destructor.
+        virtual ~PolygonInterface() { }
+        //! @brief  Resets this to the empty polygon.
+        virtual void clear(void) = 0;
+        //! @brief  Returns true if this polygon is empty.
+        virtual bool empty(void) const = 0;
+        //! @brief  Returns the number of points in this polygon.
+        virtual size_t size(void) const = 0;
+        //! @brief  Returns the ID value associated with this polygon.
+        virtual int id(void) const = 0;
+        //! @brief  Returns a specific point in the polygon.
+        //! @param[in]  index  The array index of the point to be returned.
+        virtual const Point& at(size_t index) const = 0;
+        //! @brief  Returns the bounding rectangle that contains this polygon.
+        //!
+        //! If a NULL pointer is passed for any of the arguments, then that
+        //! value is ignored and not returned.
+        //!
+        //! @param[out]  minX  The left hand side of the bounding box.
+        //! @param[out]  minY  The top of the bounding box.
+        //! @param[out]  maxX  The right hand side of the bounding box.
+        //! @param[out]  maxY  The bottom of the bounding box.
+        void getBoundingRect(double *minX, double *minY,
+                double *maxX, double *maxY) const;
+};
 
 
-typedef struct
+//! @brief  A line between two points. 
+//!
+class Edge
 {
-    Point a;
-    Point b;
-} Edge;
+    public:
+        //! The first point.
+        Point a;
+        //! The second point.
+        Point b;
+};
+
 
+//! @brief  A bounding box, represented with an Edge between top-left and
+//!         bottom-right corners.
+//!
 typedef Edge BBox;
 
 
+class Router;
+class ReferencingPolygon;
+
+
+//! @brief  A dynamic Polygon, to which points can be easily added and removed.
+//!
+//! @note The Rectangle class can be used as an easy way of constructing a
+//!       square or rectangular polygon.
+//!
+class Polygon : public PolygonInterface
+{
+    public:
+        //! @brief  Constructs an empty polygon (with zero points). 
+        Polygon();
+        //! @brief  Constructs a new polygon with n points.
+        //! 
+        //! A rectangle would be comprised of four point.  An n segment 
+        //! PolyLine (represented as a Polygon) would be comprised of n+1
+        //! points.  Whether a particular Polygon is closed or not, depends
+        //! on whether it is a Polygon or Polyline.  Shape polygons are always
+        //! considered to be closed, meaning the last point joins back to the
+        //! first point.
+        //!
+        //! @param[in]  n  Number of points in the polygon.
+        //!
+        Polygon(const int n);
+        //! @brief  Constructs a new polygon from an existing Polygon.
+        //!
+        //! @param[in]  poly  An existing polygon to copy the new polygon from.
+        //!
+        Polygon(const PolygonInterface& poly);
+        //! @brief  Resets this to the empty polygon.
+        void clear(void);
+        //! @brief  Returns true if this polygon is empty.
+        bool empty(void) const;
+        //! @brief  Returns the number of points in this polygon.
+        size_t size(void) const;
+        //! @brief  Returns the ID value associated with this polygon.
+        int id(void) const;
+        //! @brief  Returns a specific point in the polygon.
+        //! @param[in]  index  The array index of the point to be returned.
+        const Point& at(size_t index) const;
+        //! @brief  Returns a simplified Polyline, where all collinear line
+        //!         segments have been collapsed down into single line 
+        //!         segments.
+        //!
+        //! @return A new polyline with a simplified representation.
+        //!
+        Polygon simplify(void) const;
+        //! @brief  Returns a curved approximation of this multi-segment 
+        //!         PolyLine, with the corners replaced by smooth Bezier 
+        //!         curves.
+        //!
+        //! This function does not do any further obstacle avoidance with the
+        //! curves produced.  Hence, you would usually specify a curve_amount
+        //! in similar size to the space buffer around obstacles in the scene.
+        //! This way the curves will cut the corners around shapes but still
+        //! run within this buffer space.
+        //!
+        //! @param  curve_amount  Describes the distance along the end of each 
+        //!                       line segment to turn into a curve.
+        //! @param  closed        Describes whether the Polygon should be 
+        //!                       treated as closed.  Defaults to false.
+        //! @return A new polyline (polygon) representing the curved path.
+        //!         Its points represent endpoints of line segments and 
+        //!         Bezier spline control points.  The Polygon::ts vector for
+        //!         this returned polygon is populated with a character for 
+        //!         each point describing its type.
+        //! @sa     ts
+        Polygon curvedPolyline(const double curve_amount, 
+                const bool closed = false) const;
+        //! @brief  Translates the polygon position by a relative amount.
+        //!
+        //! @param[in]  xDist  Distance to move polygon in the x dimension.
+        //! @param[in]  yDist  Distance to move polygon in the y dimension.
+        void translate(const double xDist, const double yDist);
+
+        //! @brief  An ID for the polygon.
+        int _id;
+        //! @brief  A vector of the points that make up the Polygon.
+        std::vector<Point> ps;
+        //! @brief  If used, denotes whether the corresponding point in ps is 
+        //!         a move-to operation or a Bezier curve-to.
+        //! 
+        //! Each character describes the drawing operation for the 
+        //! corresponding point in the ps vector.  Possible values are:
+        //!  -  'M': A moveto operation, marks the first point;
+        //!  -  'L': A lineto operation, is a line from the previous point to
+        //!     the current point; or
+        //!  -  'C': A curveto operation, three consecutive 'C' points 
+        //!     (along with the previous point) describe the control points 
+        //!     of a Bezier curve.
+        //!  -  'Z': Closes the path (used for cluster boundaries).
+        //!
+        //! @note   This vector will currently only be populated for polygons 
+        //!         returned by curvedPolyline().  
+        std::vector<char> ts;
+};
+
+
+//! @brief  A multi-segment line, represented with the Polygon class.
+//!
+typedef Polygon PolyLine;
+
+
+//! @brief  A Polygon which just references its points from other Polygons.
+//!          
+//! This type of Polygon is used to accurately represent cluster boundaries 
+//! made up from the corner points of shapes.
+//!
+class ReferencingPolygon : public PolygonInterface
+{
+    public:
+        ReferencingPolygon();
+        ReferencingPolygon(const Polygon& poly, const Router *router);
+        void clear(void);
+        bool empty(void) const;
+        size_t size(void) const;
+        int id(void) const;
+        const Point& at(size_t index) const;
+
+        int _id;
+        std::vector<std::pair<const Polygon *, unsigned short> > ps;
+};
+
+
+//! @brief  A Rectangle, a simpler way to define the polygon for square or
+//!         rectangular shapes.
+//!
+class Rectangle : public Polygon
+{
+    public:
+        //! @brief  Constructs a rectangular polygon given two opposing 
+        //!         corner points.
+        //!
+        //! @param[in]  topLeft      The first corner point of the rectangle.
+        //! @param[in]  bottomRight  The opposing corner point of the rectangle.
+        //!
+        Rectangle(const Point& topLeft, const Point& bottomRight);
+        
+        //! @brief  Constructs a rectangular polygon given the centre, width
+        //!         and height.
+        //!
+        //! @param[in]  centre  The centre of the rectangle, specified as 
+        //!                     a point.
+        //! @param[in]  width   The width of the rectangle.
+        //! @param[in]  height  The height of the rectangle.
+        //!
+        Rectangle(const Point& centre, const double width, const double height);
+};
+
+
 }
 
 #endif
diff --git a/src/libavoid/graph.cpp b/src/libavoid/graph.cpp
index 1970212df..728f8c085 100644
--- a/src/libavoid/graph.cpp
+++ b/src/libavoid/graph.cpp
@@ -2,56 +2,61 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
+#include <cmath>
+
 #include "libavoid/debug.h"
 #include "libavoid/graph.h"
 #include "libavoid/connector.h"
 #include "libavoid/geometry.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/timer.h"
 #include "libavoid/vertices.h"
 #include "libavoid/router.h"
+#include "libavoid/assertions.h"
 
-#include <math.h>
 
 using std::pair;
 
 namespace Avoid {
 
 
-EdgeInf::EdgeInf(VertInf *v1, VertInf *v2)
-    : lstPrev(NULL)
-    , lstNext(NULL)
-    , _blocker(0)
-    , _router(NULL)
-    , _added(false)
-    , _visible(false)
-    , _v1(v1)
-    , _v2(v2)
-    , _dist(-1)
+EdgeInf::EdgeInf(VertInf *v1, VertInf *v2, const bool orthogonal)
+    : lstPrev(NULL),
+      lstNext(NULL),
+      _blocker(0),
+      _router(NULL),
+      _added(false),
+      _visible(false),
+      _orthogonal(orthogonal),
+      _v1(v1),
+      _v2(v2),
+      _dist(-1)
 {
     // Not passed NULL values.
-    assert(v1 && v2);
+    COLA_ASSERT(v1 && v2);
 
     // We are in the same instance
-    assert(_v1->_router == _v2->_router);
+    COLA_ASSERT(_v1->_router == _v2->_router);
     _router = _v1->_router;
 
     _conns.clear();
@@ -67,25 +72,142 @@ EdgeInf::~EdgeInf()
 }
 
 
+// Gives an order value between 0 and 3 for the point c, given the last
+// segment was from a to b.  Returns the following value:
+//    0 : Point c is directly backwards from point b.
+//    1 : Point c is a left-hand 90 degree turn.
+//    2 : Point c is a right-hand 90 degree turn.
+//    3 : Point c is straight ahead (collinear).
+//
+static inline int orthogTurnOrder(const Point& a, const Point& b, 
+        const Point& c)
+{
+    // We should only be calling this with orthogonal points, 
+    COLA_ASSERT((c.x == b.x) || (c.y == b.y));
+    COLA_ASSERT((a.x == b.x) || (a.y == b.y));
+
+    int direction = vecDir(a, b, c);
+
+    if (direction > 0)
+    {
+        // Counterclockwise := left
+        return 1;
+    }
+    else if (direction < 0)
+    {
+        // Clockwise := right
+        return 2;
+    }
+
+    if (b.x == c.x)
+    {
+        if ( ((a.y < b.y) && (c.y < b.y)) || 
+             ((a.y > b.y) && (c.y > b.y)) ) 
+        {
+            // Behind.
+            return 0;
+        }
+    }
+    else
+    {
+        if ( ((a.x < b.x) && (c.x < b.x)) || 
+             ((a.x > b.x) && (c.x > b.x)) ) 
+        {
+            // Behind.
+            return 0;
+        }
+    }
+
+    // Ahead.
+    return 3;
+}
+
+
+// Returns a less than operation for a set exploration order for orthogonal
+// searching.  Forward, then left, then right.  Or if there is no previous 
+// point, then the order is north, east, south, then west.
+// Note: This method assumes the two Edges that share a common point.
+bool EdgeInf::rotationLessThan(const VertInf *lastV, const EdgeInf *rhs) const
+{
+    if ((_v1 == rhs->_v1) && (_v2 == rhs->_v2))
+    {
+        // Effectively the same visibility edge, so they are equal.
+        return false;
+    }
+    VertInf *lhsV = NULL, *rhsV = NULL, *commonV = NULL;
+    
+    // Determine common Point and the comparison point on the left- and
+    // the right-hand-side.
+    if (_v1 == rhs->_v1)
+    {
+        commonV = _v1;
+        lhsV = _v2;
+        rhsV = rhs->_v2;
+    }
+    else if (_v1 == rhs->_v2)
+    {
+        commonV = _v1;
+        lhsV = _v2;
+        rhsV = rhs->_v1;
+    }
+    else if (_v2 == rhs->_v1)
+    {
+        commonV = _v2;
+        lhsV = _v1;
+        rhsV = rhs->_v2;
+    }
+    else if (_v2 == rhs->_v2)
+    {
+        commonV = _v2;
+        lhsV = _v1;
+        rhsV = rhs->_v1;
+    }
+
+    const Point& lhsPt = lhsV->point;
+    const Point& rhsPt = rhsV->point;
+    const Point& commonPt = commonV->point;
+    
+    // If no lastPt, use one directly to the left;
+    Point lastPt = (lastV) ? lastV->point : Point(commonPt.x - 10,  commonPt.y);
+
+    int lhsVal = orthogTurnOrder(lastPt, commonPt, lhsPt);
+    int rhsVal = orthogTurnOrder(lastPt, commonPt, rhsPt);
+
+    return lhsVal < rhsVal;
+}
+
+
 void EdgeInf::makeActive(void)
 {
-    assert(_added == false);
+    COLA_ASSERT(_added == false);
 
-    if (_visible)
+    if (_orthogonal)
     {
-        _router->visGraph.addEdge(this);
-        _pos1 = _v1->visList.insert(_v1->visList.begin(), this);
-        _v1->visListSize++;
-        _pos2 = _v2->visList.insert(_v2->visList.begin(), this);
-        _v2->visListSize++;
+        COLA_ASSERT(_visible);
+        _router->visOrthogGraph.addEdge(this);
+        _pos1 = _v1->orthogVisList.insert(_v1->orthogVisList.begin(), this);
+        _v1->orthogVisListSize++;
+        _pos2 = _v2->orthogVisList.insert(_v2->orthogVisList.begin(), this);
+        _v2->orthogVisListSize++;
     }
-    else // if (invisible)
+    else
     {
-        _router->invisGraph.addEdge(this);
-        _pos1 = _v1->invisList.insert(_v1->invisList.begin(), this);
-        _v1->invisListSize++;
-        _pos2 = _v2->invisList.insert(_v2->invisList.begin(), this);
-        _v2->invisListSize++;
+        if (_visible)
+        {
+            _router->visGraph.addEdge(this);
+            _pos1 = _v1->visList.insert(_v1->visList.begin(), this);
+            _v1->visListSize++;
+            _pos2 = _v2->visList.insert(_v2->visList.begin(), this);
+            _v2->visListSize++;
+        }
+        else // if (invisible)
+        {
+            _router->invisGraph.addEdge(this);
+            _pos1 = _v1->invisList.insert(_v1->invisList.begin(), this);
+            _v1->invisListSize++;
+            _pos2 = _v2->invisList.insert(_v2->invisList.begin(), this);
+            _v2->invisListSize++;
+        }
     }
     _added = true;
 }
@@ -93,23 +215,35 @@ void EdgeInf::makeActive(void)
 
 void EdgeInf::makeInactive(void)
 {
-    assert(_added == true);
+    COLA_ASSERT(_added == true);
 
-    if (_visible)
+    if (_orthogonal)
     {
-        _router->visGraph.removeEdge(this);
-        _v1->visList.erase(_pos1);
-        _v1->visListSize--;
-        _v2->visList.erase(_pos2);
-        _v2->visListSize--;
+        COLA_ASSERT(_visible);
+        _router->visOrthogGraph.removeEdge(this);
+        _v1->orthogVisList.erase(_pos1);
+        _v1->orthogVisListSize--;
+        _v2->orthogVisList.erase(_pos2);
+        _v2->orthogVisListSize--;
     }
-    else // if (invisible)
+    else
     {
-        _router->invisGraph.removeEdge(this);
-        _v1->invisList.erase(_pos1);
-        _v1->invisListSize--;
-        _v2->invisList.erase(_pos2);
-        _v2->invisListSize--;
+        if (_visible)
+        {
+            _router->visGraph.removeEdge(this);
+            _v1->visList.erase(_pos1);
+            _v1->visListSize--;
+            _v2->visList.erase(_pos2);
+            _v2->visListSize--;
+        }
+        else // if (invisible)
+        {
+            _router->invisGraph.removeEdge(this);
+            _v1->invisList.erase(_pos1);
+            _v1->invisListSize--;
+            _v2->invisList.erase(_pos2);
+            _v2->invisListSize--;
+        }
     }
     _blocker = 0;
     _conns.clear();
@@ -119,11 +253,12 @@ void EdgeInf::makeInactive(void)
 
 void EdgeInf::setDist(double dist)
 {
-    //assert(dist != 0);
+    //COLA_ASSERT(dist != 0);
 
     if (_added && !_visible)
     {
         makeInactive();
+        COLA_ASSERT(!_added);
     }
     if (!_added)
     {
@@ -135,6 +270,12 @@ void EdgeInf::setDist(double dist)
 }
 
 
+bool EdgeInf::added(void)
+{
+    return _added;
+}
+
+
 void EdgeInf::alertConns(void)
 {
     FlagList::iterator finish = _conns.end();
@@ -161,11 +302,12 @@ void EdgeInf::addCycleBlocker(void)
 
 void EdgeInf::addBlocker(int b)
 {
-    assert(_router->InvisibilityGrph);
+    COLA_ASSERT(_router->InvisibilityGrph);
 
     if (_added && _visible)
     {
         makeInactive();
+        COLA_ASSERT(!_added);
     }
     if (!_added)
     {
@@ -232,8 +374,11 @@ void EdgeInf::checkVis(void)
         cone1 = inValidRegion(_router->IgnoreRegions, i->shPrev->point,
                 iPoint, i->shNext->point, jPoint);
     }
-    else
+    else if (_router->IgnoreRegions == false)
     {
+        // If Ignoring regions then this case is already caught by 
+        // the invalid regions, so only check it when not ignoring
+        // regions.
         ShapeSet& ss = _router->contains[iID];
 
         if ((jID.isShape) && (ss.find(jID.objID) != ss.end()))
@@ -253,8 +398,11 @@ void EdgeInf::checkVis(void)
             cone2 = inValidRegion(_router->IgnoreRegions, j->shPrev->point,
                     jPoint, j->shNext->point, iPoint);
         }
-        else
+        else if (_router->IgnoreRegions == false)
         {
+            // If Ignoring regions then this case is already caught by 
+            // the invalid regions, so only check it when not ignoring
+            // regions.
             ShapeSet& ss = _router->contains[jID];
 
             if ((iID.isShape) && (ss.find(iID.objID) != ss.end()))
@@ -274,7 +422,7 @@ void EdgeInf::checkVis(void)
         db_printf("\tSetting visibility edge... \n\t\t");
         db_print();
 
-        double d = dist(iPoint, jPoint);
+        double d = euclideanDist(iPoint, jPoint);
 
         setDist(d);
 
@@ -316,26 +464,39 @@ int EdgeInf::firstBlocker(void)
     }
 
     VertInf *last = _router->vertices.end();
+    unsigned int lastId = 0;
+    bool seenIntersectionAtEndpoint = false;
     for (VertInf *k = _router->vertices.shapesBegin(); k != last; )
     {
         VertID kID = k->id;
-        if ((ss.find(kID.objID) != ss.end()))
+        if (k->id == dummyOrthogID)
+        {
+            // Don't include orthogonal dummy vertices.
+            k = k->lstNext;
+            continue;
+        }
+        if (kID.objID != lastId)
         {
-            unsigned int shapeID = kID.objID;
-            db_printf("Endpoint is inside shape %u so ignore shape edges.\n",
-                    kID.objID);
-            // One of the endpoints is inside this shape so ignore it.
-            while ((k != last) && (k->id.objID == shapeID))
+            if ((ss.find(kID.objID) != ss.end()))
             {
-                // And skip the other vertices from this shape.
-                k = k->lstNext;
+                unsigned int shapeID = kID.objID;
+                db_printf("Endpoint is inside shape %u so ignore shape "
+                        "edges.\n", kID.objID);
+                // One of the endpoints is inside this shape so ignore it.
+                while ((k != last) && (k->id.objID == shapeID))
+                {
+                    // And skip the other vertices from this shape.
+                    k = k->lstNext;
+                }
+                continue;
             }
-            continue;
+            seenIntersectionAtEndpoint = false;
+            lastId = kID.objID;
         }
         Point& kPoint = k->point;
         Point& kPrevPoint = k->shPrev->point;
-
-        if (segmentIntersect(pti, ptj, kPrevPoint, kPoint))
+        if (segmentShapeIntersect(pti, ptj, kPrevPoint, kPoint, 
+                    seenIntersectionAtEndpoint))
         {
             ss.clear();
             return kID.objID;
@@ -358,9 +519,17 @@ bool EdgeInf::isBetween(VertInf *i, VertInf *j)
 }
 
 
+    // Returns true if this edge is a vertical or horizontal line segment.
+bool EdgeInf::isOrthogonal(void) const
+{
+    return ((_v1->point.x == _v2->point.x) || 
+            (_v1->point.y == _v2->point.y));
+}
+
+
 VertInf *EdgeInf::otherVert(VertInf *vert)
 {
-    assert((vert == _v1) || (vert == _v2));
+    COLA_ASSERT((vert == _v1) || (vert == _v2));
 
     if (vert == _v1)
     {
@@ -372,12 +541,17 @@ VertInf *EdgeInf::otherVert(VertInf *vert)
 
 EdgeInf *EdgeInf::checkEdgeVisibility(VertInf *i, VertInf *j, bool knownNew)
 {
+    // This is for polyline routing, so check we're not 
+    // considering orthogonal vertices.
+    COLA_ASSERT(i->id != dummyOrthogID);
+    COLA_ASSERT(j->id != dummyOrthogID);
+    
     Router *router = i->_router;
     EdgeInf *edge = NULL;
 
     if (knownNew)
     {
-        assert(existingEdge(i, j) == NULL);
+        COLA_ASSERT(existingEdge(i, j) == NULL);
         edge = new EdgeInf(i, j);
     }
     else
@@ -399,22 +573,17 @@ EdgeInf *EdgeInf::checkEdgeVisibility(VertInf *i, VertInf *j, bool knownNew)
 }
 
 
+    // XXX: This function is ineffecient, and shouldn't even really be
+    //      required.
 EdgeInf *EdgeInf::existingEdge(VertInf *i, VertInf *j)
 {
     VertInf *selected = NULL;
 
-    if (i->visListSize <= j->visListSize)
-    {
-        selected = i;
-    }
-    else
-    {
-        selected = j;
-    }
-
+    // Look through poly-line visibility edges.
+    selected = (i->visListSize <= j->visListSize) ? i : j;
     EdgeInfList& visList = selected->visList;
-    EdgeInfList::iterator finish = visList.end();
-    for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
+    EdgeInfList::const_iterator finish = visList.end();
+    for (EdgeInfList::const_iterator edge = visList.begin(); edge != finish;
             ++edge)
     {
         if ((*edge)->isBetween(i, j))
@@ -423,18 +592,24 @@ EdgeInf *EdgeInf::existingEdge(VertInf *i, VertInf *j)
         }
     }
 
-    if (i->invisListSize <= j->invisListSize)
-    {
-        selected = i;
-    }
-    else
+    // Look through orthogonal visbility edges.
+    selected = (i->orthogVisListSize <= j->orthogVisListSize) ? i : j;
+    EdgeInfList& orthogVisList = selected->orthogVisList;
+    finish = orthogVisList.end();
+    for (EdgeInfList::const_iterator edge = orthogVisList.begin(); 
+            edge != finish; ++edge)
     {
-        selected = j;
+        if ((*edge)->isBetween(i, j))
+        {
+            return (*edge);
+        }
     }
 
+    // Look through poly-line invisbility edges.
+    selected = (i->invisListSize <= j->invisListSize) ? i : j;
     EdgeInfList& invisList = selected->invisList;
     finish = invisList.end();
-    for (EdgeInfList::iterator edge = invisList.begin(); edge != finish;
+    for (EdgeInfList::const_iterator edge = invisList.begin(); edge != finish;
             ++edge)
     {
         if ((*edge)->isBetween(i, j))
@@ -450,19 +625,45 @@ EdgeInf *EdgeInf::existingEdge(VertInf *i, VertInf *j)
 //===========================================================================
 
 
-EdgeList::EdgeList()
-    : _firstEdge(NULL)
-    , _lastEdge(NULL)
-    , _count(0)
+EdgeList::EdgeList(bool orthogonal)
+    : _orthogonal(orthogonal),
+      _firstEdge(NULL),
+      _lastEdge(NULL),
+      _count(0)
+{
+}
+
+
+EdgeList::~EdgeList()
+{
+    clear();
+}
+
+
+void EdgeList::clear(void)
+{
+    while (_firstEdge)
+    {
+        delete _firstEdge;
+    }
+    COLA_ASSERT(_count == 0);
+    _lastEdge = NULL;
+}
+
+
+int EdgeList::size(void) const
 {
+    return _count;
 }
 
 
 void EdgeList::addEdge(EdgeInf *edge)
 {
+    COLA_ASSERT(!_orthogonal || edge->isOrthogonal());
+    
     if (_firstEdge == NULL)
     {
-        assert(_lastEdge == NULL);
+        COLA_ASSERT(_lastEdge == NULL);
 
         _lastEdge = edge;
         _firstEdge = edge;
@@ -472,7 +673,7 @@ void EdgeList::addEdge(EdgeInf *edge)
     }
     else
     {
-        assert(_lastEdge != NULL);
+        COLA_ASSERT(_lastEdge != NULL);
 
         _lastEdge->lstNext = edge;
         edge->lstPrev = _lastEdge;
diff --git a/src/libavoid/graph.h b/src/libavoid/graph.h
index 05f03a988..db776b80b 100644
--- a/src/libavoid/graph.h
+++ b/src/libavoid/graph.h
@@ -2,24 +2,27 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_GRAPH_H
 #define AVOID_GRAPH_H
 
@@ -31,6 +34,7 @@
 
 namespace Avoid {
 
+
 class ConnRef;
 class Router;
 
@@ -42,8 +46,8 @@ typedef std::list<bool *> FlagList;
 class EdgeInf
 {
     public:
-        EdgeInf(VertInf *v1, VertInf *v2);
-        virtual ~EdgeInf();
+        EdgeInf(VertInf *v1, VertInf *v2, const bool orthogonal = false);
+        ~EdgeInf();
         inline double getDist(void)
         {
             return _dist;
@@ -53,6 +57,9 @@ class EdgeInf
         void addConn(bool *flag);
         void addCycleBlocker(void);
         void addBlocker(int b);
+        bool added(void);
+        bool isOrthogonal(void) const;
+        bool rotationLessThan(const VertInf* last, const EdgeInf *rhs) const;
 
         std::pair<VertID, VertID> ids(void);
         std::pair<Point, Point> points(void);
@@ -70,6 +77,7 @@ class EdgeInf
         Router *_router;
         bool _added;
         bool _visible;
+        bool _orthogonal;
         VertInf *_v1;
         VertInf *_v2;
         EdgeInfList::iterator _pos1;
@@ -87,12 +95,17 @@ class EdgeInf
 class EdgeList
 {
     public:
-        EdgeList();
-        void addEdge(EdgeInf *edge);
-        void removeEdge(EdgeInf *edge);
+        friend class EdgeInf;
+        EdgeList(bool orthogonal = false);
+        ~EdgeList();
+        void clear(void);
         EdgeInf *begin(void);
         EdgeInf *end(void);
+        int size(void) const;
     private:
+        void addEdge(EdgeInf *edge);
+        void removeEdge(EdgeInf *edge);
+        bool _orthogonal;
         EdgeInf *_firstEdge;
         EdgeInf *_lastEdge;
         unsigned int _count;
diff --git a/src/libavoid/libavoid.h b/src/libavoid/libavoid.h
index d598c6c74..1d4cd1fdc 100644
--- a/src/libavoid/libavoid.h
+++ b/src/libavoid/libavoid.h
@@ -2,29 +2,37 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file  libavoid.h
+//! @brief Standard libavoid include file which includes all libavoid 
+//!        header files.
+
+//! @namespace Avoid
+//! @brief The namespace used by code in the libavoid library.
+
 #ifndef AVOID_LIBAVOID_H
 #define AVOID_LIBAVOID_H
 
 #include "libavoid/geomtypes.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/connector.h"
 #include "libavoid/graph.h"
 #include "libavoid/debug.h"
@@ -32,7 +40,6 @@
 #include "libavoid/makepath.h"
 #include "libavoid/vertices.h"
 #include "libavoid/visibility.h"
-#include "libavoid/static.h"
 #include "libavoid/router.h"
 
 #endif
diff --git a/src/libavoid/makepath.cpp b/src/libavoid/makepath.cpp
index 3a57f8e4e..4e15dbca9 100644
--- a/src/libavoid/makepath.cpp
+++ b/src/libavoid/makepath.cpp
@@ -2,43 +2,105 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
  *
- * --------------------------------------------------------------------
- * The dijkstraPath function is based on code published and described
- * in "Algorithms in C" (Second Edition), 1990, by Robert Sedgewick.
- * --------------------------------------------------------------------
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
+#include <algorithm>
+#include <vector>
+#include <climits>
+#define _USE_MATH_DEFINES
+#include <cmath>
+
 #include "libavoid/vertices.h"
 #include "libavoid/makepath.h"
 #include "libavoid/geometry.h"
 #include "libavoid/connector.h"
 #include "libavoid/graph.h"
 #include "libavoid/router.h"
-#include <algorithm>
-#include <vector>
-#include <climits>
-#include <limits.h>
-#include <math.h>
+#include "libavoid/debug.h"
+#include "libavoid/assertions.h"
+#ifdef ASTAR_DEBUG
+  #include <SDL_gfxPrimitives.h>
+#endif
 
 namespace Avoid {
 
+class ANode
+{
+    public:
+        VertInf* inf;
+        double g;        // Gone
+        double h;        // Heuristic
+        double f;        // Formula f = g + h
+        
+        int prevIndex;   // Index into DONE for the previous ANode.
+        int timeStamp;   // Timestamp used to determine explaration order of
+                         // seemingly equal paths during orthogonal routing.
+
+        ANode(VertInf *vinf, int time)
+            : inf(vinf),
+              g(0),
+              h(0),
+              f(0),
+              prevIndex(-1),
+              timeStamp(time)
+        {
+        }
+        ANode()
+            : inf(NULL),
+              g(0),
+              h(0),
+              f(0),
+              prevIndex(-1),
+              timeStamp(-1)
+        {
+        }
+};
+
+
+// This returns the opposite result (>) so that when used with stl::make_heap, 
+// the head node of the heap will be the smallest value, rather than the 
+// largest.  This saves us from having to sort the heap (and then reorder
+// it back into a heap) when getting the next node to examine.  This way we
+// get better complexity -- logarithmic pushs and pops to the heap.
+//
+bool operator<(const ANode &a, const ANode &b)
+{
+    if (a.f != b.f)
+    {
+        return a.f > b.f;
+    }
+    if (a.timeStamp != b.timeStamp)
+    {
+        // Tiebreaker, if two paths have equal cost, then choose the one with
+        // the highest timeStamp.  This corresponds to the furthest point
+        // explored along the straight-line path.  When exploring we give the
+        // directions the following timeStamps; left:1, right:2 and forward:3,
+        // then we always try to explore forward first.
+        return a.timeStamp < b.timeStamp;
+    }
+    COLA_ASSERT(a.prevIndex != b.prevIndex);
+    return a.prevIndex > b.prevIndex;
+}
+
 
 static double Dot(const Point& l, const Point& r)
 {
@@ -56,6 +118,13 @@ static double CrossLength(const Point& l, const Point& r)
 //
 static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
 {
+    if ((p1.x == p2.x && p1.y == p2.y) || (p2.x == p3.x && p2.y == p3.y))
+    {
+        // If two of the points are the same, then we can't say anything
+        // about the angle between.  Treat them as being collinear.
+        return M_PI;
+    }
+
     Point v1(p1.x - p2.x, p1.y - p2.y);
     Point v2(p3.x - p2.x, p3.y - p2.y);
 
@@ -63,21 +132,45 @@ static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
 }
 
 
+// Construct a temporary Polygon path given several VertInf's for a connector.
+//
+static void constructPolygonPath(Polygon& connRoute, VertInf *inf2, 
+        VertInf *inf3, std::vector<ANode>& done, int inf1Index)
+{
+    int routeSize = 2;
+    for (int curr = inf1Index; curr >= 0; curr = done[curr].prevIndex)
+    {
+        routeSize += 1;
+    }
+    connRoute.ps.resize(routeSize);
+    connRoute.ps[routeSize - 1] = inf3->point;
+    connRoute.ps[routeSize - 2] = inf2->point;
+    routeSize -= 3;
+    for (int curr = inf1Index; curr >= 0; curr = done[curr].prevIndex)
+    {
+        connRoute.ps[routeSize] = done[curr].inf->point;
+        routeSize -= 1;
+    }
+}
+
+
 // Given the two points for a new segment of a path (inf2 & inf3)
 // as well as the distance between these points (dist), as well as
 // possibly the previous point (inf1) [from inf1--inf2], return a
 // cost associated with this route.
 //
-double cost(ConnRef *lineRef, const double dist, VertInf *inf1,
-        VertInf *inf2, VertInf *inf3)
+static double cost(ConnRef *lineRef, const double dist, VertInf *inf2, 
+        VertInf *inf3, std::vector<ANode>& done, int inf1Index)
 {
+    VertInf *inf1 = (inf1Index >= 0) ?  done[inf1Index].inf : NULL;
     double result = dist;
+    Polygon connRoute;
 
     Router *router = inf2->_router;
-    if (inf2->pathNext != NULL)
+    if (inf1 != NULL)
     {
-        double& angle_penalty = router->angle_penalty;
-        double& segmt_penalty = router->segmt_penalty;
+        const double angle_penalty = router->routingPenalty(anglePenalty);
+        const double segmt_penalty = router->routingPenalty(segmentPenalty);
 
         // This is not the first segment, so there is a bend
         // between it and the last one in the existing path.
@@ -89,29 +182,83 @@ double cost(ConnRef *lineRef, const double dist, VertInf *inf1,
 
             double rad = M_PI - angleBetween(p1, p2, p3);
 
-            // Make `xval' between 0--10 then take its log so small
-            // angles are not penalised as much as large ones.
-            //
-            double xval = rad * 10 / M_PI;
-            double yval = xval * log10(xval + 1) / 10.5;
-            result += (angle_penalty * yval);
-            //printf("deg from straight: %g\tpenalty: %g\n",
-            //        rad * 180 / M_PI, (angle_penalty * yval));
-
-            // Don't penalise as an extra segment if there is no turn.
-            if (rad > 0.0005)
+            if (rad > 0)
+            {
+                // Make `xval' between 0--10 then take its log so small
+                // angles are not penalised as much as large ones.
+                //
+                double xval = rad * 10 / M_PI;
+                double yval = xval * log10(xval + 1) / 10.5;
+                result += (angle_penalty * yval);
+                //db_printf("deg from straight: %g\tpenalty: %g\n",
+                //        rad * 180 / M_PI, (angle_penalty * yval));
+            }
+
+            if (rad == M_PI)
             {
+                // Needs to double back
+                result += (2 * segmt_penalty);
+            }
+            else if (rad > 0)
+            {
+                // Only penalise as an extra segment if the two 
+                // segments are not collinear.
                 result += segmt_penalty;
             }
         }
     }
 
-    if (lineRef->doesHateCrossings() && (router->crossing_penalty > 0))
+    if (!router->_inCrossingPenaltyReroutingStage)
     {
-        Point& a1 = inf2->point;
-        Point& a2 = inf3->point;
+        // Return here if we ar not in the postprocessing stage 
+        return result;
+    }
 
-        ConnRefList::iterator curr, finish = router->connRefs.end();
+    const double cluster_crossing_penalty = 
+            router->routingPenalty(clusterCrossingPenalty);
+    // XXX: Clustered routing doesn't yet work with orthogonal connectors.
+    if (router->ClusteredRouting && !router->clusterRefs.empty() &&
+            (cluster_crossing_penalty > 0) && 
+            (lineRef->routingType() != ConnType_Orthogonal))
+    {
+        if (connRoute.empty())
+        {
+            constructPolygonPath(connRoute, inf2, inf3, done, inf1Index);
+        }
+        // There are clusters so do cluster routing.
+        for (ClusterRefList::const_iterator cl = router->clusterRefs.begin(); 
+                cl != router->clusterRefs.end(); ++cl)
+        {
+            ReferencingPolygon& cBoundary = (*cl)->polygon();
+            COLA_ASSERT(cBoundary.ps[0] != cBoundary.ps[cBoundary.size() - 1]);
+            for (size_t j = 0; j < cBoundary.size(); ++j)
+            {
+                // Cluster boundary points should correspond to shape 
+                // vertices and hence already be in the list of vertices.
+                COLA_ASSERT(router->vertices.getVertexByPos(cBoundary.at(j))!=NULL);
+            }
+            
+            bool isConn = false;
+            Polygon dynamic_c_boundary(cBoundary);
+            Polygon dynamic_conn_route(connRoute);
+            const bool finalSegment = (inf3 == lineRef->dst());
+            CrossingsInfoPair crossings = countRealCrossings(
+                    dynamic_c_boundary, isConn, dynamic_conn_route, 
+                    connRoute.size() - 1, true, finalSegment);
+            result += (crossings.first * cluster_crossing_penalty);
+        }
+    }
+
+    const double shared_path_penalty = 
+            router->routingPenalty(fixedSharedPathPenalty);
+    if (shared_path_penalty > 0)
+    {
+        // Penalises shared paths, except if the connectors shared an endpoint.
+        if (connRoute.empty())
+        {
+            constructPolygonPath(connRoute, inf2, inf3, done, inf1Index);
+        }
+        ConnRefList::const_iterator curr, finish = router->connRefs.end();
         for (curr = router->connRefs.begin(); curr != finish; ++curr)
         {
             ConnRef *connRef = *curr;
@@ -120,306 +267,237 @@ double cost(ConnRef *lineRef, const double dist, VertInf *inf1,
             {
                 continue;
             }
-            Avoid::PolyLine& route2 = connRef->route();
-            for (int j = 1; j < route2.pn; ++j)
-            {
-                Avoid::Point& b1 = route2.ps[j - 1];
-                Avoid::Point& b2 = route2.ps[j];
+            const Avoid::PolyLine& route2 = connRef->route();
             
-                if (((a1 == b1) && (a2 == b2)) ||
-                    ((a2 == b1) && (a1 == b2)))
-                {
-                    // Route along same segment: no penalty.  We detect
-                    // crossovers when we see the segments diverge.
-                    continue;
-                }
-
-                if ((a2 == b2) || (a2 == b1) || (b2 == a1))
-                {
-                    // Each crossing that is at a vertex in the 
-                    // visibility graph gets noticed four times.
-                    // We ignore three of these cases.
-                    // This also catches the case of a shared path,
-                    // but this is one that terminates at a common
-                    // endpoint, so we don't care about it.
-                    continue;
-                }
-
-                if (a1 == b1)
-                {
-                    if (j == 1)
-                    {
-                        // common source point.
-                        continue;
-                    }
-                    Avoid::Point& b0 = route2.ps[j - 2];
-                    // The segments share an endpoint -- a1==b1.
-                    if (a2 == b0)
-                    {
-                        // a2 is not a split, continue.
-                        continue;
-                    }
-                    
-                    // If here, then we know that a2 != b2
-                    // And a2 and its pair in b are a split.
-                    assert(a2 != b2);
-
-                    if (inf2->pathNext == NULL)
-                    {
-                        continue;
-                    }
-                    Avoid::Point& a0 = inf1->point;
-
-                    if ((a0 == b0) || (a0 == b2))
-                    {
-                        //printf("Shared path... ");
-                        bool normal = (a0 == b0) ? true : false;
-                        // Determine direction we have to look through
-                        // the points of connector b.
-                        int dir = normal ? -1 : 1;
-                        
-                        int traceJ = j - 1 + dir;
-                        
-                        int endCornerSide = Avoid::cornerSide(
-                                a0, a1, a2, normal ? b2 : b0);
-
-                        
-                        VertInf *traceInf1 = inf2->pathNext;
-                        VertInf *traceInf2 = inf2;
-                        VertInf *traceInf3 = inf3;
-                        while (traceInf1 &&
-                                (traceJ >= 0) && (traceJ < route2.pn) &&
-                                (traceInf1->point == route2.ps[traceJ]))
-                        {
-                            traceInf3 = traceInf2;
-                            traceInf2 = traceInf1;
-                            traceInf1 = traceInf1->pathNext;
-                            traceJ += dir;
-                        }
-                        
-                        if (!traceInf1 ||
-                                (traceJ < 0) || (traceJ >= route2.pn))
-                        {
-                            //printf("common source or destination.\n");
-                            // The connectors have a shared path, but it
-                            // comes from a common source point.
-                            // XXX: There might be a better way to
-                            //      check this by asking the connectors
-                            //      for the IDs of the attached shapes.
-                            continue;
-                        }
-                        
-                        int startCornerSide = Avoid::cornerSide(
-                                traceInf1->point, traceInf2->point,
-                                traceInf3->point, route2.ps[traceJ]);
-                        
-                        if (endCornerSide != startCornerSide)
-                        {
-                            //printf("crosses.\n");
-                            result += router->crossing_penalty;
-                        }
-                        else
-                        {
-                            //printf("doesn't cross.\n");
-                        }
-                    }
-                    else
-                    {
-                        // The connectors cross or touch at this point.
-                        //printf("Cross or touch at point... ");
-                    
-                        int side1 = Avoid::cornerSide(a0, a1, a2, b0);
-                        int side2 = Avoid::cornerSide(a0, a1, a2, b2);
-
-                        if (side1 != side2)
-                        {
-                            //printf("cross.\n");
-                            // The connectors cross at this point.
-                            result += router->crossing_penalty;
-                        }
-                        else
-                        {
-                            //printf("touch.\n");
-                            // The connectors touch at this point.
-                        }
-                    }
-                    continue;
-                }
+            bool isConn = true;
+            Polygon dynamic_route2(route2);
+            Polygon dynamic_conn_route(connRoute);
+            CrossingsInfoPair crossings = countRealCrossings(
+                    dynamic_route2, isConn, dynamic_conn_route, 
+                    connRoute.size() - 1, false, false, NULL, NULL,
+                    connRef, lineRef);
+
+            if ((crossings.second & CROSSING_SHARES_PATH) &&
+                    (crossings.second & CROSSING_SHARES_FIXED_SEGMENT) &&
+                    !(crossings.second & CROSSING_SHARES_PATH_AT_END))
+            {
+                // Penalise unecessary shared paths in the middle of
+                // connectors.
+                result += shared_path_penalty;
+            }
+        }
+    }
 
-                double xc, yc;
-                int intersectResult = Avoid::segmentIntersectPoint(
-                        a1, a2, b1, b2, &xc, &yc);
+    const double crossing_penalty = router->routingPenalty(crossingPenalty);
+    if (lineRef->doesHateCrossings() && (crossing_penalty > 0))
+    {
+        if (connRoute.empty())
+        {
+            constructPolygonPath(connRoute, inf2, inf3, done, inf1Index);
+        }
+        ConnRefList::const_iterator curr, finish = router->connRefs.end();
+        for (curr = router->connRefs.begin(); curr != finish; ++curr)
+        {
+            ConnRef *connRef = *curr;
 
-                if (intersectResult == Avoid::DO_INTERSECT)
-                {
-                    result += router->crossing_penalty;
-                }
+            if (connRef->id() == lineRef->id())
+            {
+                continue;
             }
+            const Avoid::PolyLine& route2 = connRef->route();
+            
+            bool isConn = true;
+            Polygon dynamic_route2(route2);
+            Polygon dynamic_conn_route(connRoute);
+            CrossingsInfoPair crossings = countRealCrossings(
+                    dynamic_route2, isConn, dynamic_conn_route, 
+                    connRoute.size() - 1, true);
+            result += (crossings.first * crossing_penalty);
         }
     }
-    
+
     return result;
 }
 
 
-// Returns the best path from src to tar using the cost function.
-//
-// The path is worked out via Dijkstra's algorithm, and is encoded via
-// pathNext links in each of the VerInfs along the path.
-//
-// Based on the code of 'matrixpfs'.
-//
-static void dijkstraPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
+static double estimatedCost(ConnRef *lineRef, const Point *last, 
+        const Point& a, const Point& b)
 {
-    Router *router = src->_router;
-
-    double unseen = (double) __INT_MAX__;
-
-    // initialize arrays
-    VertInf *finish = router->vertices.end();
-    for (VertInf *t = router->vertices.connsBegin(); t != finish; t = t->lstNext)
+    if (lineRef->routingType() == ConnType_PolyLine)
     {
-        t->pathNext = NULL;
-        t->pathDist = -unseen;
+        return euclideanDist(a, b);
     }
-
-    VertInf *min = src;
-    while (min != tar)
+    else // Orthogonal
     {
-        VertInf *k = min;
-        min = NULL;
-
-        k->pathDist *= -1;
-        if (k->pathDist == unseen)
+        // XXX: This currently just takes into account the compulsory
+        //      bend but will have to be updated when port direction 
+        //      information is available.
+        int num_penalties = 0;
+        double xmove = b.x - a.x;
+        double ymove = b.y - a.y;
+        if (!last)
         {
-            k->pathDist = 0;
-        }
-
-        EdgeInfList& visList = k->visList;
-        EdgeInfList::iterator finish = visList.end();
-        for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
-                ++edge)
-        {
-            VertInf *t = (*edge)->otherVert(k);
-            VertID tID = t->id;
-
-            // Only check shape verticies, or endpoints.
-            if ((t->pathDist < 0) &&
-                    ((tID.objID == src->id.objID) || tID.isShape))
+            // Just two points.
+            if ((xmove != 0) && (ymove != 0))
             {
-                double kt_dist = (*edge)->getDist();
-                double priority = k->pathDist +
-                        cost(lineRef, kt_dist, k->pathNext, k, t);
-
-                if ((kt_dist != 0) && (t->pathDist < -priority))
-                {
-                    t->pathDist = -priority;
-                    t->pathNext = k;
-                }
-                if ((min == NULL) || (t->pathDist > min->pathDist))
-                {
-                    min = t;
-                }
+                num_penalties += 1;
             }
         }
-        EdgeInfList& invisList = k->invisList;
-        finish = invisList.end();
-        for (EdgeInfList::iterator edge = invisList.begin(); edge != finish;
-                ++edge)
+        else
         {
-            VertInf *t = (*edge)->otherVert(k);
-            VertID tID = t->id;
-
-            // Only check shape verticies, or endpoints.
-            if ((t->pathDist < 0) &&
-                    ((tID.objID == src->id.objID) || tID.isShape > 0))
+            // We have three points, so we know the direction of the 
+            // previous segment.
+            double rad = M_PI - angleBetween(*last, a, b);
+            if (rad > (M_PI / 2))            
             {
-                if ((min == NULL) || (t->pathDist > min->pathDist))
-                {
-                    min = t;
-                }
+                // Target point is back in the direction of the first point,
+                // so at least two bends are required.
+                num_penalties += 2;
+            }
+            else if (rad > 0)
+            {
+                // To the side, so at least one bend.
+                num_penalties += 1;
             }
         }
+        double penalty = num_penalties * 
+                lineRef->router()->routingPenalty(segmentPenalty);
+
+        return manhattanDist(a, b) + penalty;
     }
 }
 
 
-class ANode
+class CmpVisEdgeRotation 
 {
     public:
-        VertInf* inf;
-        double g;        // Gone
-        double h;        // Heuristic
-        double f;        // Formula f = g + h
-        VertInf *pp;
-
-        ANode(VertInf *vinf)
-            : inf(vinf)
-            , g(0)
-            , h(0)
-            , f(0)
-            , pp(NULL)
+        CmpVisEdgeRotation(const VertInf* lastPt)
+            : _lastPt(lastPt)
         {
         }
-        ANode()
-            : inf(NULL)
-            , g(0)
-            , h(0)
-            , f(0)
-            , pp(NULL)
+        bool operator() (const EdgeInf* u, const EdgeInf* v) const 
         {
+            return u->rotationLessThan(_lastPt, v);
         }
+    private:
+        const VertInf *_lastPt;
 };
 
-bool operator<(const ANode &a, const ANode &b)
-{
-    return a.f < b.f;
-}
-
-
-bool operator>(const ANode &a, const ANode &b)
-{
-    return a.f > b.f;
-}
-
 
 // Returns the best path from src to tar using the cost function.
 //
 // The path is worked out using the aStar algorithm, and is encoded via
-// pathNext links in each of the VerInfs along the path.
+// prevIndex values for each ANode which point back to the previous ANode's
+// position in the DONE vector.  At completion, this order is written into
+// the pathNext links in each of the VerInfs along the path.
 //
 // The aStar STL code is based on public domain code available on the
 // internet.
 //
-static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
+static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar, 
+        VertInf *start)
 {
+    bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);
+
+    double (*dist)(const Point& a, const Point& b) = 
+        (isOrthogonal) ? manhattanDist : euclideanDist;
+
     std::vector<ANode> PENDING;     // STL Vectors chosen because of rapid
     std::vector<ANode> DONE;        // insertions/deletions at back,
     ANode Node, BestNode;           // Temporary Node and BestNode
     bool bNodeFound = false;        // Flag if node is found in container
+    int timestamp = 1;
 
-    tar->pathNext = NULL;
+    if (start == NULL)
+    {
+        start = src;
+    }
+
+    Router *router = lineRef->router();
+    if (router->RubberBandRouting && (start != src))
+    {
+        COLA_ASSERT(router->IgnoreRegions == true);
+        
+        const PolyLine& currRoute = lineRef->route();
+        VertInf *last = NULL;
+        int rIndx = 0;
+        while (last != start)
+        {
+            const Point& pnt = currRoute.at(rIndx);
+            bool isShape = (rIndx > 0);
+            VertID vID(pnt.id, isShape, pnt.vn);
+
+#ifdef PATHDEBUG
+            db_printf("/// %d %d %d\n", pnt.id, (int) isShape, pnt.vn);
+#endif
+            VertInf *curr = router->vertices.getVertexByID(vID);
+            COLA_ASSERT(curr != NULL);
+
+            Node = ANode(curr, timestamp++);
+            if (!last)
+            {
+                Node.g = 0;
+                Node.h = estimatedCost(lineRef, NULL, Node.inf->point, 
+                        tar->point);
+                Node.f = Node.g + Node.h;
+            }
+            else
+            {
+                double edgeDist = dist(BestNode.inf->point, curr->point);
+
+                Node.g = BestNode.g + cost(lineRef, edgeDist, BestNode.inf, 
+                        Node.inf, DONE, BestNode.prevIndex);
+
+                // Calculate the Heuristic.
+                Node.h = estimatedCost(lineRef, &(BestNode.inf->point),
+                        Node.inf->point, tar->point);
+
+                // The A* formula
+                Node.f = Node.g + Node.h;
+                
+                // Point parent to last BestNode (pushed onto DONE)
+                Node.prevIndex = DONE.size() - 1;
+            }
+
+            if (curr != start)
+            {
+                BestNode = Node;
+
+                DONE.push_back(BestNode);
+            }
+            else
+            {
+                PENDING.push_back(Node);
+            }
 
-    // Create the start node
-    Node = ANode(src);
-    Node.g = 0;
-    Node.h = dist(Node.inf->point, tar->point);
-    Node.f = Node.g + Node.h;
-    // Set a null parent, so cost function knows this is the first segment.
-    Node.pp = NULL;
+            rIndx++;
+            last = curr;
+        }
+    }
+    else
+    {
+        // Create the start node
+        Node = ANode(src, timestamp++);
+        Node.g = 0;
+        Node.h = estimatedCost(lineRef, NULL, Node.inf->point, tar->point);
+        Node.f = Node.g + Node.h;
+        // Set a null parent, so cost function knows this is the first segment.
+
+        // Populate the PENDING container with the first location
+        PENDING.push_back(Node);
+    }
+
+    tar->pathNext = NULL;
 
-    // Populate the PENDING container with the first location
-    PENDING.push_back(Node);
     // Create a heap from PENDING for sorting
     using std::make_heap; using std::push_heap; using std::pop_heap;
     make_heap( PENDING.begin(), PENDING.end() );
 
     while (!PENDING.empty())
     {
-        // Ascending sort based on overloaded operators below
-        sort_heap(PENDING.begin(), PENDING.end());
-
-        // Set the Node with lowest f value to BESTNODE
+        // Set the Node with lowest f value to BESTNODE.
+        // Since the ANode operator< is reversed, the head of the
+        // heap is the node with the lowest f value.
         BestNode = PENDING.front();
 
         // Pop off the heap.  Actually this moves the
@@ -427,38 +505,118 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
         // is not actually removed since the pop is to
         // the heap and not the container.
         pop_heap(PENDING.begin(), PENDING.end());
-
-
         // Remove node from right (the value we pop_heap'd)
         PENDING.pop_back();
 
         // Push the BestNode onto DONE
-        BestNode.inf->pathNext = BestNode.pp;
         DONE.push_back(BestNode);
 
+        VertInf *prevInf = (BestNode.prevIndex >= 0) ?
+                DONE[BestNode.prevIndex].inf : NULL;
 #if 0
-        printf("Considering... ");
-        BestNode.ID->print(stdout);
-        printf(" - g: %3.1f h: %3.1f f: %3.1f back: ", BestNode.g, BestNode.h,
-                BestNode.f);
-        BestNode.pp.print(stdout);
-        printf("\n");
+        db_printf("Considering... ");
+        db_printf(" %g %g  ", BestNode.inf->point.x, BestNode.inf->point.y);
+        BestNode.inf->id.db_print();
+        db_printf(" - g: %3.1f h: %3.1f back: ", BestNode.g, BestNode.h);
+        if (prevInf)
+        {
+            db_printf(" %g %g", prevInf->point.x, prevInf->point.y);
+            //prevInf->id.db_print();
+        }
+        db_printf("\n");
+#endif
+
+#if defined(ASTAR_DEBUG)
+        if (router->avoid_screen)
+        {
+            int canx = 151;
+            int cany = 55;
+            int radius = 5;
+            ANode curr;
+            for (curr = BestNode; curr.prevIndex >= 0; 
+                    curr = DONE[curr.prevIndex])
+            {
+                filledCircleRGBA(router->avoid_screen, 
+                        (int) curr.inf->point.x + canx,
+                        (int) curr.inf->point.y + cany, 
+                        radius, 0, 0, 255, 128);
+            }
+            filledCircleRGBA(router->avoid_screen, 
+                    (int) BestNode.inf->point.x + canx,
+                    (int) BestNode.inf->point.y + cany, 
+                    radius, 255, 0, 0, 255);
+
+            SDL_Flip(router->avoid_screen);
+            //SDL_Delay(500);
+
+            filledCircleRGBA(router->avoid_screen, 
+                    (int) BestNode.inf->point.x + canx,
+                    (int) BestNode.inf->point.y + cany, 
+                    radius, 255, 255, 255, 255);
+            filledCircleRGBA(router->avoid_screen, 
+                    (int) BestNode.inf->point.x + canx,
+                    (int) BestNode.inf->point.y + cany, 
+                    radius, 0, 255, 0, 128);
+            for (curr = BestNode; curr.prevIndex >= 0; 
+                    curr = DONE[curr.prevIndex])
+            {
+                filledCircleRGBA(router->avoid_screen, 
+                        (int) curr.inf->point.x + canx,
+                        (int) curr.inf->point.y + cany, 
+                        radius, 255, 255, 255, 255);
+                filledCircleRGBA(router->avoid_screen, 
+                        (int) curr.inf->point.x + canx,
+                        (int) curr.inf->point.y + cany, 
+                        radius, 0, 255, 0, 128);
+            }
+        }
 #endif
 
         // If at destination, break and create path below
         if (BestNode.inf == tar)
         {
+#ifdef PATHDEBUG
+            db_printf("Cost: %g\n", BestNode.f);
+#endif
             //bPathFound = true; // arrived at destination...
+            
+            // Correct all the pathNext pointers.
+            ANode curr;
+            int currIndex = DONE.size() - 1;
+            for (curr = BestNode; curr.prevIndex > 0; 
+                    curr = DONE[curr.prevIndex])
+            {
+                COLA_ASSERT(curr.prevIndex < currIndex);   
+                curr.inf->pathNext = DONE[curr.prevIndex].inf;
+                currIndex = curr.prevIndex;
+            }
+            // Check that we've gone through the complete path.
+            COLA_ASSERT(curr.prevIndex == 0);
+            // Fill in the final pathNext pointer.
+            curr.inf->pathNext = DONE[curr.prevIndex].inf;
+
             break;
         }
 
         // Check adjacent points in graph
-        EdgeInfList& visList = BestNode.inf->visList;
-        EdgeInfList::iterator finish = visList.end();
-        for (EdgeInfList::iterator edge = visList.begin(); edge != finish;
-                ++edge)
+        EdgeInfList& visList = (!isOrthogonal) ?
+                BestNode.inf->visList : BestNode.inf->orthogVisList;
+        if (isOrthogonal)
+        {
+            // We would like to explore in a structured way, 
+            // so sort the points in the visList...
+            CmpVisEdgeRotation compare(prevInf);
+            visList.sort(compare);
+        }
+        EdgeInfList::const_iterator finish = visList.end();
+        for (EdgeInfList::const_iterator edge = visList.begin(); 
+                edge != finish; ++edge)
         {
-            Node.inf = (*edge)->otherVert(BestNode.inf);
+            Node = ANode((*edge)->otherVert(BestNode.inf), timestamp++);
+
+            // Set the index to the previous ANode that we reached
+            // this ANode through (the last BestNode pushed onto DONE).
+            Node.prevIndex = DONE.size() - 1;
 
             // Only check shape verticies, or the tar endpoint.
             if (!(Node.inf->id.isShape) && (Node.inf != tar))
@@ -466,6 +624,15 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 continue;
             }
 
+            VertInf *prevInf = (BestNode.prevIndex >= 0) ?
+                    DONE[BestNode.prevIndex].inf : NULL;
+
+            // Don't bother looking at the segment we just arrived along.
+            if (prevInf && (prevInf == Node.inf))
+            {
+                continue;
+            }
+
             double edgeDist = (*edge)->getDist();
 
             if (edgeDist == 0)
@@ -473,31 +640,49 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 continue;
             }
 
-            VertInf *prevInf = BestNode.inf->pathNext;
+            if (!router->_orthogonalRouting &&
+                  (!router->RubberBandRouting || (start == src)) && 
+                  (validateBendPoint(prevInf, BestNode.inf, Node.inf) == false))
+            {
+                // The bendpoint is not valid, i.e., is a zigzag corner, so...
+                continue;
+                // For RubberBand routing we want to allow these routes and
+                // unwind them later, otherwise instead or unwinding, paths
+                // can go the *really* long way round.
+            }
 
-            Node.g = BestNode.g + cost(lineRef, edgeDist, prevInf,
-                    BestNode.inf, Node.inf);
+            Node.g = BestNode.g + cost(lineRef, edgeDist, BestNode.inf, 
+                    Node.inf, DONE, BestNode.prevIndex);
 
             // Calculate the Heuristic.
-            Node.h = dist(Node.inf->point, tar->point);
+            Node.h = estimatedCost(lineRef, &(BestNode.inf->point),
+                    Node.inf->point, tar->point);
 
             // The A* formula
             Node.f = Node.g + Node.h;
-            // Point parent to last BestNode (pushed onto DONE)
-            Node.pp = BestNode.inf;
+
+#if 0
+            db_printf("-- Adding: %g %g  ", Node.inf->point.x, 
+                    Node.inf->point.y);
+            Node.inf->id.db_print();
+            db_printf(" - g: %3.1f h: %3.1f \n", Node.g, Node.h);
+#endif
 
             bNodeFound = false;
 
             // Check to see if already on PENDING
             for (unsigned int i = 0; i < PENDING.size(); i++)
             {
-                if (Node.inf == PENDING.at(i).inf)
-                {   // If already on PENDING
-                    if (Node.g < PENDING.at(i).g)
+                ANode& ati = PENDING.at(i);
+                if ((Node.inf == ati.inf) &&
+                        (DONE[Node.prevIndex].inf == DONE[ati.prevIndex].inf))
+                {
+                    // If already on PENDING
+                    if (Node.g < ati.g)
                     {
-                        PENDING.at(i).g = Node.g;
-                        PENDING.at(i).f = Node.g + PENDING.at(i).h;
-                        PENDING.at(i).pp = Node.pp;
+                        PENDING[i] = Node;
+
+                        make_heap( PENDING.begin(), PENDING.end() );
                     }
                     bNodeFound = true;
                     break;
@@ -508,15 +693,14 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 // Check to see if already on DONE
                 for (unsigned int i = 0; i < DONE.size(); i++)
                 {
-                    if (Node.inf == DONE.at(i).inf)
+                    ANode& ati = DONE.at(i);
+                    if ((Node.inf == ati.inf) && 
+                            (DONE[Node.prevIndex].inf == DONE[ati.prevIndex].inf))
                     {
                         // If on DONE, Which has lower gone?
-                        if (Node.g < DONE.at(i).g)
+                        if (Node.g < ati.g)
                         {
-                            DONE.at(i).g = Node.g;
-                            DONE.at(i).f = Node.g + DONE.at(i).h;
-                            DONE.at(i).pp = Node.pp;
-                            DONE.at(i).inf->pathNext = Node.pp;
+                            DONE[i] = Node;
                         }
                         bNodeFound = true;
                         break;
@@ -530,26 +714,24 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
                 PENDING.push_back(Node);
                 // Push NewNode onto heap
                 push_heap( PENDING.begin(), PENDING.end() );
-                // Re-Assert heap, or will be short by one
-                make_heap( PENDING.begin(), PENDING.end() );
 
 #if 0
+                using std::cout; using std::endl;
                 // Display PENDING and DONE containers (For Debugging)
                 cout << "PENDING:   ";
-                for (int i = 0; i < PENDING.size(); i++)
+                for (unsigned int i = 0; i < PENDING.size(); i++)
                 {
-                    cout << PENDING.at(i).x << "," << PENDING.at(i).y << ",";
-                    cout << PENDING.at(i).g << "," << PENDING.at(i).h << "  ";
+                    cout << PENDING.at(i).g << "," << PENDING.at(i).h << ",";
+                    cout << PENDING.at(i).inf << "," << PENDING.at(i).pp << "  ";
                 }
                 cout << endl;
                 cout << "DONE:   ";
-                for (int i = 0; i < DONE.size(); i++)
+                for (unsigned int i = 0; i < DONE.size(); i++)
                 {
-                    cout << DONE.at(i).x << "," << DONE.at(i).y << ",";
-                    cout << DONE.at(i).g << "," << DONE.at(i).h << "  ";
+                    cout << DONE.at(i).g << "," << DONE.at(i).h << ",";
+                    cout << DONE.at(i).inf << "," << DONE.at(i).pp << "  ";
                 }
                 cout << endl << endl;
-                int ch = _getch();
 #endif
             }
         }
@@ -559,75 +741,52 @@ static void aStarPath(ConnRef *lineRef, VertInf *src, VertInf *tar)
 
 // Returns the best path for the connector referred to by lineRef.
 //
-// The path encoded in the pathNext links in each of the VerInfs
+// The path encoded in the pathNext links in each of the VertInfs
 // backwards along the path, from the tar back to the source.
 //
 void makePath(ConnRef *lineRef, bool *flag)
 {
+    bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);
     Router *router = lineRef->router();
     VertInf *src = lineRef->src();
     VertInf *tar = lineRef->dst();
+    VertInf *start = lineRef->start();
 
-    // If the connector hates crossings then we want to examine direct paths:
-    bool examineDirectPath = lineRef->doesHateCrossings();
-    
     // TODO: Could be more efficient here.
-    EdgeInf *directEdge = EdgeInf::existingEdge(src, tar);
-    if (!(router->IncludeEndpoints) && directVis(src, tar))
-    {
-        Point p = src->point;
-        Point q = tar->point;
-
-        assert(directEdge == NULL);
-
-        directEdge = new EdgeInf(src, tar);
-        tar->pathNext = src;
-        directEdge->setDist(dist(p, q));
-        directEdge->addConn(flag);
-
-        return;
-    }
-    else if (router->IncludeEndpoints && directEdge &&
-            (directEdge->getDist() > 0) && !examineDirectPath)
+    if (isOrthogonal)
     {
-        tar->pathNext = src;
-        directEdge->addConn(flag);
+        aStarPath(lineRef, src, tar, start);
     }
-    else
+    else // if (!isOrthogonal)
     {
-        // Mark the path endpoints as not being able to see
-        // each other.  This is true if we are here.
-        if (!(router->IncludeEndpoints) && router->InvisibilityGrph)
-        {
-            if (!directEdge)
-            {
-                directEdge = new EdgeInf(src, tar);
-            }
-            directEdge->addBlocker(0);
-        }
-
-        if (router->UseAStarSearch)
+        EdgeInf *directEdge = EdgeInf::existingEdge(src, tar);
+        // If the connector hates crossings or there are clusters present,
+        // then we want to examine direct paths:
+        bool examineDirectPath = lineRef->doesHateCrossings() || 
+                !(router->clusterRefs.empty());
+        
+        if ((start == src) && directEdge && (directEdge->getDist() > 0) && 
+                !examineDirectPath)
         {
-            aStarPath(lineRef, src, tar);
+            tar->pathNext = src;
+            directEdge->addConn(flag);
         }
         else
         {
-            dijkstraPath(lineRef, src, tar);
+            aStarPath(lineRef, src, tar, start);
         }
+    }
 
 #if 0
-        PointMap::iterator t;
-        for (VertInf *t = vertices.connsBegin(); t != vertices.end();
-                t = t->lstNext)
-        {
-
-            t->id.print();
-            printf(" -> ");
-            t->pathNext->id.print();
-            printf("\n");
-        }
-#endif
+    for (VertInf *t = vertices.connsBegin(); t != vertices.end();
+            t = t->lstNext)
+    {
+        t->id.db_print();
+        db_printf(" -> ");
+        t->pathNext->id.db_print();
+        db_printf("\n");
     }
+#endif
 }
 
 
diff --git a/src/libavoid/makepath.h b/src/libavoid/makepath.h
index 4d68a01e3..b40bfbc3d 100644
--- a/src/libavoid/makepath.h
+++ b/src/libavoid/makepath.h
@@ -2,24 +2,27 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2008  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_MAKEPATH_H
 #define AVOID_MAKEPATH_H
 
diff --git a/src/libavoid/polyutil.cpp b/src/libavoid/polyutil.cpp
deleted file mode 100644
index 1b4b0c619..000000000
--- a/src/libavoid/polyutil.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * vim: ts=4 sw=4 et tw=0 wm=0
- *
- * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
-*/
-
-#include <cassert>
-#include <cstdlib>
-
-#include "libavoid/polyutil.h"
-#include "libavoid/geomtypes.h"
-#include "libavoid/vertices.h"
-#include "libavoid/shape.h"
-
-namespace Avoid {
-
-
-Polygn newPoly(int size)
-{
-    Polygn newpoly;
-
-    newpoly.id = 0;
-    newpoly.pn = size;
-    newpoly.ps = (Point *) calloc(size, sizeof(Point));
-    if (!newpoly.ps)
-    {
-        fprintf(stderr,
-                "Error: Unable to allocate Point array in Avoid::newPoly\n");
-        abort();
-    }
-    return newpoly;
-}
-
-
-Polygn copyPoly(Polygn poly)
-{
-    Polygn newpoly = newPoly(poly.pn);
-
-    newpoly.id = poly.id;
-    for (int i = 0; i < poly.pn; i++)
-    {
-        newpoly.ps[i] = poly.ps[i];
-    }
-    return newpoly;
-}
-
-
-Polygn copyPoly(ShapeRef *shape)
-{
-    Polygn poly = shape->poly();
-    Polygn newpoly = newPoly(poly.pn);
-
-    newpoly.id = poly.id;
-    for (int i = 0; i < poly.pn; i++)
-    {
-        newpoly.ps[i] = poly.ps[i];
-    }
-    return newpoly;
-}
-
-
-void freePoly(Polygn& poly)
-{
-    std::free(poly.ps);
-}
-
-
-void freePtrPoly(Polygn *poly)
-{
-    std::free(poly->ps);
-    std::free(poly);
-}
-
-
-}
-
diff --git a/src/libavoid/polyutil.h b/src/libavoid/polyutil.h
deleted file mode 100644
index 9340df5f4..000000000
--- a/src/libavoid/polyutil.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * vim: ts=4 sw=4 et tw=0 wm=0
- *
- * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
-*/
-
-#include "libavoid/geometry.h"
-#include "libavoid/shape.h"
-
-#ifndef AVOID_POLYUTIL_H
-#define AVOID_POLYUTIL_H
-
-namespace Avoid {
-
-
-extern Polygn newPoly(int size);
-extern Polygn copyPoly(Polygn);
-extern Polygn copyPoly(ShapeRef *shape);
-extern void freePoly(Polygn&);
-extern void freePtrPoly(Polygn *argpoly);
-
-
-}
-
-#endif
diff --git a/src/libavoid/region.cpp b/src/libavoid/region.cpp
deleted file mode 100644
index 5a46d7cbb..000000000
--- a/src/libavoid/region.cpp
+++ /dev/null
@@ -1,858 +0,0 @@
-/*
- * vim: ts=4 sw=4 et tw=0 wm=0
- *
- * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
-*/
-
-#include <stdio.h>
-#include <cstdlib>
-#include <algorithm>
-#include <cassert>
-#include <cmath>
-
-#include "libavoid/shape.h"
-#include "libavoid/region.h"
-
-
-namespace Avoid {
-
-Region *centerRegion = NULL;
-
-static const BBox screenBBox =
-        {Point(-INFINITY, -INFINITY), Point(INFINITY, INFINITY)};
-
-
-Region::Region()
-    : _bbox(screenBBox)
-    , _left(NULL), _right(NULL), _up(NULL), _down(NULL)
-{
-    _blocks.clear();
-}
-
-
-Region::Region(double x1, double y1, double x2, double y2)
-    : _left(NULL), _right(NULL), _up(NULL), _down(NULL)
-{
-    _bbox.a.x = x1;
-    _bbox.a.y = y1;
-    _bbox.b.x = x2;
-    _bbox.b.y = y2;
-
-    _blocks.clear();
-}
-
-
-static const unsigned int R_INSIDE = 0;
-static const unsigned int R_LEFT   = 1;
-static const unsigned int R_LEDGE  = 2;
-static const unsigned int R_RIGHT  = 4;
-static const unsigned int R_REDGE  = 8;
-static const unsigned int R_ABOVE  = 16;
-static const unsigned int R_TEDGE  = 32;
-static const unsigned int R_BELOW  = 64;
-static const unsigned int R_BEDGE  = 128;
-
-static const unsigned int R_NONE   = R_INSIDE;
-static const unsigned int R_UP     = R_ABOVE;
-static const unsigned int R_DOWN   = R_BELOW;
-static const unsigned int R_HORI   = R_LEFT | R_RIGHT;
-static const unsigned int R_VERT   = R_UP | R_DOWN;
-
-
-static void printBBox(const char *label, const BBox &bbox)
-{
-    if (label)
-    {
-        printf("%s: ", label);
-    }
-    printf("(%.2f, %.2f)-(%.2f, %.2f)\n", bbox.a.x, bbox.a.y,
-            bbox.b.x, bbox.b.y);
-}
-
-
-bool Region::overlapCheck(BBox& bbox, unsigned int& p)
-{
-    p = R_INSIDE;
-    Point& a = bbox.a;
-    Point& b = bbox.b;
-    Point& r = _bbox.a;
-    Point& s = _bbox.b;
-    
-    if (s.x <= a.x)
-    {
-        // Wholly right.
-        p = R_RIGHT;
-        return false;
-    }
-    else if (r.x >= b.x)
-    {
-        // Wholly left.
-        p = R_LEFT;
-        return false;
-    }
-    
-    if (s.y <= a.y)
-    {
-        // Wholly below.
-        p = R_BELOW;
-        return false;
-    }
-    else if (r.y >= b.y)
-    {
-        // Wholly above.
-        p = R_ABOVE;
-        return false;
-    }
-    
-    if (a.y == r.y)
-    {
-        // Shared top edge.
-        p |= R_TEDGE;
-    }
-    else if (a.y < r.y)
-    {
-        // Need to split above.
-        p |= R_ABOVE;
-    }
-    
-    if (b.y == s.y)
-    {
-        // Shared bottom edge.
-        p |= R_BEDGE;
-    }
-    else if (b.y > s.y)
-    {
-        // Need to split below.
-        p |= R_BELOW;
-    }
-
-    if (a.x == r.x)
-    {
-        // Shared left edge.
-        p |= R_LEDGE;
-    }
-    else if (a.x < r.x)
-    {
-        // Need to split left.
-        p |= R_LEFT;
-    }
-
-    if (b.x == s.x)
-    {
-        // Shared right edge.
-        p |= R_REDGE;
-    }
-    else if (b.x > s.x)
-    {
-        // Need to split right.
-        p |= R_RIGHT;
-    }
-    
-    return true;
-}
-
-
-void Region::getBBox(BBox& bb)
-{
-    bb.a = _bbox.a;
-    bb.b = _bbox.b;
-}
-
-
-Region *Region::up(void)
-{
-    return _up;
-}
-
-
-Region *Region::down(void)
-{
-    return _down;
-}
-
-
-Region *Region::left(void)
-{
-    return _left;
-}
-
-
-Region *Region::right(void)
-{
-    return _right;
-}
-
-
-bool Region::isBlock(void)
-{
-    return !(_blocks.empty());
-}
-
-
-void Region::initialSplit(BBox& bbox, unsigned int pos, unsigned int& shapeId)
-{
-    Point& n1 = bbox.a;
-    Point& n2 = bbox.b;
-    Region *newR = this;
-    Region *left, *right, *top, *bottom;
-    
-    if (pos == R_INSIDE)
-    {
-        split(n2.y, R_HORI);
-        split(n2.x, R_VERT);
-        newR = split(n1.y, R_HORI);
-        newR = newR->split(n1.x, R_VERT);
-        
-        printf("%p - list %d add %d\n", newR,
-                (int) newR->_blocks.size(), shapeId);
-        newR->_blocks.push_back((int) shapeId);
-        newR->_blocks.sort();
-        newR->_blocks.unique();
-    }
-    else
-    {
-        Region *tar = NULL;
-        tar = newR->findRegion(n1.x, R_VERT);
-        if (pos & R_LEFT)
-        {
-            if (n1.x == tar->_bbox.a.x)
-            {
-                newR = tar->_right;
-            }
-            else if (n1.x == tar->_bbox.b.x)
-            {
-                newR = tar;
-            }
-            else
-            {
-                newR = tar->split(n1.x, R_VERT);
-            }
-        }
-        else if (!(pos & R_LEDGE))
-        {
-            newR = tar->split(n1.x, R_VERT);
-        }
-        left = newR;
-        
-        tar = left->findRegion(n1.y, R_HORI);
-        if (pos & R_ABOVE)
-        {
-            if (n1.y == tar->_bbox.a.y)
-            {
-                newR = tar->_down;
-            }
-            else if (n1.y == tar->_bbox.b.y)
-            {
-                newR = tar;
-            }
-            else
-            {
-                newR = tar->split(n1.y, R_HORI);
-            }
-        }
-        else if (!(pos & R_TEDGE))
-        {
-            newR = tar->split(n1.y, R_HORI);
-        }
-        top = newR;
-        
-        right = newR;
-        tar = newR->findRegion(n2.x, R_VERT);
-        if (pos & R_RIGHT)
-        {
-        
-            if (n2.x == tar->_bbox.a.x)
-            {
-                right = tar->_left;
-            }
-            else if (n2.x == tar->_bbox.b.x)
-            {
-                right = tar;
-            }
-            else
-            {
-                tar->split(n2.x, R_VERT);
-                right = tar;
-            }
-        }
-        else if (!(pos & R_REDGE))
-        {
-            tar->split(n2.x, R_VERT);
-            right = tar;
-        }
-
-        bottom = right;
-        tar = right->findRegion(n2.y, R_HORI);
-        if (pos & R_BELOW)
-        {
-            if (n2.y == tar->_bbox.a.y)
-            {
-                bottom = tar->_up;
-            }
-            else if (n2.y == tar->_bbox.b.y)
-            {
-                bottom = tar;
-            }
-            else
-            {
-                tar->split(n2.y, R_HORI);
-                bottom = tar;
-            }
-        }
-        else if (!(pos & R_BEDGE))
-        {
-            tar->split(n2.y, R_HORI);
-            bottom = tar;
-        } 
-
-        // top is actually top-left, and bottom is bottom-right.
-        Region *curr = top, *cptr = NULL;
-        while (curr->_bbox.b.y <= bottom->_bbox.b.y)
-        {
-            cptr = curr;
-            while (cptr->_bbox.b.x <= bottom->_bbox.b.x)
-            {
-                printf("%p - list %d add %d\n", cptr,
-                        (int) cptr->_blocks.size(), shapeId);
-                cptr->_blocks.push_back((int) shapeId);
-                cptr->_blocks.sort();
-                cptr->_blocks.unique();
-
-                cptr = cptr->_right;
-            }
-
-            curr = curr->_down;
-        }
-    }
-}
-
-
-// Returns the region containing the value 'pos' in the direction 'dir'.
-// Thus, if looking for the x value 55, you would pass R_VERT as 'dir'.
-// 'forMerge' specifies that the left or top block of a pair of regions
-// with the split value of 'pos' should be returned.
-Region *Region::findRegion(double pos, unsigned int dir, const bool forMerge)
-{
-    Region *curr = this;
-
-    if (dir & R_VERT)
-    {
-        while (pos > curr->_bbox.b.x)
-        {
-            curr = curr->_right;
-        }
-        while (pos < curr->_bbox.a.x)
-        {
-            curr = curr->_left;
-        }
-        if (forMerge)
-        {
-            if (pos == curr->_bbox.a.x)
-            {
-                curr = curr->_left;
-            }
-            if (pos != curr->_bbox.b.x)
-            {
-                // 'pos' is not on the boundary.
-                return NULL;
-            }
-        }
-    }
-    else if (dir & R_HORI)
-    {
-        while (pos > curr->_bbox.b.y)
-        {
-            curr = curr->_down;
-        }
-        while (pos < curr->_bbox.a.y)
-        {
-            curr = curr->_up;
-        }
-        if (forMerge)
-        {
-            if (pos == curr->_bbox.a.y)
-            {
-                curr = curr->_up;
-            }
-            if (pos != curr->_bbox.b.y)
-            {
-                // 'pos' is not on the boundary.
-                return NULL;
-            }
-        }
-    }
-    return curr;
-}
-
-
-Region *Region::split(double pos, unsigned int dir)
-{
-    Region *newR = NULL;
-    bool first = true;
-
-    if (dir & R_VERT)
-    {
-        newR = splitDir(pos, R_UP, first);
-        if (_down) _down->splitDir(pos, R_DOWN);
-    }
-    else if (dir & R_HORI)
-    {
-        newR = splitDir(pos, R_RIGHT, first);
-        if (_left)  _left->splitDir(pos, R_LEFT);
-    }
-    return newR;
-}
-
-
-void Region::merge(unsigned int dir)
-{
-    bool first = true;
-
-    if (dir & R_VERT)
-    {
-        mergeDir(R_UP, first);
-        if (_down) _down->mergeDir(R_DOWN);
-    }
-    else if (dir & R_HORI)
-    {
-        mergeDir(R_RIGHT, first);
-        if (_left)  _left->mergeDir(R_LEFT);
-    }
-}
-
-
-void Region::mergeRegion(Region *src)
-{
-    assert(src != NULL);
-
-    if (src == _left)
-    {
-        pairHor(src->_left, this);
-        _bbox.a.x = src->_bbox.a.x;
-    }
-    else if (src == _right)
-    {
-        pairHor(this, src->_right);
-        _bbox.b.x = src->_bbox.b.x;
-    }
-    else if (src == _up)
-    {
-        pairVer(src->_up, this);
-        _bbox.a.y = src->_bbox.a.y;
-    }
-    else if (src == _down)
-    {
-        pairVer(this, src->_down);
-        _bbox.b.y = src->_bbox.b.y;
-    }
-    else
-    {
-        fprintf(stderr, "Error: Avoid::Region::merge(): "
-                "Argument not adjoining region.\n");
-        abort();
-    }
-    mergeAttributes(src);
-    printf("DEL %p\n", src);
-    delete src;
-}
-
-
-Region *Region::splitDir(double pos, unsigned int dir, bool first)
-{
-    Point& o1 = _bbox.a;
-    Point& o2 = _bbox.b;
-
-    Region *newR = NULL;
-    
-    if (dir & R_VERT)
-    {
-        assert(pos > _bbox.a.x);
-        assert(pos < _bbox.b.x);
-        
-        // Vertical recursion:
-       
-        // Create new block.
-        Region *r  = new Region(pos, o1.y, o2.x, o2.y);
-        printf("NEW %p\n", r);
-        r->copyAttributes(this);
-
-        Region *o_up    = _up;
-        Region *o_down  = _down;
-
-        // Resize old block.
-        o2.x = pos;
-
-        pairHor(r, _right);
-        pairHor(this, r);
-        
-        if (dir & R_UP)
-        {
-            if (!first)  pairVer(r, _down->_right);
-        
-            if (o_up)    o_up->splitDir(pos, R_UP);
-        }
-        else if (dir & R_DOWN)
-        {
-            if (!first)  pairVer(_up->_right, r);
-        
-            if (o_down)  o_down->splitDir(pos, R_DOWN);
-        }
-        newR = r;
-    }
-    else if (dir & R_HORI)
-    {
-        // Vertical recursion:
-
-        // Create new block.
-        Region *b  = new Region(o1.x, pos, o2.x, o2.y);
-        printf("NEW %p\n", b);
-        b->copyAttributes(this);
-
-        Region *o_left  = _left;
-        Region *o_right = _right;
-    
-        // Resize old block.
-        o2.y = pos;
-        
-        pairVer(b, _down);
-        pairVer(this, b);
-
-        if (dir & R_LEFT)
-        {
-            if (!first)  pairHor(b, _right->_down);
-        
-            if (o_left)  o_left->splitDir(pos, R_LEFT);
-        }
-        else if (dir & R_RIGHT)
-        {
-            if (!first)  pairHor(_left->_down, b);
-        
-            if (o_right) o_right->splitDir(pos, R_RIGHT);
-        }
-        newR = b;
-    }
-    return newR;
-}
-
-
-void Region::mergeDir(unsigned int dir, bool first)
-{
-    if (dir & R_VERT)
-    {
-        assert(_right != NULL);
-
-        mergeRegion(_right);
-
-        if (dir & R_UP)
-        {
-            if (_up)    _up->mergeDir(dir, R_UP);
-        }
-        else if (dir & R_DOWN)
-        {
-            if (_down)  _down->mergeDir(dir, R_DOWN);
-        }
-    }
-    else if (dir & R_HORI)
-    {
-        assert(_down != NULL);
-
-        mergeRegion(_down);
-
-        if (dir & R_LEFT)
-        {
-            if (_left)  _left->mergeDir(dir, R_LEFT);
-        }
-        else if (dir & R_RIGHT)
-        {
-            if (_right) _right->mergeDir(dir, R_RIGHT);
-        }
-    }
-}
-
-
-void Region::copyAttributes(Region *src)
-{
-    _blocks = src->_blocks;
-}
-
-
-void Region::mergeAttributes(Region *src)
-{
-    _blocks.merge(src->_blocks);
-    _blocks.sort();
-    _blocks.unique();
-}
-
-
-//---------------------------
-// Static member functions:
-
-
-void Region::pairHor(Region *l, Region *r)
-{
-    if (l)  l->_right = r;
-    if (r)  r->_left  = l;
-}
-
-
-void Region::pairVer(Region *a, Region *b)
-{
-    if (a)  a->_down = b;
-    if (b)  b->_up   = a;
-}
-
-
-void Region::addShape(ShapeRef *shape)
-{
-    if (!centerRegion)
-    {
-        // Add new default region.
-        centerRegion = new Region();
-        printf("NEW %p\n", centerRegion);
-    }
-    BBox bbox;
-    // Get bounding box for added shape.
-    shape->boundingBox(bbox);
-    printBBox("Add", bbox);
-    
-    unsigned int shapeId = shape->id();
-
-    // Find starting point for overlap
-    Region *curr = centerRegion;
-    unsigned int pos = R_INSIDE;
-    while (!(curr->overlapCheck(bbox, pos)))
-    {
-        if (pos & R_LEFT)
-        {
-            curr = curr->_left;
-        }
-        else if (pos & R_RIGHT)
-        {
-            curr = curr->_right;
-        }
-        else if (pos & R_ABOVE)
-        {
-            curr = curr->_up;
-        }
-        else if (pos & R_BELOW)
-        {
-            curr = curr->_down;
-        }
-    }
-    
-    curr->initialSplit(bbox, pos, shapeId);
-    centerRegion = curr;
-}
-
-
-void Region::removeShape(ShapeRef *shape)
-{
-    const bool forMerge = true;
-
-    BBox bbox;
-    // Get bounding box for added shape.
-    shape->boundingBox(bbox);
-    printBBox("Remove", bbox);
-
-    unsigned int shapeId = shape->id();
-    
-    Region *aboveTop = centerRegion->findRegion(bbox.a.y, R_HORI, forMerge);
-    Region *aboveBottom = aboveTop->findRegion(bbox.b.y, R_HORI, forMerge);
-    Region *leftOfLeft = aboveBottom->findRegion(bbox.a.x, R_VERT, forMerge);
-    Region *leftOfRight = leftOfLeft->findRegion(bbox.b.x, R_VERT, forMerge);
-    
-    assert(aboveTop    != NULL);
-    assert(aboveBottom != NULL);
-    assert(leftOfLeft  != NULL);
-    assert(leftOfRight != NULL);
-
-    // Find Top Left.
-    Region *topLeft = leftOfLeft->_right;
-    while (topLeft->_bbox.a.y != aboveTop->_bbox.b.y)
-    {
-        topLeft = topLeft->_up;
-    }
-    
-    // Find Bottom Right.
-    Region *botRight = leftOfRight;
-    while (botRight->_bbox.b.y != aboveBottom->_bbox.b.y)
-    {
-        botRight = botRight->_down;
-    }
-    
-    // Clear blocking flag.
-    Region *curr = topLeft, *cptr = NULL;
-    while (curr->_bbox.b.y <= botRight->_bbox.b.y)
-    {
-        cptr = curr;
-        while (cptr->_bbox.b.x <= botRight->_bbox.b.x)
-        {
-            ShapeList& blocks = cptr->_blocks;
-            
-            assert(std::find(blocks.begin(), blocks.end(), (int) shapeId) !=
-                    blocks.end());
-            printf("%p - list %d remove %d\n", cptr,
-                    (int) blocks.size(), shapeId);
-            cptr->_blocks.remove((int) shapeId);
-
-            cptr = cptr->_right;
-        }
-
-        curr = curr->_down;
-    }
-    
-    // These two are safe since they don't invalidate the pointers
-    // to the regions that are inside the shape.
-    if (aboveBottom->safeToMerge(R_HORI))
-    {
-        aboveBottom->merge(R_HORI);
-    }
-    if (leftOfRight->safeToMerge(R_VERT))
-    {
-        leftOfRight->merge(R_VERT);
-    }
-    
-    // Be a bit more careful with these.
-    double leftX = leftOfLeft->_bbox.b.x;
-    if (aboveTop->safeToMerge(R_HORI))
-    {
-        aboveTop->merge(R_HORI);
-    }
-    // leftOfLeft may have been freed, so look for the new block at
-    // that position.
-    leftOfLeft = aboveTop->findRegion(leftX, R_VERT, forMerge);
-    assert(leftOfLeft);
-    if (leftOfLeft->safeToMerge(R_VERT))
-    {
-        leftOfLeft->merge(R_VERT);
-    }
-}
-
-
-bool Region::safeToMerge(unsigned int dir)
-{
-    bool unsafe = false;
-
-    if (dir == R_HORI)
-    {
-        printf("safeToMerge? y = %.3f... ", _bbox.b.y); 
-        unsafe |= unsafeToMerge(R_RIGHT);
-        if (_left)  unsafe |= _left->unsafeToMerge(R_LEFT);
-    }
-    else if (dir == R_VERT)
-    {
-        printf("safeToMerge? x = %.3f... ", _bbox.b.x); 
-        unsafe |= unsafeToMerge(R_DOWN);
-        if (_up)  unsafe |= _up->unsafeToMerge(R_UP);
-    }
-    printf("%s.\n", (unsafe) ? "no" : "yes");
-        
-    return !unsafe;
-}
-
-
-bool Region::unsafeToMerge(unsigned int dir)
-{
-    bool unsafe = false;
-    
-    if (dir & R_HORI)
-    {
-        // If they are not the same on both sides then we can merge.
-        if (_blocks != _down->_blocks)
-        {
-            printf("\n  _blocks:\n    ");
-            for (ShapeList::iterator i = _blocks.begin(); i != _blocks.end();
-                    ++i)
-            {
-                printf("%d ", *i);
-            }
-            printf("\n  _down->_blocks:\n    ");
-            for (ShapeList::iterator i = _down->_blocks.begin();
-                    i != _down->_blocks.end(); ++i)
-            {
-                printf("%d ", *i);
-            }
-            unsafe |= true;
-            printf("\n");
-        }
-
-        if ((dir == R_LEFT) && _left)
-        {
-            unsafe |= _left->unsafeToMerge(dir);
-        }
-        else if ((dir == R_RIGHT) && _right)
-        {
-            unsafe |= _right->unsafeToMerge(dir);
-        }
-    }
-    else if (dir & R_VERT)
-    {
-        if (_blocks !=  _right->_blocks)
-        {
-            printf("\n  _blocks:\n    ");
-            for (ShapeList::iterator i = _blocks.begin(); i != _blocks.end();
-                    ++i)
-            {
-                printf("%d ", *i);
-            }
-            printf("\n  _right->_blocks:\n    ");
-            for (ShapeList::iterator i = _right->_blocks.begin();
-                    i != _right->_blocks.end(); ++i)
-            {
-                printf("%d ", *i);
-            }
-            unsafe |= true;
-            printf("\n");
-        }
-        
-        if ((dir == R_UP) && _up)
-        {
-            unsafe |= _up->unsafeToMerge(dir);
-        }
-        else if ((dir == R_DOWN) && _down)
-        {
-            unsafe |= _down->unsafeToMerge(dir);
-        }
-    }
-    return unsafe;
-        
-}
-
-
-Region *Region::getTopLeftRegion(void)
-{
-    Region *curr = centerRegion;
-
-    while (curr && (curr->_up || curr->_left))
-    {
-        if (curr->_up)
-        {
-            curr = curr->_up;
-        }
-        else
-        {
-            curr = curr->_left;
-        }
-    }
-    return curr;
-}
-
-
-}
-
diff --git a/src/libavoid/region.h b/src/libavoid/region.h
deleted file mode 100644
index 6a44ccb08..000000000
--- a/src/libavoid/region.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * vim: ts=4 sw=4 et tw=0 wm=0
- *
- * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
-*/
-
-#ifndef AVOID_REGION_H
-#define AVOID_REGION_H
-
-#include <list>
-#include "libavoid/geomtypes.h"
-
-namespace Avoid {
-
-class ShapeRef;
-
-typedef std::list<int> ShapeList;
-
-
-class Region
-{
-    public:
-        Region();
-        Region(double x1, double y1, double x2, double y2);
-
-        bool overlapCheck(BBox& bbox, unsigned int& p);
-        void getBBox(BBox& bb);
-        Region *up(void);
-        Region *down(void);
-        Region *left(void);
-        Region *right(void);
-        void initialSplit(BBox& bbox, unsigned int pos, unsigned int& shapeId);
-        bool isBlock(void);
-        
-        static void pairHor(Region *l, Region *r);
-        static void pairVer(Region *a, Region *b);
-        static void addShape(ShapeRef *shape);
-        static void removeShape(ShapeRef *shape);
-        static Region *getTopLeftRegion(void);
-
-    private:
-        BBox _bbox;
-        Region *_left;
-        Region *_right;
-        Region *_up;
-        Region *_down;
-        ShapeList _blocks;
-        
-        Region *split(double pos, unsigned int dir); 
-        void merge(unsigned int dir);
-        void mergeRegion(Region *src); 
-        Region *findRegion(double pos, unsigned int dir,
-                const bool forMerge = false);
-        Region *splitDir(double pos, unsigned int dir, bool first = false); 
-        void mergeDir(unsigned int dir, bool first = false); 
-        void copyAttributes(Region *src);
-        void mergeAttributes(Region *src);
-        bool safeToMerge(unsigned int dir);
-        bool unsafeToMerge(unsigned int dir);
-};
-
-
-}
-
-#endif
diff --git a/src/libavoid/router.cpp b/src/libavoid/router.cpp
index df0bacd02..754c464b4 100644
--- a/src/libavoid/router.cpp
+++ b/src/libavoid/router.cpp
@@ -2,302 +2,667 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
-#include <cstdlib>
+
+#include <algorithm>
+#include <cmath>
+
 #include "libavoid/shape.h"
 #include "libavoid/router.h"
 #include "libavoid/visibility.h"
 #include "libavoid/connector.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/debug.h"
-#include "libavoid/region.h"
-#include "math.h"
-
-//#define ORTHOGONAL_ROUTING
+#include "libavoid/orthogonal.h"
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
 
-static const unsigned int infoAdd = 1;
-static const unsigned int infoDel = 2;
-static const unsigned int infoMov = 3;
+enum ActionType {
+    ShapeMove,
+    ShapeAdd,
+    ShapeRemove,
+    ConnChange
+};
 
+typedef std::list<std::pair<unsigned int, ConnEnd> > ConnUpdateList;
 
-class MoveInfo {
+class ActionInfo {
     public:
-        MoveInfo(ShapeRef *s, Polygn *p, bool fM)
-            : shape(s)
-            , newPoly(copyPoly(*p))
-            , firstMove(fM)
-        { }
-        ~MoveInfo()
+        ActionInfo(ActionType t, ShapeRef *s, const Polygon& p, bool fM)
+            : type(t),
+              objPtr(s),
+              newPoly(p),
+              firstMove(fM)
+        {
+            COLA_ASSERT(type == ShapeMove);
+        }
+        ActionInfo(ActionType t, ShapeRef *s)
+            : type(t),
+              objPtr(s)
+        {
+            COLA_ASSERT(type != ConnChange);
+        }
+        ActionInfo(ActionType t, ConnRef *c)
+            : type(t),
+              objPtr(c)
+        {
+            COLA_ASSERT(type == ConnChange);
+        }
+        ~ActionInfo()
+        {
+        }
+        ShapeRef *shape(void) const
+        {
+            COLA_ASSERT((type == ShapeMove) || (type == ShapeAdd) || 
+                    (type == ShapeRemove));
+            return (static_cast<ShapeRef *> (objPtr));
+        }
+        ConnRef *conn(void) const
+        {
+            COLA_ASSERT(type == ConnChange);
+            return (static_cast<ConnRef *> (objPtr));
+        }
+        bool operator==(const ActionInfo& rhs) const
+        {
+            return (type == rhs.type) && (objPtr == rhs.objPtr);
+        }
+        bool operator<(const ActionInfo& rhs) const
         {
-            freePoly(newPoly);
+            if (type != rhs.type)
+            {
+                return type < rhs.type;
+            }
+            return objPtr < rhs.objPtr;
         }
-        ShapeRef *shape;
-        Polygn newPoly;
+        ActionType type;
+        void *objPtr;
+        Polygon newPoly;
         bool firstMove;
+        ConnUpdateList conns;
 };
 
 
-
-Router::Router()
-    : PartialTime(false)
-    , SimpleRouting(false)
-    , segmt_penalty(0)
-    , angle_penalty(0)
-    , crossing_penalty(200)
-    // Algorithm options:
-    , UseAStarSearch(true)
-    , IgnoreRegions(true)
-    , SelectiveReroute(true)
-    , IncludeEndpoints(true)
-    , UseLeesAlgorithm(false)
-    , InvisibilityGrph(true)
-    , ConsolidateMoves(true)
-    , PartialFeedback(false)
-    // Instrumentation:
-    , st_checked_edges(0)
-#ifdef LINEDEBUG
-    , avoid_screen(NULL)
+Router::Router(const unsigned int flags)
+    : visOrthogGraph(true),
+      PartialTime(false),
+      SimpleRouting(false),
+      ClusteredRouting(true),
+      // Poly-line algorithm options:
+      IgnoreRegions(true),
+      UseLeesAlgorithm(true),
+      InvisibilityGrph(true),
+      // General algorithm options:
+      SelectiveReroute(true),
+      PartialFeedback(false),
+      RubberBandRouting(false),
+      // Instrumentation:
+      st_checked_edges(0),
+#ifdef LIBAVOID_SDL
+      avoid_screen(NULL),
 #endif
-{ }
+      _largestAssignedId(0),
+      _consolidateActions(true),
+      _orthogonalNudgeDistance(4.0),
+      // Mode options:
+      _polyLineRouting(false),
+      _orthogonalRouting(false),
+      _staticGraphInvalidated(true),
+      _inCrossingPenaltyReroutingStage(false)
+{
+    // At least one of the Routing modes must be set.
+    COLA_ASSERT(flags & (PolyLineRouting | OrthogonalRouting));
 
+    if (flags & PolyLineRouting)
+    {
+        _polyLineRouting = true;
+    }
+    if (flags & OrthogonalRouting)
+    {
+        _orthogonalRouting = true;
+    }
 
+    for (size_t p = 0; p < lastPenaltyMarker; ++p)
+    {
+        _routingPenalties[p] = 0.0;
+    }
+    _routingPenalties[clusterCrossingPenalty] = 4000;
+}
 
 
-void Router::addShape(ShapeRef *shape)
+Router::~Router()
 {
-    unsigned int pid = shape->id();
-    Polygn poly = shape->poly();
+    // Delete remaining connectors.
+    ConnRefList::iterator conn = connRefs.begin();
+    while (conn != connRefs.end())
+    {
+        db_printf("Deleting connector %u in ~Router()\n", (*conn)->id());
+        delete *conn;
+        conn = connRefs.begin();
+    }
 
-    adjustContainsWithAdd(poly, pid);
-    
-    // o  Check all visibility edges to see if this one shape
-    //    blocks them.
-    newBlockingShape(&poly, pid);
+    // Remove remaining shapes.
+    ShapeRefList::iterator shape = shapeRefs.begin();
+    while (shape != shapeRefs.end())
+    {
+        ShapeRef *shapePtr = *shape;
+        db_printf("Deleting shape %u in ~Router()\n", shapePtr->id());
+        if (shapePtr->isActive())
+        {
+            shapePtr->removeFromGraph();
+            shapePtr->makeInactive();
+        }
+        delete shapePtr;
+        shape = shapeRefs.begin();
+    }
+
+    // Cleanup orphaned orthogonal graph vertices.
+    destroyOrthogonalVisGraph();
+
+    COLA_ASSERT(connRefs.size() == 0);
+    COLA_ASSERT(shapeRefs.size() == 0);
+    COLA_ASSERT(visGraph.size() == 0);
+    COLA_ASSERT(invisGraph.size() == 0);
+}
 
-#ifdef ORTHOGONAL_ROUTING
-    Region::addShape(shape);
-#endif
 
-    // o  Calculate visibility for the new vertices.
-    if (UseLeesAlgorithm)
+void Router::modifyConnector(ConnRef *conn, const unsigned int type,
+        const ConnEnd& connEnd)
+{
+    ActionInfo modInfo(ConnChange, conn);
+    
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found == actionList.end())
     {
-        shapeVisSweep(shape);
+        modInfo.conns.push_back(std::make_pair(type, connEnd));
+        actionList.push_back(modInfo);
     }
     else
     {
-        shapeVis(shape);
+        found->conns.push_back(std::make_pair(type, connEnd));
+    }
+
+    if (!_consolidateActions)
+    {
+        processTransaction();
     }
-    callbackAllInvalidConnectors();
 }
 
 
-void Router::delShape(ShapeRef *shape)
+void Router::modifyConnector(ConnRef *conn)
 {
-    unsigned int pid = shape->id();
+    ActionInfo modInfo(ConnChange, conn);
+    
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found == actionList.end())
+    {
+        actionList.push_back(modInfo);
+    }
 
-    // Delete items that are queued in the movList.
-    for (MoveInfoList::iterator it = moveList.begin(); it != moveList.end(); )
+    if (!_consolidateActions)
     {
-        if ((*it)->shape->id() == pid)
-        {
-            MoveInfoList::iterator doomed = it;
-            ++it;
-            moveList.erase(doomed);
-        }
-        else
-        {
-            ++it;
-        }
+        processTransaction();
     }
+}
 
-    // o  Remove entries related to this shape's vertices
-    shape->removeFromGraph();
-    
-    if (SelectiveReroute)
+
+void Router::removeQueuedConnectorActions(ConnRef *conn)
+{
+    ActionInfo modInfo(ConnChange, conn);
+
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), modInfo);
+    if (found != actionList.end())
     {
-        markConnectors(shape);
+        actionList.erase(found);
     }
+}
 
-    adjustContainsWithDel(pid);
-    
-#ifdef ORTHOGONAL_ROUTING
-    Region::removeShape(shape);
-#endif
 
-    delete shape;
+void Router::addShape(ShapeRef *shape)
+{
+    // There shouldn't be remove events or move events for the same shape
+    // already in the action list.
+    // XXX: Possibly we could handle this by ordering them intelligently.
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeRemove, shape)) == actionList.end());
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeMove, shape)) == actionList.end());
+
+    ActionInfo addInfo(ShapeAdd, shape);
     
-    // o  Check all edges that were blocked by this shape.
-    if (InvisibilityGrph)
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), addInfo);
+    if (found == actionList.end())
     {
-        checkAllBlockedEdges(pid);
+        actionList.push_back(addInfo);
     }
-    else
+
+    if (!_consolidateActions)
     {
-        // check all edges not in graph
-        checkAllMissingEdges();
+        processTransaction();
     }
-    callbackAllInvalidConnectors();
 }
 
 
-void Router::moveShape(ShapeRef *shape, Polygn *newPoly, const bool first_move)
+void Router::removeShape(ShapeRef *shape)
 {
+    // There shouldn't be add events events for the same shape already 
+    // in the action list.
+    // XXX: Possibly we could handle this by ordering them intelligently.
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeAdd, shape)) == actionList.end());
+
+    // Delete any ShapeMove entries for this shape in the action list.
+    ActionInfoList::iterator found = find(actionList.begin(), 
+            actionList.end(), ActionInfo(ShapeMove, shape));
+    if (found != actionList.end())
+    {
+        actionList.erase(found);
+    }
+
+    // Add the ShapeRemove entry.
+    ActionInfo remInfo(ShapeRemove, shape);
+    found = find(actionList.begin(), actionList.end(), remInfo);
+    if (found == actionList.end())
+    {
+        actionList.push_back(remInfo);
+    }
+
+    if (!_consolidateActions)
+    {
+        processTransaction();
+    }
+}
+
+
+void Router::moveShape(ShapeRef *shape, const double xDiff, const double yDiff)
+{
+    Polygon newPoly = shape->polygon();
+    newPoly.translate(xDiff, yDiff);
+
+    moveShape(shape, newPoly);
+}
+
+
+void Router::moveShape(ShapeRef *shape, const Polygon& newPoly, 
+        const bool first_move)
+{
+    // There shouldn't be remove events or add events for the same shape
+    // already in the action list.
+    // XXX: Possibly we could handle this by ordering them intelligently.
+    COLA_ASSERT(find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeRemove, shape)) == actionList.end());
+    
+    if (find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeAdd, shape)) != actionList.end())
+    {
+        // The Add is enough, no need for the Move action too.
+        return;
+    }
+
+    ActionInfo moveInfo(ShapeMove, shape, newPoly, first_move);
     // Sanely cope with the case where the user requests moving the same
     // shape multiple times before rerouting connectors.
-    bool alreadyThere = false;
-    unsigned int id = shape->id();
-    MoveInfoList::iterator finish = moveList.end();
-    for (MoveInfoList::iterator it = moveList.begin(); it != finish; ++it)
+    ActionInfoList::iterator found = 
+            find(actionList.begin(), actionList.end(), moveInfo);
+
+    if (found != actionList.end())
     {
-        if ((*it)->shape->id() == id)
+        if (!SimpleRouting)
         {
-            if (!SimpleRouting)
-            {
-                fprintf(stderr,
-                        "warning: multiple moves requested for shape %d.\n",
-                        (int) id);
-            }
-            // Just update the MoveInfo with the second polygon, but
-            // leave the firstMove setting alone.
-            (*it)->newPoly = copyPoly(*newPoly);
-            alreadyThere = true;
+            db_printf("warning: multiple moves requested for shape %d "
+                    "within a single transaction.\n", (int) shape->id());
         }
+        // Just update the ActionInfo with the second polygon, but
+        // leave the firstMove setting alone.
+        found->newPoly = newPoly;
+    }
+    else 
+    {
+        actionList.push_back(moveInfo);
     }
 
-    if (!alreadyThere)
+    if (!_consolidateActions)
     {
-        MoveInfo *moveInfo = new MoveInfo(shape, newPoly, first_move);
-        moveList.push_back(moveInfo);
+        processTransaction();
     }
+}
+
+
+void Router::setStaticGraphInvalidated(const bool invalidated)
+{
+    _staticGraphInvalidated = invalidated;
+}
+
+
+void Router::destroyOrthogonalVisGraph(void)
+{
+    // Remove orthogonal visibility graph edges.
+    visOrthogGraph.clear();
 
-    if (!ConsolidateMoves)
+    // Remove the now orphaned vertices.
+    VertInf *curr = vertices.shapesBegin();
+    while (curr)
     {
-        processMoves();
+        if (curr->orphaned() && (curr->id == dummyOrthogID))
+        {
+            VertInf *following = vertices.removeVertex(curr);
+            delete curr;
+            curr = following;
+            continue;
+        }
+        curr = curr->lstNext;
     }
 }
 
 
-void Router::processMoves(void)
+void Router::regenerateStaticBuiltGraph(void)
 {
-    // If SimpleRouting, then don't update yet.
-    if (moveList.empty() || SimpleRouting)
+    // Here we do talks involved in updating the static-built visibility 
+    // graph (if necessary) before we do any routing.
+    if (_staticGraphInvalidated)
     {
-        return;
+        if (_orthogonalRouting)
+        {
+            destroyOrthogonalVisGraph();
+
+            timers.Register(tmOrthogGraph, timerStart);
+            // Regenerate a new visibility graph.
+            generateStaticOrthogonalVisGraph(this);
+            
+            timers.Stop();
+        }
+        _staticGraphInvalidated = false;
     }
+}
+
 
-    MoveInfoList::iterator curr;
-    MoveInfoList::iterator finish = moveList.end();
-    for (curr = moveList.begin(); curr != finish; ++curr)
+bool Router::shapeInQueuedActionList(ShapeRef *shape) const
+{
+    bool foundAdd = find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeAdd, shape)) != actionList.end();
+    bool foundRem = find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeRemove, shape)) != actionList.end();
+    bool foundMove = find(actionList.begin(), actionList.end(), 
+                ActionInfo(ShapeMove, shape)) != actionList.end();
+
+    return (foundAdd || foundRem || foundMove);
+}
+
+
+bool Router::transactionUse(void) const
+{
+    return _consolidateActions;
+}
+
+
+void Router::setTransactionUse(const bool transactions)
+{
+    _consolidateActions = transactions;
+}
+
+
+bool Router::processTransaction(void)
+{
+    bool notPartialTime = !(PartialFeedback && PartialTime);
+    bool seenShapeMovesOrDeletes = false;
+
+    // If SimpleRouting, then don't update here.
+    if (actionList.empty() || SimpleRouting)
+    {
+        return false;
+    }
+
+    actionList.sort();
+    ActionInfoList::iterator curr;
+    ActionInfoList::iterator finish = actionList.end();
+    for (curr = actionList.begin(); curr != finish; ++curr)
     {
-        MoveInfo *moveInf = *curr;
-        ShapeRef *shape = moveInf->shape;
-        Polygn *newPoly = &(moveInf->newPoly);
-        bool first_move = moveInf->firstMove;
+        ActionInfo& actInf = *curr;
+        if (!((actInf.type == ShapeRemove) || (actInf.type == ShapeMove)))
+        {
+            // Not a move or remove action, so don't do anything.
+            continue;
+        }
+        seenShapeMovesOrDeletes = true;
+
+        ShapeRef *shape = actInf.shape();
+        bool isMove = (actInf.type == ShapeMove);
+        bool first_move = actInf.firstMove;
 
         unsigned int pid = shape->id();
-        bool notPartialTime = !(PartialFeedback && PartialTime);
 
         // o  Remove entries related to this shape's vertices
         shape->removeFromGraph();
         
-        if (SelectiveReroute && (notPartialTime || first_move))
+        if (SelectiveReroute && (!isMove || notPartialTime || first_move))
         {
             markConnectors(shape);
         }
 
         adjustContainsWithDel(pid);
         
-#ifdef ORTHOGONAL_ROUTING
-        Region::removeShape(shape);
-#endif
-
-        shape->setNewPoly(*newPoly);
-
-        adjustContainsWithAdd(*newPoly, pid);
-
         // Ignore this shape for visibility.
         // XXX: We don't really need to do this if we're not using Partial
         //      Feedback.  Without this the blocked edges still route
         //      around the shape until it leaves the connector.
         shape->makeInactive();
-        
     }
     
-    if (InvisibilityGrph)
+    if (seenShapeMovesOrDeletes && _polyLineRouting)
     {
-        for (curr = moveList.begin(); curr != finish; ++curr)
+        if (InvisibilityGrph)
         {
-            MoveInfo *moveInf = *curr;
-            ShapeRef *shape = moveInf->shape;
-            unsigned int pid = shape->id();
-            
-            // o  Check all edges that were blocked by this shape.
-            checkAllBlockedEdges(pid);
+            for (curr = actionList.begin(); curr != finish; ++curr)
+            {
+                ActionInfo& actInf = *curr;
+                if (!((actInf.type == ShapeRemove) || 
+                            (actInf.type == ShapeMove)))
+                {
+                    // Not a move or remove action, so don't do anything.
+                    continue;
+                }
+
+                // o  Check all edges that were blocked by this shape.
+                checkAllBlockedEdges(actInf.shape()->id());
+            }
+        }
+        else
+        {
+            // check all edges not in graph
+            checkAllMissingEdges();
         }
-    }
-    else
-    {
-        // check all edges not in graph
-        checkAllMissingEdges();
     }
 
-    while ( ! moveList.empty() )
+    for (curr = actionList.begin(); curr != finish; ++curr)
     {
-        MoveInfo *moveInf = moveList.front();
-        ShapeRef *shape = moveInf->shape;
-        Polygn *newPoly = &(moveInf->newPoly);
+        ActionInfo& actInf = *curr;
+        if (!((actInf.type == ShapeAdd) || (actInf.type == ShapeMove)))
+        {
+            // Not a move or add action, so don't do anything.
+            continue;
+        }
+
+        ShapeRef *shape = actInf.shape();
+        Polygon& newPoly = actInf.newPoly;
+        bool isMove = (actInf.type == ShapeMove);
 
         unsigned int pid = shape->id();
-        bool notPartialTime = !(PartialFeedback && PartialTime);
 
         // Restore this shape for visibility.
         shape->makeActive();
+        
+        if (isMove)
+        {
+            shape->setNewPoly(newPoly);
+        }
+        const Polygon& shapePoly = shape->polygon();
 
-#ifdef ORTHOGONAL_ROUTING
-        Region::addShape(shape);
-#endif
+        adjustContainsWithAdd(shapePoly, pid);
 
-        // o  Check all visibility edges to see if this one shape
-        //    blocks them.
-        if (notPartialTime)
+        if (_polyLineRouting)
         {
-            newBlockingShape(newPoly, pid);
+            // o  Check all visibility edges to see if this one shape
+            //    blocks them.
+            if (!isMove || notPartialTime)
+            {
+                newBlockingShape(shapePoly, pid);
+            }
+
+            // o  Calculate visibility for the new vertices.
+            if (UseLeesAlgorithm)
+            {
+                shapeVisSweep(shape);
+            }
+            else
+            {
+                shapeVis(shape);
+            }
         }
+    }
 
-        // o  Calculate visibility for the new vertices.
-        if (UseLeesAlgorithm)
+    // Update connector endpoints.
+    for (curr = actionList.begin(); curr != finish; ++curr)
+    {
+        ActionInfo& actInf = *curr;
+        if (actInf.type != ConnChange)
         {
-            shapeVisSweep(shape);
+            continue;
         }
-        else
+        for (ConnUpdateList::iterator conn = actInf.conns.begin();
+                conn != actInf.conns.end(); ++conn)
         {
-            shapeVis(shape);
+            actInf.conn()->updateEndPoint(conn->first, conn->second);
         }
-        
-        moveList.pop_front();
-        delete moveInf;
     }
-    callbackAllInvalidConnectors();
+    // Clear the actionList.
+    actionList.clear();
+    
+    _staticGraphInvalidated = true;
+    rerouteAndCallbackConnectors();
+
+    return true;
+}
+
+
+void Router::addCluster(ClusterRef *cluster)
+{
+    cluster->makeActive();
+    
+    unsigned int pid = cluster->id();
+    ReferencingPolygon& poly = cluster->polygon();
+
+    adjustClustersWithAdd(poly, pid);
+}
+
+
+void Router::delCluster(ClusterRef *cluster)
+{
+    cluster->makeInactive();
+    
+    unsigned int pid = cluster->id();
+    
+    adjustClustersWithDel(pid);
+}
+
+
+void Router::setOrthogonalNudgeDistance(const double dist)
+{
+    COLA_ASSERT(dist >= 0);
+    _orthogonalNudgeDistance = dist;
+}
+
+
+double Router::orthogonalNudgeDistance(void) const
+{
+    return _orthogonalNudgeDistance;
+}
+
+
+unsigned int Router::assignId(const unsigned int suggestedId)
+{
+    // If the suggestedId is zero, then we assign the object the next
+    // smallest unassigned ID, otherwise we trust the ID given is unique.
+    unsigned int assignedId = (suggestedId == 0) ? 
+            (_largestAssignedId + 1) : suggestedId;
+    
+    // Have the router record if this ID is larger than the _largestAssignedId.
+    _largestAssignedId = std::max(_largestAssignedId, assignedId);
+
+    // If assertions are enabled, then we check that this ID really is unique.
+    COLA_ASSERT(idIsUnique(assignedId));
+
+    return assignedId;
+}
+
+
+    // Returns whether the given ID is unique among all objects known by the
+    // router.  Outputs a warning if the ID is found ore than once. 
+    // It is expected this is only going to be called from assertions while
+    // debugging, so efficiency is not an issue and we just iterate over all
+    // objects.
+bool Router::idIsUnique(const unsigned int id) const 
+{
+    unsigned int count = 0;
+
+    // Examine shapes.
+    for (ShapeRefList::const_iterator i = shapeRefs.begin(); 
+            i != shapeRefs.end(); ++i) 
+    {
+        if ((*i)->id() == id)
+        {
+            count++;
+        }
+    }
+
+    // Examine connectors.
+    for (ConnRefList::const_iterator i = connRefs.begin(); 
+            i != connRefs.end(); ++i) 
+    {
+        if ((*i)->id() == id)
+        {
+            count++;
+        }
+    }
+
+    // Examine clusters.
+    for (ClusterRefList::const_iterator i = clusterRefs.begin(); 
+            i != clusterRefs.end(); ++i) 
+    {
+        if ((*i)->id() == id)
+        {
+            count++;
+        }
+    }
+
+    if (count > 1)
+    {
+        db_printf("Warning:\tlibavoid object ID %d not unique.\n", id);
+        return false;
+    }
+    return true;
 }
 
 
@@ -313,13 +678,15 @@ void Router::processMoves(void)
 void Router::attachedConns(IntList &conns, const unsigned int shapeId,
         const unsigned int type)
 {
-    ConnRefList::iterator fin = connRefs.end();
-    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) {
-
-        if ((type & runningTo) && ((*i)->_dstId == shapeId)) {
+    ConnRefList::const_iterator fin = connRefs.end();
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        if ((type & runningTo) && ((*i)->_dstId == shapeId)) 
+        {
             conns.push_back((*i)->_id);
         }
-        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) {
+        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) 
+        {
             conns.push_back((*i)->_id);
         }
     }
@@ -331,16 +698,19 @@ void Router::attachedConns(IntList &conns, const unsigned int shapeId,
 void Router::attachedShapes(IntList &shapes, const unsigned int shapeId,
         const unsigned int type)
 {
-    ConnRefList::iterator fin = connRefs.end();
-    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) {
-        if ((type & runningTo) && ((*i)->_dstId == shapeId)) {
+    ConnRefList::const_iterator fin = connRefs.end();
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        if ((type & runningTo) && ((*i)->_dstId == shapeId)) 
+        {
             if ((*i)->_srcId != 0)
             {
                 // Only if there is a shape attached to the other end.
                 shapes.push_back((*i)->_srcId);
             }
         }
-        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) {
+        else if ((type & runningFrom) && ((*i)->_srcId == shapeId)) 
+        {
             if ((*i)->_dstId != 0)
             {
                 // Only if there is a shape attached to the other end.
@@ -351,18 +721,127 @@ void Router::attachedShapes(IntList &shapes, const unsigned int shapeId,
 }
 
 
-    // It's intended this function is called after shape movement has 
-    // happened to alert connectors that they need to be rerouted.
-void Router::callbackAllInvalidConnectors(void)
+    // It's intended this function is called after visibility changes 
+    // resulting from shape movement have happened.  It will alert 
+    // rerouted connectors (via a callback) that they need to be redrawn.
+void Router::rerouteAndCallbackConnectors(void)
+{
+    std::set<ConnRef *> reroutedConns;
+    ConnRefList::const_iterator fin = connRefs.end();
+    
+    // Updating the orthogonal visibility graph if necessary. 
+    regenerateStaticBuiltGraph();
+
+    timers.Register(tmOrthogRoute, timerStart);
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        (*i)->_needs_repaint = false;
+        bool rerouted = (*i)->generatePath();
+        if (rerouted)
+        {
+            reroutedConns.insert(*i);
+        }
+    }
+    timers.Stop();
+
+    // Find and reroute crossing connectors if crossing penalties are set.
+    improveCrossings();
+
+    // Perform centring and nudging for othogonal routes.
+    improveOrthogonalRoutes(this);
+
+    // Alert connectors that they need redrawing.
+    for (ConnRefList::const_iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        (*i)->_needs_repaint = true;
+        (*i)->performCallback();
+    }
+}
+
+
+typedef std::set<ConnRef *> ConnRefSet;
+
+void Router::improveCrossings(void)
 {
+    const double crossing_penalty = routingPenalty(crossingPenalty);
+    const double shared_path_penalty = routingPenalty(fixedSharedPathPenalty);
+    if ((crossing_penalty == 0) && (shared_path_penalty == 0))
+    {
+        // No penalties, return.
+        return;
+    }
+    
+    // Find crossings and reroute connectors.
+    _inCrossingPenaltyReroutingStage = true;
+    ConnRefSet crossingConns;
     ConnRefList::iterator fin = connRefs.end();
-    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) {
-        (*i)->handleInvalid();
+    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        Avoid::Polygon& iRoute = (*i)->routeRef();
+        ConnRefList::iterator j = i;
+        for (++j; j != fin; ++j) 
+        {
+            if ((crossingConns.find(*i) != crossingConns.end()) && 
+                    (crossingConns.find(*j) != crossingConns.end()))
+            {
+                // We already know both these have crossings.
+                continue;
+            }
+            // Determine if this pair cross.
+            Avoid::Polygon& jRoute = (*j)->routeRef();
+            CrossingsInfoPair crossingInfo = std::make_pair(0, 0);
+            bool meetsPenaltyCriteria = false;
+            for (size_t jInd = 1; jInd < jRoute.size(); ++jInd)
+            {
+                const bool finalSegment = ((jInd + 1) == jRoute.size());
+                CrossingsInfoPair crossingInfo = countRealCrossings(
+                        iRoute, true, jRoute, jInd, false, 
+                        finalSegment, NULL, NULL, *i, *j);
+                
+                if ((shared_path_penalty > 0) && 
+                    (crossingInfo.second & CROSSING_SHARES_PATH) && 
+                    (crossingInfo.second & CROSSING_SHARES_FIXED_SEGMENT) && 
+                    !(crossingInfo.second & CROSSING_SHARES_PATH_AT_END)) 
+                {
+                    // We are penalising fixedSharedPaths and there is a
+                    // fixedSharedPath.
+                    meetsPenaltyCriteria = true;
+                    break;
+                }
+                else if ((crossing_penalty > 0) && (crossingInfo.first > 0))
+                {
+                    // We are penalising crossings and this is a crossing.
+                    meetsPenaltyCriteria = true;
+                    break;
+                }
+            }
+            if (meetsPenaltyCriteria)
+            {
+                crossingConns.insert(*i);
+                crossingConns.insert(*j);
+            }
+        }
+    }
+
+    for (ConnRefSet::iterator i = crossingConns.begin(); 
+            i != crossingConns.end(); ++i)
+    {
+        ConnRef *conn = *i;
+        conn->makePathInvalid();
+        // XXX: Could we free these routes here for extra savings?
+        // conn->freeRoutes();
+    }
+    for (ConnRefSet::iterator i = crossingConns.begin(); 
+            i != crossingConns.end(); ++i)
+    {
+        ConnRef *conn = *i;
+        conn->generatePath();
     }
+    _inCrossingPenaltyReroutingStage = false;
 }
 
 
-void Router::newBlockingShape(Polygn *poly, int pid)
+void Router::newBlockingShape(const Polygon& poly, int pid)
 {
     // o  Check all visibility edges to see if this one shape
     //    blocks them.
@@ -382,8 +861,11 @@ void Router::newBlockingShape(Polygn *poly, int pid)
             Point e2 = points.second;
             bool blocked = false;
 
-            bool ep_in_poly1 = !(eID1.isShape) ? inPoly(*poly, e1) : false;
-            bool ep_in_poly2 = !(eID2.isShape) ? inPoly(*poly, e2) : false;
+            bool countBorder = false;
+            bool ep_in_poly1 = !(eID1.isShape) ? 
+                    inPoly(poly, e1, countBorder) : false;
+            bool ep_in_poly2 = !(eID2.isShape) ? 
+                    inPoly(poly, e2, countBorder) : false;
             if (ep_in_poly1 || ep_in_poly2)
             {
                 // Don't check edges that have a connector endpoint
@@ -391,10 +873,14 @@ void Router::newBlockingShape(Polygn *poly, int pid)
                 continue;
             }
 
-            for (int pt_i = 0; pt_i < poly->pn; pt_i++)
+            bool seenIntersectionAtEndpoint = false;
+            for (size_t pt_i = 0; pt_i < poly.size(); ++pt_i)
             {
-                int pt_n = (pt_i == (poly->pn - 1)) ? 0 : pt_i + 1;
-                if (segmentIntersect(e1, e2, poly->ps[pt_i], poly->ps[pt_n]))
+                size_t pt_n = (pt_i == (poly.size() - 1)) ? 0 : pt_i + 1;
+                const Point& pi = poly.ps[pt_i];
+                const Point& pn = poly.ps[pt_n];
+                if (segmentShapeIntersect(e1, e2, pi, pn, 
+                        seenIntersectionAtEndpoint))
                 {
                     blocked = true;
                     break;
@@ -422,7 +908,7 @@ void Router::newBlockingShape(Polygn *poly, int pid)
 
 void Router::checkAllBlockedEdges(int pid)
 {
-    assert(InvisibilityGrph);
+    COLA_ASSERT(InvisibilityGrph);
 
     for (EdgeInf *iter = invisGraph.begin(); iter != invisGraph.end() ; )
     {
@@ -444,18 +930,9 @@ void Router::checkAllBlockedEdges(int pid)
 
 void Router::checkAllMissingEdges(void)
 {
-    assert(!InvisibilityGrph);
+    COLA_ASSERT(!InvisibilityGrph);
 
-    VertInf *first = NULL;
-
-    if (IncludeEndpoints)
-    {
-        first = vertices.connsBegin();
-    }
-    else
-    {
-        first = vertices.shapesBegin();
-    }
+    VertInf *first = vertices.connsBegin();
 
     VertInf *pend = vertices.end();
     for (VertInf *i = first; i != pend; i = i->lstNext)
@@ -489,39 +966,79 @@ void Router::checkAllMissingEdges(void)
 void Router::generateContains(VertInf *pt)
 {
     contains[pt->id].clear();
+    enclosingClusters[pt->id].clear();
 
-    ShapeRefList::iterator finish = shapeRefs.end();
-    for (ShapeRefList::iterator i = shapeRefs.begin(); i != finish; ++i)
+    // Don't count points on the border as being inside.
+    bool countBorder = false;
+
+    // Compute enclosing shapes.
+    ShapeRefList::const_iterator finish = shapeRefs.end();
+    for (ShapeRefList::const_iterator i = shapeRefs.begin(); i != finish; ++i)
     {
-        Polygn poly = copyPoly(*i);
-        if (inPoly(poly, pt->point))
+        if (inPoly((*i)->polygon(), pt->point, countBorder))
         {
             contains[pt->id].insert((*i)->id());
         }
-        freePoly(poly);
+    }
+
+    // Computer enclosing Clusters
+    ClusterRefList::const_iterator clFinish = clusterRefs.end();
+    for (ClusterRefList::const_iterator i = clusterRefs.begin(); 
+            i != clFinish; ++i)
+    {
+        if (inPolyGen((*i)->polygon(), pt->point))
+        {
+            enclosingClusters[pt->id].insert((*i)->id());
+        }
     }
 }
 
 
-void Router::adjustContainsWithAdd(const Polygn& poly, const int p_shape)
+void Router::adjustClustersWithAdd(const PolygonInterface& poly, 
+        const int p_cluster)
 {
     for (VertInf *k = vertices.connsBegin(); k != vertices.shapesBegin();
             k = k->lstNext)
     {
-        if (inPoly(poly, k->point))
+        if (inPolyGen(poly, k->point))
         {
-            contains[k->id].insert(p_shape);
+            enclosingClusters[k->id].insert(p_cluster);
         }
     }
 }
 
 
-void Router::adjustContainsWithDel(const int p_shape)
+void Router::adjustClustersWithDel(const int p_cluster)
+{
+    for (ContainsMap::iterator k = enclosingClusters.begin();
+            k != enclosingClusters.end(); ++k)
+    {
+        (*k).second.erase(p_cluster);
+    }
+}
+
+
+void Router::adjustContainsWithAdd(const Polygon& poly, const int p_shape)
 {
+    // Don't count points on the border as being inside.
+    bool countBorder = false;
+
     for (VertInf *k = vertices.connsBegin(); k != vertices.shapesBegin();
             k = k->lstNext)
     {
-        contains[k->id].erase(p_shape);
+        if (inPoly(poly, k->point, countBorder))
+        {
+            contains[k->id].insert(p_shape);
+        }
+    }
+}
+
+
+void Router::adjustContainsWithDel(const int p_shape)
+{
+    for (ContainsMap::iterator k = contains.begin(); k != contains.end(); ++k)
+    {
+        (*k).second.erase(p_shape);
     }
 }
 
@@ -537,14 +1054,21 @@ static double AngleAFromThreeSides(const double a, const double b,
 
 void Router::markConnectors(ShapeRef *shape)
 {
-    assert(SelectiveReroute);
+    if (RubberBandRouting)
+    {
+        // When rubber-band routing, we do no reroute connectors that
+        // may have a better route, only invalid connectors.
+        return;
+    }
+
+    COLA_ASSERT(SelectiveReroute);
 
-    ConnRefList::iterator end = connRefs.end();
-    for (ConnRefList::iterator it = connRefs.begin(); it != end; ++it)
+    ConnRefList::const_iterator end = connRefs.end();
+    for (ConnRefList::const_iterator it = connRefs.begin(); it != end; ++it)
     {
         ConnRef *conn = (*it);
 
-        if (conn->_route.pn == 0)
+        if (conn->_route.empty())
         {
             // Ignore uninitialised connectors.
             continue;
@@ -556,7 +1080,7 @@ void Router::markConnectors(ShapeRef *shape)
         }
 
         Point start = conn->_route.ps[0];
-        Point end = conn->_route.ps[conn->_route.pn - 1];
+        Point end = conn->_route.ps[conn->_route.size() - 1];
 
         double conndist = conn->_route_dist;
 
@@ -609,12 +1133,12 @@ void Router::markConnectors(ShapeRef *shape)
                 Point n_p2(p2.x - p1.x, p2.y - p1.y);
                 Point n_start(start.x - p1.x, start.y - p1.y);
                 Point n_end(end.x - p1.x, end.y - p1.y);
-                //printf("n_p2:    (%.1f, %.1f)\n", n_p2.x, n_p2.y);
-                //printf("n_start: (%.1f, %.1f)\n", n_start.x, n_start.y);
-                //printf("n_end:   (%.1f, %.1f)\n", n_end.x, n_end.y);
+                //db_printf("n_p2:    (%.1f, %.1f)\n", n_p2.x, n_p2.y);
+                //db_printf("n_start: (%.1f, %.1f)\n", n_start.x, n_start.y);
+                //db_printf("n_end:   (%.1f, %.1f)\n", n_end.x, n_end.y);
 
                 double theta = 0 - atan2(n_p2.y, n_p2.x);
-                //printf("theta = %.2f\n", theta * (180 / PI));
+                //db_printf("theta = %.2f\n", theta * (180 / PI));
 
                 Point r_p1(0, 0);
                 Point r_p2 = n_p2;
@@ -630,16 +1154,15 @@ void Router::markConnectors(ShapeRef *shape)
                 start.y = cosv * n_start.y + sinv * n_start.x;
                 end.x = cosv * n_end.x - sinv * n_end.y;
                 end.y = cosv * n_end.y + sinv * n_end.x;
-                //printf("r_p2:    (%.1f, %.1f)\n", r_p2.x, r_p2.y);
-                //printf("r_start: (%.1f, %.1f)\n", start.x, start.y);
-                //printf("r_end:   (%.1f, %.1f)\n", end.x, end.y);
+                //db_printf("r_p2:    (%.1f, %.1f)\n", r_p2.x, r_p2.y);
+                //db_printf("r_start: (%.1f, %.1f)\n", start.x, start.y);
+                //db_printf("r_end:   (%.1f, %.1f)\n", end.x, end.y);
 
-                if (((int) r_p2.y) != 0)
+                // This might be slightly off.
+                if (fabs(r_p2.y) > 0.0001)
                 {
-                    printf("r_p2.y: %f != 0\n", r_p2.y);
-                    std::abort();
+                    db_printf("r_p2.y: %f != 0\n", r_p2.y);
                 }
-                // This might be slightly off.
                 r_p2.y = 0;
 
                 offy = r_p1.y;
@@ -679,13 +1202,13 @@ void Router::markConnectors(ShapeRef *shape)
                 x = ((b*c) + (a*d)) / (b + d);
             }
 
-            //printf("%.1f, %.1f, %.1f, %.1f\n", a, b, c, d);
-            //printf("x = %.1f\n", x);
+            //db_printf("%.1f, %.1f, %.1f, %.1f\n", a, b, c, d);
+            //db_printf("x = %.1f\n", x);
 
             x = std::max(min, x);
             x = std::min(max, x);
 
-            //printf("x = %.1f\n", x);
+            //db_printf("x = %.1f\n", x);
 
             Point xp;
             if (p1.x == p2.x)
@@ -698,20 +1221,20 @@ void Router::markConnectors(ShapeRef *shape)
                 xp.x = x;
                 xp.y = offy;
             }
-            //printf("(%.1f, %.1f)\n", xp.x, xp.y);
+            //db_printf("(%.1f, %.1f)\n", xp.x, xp.y);
 
-            e1 = dist(start, xp);
-            e2 = dist(xp, end);
+            e1 = euclideanDist(start, xp);
+            e2 = euclideanDist(xp, end);
             estdist = e1 + e2;
 
 
-            //printf("is %.1f < %.1f\n", estdist, conndist);
+            //db_printf("is %.1f < %.1f\n", estdist, conndist);
             if (estdist < conndist)
             {
 #ifdef SELECTIVE_DEBUG
                 //double angle = AngleAFromThreeSides(dist(start, end),
                 //        e1, e2);
-                printf("[%3d] - Possible better path found (%.1f < %.1f)\n",
+                db_printf("[%3d] - Possible better path found (%.1f < %.1f)\n",
                         conn->_id, estdist, conndist);
 #endif
                 conn->_needs_reroute_flag = true;
@@ -723,6 +1246,81 @@ void Router::markConnectors(ShapeRef *shape)
 }
 
 
+ConnType Router::validConnType(const ConnType select) const
+{
+    if (select != ConnType_None)
+    {
+        if ((select == ConnType_Orthogonal) && _orthogonalRouting)
+        {
+            return ConnType_Orthogonal;
+        }
+        else if ((select == ConnType_PolyLine) && _polyLineRouting)
+        {
+            return ConnType_PolyLine;
+        }
+    }
+
+    if (_polyLineRouting)
+    {
+        return ConnType_PolyLine;
+    }
+    else if (_orthogonalRouting)
+    {
+        return ConnType_Orthogonal;
+    }
+    return ConnType_None;
+}
+
+
+void Router::setRoutingPenalty(const PenaltyType penType, const double penVal)
+{
+    COLA_ASSERT(penType < lastPenaltyMarker);
+    if (penVal < 0)
+    {
+        // Set some sensible penalty.
+        switch (penType)
+        {
+            case segmentPenalty:
+                _routingPenalties[penType] = 50;
+                break;
+            case fixedSharedPathPenalty:
+                _routingPenalties[penType] = 110;
+                break;
+            case anglePenalty:
+                _routingPenalties[penType] = 50;
+                break;
+            case crossingPenalty:
+                _routingPenalties[penType] = 200;
+                break;
+            case clusterCrossingPenalty:
+                _routingPenalties[penType] = 4000;
+                break;
+            default:
+                _routingPenalties[penType] = 50;
+                break;
+        }
+    }
+    else
+    {
+        _routingPenalties[penType] = penVal;
+    }
+}
+
+
+double Router::routingPenalty(const PenaltyType penType) const
+{
+    COLA_ASSERT(penType < lastPenaltyMarker);
+    return _routingPenalties[penType];
+}
+
+
+double& Router::penaltyRef(const PenaltyType penType)
+{
+    COLA_ASSERT(penType < lastPenaltyMarker);
+    return _routingPenalties[penType];
+}
+
+
 void Router::printInfo(void)
 {
     FILE *fp = stdout;
@@ -735,6 +1333,7 @@ void Router::printInfo(void)
     int st_endpoints = 0;
     int st_valid_shape_visedges = 0;
     int st_valid_endpt_visedges = 0;
+    int st_orthogonal_visedges = 0;
     int st_invalid_visedges = 0;
     VertInf *finish = vertices.end();
     for (VertInf *t = vertices.connsBegin(); t != finish; t = t->lstNext)
@@ -775,9 +1374,15 @@ void Router::printInfo(void)
     {
         st_invalid_visedges++;
     }
+    for (EdgeInf *t = visOrthogGraph.begin(); t != visOrthogGraph.end();
+            t = t->lstNext)
+    {
+        st_orthogonal_visedges++;
+    }
     fprintf(fp, "Number of shapes: %d\n", st_shapes);
     fprintf(fp, "Number of vertices: %d (%d real, %d endpoints)\n",
             st_vertices + st_endpoints, st_vertices, st_endpoints);
+    fprintf(fp, "Number of orhtog_vis_edges: %d\n", st_orthogonal_visedges);
     fprintf(fp, "Number of vis_edges: %d (%d valid [%d normal, %d endpt], "
             "%d invalid)\n", st_valid_shape_visedges + st_invalid_visedges +
             st_valid_endpt_visedges, st_valid_shape_visedges +
@@ -787,12 +1392,462 @@ void Router::printInfo(void)
     fprintf(fp, "checkVisEdge tally: %d\n", st_checked_edges);
     fprintf(fp, "----------------------\n");
 
-    fprintf(fp, "ADDS:  "); timers.Print(tmAdd);
-    fprintf(fp, "DELS:  "); timers.Print(tmDel);
-    fprintf(fp, "MOVS:  "); timers.Print(tmMov);
-    fprintf(fp, "***S:  "); timers.Print(tmSev);
-    fprintf(fp, "PTHS:  "); timers.Print(tmPth);
+    fprintf(fp, "ADDS:  "); timers.Print(tmAdd, fp);
+    fprintf(fp, "DELS:  "); timers.Print(tmDel, fp);
+    fprintf(fp, "MOVS:  "); timers.Print(tmMov, fp);
+    fprintf(fp, "***S:  "); timers.Print(tmSev, fp);
+    fprintf(fp, "PTHS:  "); timers.Print(tmPth, fp);
+    fprintf(fp, "OrthogGraph:  "); timers.Print(tmOrthogGraph, fp);
+    fprintf(fp, "OrthogRoute:  "); timers.Print(tmOrthogRoute, fp);
+    fprintf(fp, "OrthogCentre:  "); timers.Print(tmOrthogCentre, fp);
+    fprintf(fp, "OrthogNudge:  "); timers.Print(tmOrthogNudge, fp);
     fprintf(fp, "\n");
+    timers.Reset();
+}
+
+
+static const double LIMIT = 100000000;
+
+static void reduceRange(double& val)
+{
+    val = std::min(val, LIMIT);
+    val = std::max(val, -LIMIT);
+}
+
+
+//=============================================================================
+// The following methods are for testing and debugging.
+
+
+bool Router::existsOrthogonalPathOverlap(void)
+{
+    ConnRefList::iterator fin = connRefs.end();
+    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        Avoid::Polygon iRoute = (*i)->displayRoute();
+        ConnRefList::iterator j = i;
+        for (++j; j != fin; ++j) 
+        {
+            // Determine if this pair overlap
+            Avoid::Polygon jRoute = (*j)->displayRoute();
+            CrossingsInfoPair crossingInfo = std::make_pair(0, 0);
+            for (size_t jInd = 1; jInd < jRoute.size(); ++jInd)
+            {
+                const bool finalSegment = ((jInd + 1) == jRoute.size());
+                CrossingsInfoPair crossingInfo = countRealCrossings(
+                        iRoute, true, jRoute, jInd, true, 
+                        finalSegment, NULL, NULL, *i, *j);
+                
+                if ((crossingInfo.second & CROSSING_SHARES_PATH) && 
+                    (crossingInfo.second & CROSSING_SHARES_FIXED_SEGMENT) && 
+                    !(crossingInfo.second & CROSSING_SHARES_PATH_AT_END)) 
+                {
+                    // We looking for fixedSharedPaths and there is a
+                    // fixedSharedPath.
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+
+
+bool Router::existsOrthogonalTouchingCorners(void)
+{
+    ConnRefList::iterator fin = connRefs.end();
+    for (ConnRefList::iterator i = connRefs.begin(); i != fin; ++i) 
+    {
+        Avoid::Polygon iRoute = (*i)->displayRoute();
+        ConnRefList::iterator j = i;
+        for (++j; j != fin; ++j) 
+        {
+            // Determine if this pair overlap
+            Avoid::Polygon jRoute = (*j)->displayRoute();
+            CrossingsInfoPair crossingInfo = std::make_pair(0, 0);
+            for (size_t jInd = 1; jInd < jRoute.size(); ++jInd)
+            {
+                const bool finalSegment = ((jInd + 1) == jRoute.size());
+                CrossingsInfoPair crossingInfo = countRealCrossings(
+                        iRoute, true, jRoute, jInd, true, 
+                        finalSegment, NULL, NULL, *i, *j);
+                
+                if (crossingInfo.second & CROSSING_TOUCHES) 
+                {
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+
+
+void Router::outputInstanceToSVG(std::string instanceName)
+{
+    std::string filename;
+    if (!instanceName.empty())
+    {
+        filename = instanceName;
+    }
+    else
+    {
+        filename = "libavoid-debug";
+    }
+    filename += ".svg";
+    FILE *fp = fopen(filename.c_str(), "w");
+
+    if (fp == NULL)
+    {
+        return;
+    }
+
+    double minX = LIMIT;
+    double minY = LIMIT;
+    double maxX = -LIMIT;
+    double maxY = -LIMIT;
+
+    VertInf *curr = vertices.connsBegin();
+    while (curr)
+    {
+        Point p = curr->point;
+
+        reduceRange(p.x);
+        reduceRange(p.y);
+        
+        if (p.x > -LIMIT)
+        {
+            minX = std::min(minX, p.x);
+        }
+        if (p.x < LIMIT)
+        {
+            maxX = std::max(maxX, p.x);
+        }
+        if (p.y > -LIMIT)
+        {
+            minY = std::min(minY, p.y);
+        }
+        if (p.y < LIMIT)
+        {
+            maxY = std::max(maxY, p.y);
+        }
+        curr = curr->lstNext;
+    }
+    minX -= 50;
+    minY -= 50;
+    maxX += 50;
+    maxY += 50;
+
+    fprintf(fp, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
+    fprintf(fp, "<svg xmlns:inkscape=\"http://www.inkscape.org/namespaces/inkscape\" xmlns=\"http://www.w3.org/2000/svg\" width=\"100%%\" height=\"100%%\" viewBox=\"%g %g %g %g\">\n", minX, minY, maxX - minX, maxY - minY);
+
+    // Output source code to generate this instance of the router.
+    fprintf(fp, "<!-- Source code to generate this instance:\n");
+    fprintf(fp, "#include \"libavoid/libavoid.h\"\n");
+    fprintf(fp, "using namespace Avoid;\n");
+    fprintf(fp, "int main(void) {\n");
+    fprintf(fp, "    Router *router = new Router(\n");
+    fprintf(fp, "            PolyLineRouting | OrthogonalRouting);\n");
+    for (size_t p = 0; p < lastPenaltyMarker; ++p)
+    {
+        fprintf(fp, "    router->setRoutingPenalty((PenaltyType)%lu, %g);\n", 
+                p, _routingPenalties[p]);
+    }
+    fprintf(fp, "    router->setOrthogonalNudgeDistance(%g);\n\n",
+            orthogonalNudgeDistance());
+    ShapeRefList::iterator shRefIt = shapeRefs.begin();
+    while (shRefIt != shapeRefs.end())
+    {
+        ShapeRef *shRef = *shRefIt;
+        fprintf(fp, "    Polygon poly%u(%lu);\n", 
+                shRef->id(), shRef->polygon().size());
+        for (size_t i = 0; i < shRef->polygon().size(); ++i)
+        {
+            fprintf(fp, "    poly%u.ps[%lu] = Point(%g, %g);\n", 
+                    shRef->id(), i, shRef->polygon().at(i).x,
+                    shRef->polygon().at(i).y);
+        }
+        fprintf(fp, "    ShapeRef *shapeRef%u = new ShapeRef(router, poly%u, "
+                "%u);\n", shRef->id(), shRef->id(), shRef->id());
+        fprintf(fp, "    router->addShape(shapeRef%u);\n\n", shRef->id());
+        ++shRefIt;
+    }
+    ConnRefList::reverse_iterator revConnRefIt = connRefs.rbegin();
+    while (revConnRefIt != connRefs.rend())
+    {
+        ConnRef *connRef = *revConnRefIt;
+        fprintf(fp, "    ConnRef *connRef%u = new ConnRef(router, %u);\n",
+                connRef->id(), connRef->id());
+        if (connRef->src())
+        {
+            fprintf(fp, "    ConnEnd srcPt%u(Point(%g, %g), %u);\n",
+                    connRef->id(), connRef->src()->point.x,
+                    connRef->src()->point.y, connRef->src()->visDirections);
+            fprintf(fp, "    connRef%u->setSourceEndpoint(srcPt%u);\n",
+                    connRef->id(), connRef->id());
+        }
+        if (connRef->dst())
+        {
+            fprintf(fp, "    ConnEnd dstPt%u(Point(%g, %g), %u);\n",
+                    connRef->id(), connRef->dst()->point.x,
+                    connRef->dst()->point.y, connRef->dst()->visDirections);
+            fprintf(fp, "    connRef%u->setDestEndpoint(dstPt%u);\n",
+                    connRef->id(), connRef->id());
+        }
+        fprintf(fp, "    connRef%u->setRoutingType((ConnType)%u);\n\n", 
+                connRef->id(), connRef->routingType());
+        ++revConnRefIt;
+    }
+    fprintf(fp, "    router->processTransaction();\n");
+    fprintf(fp, "    router->outputInstanceToSVG();\n");
+    fprintf(fp, "    delete router;\n");
+    fprintf(fp, "    return 0;\n");
+    fprintf(fp, "};\n");
+    fprintf(fp, "-->\n");
+
+    
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "inkscape:label=\"ShapesPoly\">\n");
+    shRefIt = shapeRefs.begin();
+    while (shRefIt != shapeRefs.end())
+    {
+        ShapeRef *shRef = *shRefIt;
+    
+        fprintf(fp, "<path id=\"poly-%u\" style=\"stroke-width: 1px; "
+                "stroke: black; fill: blue; fill-opacity: 0.3;\" d=\"", 
+                shRef->id());
+        for (size_t i = 0; i < shRef->polygon().size(); ++i)
+        {
+            fprintf(fp, "%c %g,%g ", ((i == 0) ? 'M' : 'L'), 
+                    shRef->polygon().at(i).x, shRef->polygon().at(i).y);
+        }
+        fprintf(fp, "Z\" />\n");
+        ++shRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"ShapesRect\">\n");
+    shRefIt = shapeRefs.begin();
+    while (shRefIt != shapeRefs.end())
+    {
+        ShapeRef *shRef = *shRefIt;
+        double minX, minY, maxX, maxY;
+        shRef->polygon().getBoundingRect(&minX, &minY, &maxX, &maxY);
+    
+        fprintf(fp, "<rect id=\"rect-%u\" x=\"%g\" y=\"%g\" width=\"%g\" height=\"%g\" "
+                "style=\"stroke-width: 1px; stroke: black; fill: blue; fill-opacity: 0.3;\" />\n",
+                shRef->id(), minX, minY, maxX - minX, maxY - minY);
+        ++shRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "inkscape:label=\"VisGraph\""
+            ">\n");
+    EdgeInf *finish = NULL;
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"VisGraph-shape\""
+            ">\n");
+    finish = visGraph.end();
+    for (EdgeInf *t = visGraph.begin(); t != finish; t = t->lstNext)
+    {
+        std::pair<VertID, VertID> ids = t->ids();
+        bool isShape = (ids.first.isShape) && (ids.second.isShape);
+        if (!isShape)
+        {
+            continue;
+        }
+        std::pair<Point, Point> ptpair = t->points();
+        Point p1 = ptpair.first;
+        Point p2 = ptpair.second;
+        
+        reduceRange(p1.x);
+        reduceRange(p1.y);
+        reduceRange(p2.x);
+        reduceRange(p2.y);
+        
+        fprintf(fp, "<path d=\"M %g,%g L %g,%g\" "
+                "style=\"fill: none; stroke: %s; stroke-width: 1px;\" />\n", 
+                p1.x, p1.y, p2.x, p2.y,
+                (!(ids.first.isShape) || !(ids.second.isShape)) ? "green" : 
+                "red");
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"VisGraph-conn\""
+            ">\n");
+    finish = visGraph.end();
+    for (EdgeInf *t = visGraph.begin(); t != finish; t = t->lstNext)
+    {
+        std::pair<VertID, VertID> ids = t->ids();
+        bool isShape = (ids.first.isShape) && (ids.second.isShape);
+        if (isShape)
+        {
+            continue;
+        }
+        std::pair<Point, Point> ptpair = t->points();
+        Point p1 = ptpair.first;
+        Point p2 = ptpair.second;
+        
+        reduceRange(p1.x);
+        reduceRange(p1.y);
+        reduceRange(p2.x);
+        reduceRange(p2.y);
+        
+        fprintf(fp, "<path d=\"M %g,%g L %g,%g\" "
+                "style=\"fill: none; stroke: %s; stroke-width: 1px;\" />\n", 
+                p1.x, p1.y, p2.x, p2.y,
+                (!(ids.first.isShape) || !(ids.second.isShape)) ? "green" : 
+                "red");
+    }
+    fprintf(fp, "</g>\n");
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"OrthogVisGraph\">\n");
+    finish = visOrthogGraph.end();
+    for (EdgeInf *t = visOrthogGraph.begin(); t != finish; t = t->lstNext)
+    {
+        std::pair<Point, Point> ptpair = t->points();
+        Point p1 = ptpair.first;
+        Point p2 = ptpair.second;
+        
+        reduceRange(p1.x);
+        reduceRange(p1.y);
+        reduceRange(p2.x);
+        reduceRange(p2.y);
+        
+        std::pair<VertID, VertID> ids = t->ids();
+
+        fprintf(fp, "<path d=\"M %g,%g L %g,%g\" "
+                "style=\"fill: none; stroke: %s; stroke-width: 1px;\" />\n", 
+                p1.x, p1.y, p2.x, p2.y,
+                (!(ids.first.isShape) || !(ids.second.isShape)) ? "green" : 
+                "red");
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"RawConnectors\""
+            ">\n");
+    ConnRefList::iterator connRefIt = connRefs.begin();
+    while (connRefIt != connRefs.end())
+    {
+        ConnRef *connRef = *connRefIt;
+    
+        PolyLine route = connRef->route();
+        if (!route.empty())
+        {
+            fprintf(fp, "<path id=\"raw-%u\" d=\"M %g,%g ", connRef->id(),
+                    route.ps[0].x, route.ps[0].y);
+            for (size_t i = 1; i < route.size(); ++i)
+            {
+                fprintf(fp, "L %g,%g ", route.ps[i].x, route.ps[i].y);
+            }
+            fprintf(fp, "\" ");
+            if (connRef->src() && connRef->dst())
+            {
+                fprintf(fp, "debug=\"src: %d dst: %d\" ",
+                        connRef->src()->visDirections,
+                        connRef->dst()->visDirections);
+            }
+            fprintf(fp, "style=\"fill: none; stroke: black; "
+                    "stroke-width: 1px;\" />\n");
+        }
+        
+        ++connRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "style=\"display: none;\" "
+            "inkscape:label=\"CurvedDisplayConnectors\""
+            ">\n");
+    connRefIt = connRefs.begin();
+    while (connRefIt != connRefs.end())
+    {
+        ConnRef *connRef = *connRefIt;
+    
+        PolyLine route = connRef->displayRoute().curvedPolyline(8);
+        if (!route.empty())
+        {
+            fprintf(fp, "<path id=\"curved-%u\" d=\"M %g,%g ", connRef->id(),
+                    route.ps[0].x, route.ps[0].y);
+            for (size_t i = 1; i < route.size(); ++i)
+            {
+                if (route.ts[i] == 'C')
+                {
+                    fprintf(fp, "%c %g,%g %g,%g %g,%g", route.ts[i], 
+                            route.ps[i].x, route.ps[i].y,
+                            route.ps[i+1].x, route.ps[i+1].y,
+                            route.ps[i+2].x, route.ps[i+2].y);
+                    i += 2;
+                }
+                else
+                {
+                    fprintf(fp, "%c %g,%g ", route.ts[i], 
+                            route.ps[i].x, route.ps[i].y);
+                }
+            }
+            fprintf(fp, "\" ");
+            if (connRef->src() && connRef->dst())
+            {
+                fprintf(fp, "debug=\"src: %d dst: %d\" ",
+                        connRef->src()->visDirections,
+                        connRef->dst()->visDirections);
+            }
+            fprintf(fp, "style=\"fill: none; stroke: black; "
+                    "stroke-width: 1px;\" />\n");
+        }
+        
+        ++connRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+
+    fprintf(fp, "<g inkscape:groupmode=\"layer\" "
+            "inkscape:label=\"DisplayConnectors\""
+            ">\n");
+    connRefIt = connRefs.begin();
+    while (connRefIt != connRefs.end())
+    {
+        ConnRef *connRef = *connRefIt;
+    
+        PolyLine route = connRef->displayRoute();
+        if (!route.empty())
+        {
+            fprintf(fp, "<path id=\"disp-%u\" d=\"M %g,%g ", connRef->id(),
+                    route.ps[0].x, route.ps[0].y);
+            for (size_t i = 1; i < route.size(); ++i)
+            {
+                fprintf(fp, "L %g,%g ", route.ps[i].x, route.ps[i].y);
+            }
+            fprintf(fp, "\" ");
+            if (connRef->src() && connRef->dst())
+            {
+                fprintf(fp, "debug=\"src: %d dst: %d\" ",
+                        connRef->src()->visDirections,
+                        connRef->dst()->visDirections);
+            }
+            fprintf(fp, "style=\"fill: none; stroke: black; "
+                    "stroke-width: 1px;\" />\n");
+        }
+        
+        ++connRefIt;
+    }
+    fprintf(fp, "</g>\n");
+
+
+    fprintf(fp, "</svg>\n");
+    fclose(fp);
 }
 
 
diff --git a/src/libavoid/router.h b/src/libavoid/router.h
index 597f48c5e..03060b025 100644
--- a/src/libavoid/router.h
+++ b/src/libavoid/router.h
@@ -2,94 +2,325 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file    router.h
+//! @brief   Contains the interface for the Router class.
+
 
 #ifndef AVOID_ROUTER_H
 #define AVOID_ROUTER_H
 
-//#define LINEDEBUG
+#include <list>
+#include <utility>
+#include <string>
 
 #include "libavoid/shape.h"
+#include "libavoid/viscluster.h"
 #include "libavoid/graph.h"
 #include "libavoid/timer.h"
-#include <list>
-#include <utility>
-#ifdef LINEDEBUG       
+#include "libavoid/connector.h"
+
+#if defined(LINEDEBUG) || defined(ASTAR_DEBUG) || defined(LIBAVOID_SDL)
     #include <SDL.h>
+    #ifndef LIBAVOID_SDL
+        #define LIBAVOID_SDL
+    #endif
 #endif
 
 
 namespace Avoid {
 
-class ConnRef;
-typedef std::list<ConnRef *> ConnRefList;
 typedef std::list<unsigned int> IntList;
-class MoveInfo;
-typedef std::list<MoveInfo *> MoveInfoList;
+
+class ActionInfo;
+typedef std::list<ActionInfo> ActionInfoList;
+class ShapeRef;
+
+//! @brief  Flags that can be passed to the router during initialisation 
+//!         to specify options.
+enum RouterFlag
+{
+    //! @brief  This option specifies that the router should maintain the
+    //!         structures necessary to allow poly-line connector routing.
+    PolyLineRouting = 1,
+    //! @brief  This option specifies that the router should maintain the
+    //!         structures necessary to allow orthogonal connector routing.
+    OrthogonalRouting = 2
+};
 
 
 static const unsigned int runningTo = 1;
 static const unsigned int runningFrom = 2;
 static const unsigned int runningToAndFrom = runningTo | runningFrom;
 
+//! @brief  Types of penalty cases that can be used to improve the quality 
+//!         of the connector routes produced.
+enum PenaltyType
+{
+    //! @brief  This penalty is applied for each segment in the connector 
+    //!         path beyond the first.  This should always normally be set
+    //!         when doing orthogonal routing to prevent step-like connector
+    //!         paths.
+    segmentPenalty = 0,
+    //! @brief  This penalty is applied in its full amount to tight acute 
+    //!         bends in the connector path.  A smaller portion of the penalty
+    //!         is applied for slight bends, i.e., where the bend is close to
+    //!         180 degrees.  This is useful for polyline routing where there
+    //!         is some evidence that tighter corners are worse for 
+    //!         readability, but that slight bends might not be so bad, 
+    //!         especially when smoothed by curves.
+    anglePenalty,
+    //! @brief  This penalty is applied whenever a connector path crosses 
+    //!         another connector path.  It takes shared paths into 
+    //!         consideration and the penalty is only applied if there
+    //!         is an actual crossing.
+    //! @note   This penalty is still experimental!  It is not recommended
+    //!         for normal use.
+    crossingPenalty,
+    //! @brief  This penalty is applied whenever a connector path crosses 
+    //!         a cluster boundary.
+    //! @note   This penalty is still experimental!  It is not recommended
+    //!         for normal use.
+    clusterCrossingPenalty,
+    //! @brief  This penalty is applied whenever a connector path shares 
+    //!         some segments with an immovable portion of an existing 
+    //!         connector route (such as the first or last segment of a
+    //!         connector).
+    //! @note   This penalty is still experimental!  It is not recommended
+    //!         for normal use.
+    fixedSharedPathPenalty,
+    // Used for determining the size of the penalty array.  
+    // This should always we the last value in the enum.
+    lastPenaltyMarker
+};
+
+
+static const double noPenalty = 0;
+static const double chooseSensiblePenalty = -1;
+
 
+//! @brief   The Router class represents a libavoid router instance.
+//!
+//! Usually you would keep a separate Router instance for each diagram
+//! or layout you have open in your application.
+//
 class Router {
     public:
-        Router();
+        //! @brief  Constructor for router instance.
+        //!
+        //! @param[in]  flags  One or more Avoid::RouterFlag options to 
+        //!                    control the behaviour of the router.
+        Router(const unsigned int flags);
+
+        //! @brief  Destructor for router instance.
+        //!
+        //! @note   Destroying a router instance will delete all remaining
+        //!         shapes and connectors, thereby invalidating any existing
+        //!         pointers to them.
+        ~Router();
 
         ShapeRefList shapeRefs;
         ConnRefList connRefs;
+        ClusterRefList clusterRefs;
         EdgeList visGraph;
         EdgeList invisGraph;
+        EdgeList visOrthogGraph;
         ContainsMap contains;
         VertInfList vertices;
+        ContainsMap enclosingClusters;
         
         bool PartialTime;
         bool SimpleRouting;
-        double segmt_penalty;
-        double angle_penalty;
-        double crossing_penalty;
-
+        bool ClusteredRouting;
 
-        bool UseAStarSearch;
+        // Poly-line routing options:
         bool IgnoreRegions;
-        bool SelectiveReroute;
-        bool IncludeEndpoints;
         bool UseLeesAlgorithm;
         bool InvisibilityGrph;
-        bool ConsolidateMoves;
+       
+        // General routing options:
+        bool SelectiveReroute;
+        
         bool PartialFeedback;
+        bool RubberBandRouting;
+        
 
         // Instrumentation:
         Timer timers;
         int st_checked_edges;
-#ifdef LINEDEBUG
+#ifdef LIBAVOID_SDL
         SDL_Surface *avoid_screen;
 #endif
 
+        //! @brief Allows setting of the behaviour of the router in regard
+        //!        to transactions.  This controls whether transactions are
+        //!        used to queue changes and process them effeciently at once
+        //!        or they are instead processed immediately.
+        //!
+        //! It is more efficient to perform actions like shape movement,
+        //! addition or deletion as batch tasks, and reroute the necessary
+        //! connectors just once after these actions have been performed.
+        //! For this reason, libavoid allows you to group such actions into
+        //! "transactions" that are processed efficiently when the 
+        //! processTransaction() method is called.
+        //!
+        //! By default, the router will process all actions as tranactions.
+        //! If transactionUse() is set to false, then all actions will get 
+        //! processed immediately, and cause immediate routing callbacks to 
+        //! all affected connectors after each action.
+        //!
+        //! @param[in]  transactions  A boolean value specifying whether to
+        //!                           use transactions.
+        //!
+        void setTransactionUse(const bool transactions);
+
+        //! @brief Reports whether the router groups actions into transactions.
+        //!
+        //! @return A boolean value describing whether transactions are in use.
+        //!
+        //! @sa setTransactionUse
+        //! @sa processTransaction
+        //!
+        bool transactionUse(void) const;
+
+        //! @brief Finishes the current transaction and processes all the 
+        //!        queued object changes efficiently.
+        //!
+        //! This method will efficiently process all moves, additions and
+        //! deletions that have occurred since processTransaction() was 
+        //! last called.
+        //!
+        //! If transactionUse() is false, then all actions will have been 
+        //! processed immediately and this method will do nothing.
+        //!
+        //! @return A boolean value describing whether there were any actions
+        //!         to process.
+        //!
+        //! @sa setTransactionUse
+        //!
+        bool processTransaction(void);
+        
+        //! @brief Add a shape to the router scene.
+        //!
+        //! This shape will be considered to be an obstacle. Calling this 
+        //! method will cause connectors intersecting the added shape to
+        //! be marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape  Pointer reference to the shape being added.
+        //!
         void addShape(ShapeRef *shape);
-        void delShape(ShapeRef *shape);
-        void moveShape(ShapeRef *shape, Polygn *newPoly,
+
+        //! @brief Remove a shape from the router scene.
+        //!
+        //! Connectors that could have a better (usually shorter) path after
+        //! the removal of this shape will be marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape  Pointer reference to the shape being removed.
+        //!
+        void removeShape(ShapeRef *shape);
+
+        //! @brief Move or resize an existing shape within the router scene.
+        //!
+        //! A new polygon for the shape can be given to effectively move or 
+        //! resize the shape with the scene.  Connectors that intersect the 
+        //! new shape polygon, or that could have a better (usually shorter)
+        //! path after the change, will be marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape       Pointer reference to the shape being 
+        //!                         moved/resized.
+        //! @param[in]  newPoly     The new polygon boundary for the shape.
+        //! @param[in]  first_move  This option is used for some advanced 
+        //!                         (currently undocumented) behaviour and 
+        //!                         it should be ignored for the moment.
+        //!
+        void moveShape(ShapeRef *shape, const Polygon& newPoly,
                 const bool first_move = false);
-        void processMoves(void);
-        
+
+        //! @brief Move an existing shape within the router scene by a relative
+        //!        distance.
+        //!         
+        //! Connectors that intersect the shape's new position, or that could 
+        //! have a better (usually shorter) path after the change, will be 
+        //! marked as needing to be rerouted.
+        //!
+        //! @param[in]  shape       Pointer reference to the shape being moved.
+        //! @param[in]  xDiff       The distance to move the shape in the 
+        //!                         x dimension.
+        //! @param[in]  yDiff       The distance to move the shape in the 
+        //!                         y dimension.
+        //!
+        void moveShape(ShapeRef *shape, const double xDiff, const double yDiff);
+
+        //! @brief Sets a spacing distance for overlapping orthogonal 
+        //!        connectors to be nudged apart.
+        //!         
+        //! By default, this distance is set to a value of 4.
+        //!
+        //! This method does not re-trigger post-processing of connectors.
+        //! The new distance will be used the next time rerouting is performed.
+        //!
+        //! @param[in]  dist  The distance to be used for orthogonal nudging.
+        //!
+        void setOrthogonalNudgeDistance(const double dist);
+
+        //! @brief   Returns the spacing distance that overlapping orthogonal
+        //!          connecotrs are nudged apart.
+        //!
+        //! @return  The current spacing distance used for orthogonal nudging.
+        //!
+        double orthogonalNudgeDistance(void) const;
+
+        //! @brief  Sets or removes penalty values that are applied during 
+        //!         connector routing.
+        //!
+        //! By default, libavoid will produce shortest path routes between
+        //! the source and destination points for each connector.  There are
+        //! several penalties that can be applied during this stage to 
+        //! improve the aesthetics of the routes generated.  These different
+        //! penalties are specified and explained by the PenaltyType enum.
+        //! 
+        //! If a value of zero or Avoid::noPenalty is given then the penalty 
+        //! for this case will be removed.  If no penalty argument (or a 
+        //! negative value) is specified when calling this method, then a 
+        //! sensible penalty value will be automatically chosen.
+        //!
+        //! @param[in] penType  The type of penalty, a PenaltyType.
+        //! @param[in] penVal   The value to be applied for each occurance
+        //!                     of the penalty case.  
+        //!
+        void setRoutingPenalty(const PenaltyType penType, 
+                const double penVal = chooseSensiblePenalty);
+
+        //! @brief  Returns the current penalty value for a particular 
+        //!         routing penalty case.
+        //!
+        //! @param[in] penType  The type of penalty, a PenaltyType.
+        //! @return  The penalty value for the specified penalty case.
+        //!
+        double routingPenalty(const PenaltyType penType) const;
+
+        void addCluster(ClusterRef *cluster);
+        void delCluster(ClusterRef *cluster);
+
         void attachedConns(IntList &conns, const unsigned int shapeId,
                 const unsigned int type);
         void attachedShapes(IntList &shapes, const unsigned int shapeId,
@@ -98,15 +329,49 @@ class Router {
         void markConnectors(ShapeRef *shape);
         void generateContains(VertInf *pt);
         void printInfo(void);
+        void outputInstanceToSVG(std::string filename = std::string());
+        unsigned int assignId(const unsigned int suggestedId);
+        void regenerateStaticBuiltGraph(void);
+        void destroyOrthogonalVisGraph(void);
+        void setStaticGraphInvalidated(const bool invalidated);
+        ConnType validConnType(const ConnType select = ConnType_None) const;
+        bool shapeInQueuedActionList(ShapeRef *shape) const;
+        double& penaltyRef(const PenaltyType penType);
+        bool existsOrthogonalPathOverlap(void);
+        bool existsOrthogonalTouchingCorners(void);
+
     private:
-        void newBlockingShape(Polygn *poly, int pid);
+        friend class ConnRef;
+
+        void modifyConnector(ConnRef *conn);
+        void modifyConnector(ConnRef *conn, unsigned int type,
+                const ConnEnd &connEnd);
+        void removeQueuedConnectorActions(ConnRef *conn);
+        void newBlockingShape(const Polygon& poly, int pid);
         void checkAllBlockedEdges(int pid);
         void checkAllMissingEdges(void);
-        void adjustContainsWithAdd(const Polygn& poly, const int p_shape);
+        void adjustContainsWithAdd(const Polygon& poly, const int p_shape);
         void adjustContainsWithDel(const int p_shape);
-        void callbackAllInvalidConnectors(void);
+        void adjustClustersWithAdd(const PolygonInterface& poly, 
+                const int p_cluster);
+        void adjustClustersWithDel(const int p_cluster);
+        void rerouteAndCallbackConnectors(void);
+        bool idIsUnique(const unsigned int id) const;
+        void improveCrossings(void);
+
+        ActionInfoList actionList;
+        unsigned int _largestAssignedId;
+        bool _consolidateActions;
+        double _orthogonalNudgeDistance;
+        double _routingPenalties[lastPenaltyMarker];
+
+    public:
+        // Overall modes:
+        bool _polyLineRouting;
+        bool _orthogonalRouting;
 
-        MoveInfoList moveList;
+        bool _staticGraphInvalidated;
+        bool _inCrossingPenaltyReroutingStage;
 };
 
 }
diff --git a/src/libavoid/shape.cpp b/src/libavoid/shape.cpp
index c0ff2f6e8..ac3f0c193 100644
--- a/src/libavoid/shape.cpp
+++ b/src/libavoid/shape.cpp
@@ -2,53 +2,57 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2008  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
-#include <cassert>
 
 #include "libavoid/shape.h"
 #include "libavoid/graph.h"  // For alertConns
 #include "libavoid/vertices.h"
-#include "libavoid/polyutil.h"
 #include "libavoid/router.h"
+#include "libavoid/debug.h"
+#include "libavoid/assertions.h"
 
 
 namespace Avoid {
 
 
-ShapeRef::ShapeRef(Router *router, unsigned int id, Polygn& ply)
+ShapeRef::ShapeRef(Router *router, Polygon& ply, const unsigned int id)
     : _router(router)
-    , _id(id)
-    , _poly(copyPoly(ply))
+    , _poly(ply)
     , _active(false)
     , _inMoveList(false)
     , _firstVert(NULL)
     , _lastVert(NULL)
 {
+    _id = router->assignId(id);
+
     bool isShape = true;
-    VertID i = VertID(id, isShape, 0);
+    VertID i = VertID(_id, isShape, 0);
     
+    const bool addToRouterNow = false;
     VertInf *last = NULL;
     VertInf *node = NULL;
-    for (int pt_i = 0; pt_i < _poly.pn; pt_i++)
+    for (size_t pt_i = 0; pt_i < _poly.size(); ++pt_i)
     {
-        node = new VertInf(_router, i, _poly.ps[pt_i]);
+        node = new VertInf(_router, i, _poly.ps[pt_i], addToRouterNow);
 
         if (!_firstVert)
         {
@@ -69,17 +73,22 @@ ShapeRef::ShapeRef(Router *router, unsigned int id, Polygn& ply)
     
     _lastVert->shNext = _firstVert;
     _firstVert->shPrev = _lastVert;
-    
-    makeActive();
 }
 
 
 ShapeRef::~ShapeRef()
 {
-    assert(_firstVert != NULL);
-    
-    makeInactive();
+    COLA_ASSERT(!_router->shapeInQueuedActionList(this));
 
+    if (_active)
+    {
+        // Destroying a shape without calling removeShape(), so do it now.
+        _router->removeShape(this);
+        _router->processTransaction();
+    }
+
+    COLA_ASSERT(_firstVert != NULL);
+    
     VertInf *it = _firstVert;
     do
     {
@@ -90,41 +99,37 @@ ShapeRef::~ShapeRef()
     }
     while (it != _firstVert);
     _firstVert = _lastVert = NULL;
-
-    freePoly(_poly);
 }
 
 
-void ShapeRef::setNewPoly(Polygn& poly)
+void ShapeRef::setNewPoly(const Polygon& poly)
 {
-    assert(_firstVert != NULL);
-    assert(_poly.pn == poly.pn);
+    COLA_ASSERT(_firstVert != NULL);
+    COLA_ASSERT(_poly.size() == poly.size());
     
     VertInf *curr = _firstVert;
-    for (int pt_i = 0; pt_i < _poly.pn; pt_i++)
+    for (size_t pt_i = 0; pt_i < _poly.size(); ++pt_i)
     {
-        assert(curr->visListSize == 0);
-        assert(curr->invisListSize == 0);
+        COLA_ASSERT(curr->visListSize == 0);
+        COLA_ASSERT(curr->invisListSize == 0);
 
         // Reset with the new polygon point.
         curr->Reset(poly.ps[pt_i]);
         curr->pathNext = NULL;
-        curr->pathDist = 0;
         
         curr = curr->shNext;
     }
-    assert(curr == _firstVert);
+    COLA_ASSERT(curr == _firstVert);
         
-    freePoly(_poly);
-    _poly = copyPoly(poly);
+    _poly = poly;
 }
 
 
 void ShapeRef::makeActive(void)
 {
-    assert(!_active);
+    COLA_ASSERT(!_active);
     
-    // Add to connRefs list.
+    // Add to shapeRefs list.
     _pos = _router->shapeRefs.insert(_router->shapeRefs.begin(), this);
 
     // Add points to vertex list.
@@ -144,9 +149,9 @@ void ShapeRef::makeActive(void)
 
 void ShapeRef::makeInactive(void)
 {
-    assert(_active);
+    COLA_ASSERT(_active);
     
-    // Remove from connRefs list.
+    // Remove from shapeRefs list.
     _router->shapeRefs.erase(_pos);
 
     // Remove points from vertex list.
@@ -162,7 +167,13 @@ void ShapeRef::makeInactive(void)
     
     _active = false;
 }
-    
+
+
+bool ShapeRef::isActive(void) const
+{
+    return _active;
+}
+
 
 VertInf *ShapeRef::firstVert(void)
 {
@@ -176,19 +187,19 @@ VertInf *ShapeRef::lastVert(void)
 }
 
 
-unsigned int ShapeRef::id(void)
+unsigned int ShapeRef::id(void) const
 {
     return _id;
 }
 
 
-Polygn ShapeRef::poly(void)
+const Polygon& ShapeRef::polygon(void) const
 {
     return _poly;
 }
 
 
-Router *ShapeRef::router(void)
+Router *ShapeRef::router(void) const
 {
     return _router;
 }
@@ -196,13 +207,13 @@ Router *ShapeRef::router(void)
 
 void ShapeRef::boundingBox(BBox& bbox)
 {
-    assert(_poly.pn > 0);
+    COLA_ASSERT(!_poly.empty());
 
     bbox.a = bbox.b = _poly.ps[0];
     Point& a = bbox.a;
     Point& b = bbox.b;
 
-    for (int i = 1; i < _poly.pn; ++i)
+    for (size_t i = 1; i < _poly.size(); ++i)
     {
         const Point& p = _poly.ps[i];
 
@@ -223,8 +234,8 @@ void ShapeRef::removeFromGraph(void)
         
         // For each vertex.
         EdgeInfList& visList = tmp->visList;
-        EdgeInfList::iterator finish = visList.end();
-        EdgeInfList::iterator edge;
+        EdgeInfList::const_iterator finish = visList.end();
+        EdgeInfList::const_iterator edge;
         while ((edge = visList.begin()) != finish)
         {
             // Remove each visibility edge
@@ -239,6 +250,15 @@ void ShapeRef::removeFromGraph(void)
             // Remove each invisibility edge
             delete (*edge);
         }
+
+        EdgeInfList& orthogList = tmp->orthogVisList;
+        finish = orthogList.end();
+        while ((edge = orthogList.begin()) != finish)
+        {
+            // Remove each orthogonal visibility edge
+            (*edge)->alertConns();
+            delete (*edge);
+        }
     }
 }
 
@@ -251,8 +271,8 @@ void ShapeRef::markForMove(void)
     }
     else
     {
-        fprintf(stderr, "WARNING: two moves queued for same shape prior to "
-                "rerouting.\n         This is not safe.\n");
+        db_printf("WARNING: two moves queued for same shape prior to rerouting."
+                "\n         This is not safe.\n");
     }
 }
 
diff --git a/src/libavoid/shape.h b/src/libavoid/shape.h
index b654c6eea..df4c98df1 100644
--- a/src/libavoid/shape.h
+++ b/src/libavoid/shape.h
@@ -2,24 +2,30 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2008  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+//! @file    shape.h
+//! @brief   Contains the interface for the ShapeRef class.
+
+
 #ifndef AVOID_SHAPE_H
 #define AVOID_SHAPE_H
 
@@ -35,21 +41,64 @@ class ShapeRef;
 typedef std::list<ShapeRef *> ShapeRefList;
 
 
+//! @brief   The ShapeRef class represents a shape object.
+//!
+//! Shapes are obstacles that connectors must be routed around.  They can be 
+//! placed into a Router scene and can be repositioned or resized (via
+//! Router::moveShape()).
+//! 
+//! Usually, it is expected that you would create a ShapeRef for each shape 
+//! in your diagram and keep that reference in your own shape class.
+//!
 class ShapeRef
 {
     public:
-        ShapeRef(Router *router, unsigned int id, Polygn& poly);
-        virtual ~ShapeRef();
-        void setNewPoly(Polygn& poly);
+        //! @brief  Shape reference constructor.
+        //!
+        //! Creates a shape obect reference, but does not yet place it into the
+        //! Router scene.
+        //!
+        //! The poly argument will usually be the boundary of the shape in your 
+        //! application with additional buffer of several pixels on each side.
+        //! Specifying such a buffer results in connectors leaving a small 
+        //! amount of space around shapes, rather than touching them on the 
+        //! corners or edges.
+        //!
+        //! If an ID is not specified, then one will be assigned to the shape.
+        //! If assigning an ID yourself, note that it should be a unique 
+        //! positive integer.  Also, IDs are given to all objects in a scene,
+        //! so the same ID cannot be given to a shape and a connector for 
+        //! example.
+        //!
+        //! @param[in]  router  The router scene to place the shape into.
+        //! @param[in]  poly    A Polygon representing the boundary of the 
+        //!                     shape.
+        //! @param[in]  id      A unique positive integer ID for the shape.  
+        ShapeRef(Router *router, Polygon& poly, const unsigned int id = 0);
+        //! @brief  Shape reference destructor.
+        //!
+        //! This will call Router::removeShape() for this shape, if this has
+        //! not already be called.
+        ~ShapeRef();
+        
+        //! @brief   Returns the ID of this shape.
+        //! @returns The ID of the shape. 
+        unsigned int id(void) const;
+        //! @brief   Returns a reference to the polygon boundary of this shape.
+        //! @returns A reference to the polygon boundary of the shape.
+        const Polygon& polygon(void) const;
+        //! @brief   Returns a pointer to the router scene this shape is in.
+        //! @returns A pointer to the router scene for this shape.
+        Router *router(void) const;
+        
+        void setNewPoly(const Polygon& poly);
         VertInf *firstVert(void);
         VertInf *lastVert(void);
-        unsigned int id(void);
-        Polygn poly(void);
-        Router *router(void);
         void boundingBox(BBox& bbox);
 
         void makeActive(void);
         void makeInactive(void);
+        bool isActive(void) const;
 
         void removeFromGraph(void);
         void markForMove(void);
@@ -60,7 +109,7 @@ class ShapeRef
     private:
         Router *_router;
         unsigned int _id;
-        Polygn _poly;
+        Polygon _poly;
         bool _active;
         bool _inMoveList;
         ShapeRefList::iterator _pos;
diff --git a/src/libavoid/static.cpp b/src/libavoid/static.cpp
deleted file mode 100644
index 740a4f9c0..000000000
--- a/src/libavoid/static.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * vim: ts=4 sw=4 et tw=0 wm=0
- *
- * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
-*/
-
-#include <cassert>
-#include <iostream>
-#include "libavoid/vertices.h"
-#include "libavoid/connector.h"
-#include "libavoid/graph.h"
-#include "libavoid/static.h"
-#include "libavoid/shape.h"
-#include "libavoid/visibility.h"
-#include "libavoid/debug.h"
-#include "libavoid/router.h"
-
-namespace Avoid {
-
-static void computeCompleteVis(Router *router);
-
-
-// This should only be used for the static algorithm.
-//
-// XXX: If to set up the vis graph for incremental it would need 
-//      the shapeRef pointers in obs.
-//
-void CreateVisGraph(Router *router, Polygn **obs, int n_obs)
-{
-    for (int poly_i = 0; poly_i < n_obs; poly_i++)
-    {
-        unsigned int id = obs[poly_i]->id;
-        
-        new ShapeRef(router, id, *(obs[poly_i]));
-    }
-    computeCompleteVis(router);
-}
-
-
-static void computeCompleteVis(Router *router)
-{
-    VertInf *beginVert = router->vertices.shapesBegin();
-    VertInf *endVert = router->vertices.end();
-    for (VertInf *i = beginVert; i != endVert; i = i->lstNext)
-    {
-        db_printf("-- CONSIDERING --\n");
-        i->id.db_print();
-
-        for (VertInf *j = i->lstPrev ; j != NULL; j = j->lstPrev)
-        {
-            bool knownNew = true;
-            EdgeInf::checkEdgeVisibility(i, j, knownNew);
-        }
-    }
-}
-
-
-void DestroyVisGraph(Router *router)
-{
-    ShapeRefList::iterator sFinish = router->shapeRefs.end();
-    ShapeRefList::iterator sCurr;
-    
-    while ((sCurr = router->shapeRefs.begin()) != sFinish)
-    {
-        ShapeRef *shape = (*sCurr);
-
-        shape->removeFromGraph();
-        delete shape;
-    }
-    
-    ConnRefList::iterator cFinish = router->connRefs.end();
-    ConnRefList::iterator cCurr;
-    
-    while ((cCurr = router->connRefs.begin())!= cFinish)
-    {
-        ConnRef *conn = (*cCurr);
-
-        conn->removeFromGraph();
-        conn->unInitialise();
-    }
-    // Clear contains info.
-    router->contains.clear();
-
-    assert(router->vertices.connsBegin() == NULL);
-}
-
-
-}
-
diff --git a/src/libavoid/static.h b/src/libavoid/static.h
deleted file mode 100644
index 18e9ac278..000000000
--- a/src/libavoid/static.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * vim: ts=4 sw=4 et tw=0 wm=0
- *
- * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
-*/
-
-
-#ifndef AVOID_STATIC_H
-#define AVOID_STATIC_H
-
-#include "libavoid/geomtypes.h"
-
-
-namespace Avoid {
-
-class Router;
-
-
-extern void CreateVisGraph(Router *router, Polygn **obstacles,
-        int n_obstacles);
-extern void DestroyVisGraph(Router *router);
-
-}
-
-
-#endif
diff --git a/src/libavoid/timer.cpp b/src/libavoid/timer.cpp
index e4349bea9..f8600acbe 100644
--- a/src/libavoid/timer.cpp
+++ b/src/libavoid/timer.cpp
@@ -2,31 +2,34 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2008  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #include <cstdio>
 #include <cstdlib>
-#include <cassert>
-using std::abort;
 #include <climits>
 
 #include "libavoid/timer.h"
+#include "libavoid/debug.h"
+#include "libavoid/assertions.h"
 
 namespace Avoid {
 
@@ -52,9 +55,9 @@ void Timer::Reset(void)
 }
 
 
-void Timer::Register(const int t, const bool start)
+void Timer::Register(const TimerIndex t, const bool start)
 {
-    assert(t != tmNon);
+    COLA_ASSERT(t != tmNon);
 
     if (type == tmNon)
     {
@@ -73,11 +76,7 @@ void Timer::Register(const int t, const bool start)
 
 void Timer::Start(void)
 {
-    if (running)
-    {
-        fprintf(stderr, "ERROR: Timer already running in Timer::Start()\n");
-        abort();
-    }
+    COLA_ASSERT(!running);
     cStart[type] = clock();  // CPU time
     running = true;
 }
@@ -85,11 +84,7 @@ void Timer::Start(void)
 
 void Timer::Stop(void)
 {
-    if (!running)
-    {
-        fprintf(stderr, "ERROR: Timer not running in Timer::Stop()\n");
-        abort();
-    }
+    COLA_ASSERT(running);
     clock_t cStop = clock();      // CPU time
     running = false;
 
@@ -106,11 +101,7 @@ void Timer::Stop(void)
         cDiff = cStop - cStart[type];
     }
     
-    if (cDiff > LONG_MAX)
-    {
-        fprintf(stderr, "Error: cDiff overflow in Timer:Stop()\n");
-        abort();
-    }
+    COLA_ASSERT(cDiff < LONG_MAX);
 
     if (type == tmPth)
     {
@@ -136,11 +127,11 @@ void Timer::Stop(void)
 }
 
 
-void Timer::PrintAll(void)
+void Timer::PrintAll(FILE *fp)
 {
-    for (int i = 0; i < tmCount; i++)
+    for (unsigned int i = 0; i < tmCount; i++)
     {
-        Print(i);
+        Print((TimerIndex) i, fp);
     }
 }
 
@@ -148,14 +139,14 @@ void Timer::PrintAll(void)
 #define toMsec(tot) ((bigclock_t) ((tot) / (((double) CLOCKS_PER_SEC) / 1000)))
 #define toAvg(tot, cnt) ((((cnt) > 0) ? ((long double) (tot)) / (cnt) : 0))
 
-void Timer::Print(const int t)
+void Timer::Print(const TimerIndex t, FILE *fp)
 {
    bigclock_t avg = toMsec(toAvg(cTotal[t], cTally[t]));
    bigclock_t pind = toMsec(toAvg(cPath[t], cPathTally[t]));
    bigclock_t pavg = toMsec(toAvg(cPath[t], cTally[t]));
    double max = toMsec(cMax[t]); 
    double pmax = toMsec(cPathMax[t]);
-   printf("\t%lld %d %lld %.0f %lld %d %lld %.0f %lld\n",
+   fprintf(fp, "\t%lld %d %lld %.0f %lld %d %lld %.0f %lld\n",
            cTotal[t], cTally[t], avg, max,
            cPath[t], cPathTally[t], pavg, pmax, pind);
 }
diff --git a/src/libavoid/timer.h b/src/libavoid/timer.h
index a7e6081fa..9cab6d7ff 100644
--- a/src/libavoid/timer.h
+++ b/src/libavoid/timer.h
@@ -2,24 +2,27 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2008  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef PROFILE_H
 #define PROFILE_H
 
@@ -46,14 +49,20 @@ namespace Avoid {
 
 typedef unsigned long long int bigclock_t;
 
-static const int tmCount = 5;
-
-static const int tmNon = -1;
-static const int tmAdd = 0;
-static const int tmDel = 1;
-static const int tmMov = 2;
-static const int tmPth = 3;
-static const int tmSev = 4;
+enum TimerIndex 
+{
+    tmNon = 0,
+    tmAdd,
+    tmDel,
+    tmMov,
+    tmPth,
+    tmSev,
+    tmOrthogGraph,
+    tmOrthogRoute,
+    tmOrthogCentre,
+    tmOrthogNudge,
+    tmCount
+};
 
 
 static const bool timerStart = true;
@@ -64,12 +73,12 @@ class Timer
 {
     public:
         Timer();
-        void Register(const int t, const bool start = timerDelay);
+        void Register(const TimerIndex t, const bool start = timerDelay);
         void Start(void);
         void Stop(void);
         void Reset(void);
-        void Print(const int t);
-        void PrintAll(void);
+        void Print(TimerIndex, FILE *fp);
+        void PrintAll(FILE *fp);
 
     private:
         clock_t cStart[tmCount];
@@ -82,7 +91,7 @@ class Timer
 
         bool running;
         long count;
-        int type, lasttype;
+        TimerIndex type, lasttype;
 };
 
 
diff --git a/src/libavoid/vertices.cpp b/src/libavoid/vertices.cpp
index c2be955ac..85226498a 100644
--- a/src/libavoid/vertices.cpp
+++ b/src/libavoid/vertices.cpp
@@ -2,33 +2,36 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
+#include <iostream>
+#include <cstdlib>
+
 #include "libavoid/vertices.h"
 #include "libavoid/geometry.h"
 #include "libavoid/graph.h"  // For alertConns
 #include "libavoid/debug.h"
 #include "libavoid/router.h"
-
-#include <iostream>
-#include <cstdlib>
-#include <cassert>
+#include "libavoid/assertions.h"
 
 using std::ostream;
 
@@ -76,7 +79,8 @@ bool VertID::operator==(const VertID& rhs) const
     {
         return false;
     }
-    assert(isShape == rhs.isShape);
+    // XXX RubberBand search breaks this:
+    // COLA_ASSERT(isShape == rhs.isShape);
     return true;
 }
 
@@ -87,7 +91,7 @@ bool VertID::operator!=(const VertID& rhs) const
     {
         return true;
     }
-    assert(isShape == rhs.isShape);
+    COLA_ASSERT(isShape == rhs.isShape);
     return false;
 }
 
@@ -133,8 +137,8 @@ void VertID::db_print(void) const
 }
 
 
-const int VertID::src = 1;
-const int VertID::tar = 2;
+const unsigned short VertID::src = 1;
+const unsigned short VertID::tar = 2;
 
 
 ostream& operator<<(ostream& os, const VertID& vID)
@@ -144,25 +148,57 @@ ostream& operator<<(ostream& os, const VertID& vID)
 
 
 
-VertInf::VertInf(Router *router, const VertID& vid, const Point& vpoint)
-    : _router(router)
-    , id(vid)
-    , point(vpoint)
-    , lstPrev(NULL)
-    , lstNext(NULL)
-    , shPrev(NULL)
-    , shNext(NULL)
-    , visListSize(0)
-    , invisListSize(0)
-    , pathNext(NULL)
-    , pathDist(0)
+VertInf::VertInf(Router *router, const VertID& vid, const Point& vpoint, 
+        const bool addToRouter)
+    : _router(router),
+      id(vid),
+      point(vpoint),
+      lstPrev(NULL),
+      lstNext(NULL),
+      shPrev(NULL),
+      shNext(NULL),
+      visListSize(0),
+      orthogVisListSize(0),
+      invisListSize(0),
+      pathNext(NULL),
+      visDirections(ConnDirNone)
+{
+    point.id = vid.objID;
+    point.vn = vid.vn;
+
+    if (addToRouter)
+    {
+        _router->vertices.addVertex(this);
+    }
+}
+
+
+VertInf::~VertInf()
+{
+}
+
+
+void VertInf::Reset(const VertID& vid, const Point& vpoint)
 {
+    id = vid;
+    point = vpoint;
+    point.id = id.objID;
+    point.vn = id.vn;
 }
 
 
 void VertInf::Reset(const Point& vpoint)
 {
     point = vpoint;
+    point.id = id.objID;
+    point.vn = id.vn;
+}
+
+
+// Returns true if this vertex is not involved in any (in)visibility graphs.
+bool VertInf::orphaned(void)
+{
+    return (visList.empty() && invisList.empty() && orthogVisList.empty());
 }
 
 
@@ -170,15 +206,12 @@ void VertInf::removeFromGraph(const bool isConnVert)
 {
     if (isConnVert)
     {
-        assert(!(id.isShape));
+        COLA_ASSERT(!(id.isShape));
     }
 
-    VertInf *tmp = this;
-
     // For each vertex.
-    EdgeInfList& visList = tmp->visList;
-    EdgeInfList::iterator finish = visList.end();
-    EdgeInfList::iterator edge;
+    EdgeInfList::const_iterator finish = visList.end();
+    EdgeInfList::const_iterator edge;
     while ((edge = visList.begin()) != finish)
     {
         // Remove each visibility edge
@@ -186,7 +219,14 @@ void VertInf::removeFromGraph(const bool isConnVert)
         delete (*edge);
     }
 
-    EdgeInfList& invisList = tmp->invisList;
+    finish = orthogVisList.end();
+    while ((edge = orthogVisList.begin()) != finish)
+    {
+        // Remove each orthogonal visibility edge.
+        (*edge)->alertConns();
+        delete (*edge);
+    }
+
     finish = invisList.end();
     while ((edge = invisList.begin()) != finish)
     {
@@ -208,7 +248,7 @@ bool directVis(VertInf *src, VertInf *dst)
 
     // We better be part of the same instance of libavoid.
     Router *router = src->_router;
-    assert(router == dst->_router);
+    COLA_ASSERT(router == dst->_router);
 
     ContainsMap& contains = router->contains;
     if (!(pID.isShape))
@@ -239,40 +279,40 @@ bool directVis(VertInf *src, VertInf *dst)
 
 
 VertInfList::VertInfList()
-    : _firstShapeVert(NULL)
-    , _firstConnVert(NULL)
-    , _lastShapeVert(NULL)
-    , _lastConnVert(NULL)
-    , _shapeVertices(0)
-    , _connVertices(0)
+    : _firstShapeVert(NULL),
+      _firstConnVert(NULL),
+      _lastShapeVert(NULL),
+      _lastConnVert(NULL),
+      _shapeVertices(0),
+      _connVertices(0)
 {
 }
 
 
 #define checkVertInfListConditions() \
         do { \
-            assert((!_firstConnVert && (_connVertices == 0)) || \
+            COLA_ASSERT((!_firstConnVert && (_connVertices == 0)) || \
                     ((_firstConnVert->lstPrev == NULL) && (_connVertices > 0))); \
-            assert((!_firstShapeVert && (_shapeVertices == 0)) || \
+            COLA_ASSERT((!_firstShapeVert && (_shapeVertices == 0)) || \
                     ((_firstShapeVert->lstPrev == NULL) && (_shapeVertices > 0))); \
-            assert(!_lastShapeVert || (_lastShapeVert->lstNext == NULL)); \
-            assert(!_lastConnVert || (_lastConnVert->lstNext == _firstShapeVert)); \
-            assert((!_firstConnVert && !_lastConnVert) || \
+            COLA_ASSERT(!_lastShapeVert || (_lastShapeVert->lstNext == NULL)); \
+            COLA_ASSERT(!_lastConnVert || (_lastConnVert->lstNext == _firstShapeVert)); \
+            COLA_ASSERT((!_firstConnVert && !_lastConnVert) || \
                     (_firstConnVert &&  _lastConnVert) ); \
-            assert((!_firstShapeVert && !_lastShapeVert) || \
+            COLA_ASSERT((!_firstShapeVert && !_lastShapeVert) || \
                     (_firstShapeVert &&  _lastShapeVert) ); \
-            assert(!_firstShapeVert || _firstShapeVert->id.isShape); \
-            assert(!_lastShapeVert || _lastShapeVert->id.isShape); \
-            assert(!_firstConnVert || !(_firstConnVert->id.isShape)); \
-            assert(!_lastConnVert || !(_lastConnVert->id.isShape)); \
+            COLA_ASSERT(!_firstShapeVert || _firstShapeVert->id.isShape); \
+            COLA_ASSERT(!_lastShapeVert || _lastShapeVert->id.isShape); \
+            COLA_ASSERT(!_firstConnVert || !(_firstConnVert->id.isShape)); \
+            COLA_ASSERT(!_lastConnVert || !(_lastConnVert->id.isShape)); \
         } while(0)
 
 
 void VertInfList::addVertex(VertInf *vert)
 {
     checkVertInfListConditions();
-    assert(vert->lstPrev == NULL);
-    assert(vert->lstNext == NULL);
+    COLA_ASSERT(vert->lstPrev == NULL);
+    COLA_ASSERT(vert->lstNext == NULL);
 
     if (!(vert->id.isShape))
     {
@@ -329,10 +369,18 @@ void VertInfList::addVertex(VertInf *vert)
 }
 
 
-void VertInfList::removeVertex(VertInf *vert)
+// Removes a vertex from the list and returns a pointer to the vertex
+// following the removed one.
+VertInf *VertInfList::removeVertex(VertInf *vert)
 {
+    if (vert == NULL)
+    {
+        return NULL;
+    }
     // Conditions for correct data structure
     checkVertInfListConditions();
+    
+    VertInf *following = vert->lstNext;
 
     if (!(vert->id.isShape))
     {
@@ -421,6 +469,50 @@ void VertInfList::removeVertex(VertInf *vert)
     vert->lstNext = NULL;
 
     checkVertInfListConditions();
+
+    return following;
+}
+
+
+VertInf *VertInfList::getVertexByID(const VertID& id)
+{
+    VertID searchID = id;
+    if (searchID.vn == kUnassignedVertexNumber)
+    {
+        unsigned int topbit = ((unsigned int) 1) << 31;
+        if (searchID.objID & topbit)
+        {
+            searchID.objID = searchID.objID & ~topbit;
+            searchID.vn = VertID::src;
+        }
+        else
+        {
+            searchID.vn = VertID::tar;
+        }
+    }
+    VertInf *last = end();
+    for (VertInf *curr = connsBegin(); curr != last; curr = curr->lstNext)
+    {
+        if (curr->id == searchID)
+        {
+            return curr;
+        }
+    }
+    return NULL;
+}
+
+
+VertInf *VertInfList::getVertexByPos(const Point& p)
+{
+    VertInf *last = end();
+    for (VertInf *curr = shapesBegin(); curr != last; curr = curr->lstNext)
+    {
+        if (curr->point == p)
+        {
+            return curr;
+        }
+    }
+    return NULL;
 }
 
 
@@ -447,6 +539,18 @@ VertInf *VertInfList::end(void)
 }
 
 
+unsigned int VertInfList::connsSize(void) const
+{
+    return _connVertices;
+}
+
+
+unsigned int VertInfList::shapesSize(void) const
+{
+    return _shapeVertices;
+}
+
+
 }
 
 
diff --git a/src/libavoid/vertices.h b/src/libavoid/vertices.h
index 1b0dcf3aa..b07c87f95 100644
--- a/src/libavoid/vertices.h
+++ b/src/libavoid/vertices.h
@@ -2,24 +2,27 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_VERTICES_H
 #define AVOID_VERTICES_H
 
@@ -28,6 +31,7 @@
 #include <map>
 #include <iostream>
 #include <cstdio>
+
 #include "libavoid/geomtypes.h"
 
 namespace Avoid {
@@ -37,16 +41,18 @@ class Router;
 
 typedef std::list<EdgeInf *> EdgeInfList;
 
+typedef unsigned int ConnDirFlags;
+
 
 class VertID
 {
     public:
         unsigned int objID;
         bool isShape;
-        int vn;
+        unsigned short vn;
 
-        static const int src;
-        static const int tar;
+        static const unsigned short src;
+        static const unsigned short tar;
 
         VertID();
         VertID(unsigned int id, bool s, int n);
@@ -64,12 +70,23 @@ class VertID
 };
 
 
+// An ID given to all dummy vertices inserted to allow creation of the
+// orthogonal visibility graph since the vertices in the orthogonal graph 
+// mostly do not correspond to shape corners or connector endpoints.
+//
+static const VertID dummyOrthogID(0, true, 0);
+
+
 class VertInf
 {
     public:
-        VertInf(Router *router, const VertID& vid, const Point& vpoint);
+        VertInf(Router *router, const VertID& vid, const Point& vpoint,
+                const bool addToRouter = true);
+        ~VertInf();
+        void Reset(const VertID& vid, const Point& vpoint);
         void Reset(const Point& vpoint);
         void removeFromGraph(const bool isConnVert = true);
+        bool orphaned(void);
 
         Router *_router;
         VertID id;
@@ -80,28 +97,40 @@ class VertInf
         VertInf *shNext;
         EdgeInfList visList;
         unsigned int visListSize;
+        EdgeInfList orthogVisList;
+        unsigned int orthogVisListSize;
         EdgeInfList invisList;
         unsigned int invisListSize;
         VertInf *pathNext;
-        double pathDist;
+        ConnDirFlags visDirections;
 };
 
 
 bool directVis(VertInf *src, VertInf *dst);
 
 
+// A linked list of all the vertices in the router instance.  All the 
+// connector endpoints are listed first, then all the shape vertices.
+// Dunnny vertices inserted for orthogonal routing are classed as shape
+// vertices but have VertID(0, 0).
+//
 class VertInfList
 {
     public:
         VertInfList();
         void addVertex(VertInf *vert);
-        void removeVertex(VertInf *vert);
+        VertInf *removeVertex(VertInf *vert);
+        VertInf *getVertexByID(const VertID& id);
+        VertInf *getVertexByPos(const Point& p);
         VertInf *shapesBegin(void);
         VertInf *connsBegin(void);
         VertInf *end(void);
-        void stats(void)
+        unsigned int connsSize(void) const;
+        unsigned int shapesSize(void) const;
+        void stats(FILE *fp = stderr)
         {
-            printf("Conns %d, shapes %d\n", _connVertices, _shapeVertices);
+            fprintf(fp, "Conns %d, shapes %d\n", _connVertices, 
+                    _shapeVertices);
         }
     private:
         VertInf *_firstShapeVert;
diff --git a/src/libavoid/visibility.cpp b/src/libavoid/visibility.cpp
index d2b057643..089911f35 100644
--- a/src/libavoid/visibility.cpp
+++ b/src/libavoid/visibility.cpp
@@ -2,26 +2,39 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2009  Monash University
+ *
+ * --------------------------------------------------------------------
+ * The Visibility Sweep technique is based upon the method described
+ * in Section 5.2 of:
+ *     Lee, D.-T. (1978). Proximity and reachability in the plane.,
+ *     PhD thesis, Department of Electrical Engineering, 
+ *     University of Illinois, Urbana, IL.
+ * --------------------------------------------------------------------
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #include <algorithm>
 #include <cfloat>
+#define _USE_MATH_DEFINES
+#include <cmath>
 
 #include "libavoid/shape.h"
 #include "libavoid/debug.h"
@@ -30,8 +43,7 @@
 #include "libavoid/graph.h"
 #include "libavoid/geometry.h"
 #include "libavoid/router.h"
-
-#include <math.h>
+#include "libavoid/assertions.h"
 
 #ifdef LINEDEBUG
   #include "SDL_gfxPrimitives.h"
@@ -53,16 +65,7 @@ void shapeVis(ShapeRef *shape)
     VertInf *shapeBegin = shape->firstVert();
     VertInf *shapeEnd = shape->lastVert()->lstNext;
 
-    VertInf *pointsBegin = NULL;
-    if (router->IncludeEndpoints)
-    {
-        pointsBegin = router->vertices.connsBegin();
-    }
-    else
-    {
-        pointsBegin = router->vertices.shapesBegin();
-    }
-
+    VertInf *pointsBegin = router->vertices.connsBegin();
     for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
     {
         bool knownNew = true;
@@ -73,6 +76,11 @@ void shapeVis(ShapeRef *shape)
         db_printf("\tFirst Half:\n");
         for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
         {
+            if (j->id == dummyOrthogID)
+            {
+                // Don't include orthogonal dummy vertices.
+                continue;
+            }
             EdgeInf::checkEdgeVisibility(curr, j, knownNew);
         }
 
@@ -80,6 +88,11 @@ void shapeVis(ShapeRef *shape)
         VertInf *pointsEnd = router->vertices.end();
         for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext)
         {
+            if (k->id == dummyOrthogID)
+            {
+                // Don't include orthogonal dummy vertices.
+                continue;
+            }
             EdgeInf::checkEdgeVisibility(curr, k, knownNew);
         }
     }
@@ -113,7 +126,7 @@ void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
     const VertID& pID = point->id;
 
     // Make sure we're only doing ptVis for endpoints.
-    assert(!(pID.isShape));
+    COLA_ASSERT(!(pID.isShape));
 
     if ( !(router->InvisibilityGrph) )
     {
@@ -135,9 +148,14 @@ void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
         for (VertInf *k = router->vertices.shapesBegin(); k != shapesEnd;
                 k = k->lstNext)
         {
+            if (k->id == dummyOrthogID)
+            {
+                // Don't include orthogonal dummy vertices.
+                continue;
+            }
             EdgeInf::checkEdgeVisibility(point, k, knownNew);
         }
-        if (router->IncludeEndpoints && partner)
+        if (partner)
         {
             EdgeInf::checkEdgeVisibility(point, partner, knownNew);
         }
@@ -152,7 +170,6 @@ void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
 static VertInf *centerInf;
 static Point centerPoint;
 static VertID centerID;
-static double centerAngle;
 
 
 class PointPair
@@ -165,19 +182,44 @@ class PointPair
             double y = vInf->point.y - centerPoint.y;
 
             angle = pos_to_angle(x, y);
+            distance = euclideanDist(centerPoint, vInf->point);
         }
-        bool operator==(const PointPair& rhs) const
+        bool operator<(const PointPair& rhs) const
         {
-            if (vInf->id == rhs.vInf->id)
+            // Firstly order by angle.
+            if (angle == rhs.angle)
             {
-                return true;
+                // If the points are collinear, then order them in increasing
+                // distance from the point we are sweeping around.
+                if (distance == rhs.distance)
+                {
+                    // XXX: Add this assertion back if we require that 
+                    //      connector endpoints have unique IDs. For the 
+                    //      moment it is okay for them to have the same ID.
+                    //COLA_ASSERT(vInf->id != rhs.vInf->id);
+                    
+                    // If comparing two points at the same physical 
+                    // position, then order them by their VertIDs.
+                    return vInf->id < rhs.vInf->id;
+                }
+                return distance < rhs.distance;
             }
-            return false;
+            return angle < rhs.angle;
         }
         static double pos_to_angle(double x, double y)
         {
+            if (y == 0)
+            {
+                return ((x < 0) ? 180 : 0);
+            }
+            else if (x == 0)
+            {
+                return ((y < 0) ? 270 : 90);
+            }
+            
             double ang = atan(y / x);
             ang = (ang * 180) / M_PI;
+
             if (x < 0)
             {
                 ang += 180;
@@ -186,36 +228,48 @@ class PointPair
             {
                 ang += 360;
             }
+            COLA_ASSERT(ang >= 0);
+            COLA_ASSERT(ang <= 360);
             return ang;
         }
 
         VertInf    *vInf;
         double     angle;
+        double     distance;
 };
 
 
-typedef std::list<PointPair > VertList;
+typedef std::set<PointPair > VertSet;
 
 
 class EdgePair
 {
     public:
-        EdgePair(VertInf *v1, VertInf *v2, double d, double a)
-            : vInf1(v1), vInf2(v2), initdist(d), initangle(a)
+        EdgePair() :
+            vInf1(NULL), vInf2(NULL), dist1(0.0), dist2(0.0), angle(0.0),
+            angleDist(0.0)
+        {
+            // The default constuctor should never be called.  
+            // This is defined to appease the MSVC compiler.
+            COLA_ASSERT(false);
+        }
+        EdgePair(const PointPair& p1, VertInf *v) : 
+                vInf1(p1.vInf), 
+                vInf2(v),
+                dist1(p1.distance),
+                dist2(euclideanDist(vInf2->point, centerPoint)),
+                angle(p1.angle),
+                angleDist(p1.distance)
         {
-            currdist  = initdist;
-            currangle = initangle;
         }
         bool operator<(const EdgePair& rhs) const
         {
-            if (initdist == rhs.initdist)
+            COLA_ASSERT(angle == rhs.angle);
+            if (angleDist == rhs.angleDist)
             {
-                // TODO: This is a bit of a hack, should be
-                //       set by the call to the constructor.
-                return dist(centerPoint, vInf2->point) <
-                        dist(centerPoint, rhs.vInf2->point);
+                return (dist2 < rhs.dist2);
             }
-            return (initdist < rhs.initdist);
+            return (angleDist < rhs.angleDist);
         }
         bool operator==(const EdgePair& rhs) const
         {
@@ -239,37 +293,53 @@ class EdgePair
             }
             return true;
         }
-        void SetObsAng(double a)
+        void setNegativeAngle(void)
         {
-            obsAngle = fmod(initangle - (a - 180), 360);
+            angle = -1.0;
+        }
+        double setCurrAngle(const PointPair& p)
+        {
+            if (p.vInf->point == vInf1->point)
+            {
+                angleDist = dist1;
+                angle = p.angle;
+            }
+            else if (p.vInf->point == vInf2->point)
+            {
+                angleDist = dist2;
+                angle = p.angle;
+            }
+            else if (p.angle != angle)
+            {
+                COLA_ASSERT(p.angle > angle);
+                angle = p.angle;
+                Point pp;
+                int result = rayIntersectPoint(vInf1->point, vInf2->point,
+                        centerPoint, p.vInf->point, &(pp.x), &(pp.y));
+                if (result != DO_INTERSECT) 
+                {
+                    // This can happen with points that appear to have the
+                    // same angle but at are at slightly different positions
+                    angleDist = std::min(dist1, dist2);
+                }
+                else
+                {
+                    angleDist = euclideanDist(pp, centerPoint);
+                }
+            }
 
-            //db_printf("SetObsAng: %.2f  (from init %.2f, a %.2f)\n",
-            //      obsAngle, initangle, a);
+            return angleDist;
         }
 
         VertInf *vInf1;
         VertInf *vInf2;
-        double  initdist;
-        double  initangle;
-        double  currdist;
-        double  currangle;
-        double  obsAngle;
+        double dist1;
+        double dist2;
+        double angle;
+        double angleDist;
 };
 
-typedef std::set<EdgePair> EdgeSet;
-
-
-static bool ppCompare(PointPair& pp1, PointPair& pp2)
-{
-    if (pp1.angle == pp2.angle)
-    {
-        // If the points are colinear, then order them in increasing
-        // distance from the point we are sweeping around.
-        return dist(centerPoint, pp1.vInf->point) <
-                dist(centerPoint, pp2.vInf->point);
-    }
-    return pp1.angle < pp2.angle;
-}
+typedef std::list<EdgePair> SweepEdgeList;
 
 
 #define AHEAD    1
@@ -278,11 +348,11 @@ static bool ppCompare(PointPair& pp1, PointPair& pp2)
 class isBoundingShape
 {
     public:
-        // constructor remembers the value provided
-        isBoundingShape(ShapeSet& set)
-        : ss(set)
+        // Class instance remembers the ShapeSet.
+        isBoundingShape(ShapeSet& set) : 
+            ss(set)
         { }
-        // the following is an overloading of the function call operator
+        // The following is an overloading of the function call operator.
         bool operator () (const PointPair& pp)
         {
             if (pp.vInf->id.isShape &&
@@ -293,58 +363,111 @@ class isBoundingShape
             return false;
         }
     private:
+        // MSVC wants to generate the assignment operator and the default 
+        // constructor, but fails.  Therefore we declare them private and 
+        // don't implement them.
+        isBoundingShape & operator=(isBoundingShape const &);
+        isBoundingShape();
+
         ShapeSet& ss;
 };
 
 
-static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf,
-        bool lastVisible, double lastAngle, int *blocker)
+static bool sweepVisible(SweepEdgeList& T, const PointPair& point, 
+        std::set<unsigned int>& onBorderIDs, int *blocker)
 {
+    if (T.empty())
+    {
+        // No blocking edges.
+        return true;
+    }
 
-    if (!lastInf || (lastAngle != centerAngle))
+    Router *router = point.vInf->_router;
+    bool visible = true;
+
+    SweepEdgeList::const_iterator closestIt = T.begin();
+    SweepEdgeList::const_iterator end = T.end();
+    while (closestIt != end)
     {
-        // Nothing before it on the current ray
-        EdgeSet::iterator closestIt = T.begin();
-        if (closestIt != T.end())
+        if ((point.vInf->point == closestIt->vInf1->point) ||
+                (point.vInf->point == closestIt->vInf2->point))
         {
+            // If the ray intersects just the endpoint of a 
+            // blocking edge then ignore that edge.
+            ++closestIt;
+            continue;
+        }
+        break;
+    }
+    if (closestIt == end)
+    {
+        return true;
+    }
 
-            Point &e1 = (*closestIt).vInf1->point;
-            Point &e2 = (*closestIt).vInf2->point;
-
-            if (segmentIntersect(centerInf->point, currInf->point, e1, e2))
+    if (! point.vInf->id.isShape )
+    {
+        // It's a connector endpoint, so we have to ignore 
+        // edges of containing shapes for determining visibility.
+        ShapeSet& rss = router->contains[point.vInf->id];
+        while (closestIt != end)
+        {
+            if (rss.find(closestIt->vInf1->id.objID) == rss.end())
             {
-                *blocker = (*closestIt).vInf1->id.objID;
-                return false;
+                // This is not a containing edge so do the normal 
+                // test and then stop.
+                if (point.distance > closestIt->angleDist)
+                {
+                    visible = false;
+                }
+                else if ((point.distance == closestIt->angleDist) && 
+                        onBorderIDs.find(closestIt->vInf1->id.objID) != 
+                                onBorderIDs.end())
+                {
+                    // Touching, but centerPoint is on another edge of
+                    // shape shape, so count as blocking.
+                    visible = false;
+                }
+                break;
             }
+            // This was a containing edge, so consider the next along.
+            ++closestIt;
         }
     }
     else
     {
-        // There was another point before this on the ray (lastInf)
-        if (!lastVisible)
+        // Just test to see if this point is closer than the closest 
+        // edge blocking this ray.
+        if (point.distance > closestIt->angleDist)
         {
-            *blocker = -1;
-            return false;
+            visible =  false;
         }
-        else
+        else if ((point.distance == closestIt->angleDist) && 
+                onBorderIDs.find(closestIt->vInf1->id.objID) != 
+                        onBorderIDs.end())
         {
-            // Check if there is an edge in T that blocks the ray
-            // between lastInf and currInf.
-            EdgeSet::iterator tfin = T.end();
-            for (EdgeSet::iterator l = T.begin(); l != tfin; ++l)
-            {
-                Point &e1 = (*l).vInf1->point;
-                Point &e2 = (*l).vInf2->point;
+            // Touching, but centerPoint is on another edge of
+            // shape shape, so count as blocking.
+            visible = false;
+        }
+    }
 
-                if (segmentIntersect(lastInf->point, currInf->point, e1, e2))
-                {
-                    *blocker = (*l).vInf1->id.objID;
-                    return false;
-                }
-            }
+    if (!visible)
+    {
+        *blocker = (*closestIt).vInf1->id.objID;
+#ifdef LINEDEBUG
+        Point &e1 = (*closestIt).vInf1->point;
+        Point &e2 = (*closestIt).vInf2->point;
+
+        if (router->avoid_screen)
+        {
+            int canx = 151;
+            int cany = 55;
+            lineRGBA(router->avoid_screen, e1.x + canx, e1.y + cany,
+                    e2.x + canx, e2.y + cany, 0, 0, 225, 255);
         }
+#endif
     }
-    return true;
+    return visible;
 }
 
 
@@ -358,120 +481,128 @@ void vertexSweep(VertInf *vert)
     centerID = pID;
     centerPoint = pPoint;
     Point centerPt = pPoint;
-    centerAngle = -1;
 
     // List of shape (and maybe endpt) vertices, except p
     // Sort list, around
-    VertList v;
+    VertSet v;
 
     // Initialise the vertex list
+    ShapeSet& ss = router->contains[centerID];
     VertInf *beginVert = router->vertices.connsBegin();
     VertInf *endVert = router->vertices.end();
     for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext)
     {
-        if (inf->id == centerID)
+        if (inf == centerInf)
+        {
+            // Don't include the center point itself.
+            continue;
+        }
+        else if (inf->id == dummyOrthogID)
         {
-            // Don't include the center point
+            // Don't include orthogonal dummy vertices.
+            continue;
+        }
+
+        if (!(centerID.isShape) && (ss.find(inf->id.objID) != ss.end()))
+        {
+            // Don't include edge points of containing shapes.
+            unsigned int shapeID = inf->id.objID;
+            db_printf("Center is inside shape %u so ignore shape edges.\n",
+                    shapeID);
             continue;
         }
 
         if (inf->id.isShape)
         {
-            // Add shape vertex
-            v.push_back(inf);
+            // Add shape vertex.
+            v.insert(inf);
         }
         else
         {
-            if (router->IncludeEndpoints)
+            // Add connector endpoint.
+            if (centerID.isShape)
             {
-                if (centerID.isShape)
-                {
-                    // Add endpoint vertex
-                    v.push_back(inf);
-                }
-                else
+                // Center is a shape vertex, so add all endpoint vertices.
+                v.insert(inf);
+            }
+            else
+            {
+                // Center is an endpoint, so only include the other
+                // endpoint from the matching connector.
+                VertID partnerID = VertID(centerID.objID, false,
+                        (centerID.vn == 1) ? 2 : 1);
+                if (inf->id == partnerID)
                 {
-                    // Center is an endpoint, so only include the other
-                    // endpoint from the matching connector.
-                    VertID partnerID = VertID(centerID.objID, false,
-                            (centerID.vn == 1) ? 2 : 1);
-                    if (inf->id == partnerID)
-                    {
-                        v.push_back(inf);
-                    }
+                    v.insert(inf);
                 }
             }
         }
     }
-    // TODO: This should be done with a sorted data type and insertion sort.
-    v.sort(ppCompare);
-
-    EdgeSet e;
-    ShapeSet& ss = router->contains[centerID];
+    std::set<unsigned int> onBorderIDs;
 
-    // And edge to T that intersect the initial ray.
-    VertInf *last = router->vertices.end();
-    for (VertInf *k = router->vertices.shapesBegin(); k != last; )
+    // Add edges to T that intersect the initial ray.
+    SweepEdgeList e;
+    VertSet::const_iterator vbegin = v.begin();
+    VertSet::const_iterator vend = v.end();
+    for (VertSet::const_iterator t = vbegin; t != vend; ++t)
     {
-        VertID kID = k->id;
-        if (!(centerID.isShape) && (ss.find(kID.objID) != ss.end()))
-        {
-            unsigned int shapeID = kID.objID;
-            db_printf("Center is inside shape %u so ignore shape edges.\n",
-                    shapeID);
-            // One of the endpoints is inside this shape so ignore it.
-            while ((k != last) && (k->id.objID == shapeID))
-            {
-                // And skip the other vertices from this shape.
-                k = k->lstNext;
-            }
-            continue;
-        }
+        VertInf *k = t->vInf;
 
-        VertInf *kPrev = k->shPrev;
-        if ((centerInf == k) || (centerInf == kPrev))
-        {
-            k = k->lstNext;
-            continue;
-        }
+        COLA_ASSERT(centerInf != k);
+        COLA_ASSERT(centerID.isShape || (ss.find(k->id.objID) == ss.end()));
 
         Point xaxis(DBL_MAX, centerInf->point.y);
 
-        if (segmentIntersect(centerInf->point, xaxis, kPrev->point, k->point))
+        VertInf *kPrev = k->shPrev;
+        VertInf *kNext = k->shNext;
+        if (kPrev && (kPrev != centerInf) && 
+                (vecDir(centerInf->point, xaxis, kPrev->point) == AHEAD))
         {
-            double distance;
-            if (vecDir(centerInf->point, xaxis, kPrev->point) == BEHIND)
+            if (segmentIntersect(centerInf->point, xaxis, kPrev->point, 
+                        k->point))
             {
-                distance = dist(centerInf->point, kPrev->point);
+                EdgePair intPair = EdgePair(*t, kPrev);
+                e.push_back(intPair);
             }
-            else
+            if ((vecDir(kPrev->point, k->point, centerInf->point) == 0) &&
+                    inBetween(kPrev->point, k->point, centerInf->point))
             {
-                distance = dist(centerInf->point, k->point);
+                // Record that centerPoint is on an obstacle line.
+                onBorderIDs.insert(k->id.objID);
+            }
+        }
+        else if (kNext && (kNext != centerInf) && 
+                (vecDir(centerInf->point, xaxis, kNext->point) == AHEAD))
+        {
+            if (segmentIntersect(centerInf->point, xaxis, kNext->point, 
+                        k->point))
+            {
+                EdgePair intPair = EdgePair(*t, kNext);
+                e.push_back(intPair);
+            }
+            if ((vecDir(kNext->point, k->point, centerInf->point) == 0) &&
+                    inBetween(kNext->point, k->point, centerInf->point))
+            {
+                // Record that centerPoint is on an obstacle line.
+                onBorderIDs.insert(k->id.objID);
             }
-
-            EdgePair intPair = EdgePair(k, kPrev, distance, 0.0);
-            e.insert(intPair).first;
         }
-        k = k->lstNext;
     }
+    for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c)
+    {
+        (*c).setNegativeAngle();
+    }
+
 
     // Start the actual sweep.
     db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");
 
-    VertInf *lastInf     = NULL;
-    double   lastAngle   = 0;
-    bool     lastVisible = false;
-    int      lastBlocker = 0;
-
-    isBoundingShape isBounding(router->contains[centerID]);
-    VertList::iterator vfst = v.begin();
-    VertList::iterator vfin = v.end();
-    for (VertList::iterator t = vfst; t != vfin; ++t)
+    isBoundingShape isBounding(ss);
+    for (VertSet::const_iterator t = vbegin; t != vend; ++t)
     {
         VertInf *currInf = (*t).vInf;
         VertID& currID = currInf->id;
         Point&  currPt = currInf->point;
-        centerAngle = (*t).angle;
 
 #ifdef LINEDEBUG
         Sint16 ppx = (int) centerPt.x;
@@ -479,29 +610,28 @@ void vertexSweep(VertInf *vert)
 
         Sint16 cx = (int) currPt.x;
         Sint16 cy = (int) currPt.y;
+
+        int canx = 151;
+        int cany = 55;
 #endif
 
-        double currDist = dist(centerPt, currPt);
-        db_printf("Dist: %.1f.\n", currDist);
+        const double& currDist = (*t).distance;
 
         EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf);
         if (edge == NULL)
         {
             edge = new EdgeInf(centerInf, currInf);
         }
-        // Ignore vertices from bounding shapes, if sweeping round an endpoint.
-        if (!(centerID.isShape) && isBounding(*t))
+
+        for (SweepEdgeList::iterator c = e.begin(); c != e.end(); ++c)
         {
-            if (router->InvisibilityGrph)
-            {
-                // if p and t can't see each other, add blank edge
-                db_printf("\tSkipping visibility edge... \n\t\t");
-                edge->addBlocker(currInf->id.objID);
-                edge->db_print();
-            }
-            continue;
+            (*c).setCurrAngle(*t);
         }
+        e.sort();
 
+        // Check visibility.
+        int blocker = 0;
+        bool currVisible = sweepVisible(e, *t, onBorderIDs, &blocker);
 
         bool cone1 = true, cone2 = true;
         if (centerID.isShape)
@@ -519,7 +649,6 @@ void vertexSweep(VertInf *vert)
 
         if (!cone1 || !cone2)
         {
-            lastInf = NULL;
             if (router->InvisibilityGrph)
             {
                 db_printf("\tSetting invisibility edge... \n\t\t");
@@ -529,18 +658,15 @@ void vertexSweep(VertInf *vert)
         }
         else
         {
-            int blocker = 0;
-            // Check visibility.
-            bool currVisible = sweepVisible(e, currInf,
-                    lastInf, lastVisible, lastAngle, &blocker);
-            if (blocker == -1)
-            {
-                blocker = lastBlocker;
-            }
             if (currVisible)
             {
 #ifdef LINEDEBUG
-                lineRGBA(avoid_screen, ppx, ppy, cx, cy, 255, 0, 0, 32);
+                if (router->avoid_screen)
+                {
+                    lineRGBA(router->avoid_screen, ppx + canx, ppy + cany,
+                            cx + canx, cy + cany, 255, 0, 0, 75);
+                    SDL_Delay(1000);
+                }
 #endif
                 db_printf("\tSetting visibility edge... \n\t\t");
                 edge->setDist(currDist);
@@ -552,72 +678,55 @@ void vertexSweep(VertInf *vert)
                 edge->addBlocker(blocker);
                 edge->db_print();
             }
-
-            lastVisible = currVisible;
-            lastInf     = currInf;
-            lastAngle   = centerAngle;
-            lastBlocker = blocker;
+        }
+        
+        if (!(edge->added()) && !(router->InvisibilityGrph))
+        {
+            delete edge;
+            edge = NULL;
         }
 
         if (currID.isShape)
         {
             // This is a shape edge
-            Point& prevPt = currInf->shPrev->point;
-            Point& nextPt = currInf->shNext->point;
-
-            int prevDir = vecDir(centerPt, currPt, prevPt);
-            EdgePair prevPair = EdgePair(currInf, currInf->shPrev,
-                    currDist, centerAngle);
 
-            EdgeSet::iterator ePtr;
-            if (prevDir == BEHIND)
+            if (currInf->shPrev != centerInf)
             {
-                // XXX: Strangely e.find does not return the correct results.
-                // ePtr = e.find(prevPair);
-                ePtr = std::find(e.begin(), e.end(), prevPair);
-                if (ePtr != e.end())
+                Point& prevPt = currInf->shPrev->point;
+                int prevDir = vecDir(centerPt, currPt, prevPt);
+                EdgePair prevPair = EdgePair(*t, currInf->shPrev);
+
+                if (prevDir == BEHIND)
                 {
-                    e.erase(ePtr);
+                    e.remove(prevPair);
                 }
-            }
-            else if ((prevDir == AHEAD) && (currInf->shPrev != centerInf))
-            {
-                double x = prevPt.x - currPt.x;
-                double y = prevPt.y - currPt.y;
-                double angle = PointPair::pos_to_angle(x, y);
-                prevPair.SetObsAng(angle);
-
-                ePtr = e.insert(prevPair).first;
-            }
-
-
-            int nextDir = vecDir(centerPt, currPt, nextPt);
-            EdgePair nextPair = EdgePair(currInf, currInf->shNext,
-                    currDist, centerAngle);
-
-            if (nextDir == BEHIND)
-            {
-                // XXX: Strangely e.find does not return the correct results.
-                // ePtr = e.find(nextPair);
-                ePtr = std::find(e.begin(), e.end(), nextPair);
-                if (ePtr != e.end())
+                else if (prevDir == AHEAD)
                 {
-                    e.erase(ePtr);
+                    e.push_front(prevPair);
                 }
             }
-            else if ((nextDir == AHEAD) && (currInf->shNext != centerInf))
+
+            if (currInf->shNext != centerInf)
             {
-                double x = nextPt.x - currPt.x;
-                double y = nextPt.y - currPt.y;
-                double angle = PointPair::pos_to_angle(x, y);
-                nextPair.SetObsAng(angle);
+                Point& nextPt = currInf->shNext->point;
+                int nextDir = vecDir(centerPt, currPt, nextPt);
+                EdgePair nextPair = EdgePair(*t, currInf->shNext);
 
-                ePtr = e.insert(nextPair).first;
+                if (nextDir == BEHIND)
+                {
+                    e.remove(nextPair);
+                }
+                else if (nextDir == AHEAD)
+                {
+                    e.push_front(nextPair);
+                }
             }
         }
-
 #ifdef LINEDEBUG
-        SDL_Flip(avoid_screen);
+        if (router->avoid_screen)
+        {
+            SDL_Flip(router->avoid_screen);
+        }
 #endif
     }
 }
diff --git a/src/libavoid/visibility.h b/src/libavoid/visibility.h
index dd68ac692..92e56d922 100644
--- a/src/libavoid/visibility.h
+++ b/src/libavoid/visibility.h
@@ -2,24 +2,27 @@
  * vim: ts=4 sw=4 et tw=0 wm=0
  *
  * libavoid - Fast, Incremental, Object-avoiding Line Router
- * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
+ *
+ * Copyright (C) 2004-2008  Monash University
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- * 
+ * Author(s):   Michael Wybrow <mjwybrow@users.sourceforge.net>
 */
 
+
 #ifndef AVOID_VISIBILITY_H
 #define AVOID_VISIBILITY_H
 
@@ -28,7 +31,9 @@
 
 namespace Avoid {
 
-    
+class ShapeRef;
+class VertInf;
+
 extern void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
             const bool gen_contains = false);
 extern void vertexSweep(VertInf *point);
diff --git a/src/sp-conn-end-pair.cpp b/src/sp-conn-end-pair.cpp
index 4dc0230ff..98b2aec26 100644
--- a/src/sp-conn-end-pair.cpp
+++ b/src/sp-conn-end-pair.cpp
@@ -12,6 +12,8 @@
 
 #include <cstring>
 #include <string>
+#include <iostream>
+#include <glibmm/stringutils.h>
 
 #include "attributes.h"
 #include "sp-conn-end.h"
@@ -26,10 +28,10 @@
 
 
 SPConnEndPair::SPConnEndPair(SPPath *const owner)
-    : _invalid_path_connection()
-    , _path(owner)
+    : _path(owner)
     , _connRef(NULL)
     , _connType(SP_CONNECTOR_NOAVOID)
+    , _connCurvature(0.0)
     , _transformed_connection()
 {
     for (unsigned handle_ix = 0; handle_ix <= 1; ++handle_ix) {
@@ -47,14 +49,6 @@ SPConnEndPair::~SPConnEndPair()
         delete this->_connEnd[handle_ix];
         this->_connEnd[handle_ix] = NULL;
     }
-    if (_connRef) {
-        _connRef->removeFromGraph();
-        delete _connRef;
-        _connRef = NULL;
-    }
-
-    _invalid_path_connection.disconnect();
-    _transformed_connection.disconnect();
 }
 
 void
@@ -68,6 +62,18 @@ SPConnEndPair::release()
         this->_connEnd[handle_ix]->href = NULL;
         this->_connEnd[handle_ix]->ref.detach();
     }
+
+    // If the document is being destroyed then the router instance 
+    // and the ConnRefs will have been destroyed with it.
+    const bool routerInstanceExists = (_path->document->router != NULL);
+
+    if (_connRef && routerInstanceExists) {
+        _connRef->removeFromGraph();
+        delete _connRef;
+    }
+    _connRef = NULL;
+
+    _transformed_connection.disconnect();
 }
 
 void
@@ -76,16 +82,17 @@ sp_conn_end_pair_build(SPObject *object)
     sp_object_read_attr(object, "inkscape:connector-type");
     sp_object_read_attr(object, "inkscape:connection-start");
     sp_object_read_attr(object, "inkscape:connection-end");
+    sp_object_read_attr(object, "inkscape:connector-curvature");
 }
 
 
 static void
-avoid_conn_move(Geom::Matrix const */*mp*/, SPItem *moved_item)
+avoid_conn_transformed(Geom::Matrix const */*mp*/, SPItem *moved_item)
 {
-    // Reroute connector
     SPPath *path = SP_PATH(moved_item);
-    path->connEndPair.makePathInvalid();
-    sp_conn_adjust_invalid_path(path);
+    if (path->connEndPair.isAutoRoutingConn()) {
+        path->connEndPair.tellLibavoidNewEndpoints();
+    }
 }
 
 
@@ -93,16 +100,40 @@ void
 SPConnEndPair::setAttr(unsigned const key, gchar const *const value)
 {
     if (key == SP_ATTR_CONNECTOR_TYPE) {
-        if (value && (strcmp(value, "polyline") == 0)) {
-            _connType = SP_CONNECTOR_POLYLINE;
-
-            Avoid::Router *router = _path->document->router;
-            GQuark itemID = g_quark_from_string(SP_OBJECT(_path)->id);
-            _connRef = new Avoid::ConnRef(router, itemID);
-            _invalid_path_connection = connectInvalidPath(
-                    sigc::ptr_fun(&sp_conn_adjust_invalid_path));
-            _transformed_connection = _path->connectTransformed(
-                    sigc::ptr_fun(&avoid_conn_move));
+        if (value && (strcmp(value, "polyline") == 0 || strcmp(value, "orthogonal") == 0)) {
+            int newconnType = strcmp(value, "polyline") ? SP_CONNECTOR_ORTHOGONAL : SP_CONNECTOR_POLYLINE;
+            
+            if (!_connRef)
+            {
+                _connType = newconnType;
+                Avoid::Router *router = _path->document->router;
+                GQuark itemID = g_quark_from_string(SP_OBJECT(_path)->id);
+                _connRef = new Avoid::ConnRef(router, itemID);
+                switch (newconnType)
+                {
+                    case SP_CONNECTOR_POLYLINE:
+                        _connRef->setRoutingType(Avoid::ConnType_PolyLine);
+                        break;
+                    case SP_CONNECTOR_ORTHOGONAL:
+                        _connRef->setRoutingType(Avoid::ConnType_Orthogonal);
+                }
+                _transformed_connection = _path->connectTransformed(
+                        sigc::ptr_fun(&avoid_conn_transformed));
+            }
+            else
+                if (newconnType != _connType)
+                {
+                    _connType = newconnType;
+                    switch (newconnType)
+                    {
+                        case SP_CONNECTOR_POLYLINE:
+                            _connRef->setRoutingType(Avoid::ConnType_PolyLine);
+                            break;
+                        case SP_CONNECTOR_ORTHOGONAL:
+                            _connRef->setRoutingType(Avoid::ConnType_Orthogonal);
+                    }
+                    sp_conn_reroute_path(_path);
+                }
         }
         else {
             _connType = SP_CONNECTOR_NOAVOID;
@@ -111,17 +142,25 @@ SPConnEndPair::setAttr(unsigned const key, gchar const *const value)
                 _connRef->removeFromGraph();
                 delete _connRef;
                 _connRef = NULL;
-                _invalid_path_connection.disconnect();
                 _transformed_connection.disconnect();
             }
         }
         return;
-
+    }
+    else if (key == SP_ATTR_CONNECTOR_CURVATURE) {
+        if (value) {
+            _connCurvature = g_strtod(value, NULL);
+            if (_connRef && _connRef->isInitialised()) {
+                // Redraw the connector, but only if it has been initialised.
+                sp_conn_reroute_path(_path);
+            }
+        }
+        return;
     }
 
     unsigned const handle_ix = key - SP_ATTR_CONNECTION_START;
     g_assert( handle_ix <= 1 );
-    this->_connEnd[handle_ix]->setAttacherHref(value);
+    this->_connEnd[handle_ix]->setAttacherHref(value, _path);
 }
 
 void
@@ -131,11 +170,18 @@ SPConnEndPair::writeRepr(Inkscape::XML::Node *const repr) const
         if (this->_connEnd[handle_ix]->ref.getURI()) {
             char const * const attr_strs[] = {"inkscape:connection-start",
                                               "inkscape:connection-end"};
-            gchar *uri_string = this->_connEnd[handle_ix]->ref.getURI()->toString();
-            repr->setAttribute(attr_strs[handle_ix], uri_string);
-            g_free(uri_string);
+            std::ostringstream ostr;
+            ostr<<this->_connEnd[handle_ix]->ref.getURI()->toString()<<"_"<<
+                  (this->_connEnd[handle_ix]->type == ConnPointDefault ? "d":"u") <<
+                  "_" << this->_connEnd[handle_ix]->id;
+            
+            
+            repr->setAttribute(attr_strs[handle_ix], ostr.str().c_str());
         }
     }
+    repr->setAttribute("inkscape:connector-curvature", Glib::Ascii::dtostr(_connCurvature).c_str());
+    if (_connType == SP_CONNECTOR_POLYLINE || _connType == SP_CONNECTOR_ORTHOGONAL)
+        repr->setAttribute("inkscape:connector-type", _connType == SP_CONNECTOR_POLYLINE ? "polyline" : "orthogonal" );
 }
 
 void
@@ -161,19 +207,14 @@ SPConnEndPair::getAttachedItems(SPItem *h2attItem[2]) const {
 
 void
 SPConnEndPair::getEndpoints(Geom::Point endPts[]) const {
-    SPCurve *curve = _path->curve;
+    SPCurve *curve = _path->original_curve ? _path->original_curve : _path->curve;
     SPItem *h2attItem[2];
     getAttachedItems(h2attItem);
 
     for (unsigned h = 0; h < 2; ++h) {
         if ( h2attItem[h] ) {
-            Geom::OptRect bbox = h2attItem[h]->getBounds(sp_item_i2doc_affine(h2attItem[h]));
-            if (bbox) {
-                endPts[h] = bbox->midpoint();
-            } else {
-                // FIXME
-                endPts[h] = Geom::Point(0, 0);
-            }
+            g_assert(h2attItem[h]->avoidRef);
+            endPts[h] = h2attItem[h]->avoidRef->getConnectionPointPos(_connEnd[h]->type, _connEnd[h]->id);
         }
         else
         {
@@ -187,37 +228,40 @@ SPConnEndPair::getEndpoints(Geom::Point endPts[]) const {
     }
 }
 
-sigc::connection
-SPConnEndPair::connectInvalidPath(sigc::slot<void, SPPath *> slot)
-{
-    return _invalid_path_signal.connect(slot);
+gdouble
+SPConnEndPair::getCurvature(void) const {
+    return _connCurvature;
 }
 
-static void emitPathInvalidationNotification(void *ptr)
+SPConnEnd**
+SPConnEndPair::getConnEnds(void)
 {
-    // We emit a signal here rather than just calling the reroute function
-    // since this allows all the movement action computation to happen,
-    // then all connectors (that require it) will be rerouted.  Otherwise,
-    // one connector could get rerouted several times as a result of
-    // dragging a couple of shapes.
+    return _connEnd;
+}
 
-    SPPath *path = SP_PATH(ptr);
-    path->connEndPair._invalid_path_signal.emit(path);
+bool
+SPConnEndPair::isOrthogonal(void) const {
+    return _connType == SP_CONNECTOR_ORTHOGONAL;
 }
 
-void
-SPConnEndPair::rerouteFromManipulation(void)
+
+static void redrawConnectorCallback(void *ptr)
 {
-    _connRef->makePathInvalid();
-    sp_conn_adjust_path(_path);
+    SPPath *path = SP_PATH(ptr);
+    if (path->document == NULL) {
+        // This can happen when the document is being destroyed.
+        return;
+    }
+    sp_conn_redraw_path(path);
 }
 
 void
-SPConnEndPair::reroute(void)
+SPConnEndPair::rerouteFromManipulation(void)
 {
-    sp_conn_adjust_path(_path);
+    sp_conn_reroute_path_immediate(_path);
 }
 
+
 // Called from sp_path_update to initialise the endpoints.
 void
 SPConnEndPair::update(void)
@@ -231,8 +275,8 @@ SPConnEndPair::update(void)
             Avoid::Point src(endPt[0][Geom::X], endPt[0][Geom::Y]);
             Avoid::Point dst(endPt[1][Geom::X], endPt[1][Geom::Y]);
 
-            _connRef->lateSetup(src, dst);
-            _connRef->setCallback(&emitPathInvalidationNotification, _path);
+            _connRef->setEndpoints(src, dst);
+            _connRef->setCallback(&redrawConnectorCallback, _path);
         }
         // Store the ID of the objects attached to the connector.
         storeIds();
@@ -243,19 +287,25 @@ SPConnEndPair::update(void)
 void SPConnEndPair::storeIds(void)
 {
     if (_connEnd[0]->href) {
+        gchar ** href_strarray = NULL;
+        href_strarray = g_strsplit(_connEnd[0]->href, "_", 0);
         // href begins with a '#' which we don't want.
-        const char *startId = _connEnd[0]->href + 1;
+        const char *startId = href_strarray[0] + 1;
         GQuark itemId = g_quark_from_string(startId);
         _connRef->setEndPointId(Avoid::VertID::src, itemId);
+        g_strfreev(href_strarray);
     }
     else {
         _connRef->setEndPointId(Avoid::VertID::src, 0);
     }
     if (_connEnd[1]->href) {
+        gchar ** href_strarray = NULL;
+        href_strarray = g_strsplit(_connEnd[1]->href, "_", 0);
         // href begins with a '#' which we don't want.
-        const char *endId = _connEnd[1]->href + 1;
+        const char *endId = href_strarray[0] + 1;
         GQuark itemId = g_quark_from_string(endId);
         _connRef->setEndPointId(Avoid::VertID::tar, itemId);
+        g_strfreev(href_strarray);
     }
     else {
         _connRef->setEndPointId(Avoid::VertID::tar, 0);
@@ -278,15 +328,55 @@ SPConnEndPair::makePathInvalid(void)
     _connRef->makePathInvalid();
 }
 
+
+// Redraws the curve along the recalculated route
+// Straight or curved
+void recreateCurve(SPCurve *curve, Avoid::ConnRef *connRef, const gdouble curvature)
+{
+    bool straight = curvature<1e-3;
+
+    Avoid::PolyLine route = connRef->displayRoute();
+    if (!straight)
+        route = route.curvedPolyline(curvature);
+    connRef->calcRouteDist();
+    
+    curve->reset();
+
+    curve->moveto( Geom::Point(route.ps[0].x, route.ps[0].y) );
+    int pn = route.size();
+    for (int i = 1; i < pn; ++i) {
+        Geom::Point p(route.ps[i].x, route.ps[i].y);
+        if (straight) {
+            curve->lineto( p );
+        }
+        else {
+            switch (route.ts[i]) {
+                case 'M':
+                    curve->moveto( p );
+                    break;
+                case 'L':
+                    curve->lineto( p );
+                    break;
+                case 'C':
+                    g_assert( i+2<pn );
+                    curve->curveto( p, Geom::Point(route.ps[i+1].x, route.ps[i+1].y), 
+                            Geom::Point(route.ps[i+2].x, route.ps[i+2].y) );
+                    i+=2;
+                    break;
+            }
+        }
+    }
+}
+
+
 void
-SPConnEndPair::reroutePath(void)
+SPConnEndPair::tellLibavoidNewEndpoints(const bool processTransaction)
 {
     if (!isAutoRoutingConn()) {
         // Do nothing
         return;
     }
-
-    SPCurve *curve = _path->curve;
+    makePathInvalid();
 
     Geom::Point endPt[2];
     getEndpoints(endPt);
@@ -294,26 +384,34 @@ SPConnEndPair::reroutePath(void)
     Avoid::Point src(endPt[0][Geom::X], endPt[0][Geom::Y]);
     Avoid::Point dst(endPt[1][Geom::X], endPt[1][Geom::Y]);
 
-    _connRef->updateEndPoint(Avoid::VertID::src, src);
-    _connRef->updateEndPoint(Avoid::VertID::tar, dst);
-
-    _connRef->generatePath(src, dst);
-
-    Avoid::PolyLine route = _connRef->route();
-    _connRef->calcRouteDist();
+    _connRef->setEndpoints(src, dst);
+    if (processTransaction)
+    {
+        _connRef->router()->processTransaction();
+    }
+    return;
+}
 
-    curve->reset();
-    curve->moveto(endPt[0]);
 
-    for (int i = 1; i < route.pn; ++i) {
-        Geom::Point p(route.ps[i].x, route.ps[i].y);
-        curve->lineto(p);
+bool
+SPConnEndPair::reroutePathFromLibavoid(void)
+{
+    if (!isAutoRoutingConn()) {
+        // Do nothing
+        return false;
     }
 
+    SPCurve *curve = _path->original_curve ?_path->original_curve : _path->curve;
+
+    recreateCurve( curve, _connRef, _connCurvature );
+    
     Geom::Matrix doc2item = sp_item_i2doc_affine(SP_ITEM(_path)).inverse();
     curve->transform(doc2item); 
+    
+    return true;
 }
 
+
 /*
   Local Variables:
   mode:c++
diff --git a/src/sp-conn-end-pair.h b/src/sp-conn-end-pair.h
index 9b9e181db..3b011ed17 100644
--- a/src/sp-conn-end-pair.h
+++ b/src/sp-conn-end-pair.h
@@ -28,6 +28,7 @@ class Node;
 }
 }
 
+extern void recreateCurve(SPCurve *curve, Avoid::ConnRef *connRef, gdouble curvature);
 
 class SPConnEndPair {
 public:
@@ -38,19 +39,16 @@ public:
     void writeRepr(Inkscape::XML::Node *const repr) const;
     void getAttachedItems(SPItem *[2]) const;
     void getEndpoints(Geom::Point endPts[]) const;
-    void reroutePath(void);
+    gdouble getCurvature(void) const;
+    SPConnEnd** getConnEnds(void);
+    bool isOrthogonal(void) const;
+    friend void recreateCurve(SPCurve *curve, Avoid::ConnRef *connRef, gdouble curvature);
+    void tellLibavoidNewEndpoints(const bool processTransaction = false);
+    bool reroutePathFromLibavoid(void);
     void makePathInvalid(void);
     void update(void);
     bool isAutoRoutingConn(void);
     void rerouteFromManipulation(void);
-    void reroute(void);
-    sigc::connection connectInvalidPath(sigc::slot<void, SPPath *> slot);
-
-    // A signal emited by a call back from libavoid.  Used to let 
-    // connectors know when they need to reroute themselves.
-    sigc::signal<void, SPPath *> _invalid_path_signal;
-    // A sigc connection to listen for connector path invalidation.
-    sigc::connection _invalid_path_connection;
 
 private:
     SPConnEnd *_connEnd[2];
@@ -61,6 +59,7 @@ private:
     Avoid::ConnRef *_connRef;
 
     int _connType;
+    gdouble _connCurvature;
     
     // A sigc connection for transformed signal.
     sigc::connection _transformed_connection;
@@ -74,8 +73,9 @@ void sp_conn_end_pair_build(SPObject *object);
 
 // _connType options:
 enum {
-    SP_CONNECTOR_NOAVOID,    // Basic connector - a straight line.
-    SP_CONNECTOR_POLYLINE    // Object avoiding polyline.
+    SP_CONNECTOR_NOAVOID,     // Basic connector - a straight line.
+    SP_CONNECTOR_POLYLINE,    // Object avoiding polyline.
+    SP_CONNECTOR_ORTHOGONAL   // Object avoiding orthogonal polyline (only horizontal and verical segments).
 };
 
 
diff --git a/src/sp-conn-end.cpp b/src/sp-conn-end.cpp
index 0b420a98e..52914ee9e 100644
--- a/src/sp-conn-end.cpp
+++ b/src/sp-conn-end.cpp
@@ -16,7 +16,7 @@
 #include "2geom/path-intersection.h"
 
 
-static void change_endpts(SPCurve *const curve, Geom::Point const h2endPt[2]);
+static void change_endpts(SPCurve *const curve, double const endPos[2]);
 
 SPConnEnd::SPConnEnd(SPObject *const owner) :
     ref(owner),
@@ -39,45 +39,37 @@ get_nearest_common_ancestor(SPObject const *const obj, SPItem const *const objs[
 }
 
 
-static bool try_get_intersect_point_with_item_recursive(SPCurve *conn_curve, SPItem& item, 
-        const Geom::Matrix& item_transform, const bool at_start, double* intersect_pos, 
-        unsigned *intersect_index) {
-
-    double initial_pos = (at_start) ? 0.0 : std::numeric_limits<double>::max();
+static bool try_get_intersect_point_with_item_recursive(Geom::PathVector& conn_pv, SPItem* item,
+        const Geom::Matrix& item_transform, double& intersect_pos) {
 
+    double initial_pos = intersect_pos;
     // if this is a group...
-    if (SP_IS_GROUP(&item)) {
-        SPGroup* group = SP_GROUP(&item);
+    if (SP_IS_GROUP(item)) {
+        SPGroup* group = SP_GROUP(item);
         
         // consider all first-order children
-        double child_pos = initial_pos;
-        unsigned child_index;
+        double child_pos = std::numeric_limits<double>::max();
         for (GSList const* i = sp_item_group_item_list(group); i != NULL; i = i->next) {
             SPItem* child_item = SP_ITEM(i->data);
-            try_get_intersect_point_with_item_recursive(conn_curve, *child_item, 
-                    item_transform * child_item->transform, at_start, &child_pos, &child_index);
-            if (fabs(initial_pos - child_pos) > fabs(initial_pos - *intersect_pos)) {
-                // It is further away from the initial point than the current intersection
-                // point (i.e. the "outermost" intersection), so use this one.
-                *intersect_pos = child_pos;
-                *intersect_index = child_index;
-            }
+            try_get_intersect_point_with_item_recursive(conn_pv, child_item,
+                    item_transform * child_item->transform, child_pos);
+            if (intersect_pos > child_pos)
+                intersect_pos = child_pos;
         }
-        return *intersect_pos != initial_pos;
+        return intersect_pos != initial_pos;
     }
 
-    // if this is a shape...
-    if (!SP_IS_SHAPE(&item)) return false;
+    // if this is not a shape, nothing to be done
+    if (!SP_IS_SHAPE(item)) return false;
 
     // make sure it has an associated curve
-    SPCurve* item_curve = sp_shape_get_curve(SP_SHAPE(&item));
+    SPCurve* item_curve = sp_shape_get_curve(SP_SHAPE(item));
     if (!item_curve) return false;
 
     // apply transformations (up to common ancestor)
     item_curve->transform(item_transform);
 
     const Geom::PathVector& curve_pv = item_curve->get_pathvector();
-    const Geom::PathVector& conn_pv = conn_curve->get_pathvector();
     Geom::CrossingSet cross = crossings(conn_pv, curve_pv);
     // iterate over all Crossings
     for (Geom::CrossingSet::const_iterator i = cross.begin(); i != cross.end(); i++) {
@@ -85,18 +77,14 @@ static bool try_get_intersect_point_with_item_recursive(SPCurve *conn_curve, SPI
 
         for (Geom::Crossings::const_iterator i = cr.begin(); i != cr.end(); i++) {
             const Geom::Crossing& cr_pt = *i;
-            if (fabs(initial_pos - cr_pt.ta) > fabs(initial_pos - *intersect_pos)) {
-                // It is further away from the initial point than the current intersection
-                // point (i.e. the "outermost" intersection), so use this one.
-                *intersect_pos = cr_pt.ta;
-                *intersect_index = cr_pt.a;
-            }
+            if ( intersect_pos > cr_pt.ta)
+                intersect_pos = cr_pt.ta;
         }
     }
 
     item_curve->unref();
 
-    return *intersect_pos != initial_pos;
+    return intersect_pos != initial_pos;
 }
 
 
@@ -104,22 +92,36 @@ static bool try_get_intersect_point_with_item_recursive(SPCurve *conn_curve, SPI
 // and the item given.  If the item is a group, then the component items are considered.
 // The transforms given should be to a common ancestor of both the path and item.
 //
-static bool try_get_intersect_point_with_item(SPPath& conn, SPItem& item, 
+static bool try_get_intersect_point_with_item(SPPath* conn, SPItem* item,
         const Geom::Matrix& item_transform, const Geom::Matrix& conn_transform, 
-        const bool at_start, double* intersect_pos, unsigned *intersect_index) {
+        const bool at_start, double& intersect_pos) {
  
-    // We start with the intersection point either at the beginning or end of the 
-    // path, depending on whether we are considering the source or target endpoint.
-    *intersect_pos = (at_start) ? 0.0 : std::numeric_limits<double>::max();
-
     // Copy the curve and apply transformations up to common ancestor.
-    SPCurve* conn_curve = conn.curve->copy();
+    SPCurve* conn_curve = conn->curve->copy();
     conn_curve->transform(conn_transform);
 
+    Geom::PathVector conn_pv = conn_curve->get_pathvector();
+
+    // If this is not the starting point, use Geom::Path::reverse() to reverse the path
+    if (!at_start)
+    {
+        // connectors are actually a single path, so consider the first element from a Geom::PathVector
+        conn_pv[0] = conn_pv[0].reverse();
+    }
+
+    // We start with the intersection point at the end of the path
+    intersect_pos = conn_pv[0].size();
+
     // Find the intersection.
-    bool result = try_get_intersect_point_with_item_recursive(conn_curve, item, item_transform, 
-            at_start, intersect_pos, intersect_index);
-    
+    bool result = try_get_intersect_point_with_item_recursive(conn_pv, item, item_transform, intersect_pos);
+
+    if (!result)
+        // No intersection point has been found (why?)
+        // just default to connector end
+        intersect_pos = 0;
+    // If not at the starting point, recompute position with respect to original path
+    if (!at_start)
+        intersect_pos = conn_pv[0].size() - intersect_pos;
     // Free the curve copy.
     conn_curve->unref();
 
@@ -128,18 +130,15 @@ static bool try_get_intersect_point_with_item(SPPath& conn, SPItem& item,
 
 
 static void
-sp_conn_end_move_compensate(Geom::Matrix const */*mp*/, SPItem */*moved_item*/,
-                            SPPath *const path,
-                            bool const updatePathRepr = true)
+sp_conn_get_route_and_redraw(SPPath *const path, 
+        const bool updatePathRepr = true)
 {
-    // TODO: SPItem::getBounds gives the wrong result for some objects
-    //       that have internal representations that are updated later
-    //       by the sp_*_update functions, e.g., text.
-    sp_document_ensure_up_to_date(path->document);
-
     // Get the new route around obstacles.
-    path->connEndPair.reroutePath();
-
+    bool rerouted = path->connEndPair.reroutePathFromLibavoid();
+    if (!rerouted) {
+        return;
+    }
+    
     SPItem *h2attItem[2];
     path->connEndPair.getAttachedItems(h2attItem);
 
@@ -147,72 +146,80 @@ sp_conn_end_move_compensate(Geom::Matrix const */*mp*/, SPItem */*moved_item*/,
     SPObject const *const ancestor = get_nearest_common_ancestor(path_item, h2attItem);
     Geom::Matrix const path2anc(i2anc_affine(path_item, ancestor));
 
-    Geom::Point endPts[2] = { *(path->curve->first_point()), *(path->curve->last_point()) };
- 
+    // Set sensible values incase there the connector ends are not 
+    // attached to any shapes.
+    Geom::PathVector conn_pv = path->curve->get_pathvector();
+    double endPos[2] = { 0, conn_pv[0].size() };
+
+    SPConnEnd** _connEnd = path->connEndPair.getConnEnds();
     for (unsigned h = 0; h < 2; ++h) {
-        if (h2attItem[h]) {
-            // For each attached object, change the corresponding point to be
-            // at the outermost intersection with the object's path.
-            double intersect_pos;
-            unsigned intersect_index;
+        if (h2attItem[h] && _connEnd[h]->type == ConnPointDefault && _connEnd[h]->id == ConnPointPosCC) {
             Geom::Matrix h2i2anc = i2anc_affine(h2attItem[h], ancestor);
-            if ( try_get_intersect_point_with_item(*path, *h2attItem[h], h2i2anc, path2anc, 
-                        (h == 0), &intersect_pos, &intersect_index) ) {
-                const Geom::PathVector& curve = path->curve->get_pathvector();
-                endPts[h] = curve[intersect_index].pointAt(intersect_pos);
-            }
+            try_get_intersect_point_with_item(path, h2attItem[h], h2i2anc, path2anc,
+                        (h == 0), endPos[h]);
         }
     }
-    change_endpts(path->curve, endPts);
+    change_endpts(path->curve, endPos);
     if (updatePathRepr) {
-        path->requestDisplayUpdate(SP_OBJECT_MODIFIED_FLAG);
         path->updateRepr();
+        path->requestDisplayUpdate(SP_OBJECT_MODIFIED_FLAG);
     }
 }
 
-// TODO: This triggering of makeInvalidPath could be cleaned up to be
-//       another option passed to move_compensate.
+
 static void
-sp_conn_end_shape_move_compensate(Geom::Matrix const *mp, SPItem *moved_item,
+sp_conn_end_shape_move(Geom::Matrix const */*mp*/, SPItem */*moved_item*/,
                             SPPath *const path)
 {
     if (path->connEndPair.isAutoRoutingConn()) {
-        path->connEndPair.makePathInvalid();
+        path->connEndPair.tellLibavoidNewEndpoints();
     }
-    sp_conn_end_move_compensate(mp, moved_item, path);
 }
 
 
 void
-sp_conn_adjust_invalid_path(SPPath *const path)
+sp_conn_reroute_path(SPPath *const path)
 {
-    sp_conn_end_move_compensate(NULL, NULL, path);
+    if (path->connEndPair.isAutoRoutingConn()) {
+        path->connEndPair.tellLibavoidNewEndpoints();
+    }
 }
 
+
 void
-sp_conn_adjust_path(SPPath *const path)
+sp_conn_reroute_path_immediate(SPPath *const path)
 {
     if (path->connEndPair.isAutoRoutingConn()) {
-        path->connEndPair.makePathInvalid();
+        bool processTransaction = true;
+        path->connEndPair.tellLibavoidNewEndpoints(processTransaction);
     }
     // Don't update the path repr or else connector dragging is slowed by
     // constant update of values to the xml editor, and each step is also
     // needlessly remembered by undo/redo.
     bool const updatePathRepr = false;
-    sp_conn_end_move_compensate(NULL, NULL, path, updatePathRepr);
+    sp_conn_get_route_and_redraw(path, updatePathRepr);
+}
+
+void sp_conn_redraw_path(SPPath *const path)
+{
+    sp_conn_get_route_and_redraw(path);
 }
 
 
 static void
-change_endpts(SPCurve *const curve, Geom::Point const h2endPt[2])
+change_endpts(SPCurve *const curve, double const endPos[2])
 {
-#if 0
-    curve->reset();
-    curve->moveto(h2endPt[0]);
-    curve->lineto(h2endPt[1]);
-#else
-    curve->move_endpoints(h2endPt[0], h2endPt[1]);
-#endif
+    // Use Geom::Path::portion to cut the curve at the end positions
+    if (endPos[0] > endPos[1])
+    {
+        // Path is "negative", reset the curve and return
+        curve->reset();
+        return;
+    }
+    const Geom::Path& old_path = curve->get_pathvector()[0];
+    Geom::PathVector new_path_vector;
+    new_path_vector.push_back(old_path.portion(endPos[0], endPos[1]));
+    curve->set_pathvector(new_path_vector);
 }
 
 static void
@@ -234,29 +241,150 @@ sp_conn_end_detach(SPObject *const owner, unsigned const handle_ix)
 }
 
 void
-SPConnEnd::setAttacherHref(gchar const *value)
+SPConnEnd::setAttacherHref(gchar const *value, SPPath* path)
 {
     if ( value && href && ( strcmp(value, href) == 0 ) ) {
         /* No change, do nothing. */
     } else {
-        g_free(href);
-        href = NULL;
-        if (value) {
-            // First, set the href field, because sp_conn_end_href_changed will need it.
-            href = g_strdup(value);
-
-            // Now do the attaching, which emits the changed signal.
-            try {
-                ref.attach(Inkscape::URI(value));
-            } catch (Inkscape::BadURIException &e) {
-                /* TODO: Proper error handling as per
-                 * http://www.w3.org/TR/SVG11/implnote.html#ErrorProcessing.  (Also needed for
-                 * sp-use.) */
-                g_warning("%s", e.what());
+        if (!value)
+        {
+            ref.detach();
+            g_free(href);
+            href = NULL;
+        }
+        else
+        {
+
+            /* References to the connection points have the following format
+               #svguri_t_id, where #svguri is the id of the item the
+               connector is attached to, t is the type of the point, which
+               can be either "d" for default or "u" for user-defined, and
+               id is the local (inside the item) id of the connection point.
+               In the case of default points id represents the position on the
+               item (i.e. Top-Left, Centre-Centre, etc.).
+            */
+
+            gchar ** href_strarray = NULL;
+            if (href)
+                href_strarray = g_strsplit(href, "_", 0);
+            gchar ** value_strarray = g_strsplit(value, "_", 0);
+
+            g_free(href);
+            href = NULL;
+
+            bool changed = false;
+            bool validRef = true;
+
+            if ( !href_strarray || g_strcmp0(href_strarray[0], value_strarray[0]) != 0 )
+            {
+                // The href has changed, so update it.
+                changed = true;
+                // Set the href field, because sp_conn_end_href_changed will need it.
+                href = g_strdup(value);
+                // Now do the attaching, which emits the changed signal.
+                try {
+                    ref.attach(Inkscape::URI(value_strarray[0]));
+                } catch (Inkscape::BadURIException &e) {
+                    /* TODO: Proper error handling as per
+                    * http://www.w3.org/TR/SVG11/implnote.html#ErrorProcessing.  (Also needed for
+                    * sp-use.) */
+                    g_warning("%s", e.what());
+                    validRef = false;
+                }
+            }
+            // Check to see if the connection point changed and update it.
+            // 
+
+            if ( !value_strarray[1] )
+            {
+                /* Treat the old references to connection points
+                   as default points that connect to the centre
+                   of the item.
+                */
+                if ( type != ConnPointDefault )
+                {
+                    type = ConnPointDefault;
+                    changed = true;
+                }
+                if ( id != ConnPointPosCC )
+                {
+                    id = ConnPointPosCC;
+                    changed = true;
+                }
+            }
+            else
+            {
+                switch (value_strarray[1][0])
+                {
+                    case 'd':
+                        if ( type != ConnPointDefault )
+                        {
+                            type = ConnPointDefault;
+                            changed = true;
+                        }
+                        break;
+                    case 'u':
+                        if ( type != ConnPointUserDefined)
+                        {
+                            type = ConnPointUserDefined;
+                            changed = true;
+                        }
+                        break;
+                    default:
+                        g_warning("Bad reference to a connection point.");
+                        validRef = false;
+                }
+                if ( value_strarray[2] )
+                {
+                    int newId = (int) g_ascii_strtod( value_strarray[2], 0 );
+                    if ( id != newId )
+                    {
+                        id = newId;
+                        changed = true;
+                    }
+
+                }
+                else
+                {
+                    // We have a malformed reference to a connection point,
+                    // emit a warning, clear href and detach ref.
+                    changed = true;
+                    g_warning("Bad reference to a connection point.");\
+                    validRef = false;
+                }
+            }
+            
+            if ( changed )
+            {
+                // We still have to verify that the reference to the
+                // connection point is a valid one.
+                
+                // Get the item the connector is attached to
+                SPItem* item = ref.getObject();
+                if ( item && !item->avoidRef->isValidConnPointId( type, id ) )
+                {
+                    g_warning("Bad reference to a connection point.");
+                    validRef = false;
+                }
+/*                else
+                    // Update the connector
+                    if (path->connEndPair.isAutoRoutingConn()) {
+                        path->connEndPair.tellLibavoidNewEndpoints();
+                    }*/
+            }
+
+            if ( !validRef )
+            {
                 ref.detach();
+                g_free(href);
+                href = NULL;
             }
-        } else {
-            ref.detach();
+            else
+                if (!href)
+                    href = g_strdup(value);
+
+            g_strfreev(href_strarray);
+            g_strfreev(value_strarray);
         }
     }
 }
@@ -277,7 +405,7 @@ sp_conn_end_href_changed(SPObject */*old_ref*/, SPObject */*ref*/,
                 = SP_OBJECT(refobj)->connectDelete(sigc::bind(sigc::ptr_fun(&sp_conn_end_deleted),
                                                               SP_OBJECT(path), handle_ix));
             connEnd._transformed_connection
-                = SP_ITEM(refobj)->connectTransformed(sigc::bind(sigc::ptr_fun(&sp_conn_end_shape_move_compensate),
+                = SP_ITEM(refobj)->connectTransformed(sigc::bind(sigc::ptr_fun(&sp_conn_end_shape_move),
                                                                  path));
         }
     }
diff --git a/src/sp-conn-end.h b/src/sp-conn-end.h
index a565b6404..5e9dbb9da 100644
--- a/src/sp-conn-end.h
+++ b/src/sp-conn-end.h
@@ -5,6 +5,8 @@
 #include <sigc++/connection.h>
 
 #include "sp-use-reference.h"
+#include "connection-points.h"
+#include "conn-avoid-ref.h"
 
 
 class SPConnEnd {
@@ -14,6 +16,15 @@ public:
     SPUseReference ref;
     gchar *href;
 
+    /* In the following, type refers to connection point type,
+       i.e. default (one of the 9 combinations of right, centre,
+       left, top, bottom) or user-defined. The id serves to identify
+       the connection point in a list of connection points.
+    */
+
+    ConnPointType type;
+    int id;
+
     /** Change of href string (not a modification of the attributes of the referrent). */
     sigc::connection _changed_connection;
 
@@ -23,7 +34,7 @@ public:
     /** A sigc connection for transformed signal, used to do move compensation. */
     sigc::connection _transformed_connection;
 
-    void setAttacherHref(gchar const *);
+    void setAttacherHref(gchar const *, SPPath *);
 
 private:
     SPConnEnd(SPConnEnd const &);
@@ -32,8 +43,9 @@ private:
 
 void sp_conn_end_href_changed(SPObject *old_ref, SPObject *ref,
                               SPConnEnd *connEnd, SPPath *path, unsigned const handle_ix);
-void sp_conn_adjust_invalid_path(SPPath *const path);
-void sp_conn_adjust_path(SPPath *const path);
+void sp_conn_reroute_path(SPPath *const path);
+void sp_conn_reroute_path_immediate(SPPath *const path);
+void sp_conn_redraw_path(SPPath *const path);
 void sp_conn_end_detach(SPObject *const owner, unsigned const handle_ix);
 
 
diff --git a/src/sp-item.cpp b/src/sp-item.cpp
index 395048280..9f7157b99 100644
--- a/src/sp-item.cpp
+++ b/src/sp-item.cpp
@@ -412,6 +412,7 @@ sp_item_build(SPObject *object, SPDocument *document, Inkscape::XML::Node *repr)
     sp_object_read_attr(object, "inkscape:transform-center-x");
     sp_object_read_attr(object, "inkscape:transform-center-y");
     sp_object_read_attr(object, "inkscape:connector-avoid");
+    sp_object_read_attr(object, "inkscape:connection-points");
 
     if (((SPObjectClass *) (parent_class))->build) {
         (* ((SPObjectClass *) (parent_class))->build)(object, document, repr);
@@ -514,6 +515,9 @@ sp_item_set(SPObject *object, unsigned key, gchar const *value)
         case SP_ATTR_CONNECTOR_AVOID:
             item->avoidRef->setAvoid(value);
             break;
+        case SP_ATTR_CONNECTION_POINTS:
+            item->avoidRef->setConnectionPoints(value);
+            break;
         case SP_ATTR_TRANSFORM_CENTER_X:
             if (value) {
                 item->transform_center_x = g_strtod(value, NULL);
diff --git a/src/svg/stringstream.cpp b/src/svg/stringstream.cpp
index 6b9e512a1..431d5d97b 100644
--- a/src/svg/stringstream.cpp
+++ b/src/svg/stringstream.cpp
@@ -66,6 +66,31 @@ operator<<(Inkscape::SVGOStringStream &os, Geom::Point const & p)
     return os;
 }
 
+Inkscape::SVGIStringStream::SVGIStringStream():std::istringstream()
+{
+    this->imbue(std::locale::classic());
+    this->setf(std::ios::showpoint);
+
+    /* This one is (currently) needed though, as we currently use ostr.precision as a sort of
+       variable for storing the desired precision: see our two precision methods and our operator<<
+       methods for float and double. */
+    Inkscape::Preferences *prefs = Inkscape::Preferences::get();
+    this->precision(prefs->getInt("/options/svgoutput/numericprecision", 8));
+}
+
+Inkscape::SVGIStringStream::SVGIStringStream(const std::string& str):std::istringstream(str)
+{
+    this->imbue(std::locale::classic());
+    this->setf(std::ios::showpoint);
+
+    /* This one is (currently) needed though, as we currently use ostr.precision as a sort of
+       variable for storing the desired precision: see our two precision methods and our operator<<
+       methods for float and double. */
+    Inkscape::Preferences *prefs = Inkscape::Preferences::get();
+    this->precision(prefs->getInt("/options/svgoutput/numericprecision", 8));
+}
+
+
 /*
   Local Variables:
   mode:c++
diff --git a/src/svg/stringstream.h b/src/svg/stringstream.h
index 5fbf1976c..60ed74ecb 100644
--- a/src/svg/stringstream.h
+++ b/src/svg/stringstream.h
@@ -74,6 +74,13 @@ public:
     }
 };
 
+class SVGIStringStream:public std::istringstream {
+
+public:
+    SVGIStringStream();
+    SVGIStringStream(const std::string &str);
+};
+
 }
 
 Inkscape::SVGOStringStream &operator<<(Inkscape::SVGOStringStream &os, float d);
diff --git a/src/ui/icon-names.h b/src/ui/icon-names.h
index 0388b200f..76e76ea34 100644
--- a/src/ui/icon-names.h
+++ b/src/ui/icon-names.h
@@ -56,10 +56,18 @@
     "color-picker"
 #define INKSCAPE_ICON_COLOR_REMOVE \
     "color-remove"
+#define INKSCAPE_ICON_CONNECTOR_EDIT \
+    "connector-edit"
 #define INKSCAPE_ICON_CONNECTOR_AVOID \
     "connector-avoid"
 #define INKSCAPE_ICON_CONNECTOR_IGNORE \
     "connector-ignore"
+#define INKSCAPE_ICON_CONNECTOR_ORTHOGONAL \
+    "connector-orthogonal"
+#define INKSCAPE_ICON_CONNECTOR_NEW_CONNPOINT \
+    "connector-new-connpoint"
+#define INKSCAPE_ICON_CONNECTOR_REMOVE_CONNPOINT \
+    "connector-remove-connpoint"
 #define INKSCAPE_ICON_DIALOG_ALIGN_AND_DISTRIBUTE \
     "dialog-align-and-distribute"
 #define INKSCAPE_ICON_DIALOG_FILL_AND_STROKE \
diff --git a/src/widgets/toolbox.cpp b/src/widgets/toolbox.cpp
index 5f163d138..b10c60c79 100644
--- a/src/widgets/toolbox.cpp
+++ b/src/widgets/toolbox.cpp
@@ -470,13 +470,18 @@ static gchar const * ui_descr =
         "  </toolbar>"
 
         "  <toolbar name='ConnectorToolbar'>"
+        "    <toolitem action='ConnectorEditModeAction' />"
         "    <toolitem action='ConnectorAvoidAction' />"
         "    <toolitem action='ConnectorIgnoreAction' />"
+        "    <toolitem action='ConnectorOrthogonalAction' />"
+        "    <toolitem action='ConnectorCurvatureAction' />"
         "    <toolitem action='ConnectorSpacingAction' />"
         "    <toolitem action='ConnectorGraphAction' />"
         "    <toolitem action='ConnectorLengthAction' />"
         "    <toolitem action='ConnectorDirectedAction' />"
         "    <toolitem action='ConnectorOverlapAction' />"
+        "    <toolitem action='ConnectorNewConnPointAction' />"
+        "    <toolitem action='ConnectorRemoveConnPointAction' />"
         "  </toolbar>"
 
         "</ui>"
@@ -7076,6 +7081,13 @@ sp_text_toolbox_new (SPDesktop *desktop)
 //##      Connector      ##
 //#########################
 
+static void sp_connector_mode_toggled( GtkToggleAction* act, GtkObject */*tbl*/ )
+{
+    Inkscape::Preferences *prefs = Inkscape::Preferences::get();
+    prefs->setBool("/tools/connector/mode",
+                gtk_toggle_action_get_active( act ));
+}
+
 static void sp_connector_path_set_avoid(void)
 {
     cc_selection_set_avoid(true);
@@ -7087,6 +7099,106 @@ static void sp_connector_path_set_ignore(void)
     cc_selection_set_avoid(false);
 }
 
+static void sp_connector_orthogonal_toggled( GtkToggleAction* act, GObject *tbl )
+{
+    SPDesktop *desktop = (SPDesktop *) g_object_get_data( tbl, "desktop" );
+    Inkscape::Selection * selection = sp_desktop_selection(desktop);
+    SPDocument *doc = sp_desktop_document(desktop);
+
+    if (!sp_document_get_undo_sensitive(doc))
+    {
+        return;
+    }
+    
+
+    // quit if run by the _changed callbacks
+    if (g_object_get_data( tbl, "freeze" )) {
+        return;
+    }
+
+    // in turn, prevent callbacks from responding
+    g_object_set_data( tbl, "freeze", GINT_TO_POINTER(TRUE) );
+
+    bool is_orthog = gtk_toggle_action_get_active( act );
+    gchar orthog_str[] = "orthogonal";
+    gchar polyline_str[] = "polyline";
+    gchar *value = is_orthog ? orthog_str : polyline_str ;
+
+    bool modmade = false;
+    GSList *l = (GSList *) selection->itemList();
+    while (l) {
+        SPItem *item = (SPItem *) l->data;
+
+        if (cc_item_is_connector(item)) {
+            sp_object_setAttribute(item, "inkscape:connector-type",
+                    value, false);
+            item->avoidRef->handleSettingChange();
+            modmade = true;
+        }
+        l = l->next;
+    }
+
+    if (!modmade)
+    {
+        Inkscape::Preferences *prefs = Inkscape::Preferences::get();
+        prefs->setBool("/tools/connector/orthogonal", is_orthog);
+    }
+
+    sp_document_done(doc, SP_VERB_CONTEXT_CONNECTOR,
+            is_orthog ? _("Set connector type: orthogonal"): _("Set connector type: polyline"));
+
+    g_object_set_data( tbl, "freeze", GINT_TO_POINTER(FALSE) );
+}
+
+static void connector_curvature_changed(GtkAdjustment *adj, GObject* tbl)
+{
+    SPDesktop *desktop = (SPDesktop *) g_object_get_data( tbl, "desktop" );
+    Inkscape::Selection * selection = sp_desktop_selection(desktop);
+    SPDocument *doc = sp_desktop_document(desktop);
+
+    if (!sp_document_get_undo_sensitive(doc))
+    {
+        return;
+    }
+    
+
+    // quit if run by the _changed callbacks
+    if (g_object_get_data( tbl, "freeze" )) {
+        return;
+    }
+
+    // in turn, prevent callbacks from responding
+    g_object_set_data( tbl, "freeze", GINT_TO_POINTER(TRUE) );
+
+    gdouble newValue = gtk_adjustment_get_value(adj);
+    gchar value[G_ASCII_DTOSTR_BUF_SIZE];
+    g_ascii_dtostr(value, G_ASCII_DTOSTR_BUF_SIZE, newValue);
+
+    bool modmade = false;
+    GSList *l = (GSList *) selection->itemList();
+    while (l) {
+        SPItem *item = (SPItem *) l->data;
+
+        if (cc_item_is_connector(item)) {
+            sp_object_setAttribute(item, "inkscape:connector-curvature",
+                    value, false);
+            item->avoidRef->handleSettingChange();
+            modmade = true;
+        }
+        l = l->next;
+    }
+
+    if (!modmade)
+    {
+        Inkscape::Preferences *prefs = Inkscape::Preferences::get();
+        prefs->setDouble(Glib::ustring("/tools/connector/curvature"), newValue);
+    }
+
+    sp_document_done(doc, SP_VERB_CONTEXT_CONNECTOR,
+            _("Change connector curvature"));
+
+    g_object_set_data( tbl, "freeze", GINT_TO_POINTER(FALSE) );
+}
 
 
 static void connector_spacing_changed(GtkAdjustment *adj, GObject* tbl)
@@ -7183,21 +7295,37 @@ static void connector_tb_event_attr_changed(Inkscape::XML::Node *repr,
     if (g_object_get_data(G_OBJECT(tbl), "freeze")) {
         return;
     }
-    if (strcmp(name, "inkscape:connector-spacing") != 0) {
-        return;
+    if (strcmp(name, "inkscape:connector-spacing") == 0)
+    {
+        GtkAdjustment *adj = (GtkAdjustment*)
+                gtk_object_get_data(GTK_OBJECT(tbl), "spacing");
+        gdouble spacing = defaultConnSpacing;
+        sp_repr_get_double(repr, "inkscape:connector-spacing", &spacing);
+
+        gtk_adjustment_set_value(adj, spacing);
+        gtk_adjustment_value_changed(adj);
     }
 
-    GtkAdjustment *adj = (GtkAdjustment*)
-            gtk_object_get_data(GTK_OBJECT(tbl), "spacing");
-    gdouble spacing = defaultConnSpacing;
-    sp_repr_get_double(repr, "inkscape:connector-spacing", &spacing);
+    spinbutton_defocus(GTK_OBJECT(tbl));
+}
 
-    gtk_adjustment_set_value(adj, spacing);
-    gtk_adjustment_value_changed(adj);
+static void sp_connector_new_connection_point(GtkWidget *, GObject *tbl)
+{
+    SPDesktop *desktop = (SPDesktop *) g_object_get_data( tbl, "desktop" );
+    SPConnectorContext* cc = SP_CONNECTOR_CONTEXT(desktop->event_context);
 
-    spinbutton_defocus(GTK_OBJECT(tbl));
+    if (cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE)
+        cc_create_connection_point(cc);
 }
 
+static void sp_connector_remove_connection_point(GtkWidget *, GObject *tbl)
+{
+    SPDesktop *desktop = (SPDesktop *) g_object_get_data( tbl, "desktop" );
+    SPConnectorContext* cc = SP_CONNECTOR_CONTEXT(desktop->event_context);
+
+    if (cc->mode == SP_CONNECTOR_CONTEXT_EDITING_MODE)
+        cc_remove_connection_point(cc);
+}
 
 static Inkscape::XML::NodeEventVector connector_tb_repr_events = {
     NULL, /* child_added */
@@ -7207,12 +7335,42 @@ static Inkscape::XML::NodeEventVector connector_tb_repr_events = {
     NULL  /* order_changed */
 };
 
+static void sp_connector_toolbox_selection_changed(Inkscape::Selection *selection, GObject *tbl)
+{
+    GtkAdjustment *adj = GTK_ADJUSTMENT( g_object_get_data( tbl, "curvature" ) );
+    GtkToggleAction *act = GTK_TOGGLE_ACTION( g_object_get_data( tbl, "orthogonal" ) );
+    SPItem *item = selection->singleItem();
+    if (SP_IS_PATH(item))
+    {
+        gdouble curvature = SP_PATH(item)->connEndPair.getCurvature();
+        bool is_orthog = SP_PATH(item)->connEndPair.isOrthogonal();
+        gtk_toggle_action_set_active(act, is_orthog);
+        gtk_adjustment_set_value(adj, curvature);
+    }
+
+}
 
 static void sp_connector_toolbox_prep( SPDesktop *desktop, GtkActionGroup* mainActions, GObject* holder )
 {
     Inkscape::Preferences *prefs = Inkscape::Preferences::get();
     Inkscape::IconSize secondarySize = prefToSize("/toolbox/secondary", 1);
 
+    // Editing mode toggle button
+    {
+        InkToggleAction* act = ink_toggle_action_new( "ConnectorEditModeAction",
+                                                      _("EditMode"),
+                                                      _("Switch between connection point editing and connector drawing mode"),
+                                                      INKSCAPE_ICON_CONNECTOR_EDIT,
+                                                      Inkscape::ICON_SIZE_DECORATION );
+        gtk_action_group_add_action( mainActions, GTK_ACTION( act ) );
+
+        bool tbuttonstate = prefs->getBool("/tools/connector/mode");
+        gtk_toggle_action_set_active(GTK_TOGGLE_ACTION(act), ( tbuttonstate ? TRUE : FALSE ));
+        g_object_set_data( holder, "mode", act );
+        g_signal_connect_after( G_OBJECT(act), "toggled", G_CALLBACK(sp_connector_mode_toggled), holder );
+    }
+
+
     {
         InkAction* inky = ink_action_new( "ConnectorAvoidAction",
                                           _("Avoid"),
@@ -7233,17 +7391,42 @@ static void sp_connector_toolbox_prep( SPDesktop *desktop, GtkActionGroup* mainA
         gtk_action_group_add_action( mainActions, GTK_ACTION(inky) );
     }
 
+    // Orthogonal connectors toggle button
+    {
+        InkToggleAction* act = ink_toggle_action_new( "ConnectorOrthogonalAction",
+                                                      _("Orthogonal"),
+                                                      _("Make connector orthogonal or polyline"),
+                                                      INKSCAPE_ICON_CONNECTOR_ORTHOGONAL,
+                                                      Inkscape::ICON_SIZE_DECORATION );
+        gtk_action_group_add_action( mainActions, GTK_ACTION( act ) );
+
+        bool tbuttonstate = prefs->getBool("/tools/connector/orthogonal");
+        gtk_toggle_action_set_active(GTK_TOGGLE_ACTION(act), ( tbuttonstate ? TRUE : FALSE ));
+        g_object_set_data( holder, "orthogonal", act );
+        g_signal_connect_after( G_OBJECT(act), "toggled", G_CALLBACK(sp_connector_orthogonal_toggled), holder );
+    }
+
     EgeAdjustmentAction* eact = 0;
+    // Curvature spinbox
+    eact = create_adjustment_action( "ConnectorCurvatureAction",
+                                    _("Connector Curvature"), _("Curvature:"),
+                                    _("The amount of connectors curvature"),
+                                    "/tools/connector/curvature", defaultConnCurvature,
+                                    GTK_WIDGET(desktop->canvas), NULL, holder, TRUE, "inkscape:connector-curvature",
+                                    0, 100, 1.0, 10.0,
+                                    0, 0, 0,
+                                    connector_curvature_changed, 1, 0 );
+    gtk_action_group_add_action( mainActions, GTK_ACTION(eact) );
 
     // Spacing spinbox
     eact = create_adjustment_action( "ConnectorSpacingAction",
-                                     _("Connector Spacing"), _("Spacing:"),
-                                     _("The amount of space left around objects by auto-routing connectors"),
-                                     "/tools/connector/spacing", defaultConnSpacing,
-                                     GTK_WIDGET(desktop->canvas), NULL, holder, TRUE, "inkscape:connector-spacing",
-                                     0, 100, 1.0, 10.0,
-                                     0, 0, 0,
-                                     connector_spacing_changed, 1, 0 );
+                                    _("Connector Spacing"), _("Spacing:"),
+                                    _("The amount of space left around objects by auto-routing connectors"),
+                                    "/tools/connector/spacing", defaultConnSpacing,
+                                    GTK_WIDGET(desktop->canvas), NULL, holder, TRUE, "inkscape:connector-spacing",
+                                    0, 100, 1.0, 10.0,
+                                    0, 0, 0,
+                                    connector_spacing_changed, 1, 0 );
     gtk_action_group_add_action( mainActions, GTK_ACTION(eact) );
 
     // Graph (connector network) layout
@@ -7282,6 +7465,8 @@ static void sp_connector_toolbox_prep( SPDesktop *desktop, GtkActionGroup* mainA
         gtk_toggle_action_set_active(GTK_TOGGLE_ACTION(act), ( tbuttonstate ? TRUE : FALSE ));
 
         g_signal_connect_after( G_OBJECT(act), "toggled", G_CALLBACK(sp_directed_graph_layout_toggled), holder );
+        sigc::connection *connection = new sigc::connection(sp_desktop_selection(desktop)->connectChanged(sigc::bind(sigc::ptr_fun(sp_connector_toolbox_selection_changed), (GObject *)holder))
+        );
     }
 
     // Avoid overlaps toggle button
@@ -7299,6 +7484,31 @@ static void sp_connector_toolbox_prep( SPDesktop *desktop, GtkActionGroup* mainA
         g_signal_connect_after( G_OBJECT(act), "toggled", G_CALLBACK(sp_nooverlaps_graph_layout_toggled), holder );
     }
 
+
+    // New connection point button
+    {
+        InkAction* inky = ink_action_new( "ConnectorNewConnPointAction",
+                                          _("New connection point"),
+                                          _("Add a new connection point to the currently selected item"),
+                                          INKSCAPE_ICON_CONNECTOR_NEW_CONNPOINT,
+                                          secondarySize );
+        g_signal_connect_after( G_OBJECT(inky), "activate", G_CALLBACK(sp_connector_new_connection_point), holder );
+        gtk_action_group_add_action( mainActions, GTK_ACTION(inky) );
+    }
+    
+    // Remove selected connection point button
+    
+    {
+        InkAction* inky = ink_action_new( "ConnectorRemoveConnPointAction",
+                                          _("Remove connection point"),
+                                          _("Remove the currently selected connection point"),
+                                          INKSCAPE_ICON_CONNECTOR_REMOVE_CONNPOINT,
+                                          secondarySize );
+        g_signal_connect_after( G_OBJECT(inky), "activate", G_CALLBACK(sp_connector_remove_connection_point), holder );
+        gtk_action_group_add_action( mainActions, GTK_ACTION(inky) );
+    }
+    
+
     // Code to watch for changes to the connector-spacing attribute in
     // the XML.
     Inkscape::XML::Node *repr = SP_OBJECT_REPR(desktop->namedview);