From cca66b9ec4e494c1d919bff0f71a820d8afab1fa Mon Sep 17 00:00:00 2001
From: Daniel Baumann <daniel.baumann@progress-linux.org>
Date: Sun, 7 Apr 2024 20:24:48 +0200
Subject: Adding upstream version 1.2.2.

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>
---
 src/3rdparty/adaptagrams/libavoid/vpsc.cpp | 1500 ++++++++++++++++++++++++++++
 1 file changed, 1500 insertions(+)
 create mode 100644 src/3rdparty/adaptagrams/libavoid/vpsc.cpp

(limited to 'src/3rdparty/adaptagrams/libavoid/vpsc.cpp')

diff --git a/src/3rdparty/adaptagrams/libavoid/vpsc.cpp b/src/3rdparty/adaptagrams/libavoid/vpsc.cpp
new file mode 100644
index 0000000..0a8efb0
--- /dev/null
+++ b/src/3rdparty/adaptagrams/libavoid/vpsc.cpp
@@ -0,0 +1,1500 @@
+/*
+ * vim: ts=4 sw=4 et tw=0 wm=0
+ *
+ * libavoid - Fast, Incremental, Object-avoiding Line Router
+ *
+ * Copyright (C) 2005-2014  Monash University
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ * See the file LICENSE.LGPL distributed with the library.
+ *
+ * Licensees holding a valid commercial license may use this file in
+ * accordance with the commercial license agreement provided with the 
+ * library.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
+ *
+ * Author(s):  Tim Dwyer
+ *             Michael Wybrow
+ *
+ * --------------
+ *
+ * This file contains a slightly modified version of IncSolver() from libvpsc:
+ * A solver for the problem of Variable Placement with Separation Constraints.
+ * It has the following changes from the Adaptagrams VPSC version:
+ *  -  The required VPSC code has been consolidated into a single file.
+ *  -  Unnecessary code, like the Solver() class, has been removed.
+ *  -  The PairingHeap code has been replaced by a STL priority_queue.
+ *
+ * Modifications:  Michael Wybrow
+ *
+*/
+
+#include "libavoid/vpsc.h"
+
+#ifndef USELIBVPSC
+
+#include <iostream>
+#include <cmath>
+#include <sstream>
+#include <map>
+#include <cfloat>
+#include <cstdio>
+
+#include "libavoid/assertions.h"
+#include "libavoid/debug.h"
+
+
+using namespace std;
+
+namespace Avoid {
+
+static const double ZERO_UPPERBOUND=-1e-10;
+static const double LAGRANGIAN_TOLERANCE=-1e-4;
+
+
+IncSolver::IncSolver(Variables const &vs, Constraints const &cs)
+    : m(cs.size()),
+      cs(cs),
+      n(vs.size()), 
+      vs(vs),
+      needsScaling(false)
+{
+    for(unsigned i=0;i<n;++i) {
+        vs[i]->in.clear();
+        vs[i]->out.clear();
+
+        // Set needsScaling if any variables have a scale other than 1.
+        needsScaling |= (vs[i]->scale != 1);
+    }
+    for(unsigned i=0;i<m;++i) {
+        Constraint *c=cs[i];
+        c->left->out.push_back(c);
+        c->right->in.push_back(c);
+        c->needsScaling = needsScaling;
+    }
+    bs=new Blocks(vs);
+#ifdef LIBVPSC_LOGGING
+    printBlocks();
+    //COLA_ASSERT(!constraintGraphIsCyclic(n,vs));
+#endif
+
+    inactive=cs;
+    for(Constraints::iterator i=inactive.begin();i!=inactive.end();++i) {
+        (*i)->active=false;
+    }
+}
+IncSolver::~IncSolver() {
+    delete bs;
+}
+
+void IncSolver::addConstraint(Constraint *c)
+{
+    ++m;
+    c->active = false;
+    inactive.push_back(c);
+    c->left->out.push_back(c);
+    c->right->in.push_back(c);
+    c->needsScaling = needsScaling;
+}
+
+// useful in debugging
+void IncSolver::printBlocks() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    for(set<Block*>::iterator i=bs->begin();i!=bs->end();++i) {
+        Block *b=*i;
+        f<<"  "<<*b<<endl;
+    }
+    for(unsigned i=0;i<m;i++) {
+        f<<"  "<<*cs[i]<<endl;
+    }
+#endif
+}
+
+/*
+ * Stores the relative positions of the variables in their finalPosition
+ * field.
+ */
+void IncSolver::copyResult() {
+    for(Variables::const_iterator i=vs.begin();i!=vs.end();++i) {
+        Variable* v=*i;
+        v->finalPosition=v->position();
+        COLA_ASSERT(v->finalPosition==v->finalPosition);
+    }
+}
+
+struct node {
+    set<node*> in;
+    set<node*> out;
+};
+// useful in debugging - cycles would be BAD
+bool IncSolver::constraintGraphIsCyclic(const unsigned n, Variable* const vs[]) {
+    map<Variable*, node*> varmap;
+    vector<node*> graph;
+    for(unsigned i=0;i<n;i++) {
+        node *u=new node;
+        graph.push_back(u);
+        varmap[vs[i]]=u;
+    }
+    for(unsigned i=0;i<n;i++) {
+        for(vector<Constraint*>::iterator c=vs[i]->in.begin();c!=vs[i]->in.end();++c) {
+            Variable *l=(*c)->left;
+            varmap[vs[i]]->in.insert(varmap[l]);
+        }
+
+        for(vector<Constraint*>::iterator c=vs[i]->out.begin();c!=vs[i]->out.end();++c) {
+            Variable *r=(*c)->right;
+            varmap[vs[i]]->out.insert(varmap[r]);
+        }
+    }
+    while(graph.size()>0) {
+        node *u=nullptr;
+        vector<node*>::iterator i=graph.begin();
+        for(;i!=graph.end();++i) {
+            u=*i;
+            if(u->in.size()==0) {
+                break;
+            }
+        }
+        if(i==graph.end() && graph.size()>0) {
+            //cycle found!
+            return true;
+        } else {
+            graph.erase(i);
+            for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
+                node *v=*j;
+                v->in.erase(u);
+            }
+            delete u;
+        }
+    }
+    for(unsigned i=0; i<graph.size(); ++i) {
+        delete graph[i];
+    }
+    return false;
+}
+
+// useful in debugging - cycles would be BAD
+bool IncSolver::blockGraphIsCyclic() {
+    map<Block*, node*> bmap;
+    vector<node*> graph;
+    size_t length = bs->size();
+    for (size_t i = 0; i < length; ++i)
+    {
+        Block *b = bs->at(i);
+        node *u=new node;
+        graph.push_back(u);
+        bmap[b]=u;
+    }
+    for (size_t i = 0; i < length; ++i)
+    {
+        Block *b = bs->at(i);
+        b->setUpInConstraints();
+        Constraint *c=b->findMinInConstraint();
+        while(c!=nullptr) {
+            Block *l=c->left->block;
+            bmap[b]->in.insert(bmap[l]);
+            b->deleteMinInConstraint();
+            c=b->findMinInConstraint();
+        }
+
+        b->setUpOutConstraints();
+        c=b->findMinOutConstraint();
+        while(c!=nullptr) {
+            Block *r=c->right->block;
+            bmap[b]->out.insert(bmap[r]);
+            b->deleteMinOutConstraint();
+            c=b->findMinOutConstraint();
+        }
+    }
+    while(graph.size()>0) {
+        node *u=nullptr;
+        vector<node*>::iterator i=graph.begin();
+        for(;i!=graph.end();++i) {
+            u=*i;
+            if(u->in.size()==0) {
+                break;
+            }
+        }
+        if(i==graph.end() && graph.size()>0) {
+            //cycle found!
+            return true;
+        } else {
+            graph.erase(i);
+            for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
+                node *v=*j;
+                v->in.erase(u);
+            }
+            delete u;
+        }
+    }
+    for(unsigned i=0; i<graph.size(); i++) {
+        delete graph[i];
+    }
+    return false;
+}
+
+bool IncSolver::solve() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"solve_inc()..."<<endl;
+#endif
+    satisfy();
+    double lastcost = DBL_MAX, cost = bs->cost();
+    while(fabs(lastcost-cost)>0.0001) {
+        satisfy();
+        lastcost=cost;
+        cost = bs->cost();
+#ifdef LIBVPSC_LOGGING
+        f<<"  bs->size="<<bs->size()<<", cost="<<cost<<endl;
+#endif
+    }
+    copyResult();
+    return bs->size()!=n; 
+}
+/*
+ * incremental version of satisfy that allows refinement after blocks are
+ * moved.
+ *
+ *  - move blocks to new positions
+ *  - repeatedly merge across most violated constraint until no more
+ *    violated constraints exist
+ *
+ * Note: there is a special case to handle when the most violated constraint
+ * is between two variables in the same block.  Then, we must split the block
+ * over an active constraint between the two variables.  We choose the 
+ * constraint with the most negative lagrangian multiplier. 
+ */
+bool IncSolver::satisfy() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"satisfy_inc()..."<<endl;
+#endif
+    splitBlocks();
+    //long splitCtr = 0;
+    Constraint* v = nullptr;
+    //CBuffer buffer(inactive);
+    while ( (v = mostViolated(inactive)) && 
+            (v->equality || ((v->slack() < ZERO_UPPERBOUND) && !v->active)) )
+    {
+        COLA_ASSERT(!v->active);
+        Block *lb = v->left->block, *rb = v->right->block;
+        if(lb != rb) {
+            lb->merge(rb,v);
+        } else {
+            if(lb->isActiveDirectedPathBetween(v->right,v->left)) {
+                // cycle found, relax the violated, cyclic constraint
+                v->unsatisfiable=true;
+                continue;
+                //UnsatisfiableException e;
+                //lb->getActiveDirectedPathBetween(e.path,v->right,v->left);
+                //e.path.push_back(v);
+                //throw e;
+            }
+            //if(splitCtr++>10000) {
+                //throw "Cycle Error!";
+            //}
+            // constraint is within block, need to split first
+            try {
+                Constraint* splitConstraint
+                    =lb->splitBetween(v->left,v->right,lb,rb);
+                if(splitConstraint!=nullptr) {
+                    COLA_ASSERT(!splitConstraint->active);
+                    inactive.push_back(splitConstraint);
+                } else {
+                    v->unsatisfiable=true;
+                    continue;
+                }
+            } catch(UnsatisfiableException e) {
+                e.path.push_back(v);
+                /*
+                std::cerr << "Unsatisfiable:" << std::endl;
+                for(std::vector<Constraint*>::iterator r=e.path.begin();
+                        r!=e.path.end();++r)
+                {
+                    std::cerr << **r <<std::endl;
+                }
+                */
+                v->unsatisfiable=true;
+                continue;
+            }
+            if(v->slack()>=0) {
+                COLA_ASSERT(!v->active);
+                // v was satisfied by the above split!
+                inactive.push_back(v);
+                bs->insert(lb);
+                bs->insert(rb);
+            } else {
+                bs->insert(lb->merge(rb,v));
+                delete ((lb->deleted) ? lb : rb);
+            }
+        }
+#ifdef LIBVPSC_LOGGING
+        f<<"...remaining blocks="<<bs->size()<<", cost="<<bs->cost()<<endl;
+#endif
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  finished merges."<<endl;
+#endif
+    bs->cleanup();
+    bool activeConstraints=false;
+    for(unsigned i=0;i<m;i++) {
+        v=cs[i];
+        if(v->active) activeConstraints=true;
+        if(v->slack() < ZERO_UPPERBOUND) {
+            ostringstream s;
+            s<<"Unsatisfied constraint: "<<*v;
+#ifdef LIBVPSC_LOGGING
+            ofstream f(LOGFILE,ios::app);
+            f<<s.str()<<endl;
+#endif
+            throw s.str().c_str();
+        }
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  finished cleanup."<<endl;
+    printBlocks();
+#endif
+    copyResult();
+    return activeConstraints;
+}
+void IncSolver::moveBlocks() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"moveBlocks()..."<<endl;
+#endif
+    size_t length = bs->size();
+    for (size_t i = 0; i < length; ++i)
+    {
+        Block *b = bs->at(i);
+        b->updateWeightedPosition();
+        //b->posn = b->wposn / b->weight;
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  moved blocks."<<endl;
+#endif
+}
+void IncSolver::splitBlocks() {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+#endif
+    moveBlocks();
+    splitCnt=0;
+    // Split each block if necessary on min LM
+    size_t length = bs->size();
+    for (size_t i = 0; i < length; ++i)
+    {
+        Block *b = bs->at(i);
+        Constraint* v=b->findMinLM();
+        if(v!=nullptr && v->lm < LAGRANGIAN_TOLERANCE) {
+            COLA_ASSERT(!v->equality);
+#ifdef LIBVPSC_LOGGING
+            f<<"    found split point: "<<*v<<" lm="<<v->lm<<endl;
+#endif
+            splitCnt++;
+            Block *b = v->left->block, *l=nullptr, *r=nullptr;
+            COLA_ASSERT(v->left->block == v->right->block);
+            //double pos = b->posn;
+            b->split(l,r,v);
+            //l->posn=r->posn=pos;
+            //l->wposn = l->posn * l->weight;
+            //r->wposn = r->posn * r->weight;
+            l->updateWeightedPosition();
+            r->updateWeightedPosition();
+            bs->insert(l);
+            bs->insert(r);
+            b->deleted=true;
+            COLA_ASSERT(!v->active);
+            inactive.push_back(v);
+#ifdef LIBVPSC_LOGGING
+            f<<"  new blocks: "<<*l<<" and "<<*r<<endl;
+#endif
+        }
+    }
+    //if(splitCnt>0) { std::cout<<"  splits: "<<splitCnt<<endl; }
+#ifdef LIBVPSC_LOGGING
+    f<<"  finished splits."<<endl;
+#endif
+    bs->cleanup();
+}
+
+/*
+ * Scan constraint list for the most violated constraint, or the first equality
+ * constraint
+ */
+Constraint* IncSolver::mostViolated(Constraints &l)
+{
+    double slackForMostViolated = DBL_MAX;
+    Constraint* mostViolated = nullptr;
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f << "Looking for most violated..." << endl;
+#endif
+    size_t lSize = l.size();
+    size_t deleteIndex = lSize;
+    Constraint *constraint = nullptr;
+    double slack = 0;
+    for (size_t index = 0; index < lSize; ++index)
+    {
+        constraint = l[index];
+        slack = constraint->slack();
+        if (constraint->equality || slack < slackForMostViolated)
+        {
+            slackForMostViolated = slack;    
+            mostViolated = constraint;
+            deleteIndex = index;
+            if (constraint->equality)
+            {
+                break;
+            }
+        }
+    }
+    // Because the constraint list is not order dependent we just
+    // move the last element over the deletePoint and resize
+    // downwards.  There is always at least 1 element in the
+    // vector because of search.
+    if ( (deleteIndex < lSize) && 
+         (((slackForMostViolated < ZERO_UPPERBOUND) && !mostViolated->active) || 
+          mostViolated->equality) )
+    {
+        l[deleteIndex] = l[lSize-1];
+        l.resize(lSize-1);
+    }
+#ifdef LIBVPSC_LOGGING
+    if (mostViolated)
+    {
+        f << "  most violated is: " << *mostViolated << endl;
+    }
+    else
+    {
+        f << "  non found." << endl;
+    }
+#endif
+    return mostViolated;
+}
+
+
+using std::set;
+using std::vector;
+using std::iterator;
+using std::list;
+using std::copy;
+#define __NOTNAN(p) (p)==(p)
+
+
+Blocks::Blocks(vector<Variable*> const &vs) : vs(vs),nvs(vs.size()) {
+    blockTimeCtr=0;
+    m_blocks.resize(nvs);
+    for(size_t i=0;i<nvs;i++) {
+        m_blocks[i] = new Block(this, vs[i]);
+    }
+}
+Blocks::~Blocks(void)
+{
+    blockTimeCtr=0;
+    size_t length = m_blocks.size();
+    for (size_t i = 0; i < length; ++i)
+    {
+        delete m_blocks[i];
+    }
+    m_blocks.clear();
+}
+
+/*
+ * returns a list of variables with total ordering determined by the constraint 
+ * DAG
+ */
+list<Variable*> *Blocks::totalOrder() {
+    list<Variable*> *order = new list<Variable*>;
+    for(size_t i=0;i<nvs;i++) {
+        vs[i]->visited=false;
+    }
+    for(size_t i=0;i<nvs;i++) {
+        if(vs[i]->in.size()==0) {
+            dfsVisit(vs[i],order);
+        }
+    }
+    return order;
+}
+// Recursive depth first search giving total order by pushing nodes in the DAG
+// onto the front of the list when we finish searching them
+void Blocks::dfsVisit(Variable *v, list<Variable*> *order) {
+    v->visited=true;
+    vector<Constraint*>::iterator it=v->out.begin();
+    for(;it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(!c->right->visited) {
+            dfsVisit(c->right, order);
+        }
+    }    
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  order="<<*v<<endl;
+#endif
+    order->push_front(v);
+}
+/*
+ * Processes incoming constraints, most violated to least, merging with the
+ * neighbouring (left) block until no more violated constraints are found
+ */
+void Blocks::mergeLeft(Block *r) {    
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"mergeLeft called on "<<*r<<endl;
+#endif
+    r->timeStamp=++blockTimeCtr;
+    r->setUpInConstraints();
+    Constraint *c=r->findMinInConstraint();
+    while (c != nullptr && c->slack()<0) {
+#ifdef LIBVPSC_LOGGING
+        f<<"mergeLeft on constraint: "<<*c<<endl;
+#endif
+        r->deleteMinInConstraint();
+        Block *l = c->left->block;        
+        if (l->in==nullptr) l->setUpInConstraints();
+        double dist = c->right->offset - c->left->offset - c->gap;
+        if (r->vars->size() < l->vars->size()) {
+            dist=-dist;
+            std::swap(l, r);
+        }
+        blockTimeCtr++;
+        r->merge(l, c, dist);
+        r->mergeIn(l);
+        r->timeStamp=blockTimeCtr;
+        removeBlock(l);
+        c=r->findMinInConstraint();
+    }        
+#ifdef LIBVPSC_LOGGING
+    f<<"merged "<<*r<<endl;
+#endif
+}    
+/*
+ * Symmetrical to mergeLeft
+ */
+void Blocks::mergeRight(Block *l) {    
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"mergeRight called on "<<*l<<endl;
+#endif    
+    l->setUpOutConstraints();
+    Constraint *c = l->findMinOutConstraint();
+    while (c != nullptr && c->slack()<0) {        
+#ifdef LIBVPSC_LOGGING
+        f<<"mergeRight on constraint: "<<*c<<endl;
+#endif
+        l->deleteMinOutConstraint();
+        Block *r = c->right->block;
+        r->setUpOutConstraints();
+        double dist = c->left->offset + c->gap - c->right->offset;
+        if (l->vars->size() > r->vars->size()) {
+            dist=-dist;
+            std::swap(l, r);
+        }
+        l->merge(r, c, dist);
+        l->mergeOut(r);
+        removeBlock(r);
+        c=l->findMinOutConstraint();
+    }    
+#ifdef LIBVPSC_LOGGING
+    f<<"merged "<<*l<<endl;
+#endif
+}
+void Blocks::removeBlock(Block *doomed) {
+    doomed->deleted=true;
+    //erase(doomed);
+}
+
+// Clears up deleted blocks from the blocks list.
+void Blocks::cleanup(void)
+{
+    // We handle removal of items in-place by doing a single linear pass over 
+    // the vector. We use two indexes, j to refer to elements we've checked
+    // from the original list and i to refer to the current position we are
+    // writing in the updated list.
+    size_t i = 0;
+    
+    // For all items in the current blocks list...
+    size_t length = m_blocks.size();
+    for (size_t j = 0; j < length; )
+    {
+        if (m_blocks[j]->deleted)
+        {
+            // The element is deleted, so free it and increment j.
+            delete m_blocks[j];
+            ++j;
+        }
+        else
+        {
+            // The current element is still valid.
+            if (j > i)
+            {
+                // If we've not looking at same element, then copy from j to i.
+                m_blocks[i] = m_blocks[j];
+            }
+            // Increment both indexes.
+            ++i;
+            ++j;
+        }
+    }
+    m_blocks.resize(i);
+}
+/*
+ * Splits block b across constraint c into two new blocks, l and r (c's left
+ * and right sides respectively)
+ */
+void Blocks::split(Block *b, Block *&l, Block *&r, Constraint *c) {
+    b->split(l,r,c);
+    m_blocks.push_back(l);
+    m_blocks.push_back(r);
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"Split left: "<<*l<<endl;
+    f<<"Split right: "<<*r<<endl;
+#endif
+    r->posn = b->posn;
+    //COLA_ASSERT(r->weight!=0);
+    //r->wposn = r->posn * r->weight;
+    mergeLeft(l);
+    // r may have been merged!
+    r = c->right->block;
+    r->updateWeightedPosition();
+    //r->posn = r->wposn / r->weight;
+    mergeRight(r);
+    removeBlock(b);
+
+    COLA_ASSERT(__NOTNAN(l->posn));
+    COLA_ASSERT(__NOTNAN(r->posn));
+}
+/*
+ * returns the cost total squared distance of variables from their desired
+ * positions
+ */
+double Blocks::cost() {
+    double c = 0;
+    size_t length = m_blocks.size();
+    for (size_t i = 0; i < length; ++i)
+    {
+        c += m_blocks[i]->cost();
+    }
+    return c;
+}
+
+void PositionStats::addVariable(Variable* v) {
+    double ai=scale/v->scale;
+    double bi=v->offset/v->scale;
+    double wi=v->weight;
+    AB+=wi*ai*bi;
+    AD+=wi*ai*v->desiredPosition;
+    A2+=wi*ai*ai;
+    /*
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f << "adding v[" << v->id << "], blockscale=" << scale << ", despos=" 
+      << v->desiredPosition << ", ai=" << ai << ", bi=" << bi
+      << ", AB=" << AB << ", AD=" << AD << ", A2=" << A2;
+#endif
+*/
+}
+void Block::addVariable(Variable* v) {
+    v->block=this;
+    vars->push_back(v);
+    if(ps.A2==0) ps.scale=v->scale;
+    //weight+= v->weight;
+    //wposn += v->weight * (v->desiredPosition - v->offset);
+    //posn=wposn/weight;
+    ps.addVariable(v);
+    posn=(ps.AD - ps.AB) / ps.A2;
+    COLA_ASSERT(__NOTNAN(posn));
+    /*
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f << ", posn=" << posn << endl;
+#endif
+*/
+}
+Block::Block(Blocks *blocks, Variable* const v)
+    : vars(new vector<Variable*>)
+    , posn(0)
+    //, weight(0)
+    //, wposn(0)
+    , deleted(false)
+    , timeStamp(0)
+    , in(nullptr)
+    , out(nullptr)
+    , blocks(blocks)
+{
+    if(v!=nullptr) {
+        v->offset=0;
+        addVariable(v);
+    }
+}
+
+void Block::updateWeightedPosition() {
+    //wposn=0;
+    ps.AB=ps.AD=ps.A2=0;
+    for (Vit v=vars->begin();v!=vars->end();++v) {
+        //wposn += ((*v)->desiredPosition - (*v)->offset) * (*v)->weight;
+        ps.addVariable(*v);
+    }
+    posn=(ps.AD - ps.AB) / ps.A2;
+    COLA_ASSERT(__NOTNAN(posn));
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f << ", posn=" << posn << endl;
+#endif
+}
+Block::~Block(void)
+{
+    delete vars;
+    delete in;
+    delete out;
+}
+void Block::setUpInConstraints() {
+    setUpConstraintHeap(in,true);
+}
+void Block::setUpOutConstraints() {
+    setUpConstraintHeap(out,false);
+}
+void Block::setUpConstraintHeap(Heap* &h,bool in) {
+    delete h;
+    h = new Heap();
+    for (Vit i=vars->begin();i!=vars->end();++i) {
+        Variable *v=*i;
+        vector<Constraint*> *cs=in?&(v->in):&(v->out);
+        for (Cit j=cs->begin();j!=cs->end();++j) {
+            Constraint *c=*j;
+            c->timeStamp=blocks->blockTimeCtr;
+            if ( ((c->left->block != this) && in) || 
+                 ((c->right->block != this) && !in) )
+            {
+                h->push(c);
+            }
+        }
+    }
+}    
+Block* Block::merge(Block* b, Constraint* c) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  merging on: "<<*c<<",c->left->offset="<<c->left->offset<<",c->right->offset="<<c->right->offset<<endl;
+#endif
+    double dist = c->right->offset - c->left->offset - c->gap;
+    Block *l=c->left->block;
+    Block *r=c->right->block;
+    if (l->vars->size() < r->vars->size()) {
+        r->merge(l,c,dist);
+    } else {
+               l->merge(r,c,-dist);
+    }
+    Block* mergeBlock=b->deleted?this:b;
+#ifdef LIBVPSC_LOGGING
+    f<<"  merged block="<<*mergeBlock<<endl;
+#endif
+    return mergeBlock;
+}
+/*
+ * Merges b into this block across c.  Can be either a
+ * right merge or a left merge
+ * @param b block to merge into this
+ * @param c constraint being merged
+ * @param distance separation required to satisfy c
+ */
+void Block::merge(Block *b, Constraint *c, double dist) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"    merging: "<<*b<<"dist="<<dist<<endl;
+#endif
+    c->active=true;
+    //wposn+=b->wposn-dist*b->weight;
+    //weight+=b->weight;
+    for(Vit i=b->vars->begin();i!=b->vars->end();++i) {
+        Variable *v=*i;
+        //v->block=this;
+        //vars->push_back(v);
+        v->offset+=dist;
+        addVariable(v);
+    }
+#ifdef LIBVPSC_LOGGING
+    for(Vit i=vars->begin();i!=vars->end();++i) {
+        Variable *v=*i;
+        f<<"    v["<<v->id<<"]: d="<<v->desiredPosition
+            <<" a="<<v->scale<<" o="<<v->offset
+            <<endl;
+    }
+    f<<"  AD="<<ps.AD<<" AB="<<ps.AB<<" A2="<<ps.A2<<endl;
+#endif
+    //posn=wposn/weight;
+    //COLA_ASSERT(wposn==ps.AD - ps.AB);
+    posn=(ps.AD - ps.AB) / ps.A2;
+    COLA_ASSERT(__NOTNAN(posn));
+    b->deleted=true;
+}
+
+void Block::mergeIn(Block *b) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  merging constraint heaps... "<<endl;
+#endif
+    // We check the top of the heaps to remove possible internal constraints
+    findMinInConstraint();
+    b->findMinInConstraint();
+    while (!b->in->empty())
+    {
+        in->push(b->in->top());
+        b->in->pop();
+    }
+#ifdef LIBVPSC_LOGGING
+    f<<"  merged heap: "<<*in<<endl;
+#endif
+}
+void Block::mergeOut(Block *b) {    
+    findMinOutConstraint();
+    b->findMinOutConstraint();
+    while (!b->out->empty())
+    {
+        out->push(b->out->top());
+        b->out->pop();
+    }
+}
+Constraint *Block::findMinInConstraint() {
+    Constraint *v = nullptr;
+    vector<Constraint*> outOfDate;
+    while (!in->empty()) {
+        v = in->top();
+        Block *lb=v->left->block;
+        Block *rb=v->right->block;
+        // rb may not be this if called between merge and mergeIn
+#ifdef LIBVPSC_LOGGING
+        ofstream f(LOGFILE,ios::app);
+        f<<"  checking constraint ... "<<*v;
+        f<<"    timestamps: left="<<lb->timeStamp<<" right="<<rb->timeStamp<<" constraint="<<v->timeStamp<<endl;
+#endif
+        if(lb == rb) {
+            // constraint has been merged into the same block
+#ifdef LIBVPSC_LOGGING
+            if(v->slack()<0) {
+                f<<"  violated internal constraint found! "<<*v<<endl;
+                f<<"     lb="<<*lb<<endl;
+                f<<"     rb="<<*rb<<endl;
+            }
+#endif
+            in->pop();
+#ifdef LIBVPSC_LOGGING
+            f<<" ... skipping internal constraint"<<endl;
+#endif
+        } else if(v->timeStamp < lb->timeStamp) {
+            // block at other end of constraint has been moved since this
+            in->pop();
+            outOfDate.push_back(v);
+#ifdef LIBVPSC_LOGGING
+            f<<"    reinserting out of date (reinsert later)"<<endl;
+#endif
+        } else {
+            break;
+        }
+    }
+    for(Cit i=outOfDate.begin();i!=outOfDate.end();++i) {
+        v=*i;
+        v->timeStamp=blocks->blockTimeCtr;
+        in->push(v);
+    }
+    if(in->empty()) {
+        v=nullptr;
+    } else {
+        v=in->top();
+    }
+    return v;
+}
+Constraint *Block::findMinOutConstraint() {
+    if(out->empty()) return nullptr;
+    Constraint *v = out->top();
+    while (v->left->block == v->right->block) {
+        out->pop();
+        if(out->empty()) return nullptr;
+        v = out->top();
+    }
+    return v;
+}
+void Block::deleteMinInConstraint() {
+    in->pop();
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"deleteMinInConstraint... "<<endl;
+    f<<"  result: "<<*in<<endl;
+#endif
+}
+void Block::deleteMinOutConstraint() {
+    out->pop();
+}
+inline bool Block::canFollowLeft(Constraint const* c, Variable const* last) const {
+    return c->left->block==this && c->active && last!=c->left;
+}
+inline bool Block::canFollowRight(Constraint const* c, Variable const* last) const {
+    return c->right->block==this && c->active && last!=c->right;
+}
+
+// computes the derivative of v and the lagrange multipliers
+// of v's out constraints (as the recursive sum of those below.
+// Does not backtrack over u.
+// also records the constraint with minimum lagrange multiplier
+// in min_lm
+double Block::compute_dfdv(Variable* const v, Variable* const u,
+               Constraint *&min_lm) {
+    double dfdv=v->dfdv();
+    for(Cit it=v->out.begin();it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+            c->lm=compute_dfdv(c->right,v,min_lm);
+            dfdv+=c->lm*c->left->scale;
+            if(!c->equality&&(min_lm==nullptr||c->lm<min_lm->lm)) min_lm=c;
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+            c->lm=-compute_dfdv(c->left,v,min_lm);
+            dfdv-=c->lm*c->right->scale;
+            if(!c->equality&&(min_lm==nullptr||c->lm<min_lm->lm)) min_lm=c;
+        }
+    }
+    return dfdv/v->scale;
+}
+double Block::compute_dfdv(Variable* const v, Variable* const u) {
+    double dfdv = v->dfdv();
+    for(Cit it = v->out.begin(); it != v->out.end(); ++it) {
+        Constraint *c = *it;
+        if(canFollowRight(c,u)) {
+            c->lm =   compute_dfdv(c->right,v);
+            dfdv += c->lm * c->left->scale;
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c = *it;
+        if(canFollowLeft(c,u)) {
+            c->lm = - compute_dfdv(c->left,v);
+            dfdv -= c->lm * c->right->scale;
+        }
+    }
+    return dfdv/v->scale;
+}
+
+// The top level v and r are variables between which we want to find the
+// constraint with the smallest lm.  
+// Similarly, m is initially nullptr and is only assigned a value if the next
+// variable to be visited is r or if a possible min constraint is returned from
+// a nested call (rather than nullptr).
+// Then, the search for the m with minimum lm occurs as we return from
+// the recursion (checking only constraints traversed left-to-right 
+// in order to avoid creating any new violations).
+// We also do not consider equality constraints as potential split points
+bool Block::split_path(
+    Variable* r, 
+    Variable* const v, 
+    Variable* const u, 
+    Constraint* &m,
+    bool desperation=false
+    ) 
+{
+    for(Cit it(v->in.begin());it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  left split path: "<<*c<<endl;
+#endif
+            if(c->left==r) {
+                if(desperation&&!c->equality) m=c;
+                return true;
+            } else {
+                if(split_path(r,c->left,v,m)) {
+                    if(desperation && !c->equality && (!m||c->lm<m->lm)) {
+                               m=c;
+                    }
+                    return true;
+                }
+            }
+        }
+    }
+    for(Cit it(v->out.begin());it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  right split path: "<<*c<<endl;
+#endif
+            if(c->right==r) {
+                if(!c->equality) m=c;
+                return true;
+            } else {
+                if(split_path(r,c->right,v,m)) {
+                    if(!c->equality && (!m||c->lm<m->lm))
+                               m=c;
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+/*
+Block::Pair Block::compute_dfdv_between(
+        Variable* r, Variable* const v, Variable* const u, 
+        const Direction dir = NONE, bool changedDirection = false) {
+    double dfdv=v->weight*(v->position() - v->desiredPosition);
+    Constraint *m=nullptr;
+    for(Cit it(v->in.begin());it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+            if(dir==RIGHT) { 
+                changedDirection = true; 
+            }
+            if(c->left==r) {
+                       r=nullptr;
+                    if(!c->equality) m=c; 
+            }
+            Pair p=compute_dfdv_between(r,c->left,v,
+                    LEFT,changedDirection);
+            dfdv -= c->lm = -p.first;
+            if(r && p.second) 
+                m = p.second;
+        }
+    }
+    for(Cit it(v->out.begin());it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+            if(dir==LEFT) { 
+                changedDirection = true; 
+            }
+            if(c->right==r) {
+                       r=nullptr; 
+                    if(!c->equality) m=c; 
+            }
+            Pair p=compute_dfdv_between(r,c->right,v,
+                    RIGHT,changedDirection);
+            dfdv += c->lm = p.first;
+            if(r && p.second) 
+                m = changedDirection && !c->equality && c->lm < p.second->lm 
+                    ? c 
+                    : p.second;
+        }
+    }
+    return Pair(dfdv,m);
+}
+*/
+
+// resets LMs for all active constraints to 0 by
+// traversing active constraint tree starting from v,
+// not back tracking over u
+void Block::reset_active_lm(Variable* const v, Variable* const u) {
+    for(Cit it=v->out.begin();it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+            c->lm=0;
+            reset_active_lm(c->right,v);
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+            c->lm=0;
+            reset_active_lm(c->left,v);
+        }
+    }
+}
+void Block::list_active(Variable* const v, Variable* const u) {
+    for(Cit it=v->out.begin();it!=v->out.end();++it) {
+        Constraint *c=*it;
+        if(canFollowRight(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  "<<*c<<endl;
+#endif
+            list_active(c->right,v);
+        }
+    }
+    for(Cit it=v->in.begin();it!=v->in.end();++it) {
+        Constraint *c=*it;
+        if(canFollowLeft(c,u)) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  "<<*c<<endl;
+#endif
+            list_active(c->left,v);
+        }
+    }
+}
+/*
+ * finds the constraint with the minimum lagrange multiplier, that is, the constraint
+ * that most wants to split
+ */
+Constraint *Block::findMinLM() {
+    Constraint *min_lm=nullptr;
+    reset_active_lm(vars->front(),nullptr);
+    compute_dfdv(vars->front(),nullptr,min_lm);
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  langrangians: "<<endl;
+    list_active(vars->front(),nullptr);
+#endif
+    return min_lm;
+}
+Constraint *Block::findMinLMBetween(Variable* const lv, Variable* const rv) {
+    reset_active_lm(vars->front(),nullptr);
+    compute_dfdv(vars->front(),nullptr);
+    Constraint *min_lm=nullptr;
+    split_path(rv,lv,nullptr,min_lm);
+#if 0
+    if(min_lm==nullptr) {
+        split_path(rv,lv,nullptr,min_lm,true);
+    }
+#else
+    if(min_lm==nullptr) {
+        //err_printf("Couldn't find split point!\n");
+        UnsatisfiableException e;
+        getActivePathBetween(e.path,lv,rv,nullptr);
+        throw e;
+    }
+    COLA_ASSERT(min_lm!=nullptr);
+#endif
+    return min_lm;
+}
+
+// populates block b by traversing the active constraint tree adding variables as they're 
+// visited.  Starts from variable v and does not backtrack over variable u.
+void Block::populateSplitBlock(Block *b, Variable* v, Variable const* u) {
+    b->addVariable(v);
+    for (Cit c=v->in.begin();c!=v->in.end();++c) {
+        if (canFollowLeft(*c,u))
+            populateSplitBlock(b, (*c)->left, v);
+    }
+    for (Cit c=v->out.begin();c!=v->out.end();++c) {
+        if (canFollowRight(*c,u)) 
+            populateSplitBlock(b, (*c)->right, v);
+    }
+}
+/*
+ * Returns the active path between variables u and v... not back tracking over w
+ */
+bool Block::getActivePathBetween(Constraints& path, Variable const* u,
+               Variable const* v, Variable const *w) const {
+    if(u==v) return true;
+    for (Cit_const c=u->in.begin();c!=u->in.end();++c) {
+        if (canFollowLeft(*c,w)) {
+            if(getActivePathBetween(path, (*c)->left, v, u)) {
+                path.push_back(*c);
+                return true;
+            }
+        }
+    }
+    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
+        if (canFollowRight(*c,w)) {
+            if(getActivePathBetween(path, (*c)->right, v, u)) {
+                path.push_back(*c);
+                return true;
+            }
+        }
+    }
+    return false;
+}
+// Search active constraint tree from u to see if there is a directed path to v.
+// Returns true if path is found with all constraints in path having their visited flag
+// set true.
+bool Block::isActiveDirectedPathBetween(Variable const* u, Variable const* v) const {
+    if(u==v) return true;
+    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
+        if(canFollowRight(*c,nullptr)) {
+            if(isActiveDirectedPathBetween((*c)->right,v)) {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+bool Block::getActiveDirectedPathBetween(
+        Constraints& path, Variable const* u, Variable const* v) const {
+    if(u==v) return true;
+    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
+        if(canFollowRight(*c,nullptr)) {
+            if(getActiveDirectedPathBetween(path,(*c)->right,v)) {
+                path.push_back(*c);
+                return true;
+            }
+        }
+    }
+    return false;
+}
+/*
+ * Block needs to be split because of a violated constraint between vl and vr.
+ * We need to search the active constraint tree between l and r and find the constraint
+ * with min lagrangrian multiplier and split at that point.
+ * Returns the split constraint
+ */
+Constraint* Block::splitBetween(Variable* const vl, Variable* const vr,
+               Block* &lb, Block* &rb) {
+#ifdef LIBVPSC_LOGGING
+    ofstream f(LOGFILE,ios::app);
+    f<<"  need to split between: "<<*vl<<" and "<<*vr<<endl;
+#endif
+    Constraint *c=findMinLMBetween(vl, vr);
+#ifdef LIBVPSC_LOGGING
+    f<<"  going to split on: "<<*c<<endl;
+#endif
+    if(c!=nullptr) {
+        split(lb,rb,c);
+        deleted = true;
+    }
+    return c;
+}
+
+/*
+ * Creates two new blocks, l and r, and splits this block across constraint c,
+ * placing the left subtree of constraints (and associated variables) into l
+ * and the right into r.
+ */
+void Block::split(Block* &l, Block* &r, Constraint* c) {
+    c->active=false;
+    l=new Block(blocks);
+    populateSplitBlock(l,c->left,c->right);
+    //COLA_ASSERT(l->weight>0);
+    r=new Block(blocks);
+    populateSplitBlock(r,c->right,c->left);
+    //COLA_ASSERT(r->weight>0);
+}
+
+/*
+ * Computes the cost (squared euclidean distance from desired positions) of the
+ * current positions for variables in this block
+ */
+double Block::cost() {
+    double c = 0;
+    for (Vit v=vars->begin();v!=vars->end();++v) {
+        double diff = (*v)->position() - (*v)->desiredPosition;
+        c += (*v)->weight * diff * diff;
+    }
+    return c;
+}
+ostream& operator <<(ostream &os, const Block& b)
+{
+    os<<"Block(posn="<<b.posn<<"):";
+    for(Block::Vit v=b.vars->begin();v!=b.vars->end();++v) {
+        os<<" "<<**v;
+    }
+    if(b.deleted) {
+        os<<" Deleted!";
+    }
+    return os;
+}
+
+Constraint::Constraint(Variable *left, Variable *right, double gap, bool equality)
+: left(left),
+  right(right),
+  gap(gap),
+  timeStamp(0),
+  active(false),
+  equality(equality),
+  unsatisfiable(false),
+  needsScaling(true),
+  creator(nullptr)
+{
+    // In hindsight I think it's probably better to build the constraint DAG
+    // (by creating variable in/out lists) when needed, rather than in advance
+    //left->out.push_back(this);
+    //right->in.push_back(this);
+}
+Constraint::~Constraint() {
+    // see constructor: the following is just way too slow.  
+    // Better to create a
+    // new DAG on demand than maintain the lists dynamically.
+    //Constraints::iterator i;
+    //for(i=left->out.begin(); i!=left->out.end(); i++) {
+        //if(*i==this) break;
+    //}
+    //left->out.erase(i);
+    //for(i=right->in.begin(); i!=right->in.end(); i++) {
+        //if(*i==this) break;
+    //}
+    //right->in.erase(i);
+}
+std::string Constraint::toString(void) const
+{
+    std::stringstream stream;
+    stream << "Constraint: var(" << left->id << ") ";
+    if (gap < 0)
+    {
+        stream << "- " << -gap << " ";
+    }
+    else
+    {
+        stream << "+ " << gap << " ";
+    }
+    stream << ((equality) ? "==" : "<=");
+    stream << " var(" << right->id << ") ";
+    return stream.str();
+}
+
+std::ostream& operator <<(std::ostream &os, const Constraint &c)
+{
+    const char *type = c.equality ? "=" : "<=";
+    std::ostringstream lscale, rscale;
+    if (c.left->scale != 1) 
+    {
+        lscale << c.left->scale << "*";
+    }
+    if (c.right->scale != 1)
+    {
+        rscale << c.right->scale << "*";
+    }
+    os << lscale.str() << *c.left << "+" << c.gap << type << 
+          rscale.str() << *c.right;
+    if (c.left->block && c.right->block)
+    {
+        os << "(" << c.slack() << ")" << (c.active ? "-active" : "") <<
+              "(lm=" << c.lm << ")";
+    }
+    else
+    {
+        os << "(vars have no position)";
+    }
+    return os;
+}
+
+bool CompareConstraints::operator() (
+    Constraint *const &l, Constraint *const &r
+) const {
+    double const sl = 
+        l->left->block->timeStamp > l->timeStamp
+        ||l->left->block==l->right->block
+        ?-DBL_MAX:l->slack();
+    double const sr = 
+        r->left->block->timeStamp > r->timeStamp
+        ||r->left->block==r->right->block
+        ?-DBL_MAX:r->slack();
+    if(sl==sr) {
+        // arbitrary choice based on id
+        if(l->left->id==r->left->id) {
+            if(l->right->id<r->right->id) return true;
+            return false;
+        }
+        if(l->left->id<r->left->id) return true;
+        return false;
+    }
+    return sl > sr;
+}
+
+std::ostream& operator <<(std::ostream &os, const Variable &v) {
+    if(v.block)
+        os << "(" << v.id << "=" << v.position() << ")";
+    else
+        os << "(" << v.id << "=" << v.desiredPosition << ")";
+    return os;
+}
+
+typedef std::list<std::map<Variable *, double> > VarOffsetMapList;
+
+class EqualityConstraintSet
+{
+    public:
+        EqualityConstraintSet(Variables vs)
+        {
+            for (size_t i = 0; i < vs.size(); ++i)
+            {
+                std::map<Variable *, double> varSet;
+                varSet[vs[i]] = 0;
+                variableGroups.push_back(varSet);
+            }
+        }
+        bool isRedundant(Variable *lhs, Variable *rhs, double sep)
+        {
+            VarOffsetMapList::iterator lhsSet = setForVar(lhs);
+            VarOffsetMapList::iterator rhsSet = setForVar(rhs);
+            if (lhsSet == rhsSet)
+            {
+                // Check if this is a redundant constraint.
+                if (fabs(((*lhsSet)[lhs] + sep) - (*rhsSet)[rhs]) < 0.0001)
+                {
+                    // If so, return true.
+                    return true;
+                }
+            }
+            return false;
+        }
+        void mergeSets(Variable *lhs, Variable *rhs, double sep)
+        {
+            VarOffsetMapList::iterator lhsSet = setForVar(lhs);
+            VarOffsetMapList::iterator rhsSet = setForVar(rhs);
+            if (lhsSet == rhsSet)
+            {
+                return;
+            }
+
+            double rhsOldOffset = (*rhsSet)[rhs];
+            double rhsNewOffset = (*lhsSet)[lhs] + sep;
+            double offset = rhsNewOffset - rhsOldOffset;
+
+            // Update offsets
+            for (std::map<Variable *, double>::iterator it = rhsSet->begin();
+                    it != rhsSet->end(); ++it)
+            {
+                it->second += offset;
+            }
+            
+            // Merge rhsSet into lhsSet
+            lhsSet->insert(rhsSet->begin(), rhsSet->end());
+            variableGroups.erase(rhsSet);
+        }
+
+    private:
+        VarOffsetMapList::iterator setForVar(Variable *var)
+        {
+            for (VarOffsetMapList::iterator it = variableGroups.begin();
+                    it != variableGroups.end(); ++it)
+            {
+                if (it->find(var) != it->end())
+                {
+                    return it;
+                }
+            }
+            return variableGroups.end();
+        }
+
+    VarOffsetMapList variableGroups;
+};
+
+Constraints constraintsRemovingRedundantEqualities(Variables const &vars, 
+        Constraints const &constraints)
+{
+    EqualityConstraintSet equalitySets(vars);
+    Constraints cs = Constraints(constraints.size());
+    int csSize = 0;
+
+    for (unsigned i = 0; i < constraints.size(); ++i)
+    {
+        Constraint *c = constraints[i];
+        if (c->equality)
+        {
+            if (!equalitySets.isRedundant(c->left, c->right, c->gap))
+            {
+                // Only add non-redundant equalities
+                equalitySets.mergeSets(c->left, c->right, c->gap);
+                cs[csSize++] = c;
+            }
+        }
+        else
+        {
+            // Add all non-equalities
+            cs[csSize++] = c;
+        }
+    }
+    cs.resize(csSize);
+    return cs;
+}
+
+
+}
+
+#endif
-- 
cgit v1.2.3