diff options
Diffstat (limited to 'src/3rdparty/adaptagrams/libavoid/vpsc.cpp')
| -rw-r--r-- | src/3rdparty/adaptagrams/libavoid/vpsc.cpp | 1500 |
1 files changed, 1500 insertions, 0 deletions
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 |