src/libavoid/vpsc.cpp

   1 /*
   2  * vim: ts=4 sw=4 et tw=0 wm=0
   3  *
   4  * libavoid - Fast, Incremental, Object-avoiding Line Router
   5  *
   6  * Copyright (C) 2005-2009  Monash University
   7  *
   8  * This library is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU Lesser General Public
  10  * License as published by the Free Software Foundation; either
  11  * version 2.1 of the License, or (at your option) any later version.
  12  * See the file LICENSE.LGPL distributed with the library.
  13  *
  14  * Licensees holding a valid commercial license may use this file in
  15  * accordance with the commercial license agreement provided with the
  16  * library.
  17  *
  18  * This library is distributed in the hope that it will be useful,
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  21  *
  22  * Author(s):   Tim Dwyer  <Tim.Dwyer@csse.monash.edu.au>
  23  *
  24  * --------------
  25  *
  26  * This file contains a slightly modified version of Solver() from libvpsc:
  27  * A solver for the problem of Variable Placement with Separation Constraints.
  28  * It has the following changes from the Adaptagrams VPSC version:
  29  *  -  The required VPSC code has been consolidated into a single file.
  30  *  -  Unnecessary code (like Solver) has been removed.
  31  *  -  The PairingHeap code has been replaced by a STL priority_queue.
  32  *
  33  * Modifications:  Michael Wybrow  <mjwybrow@users.sourceforge.net>
  34  *
  35 */
  36
  37 #include <iostream>
  38 #include <math.h>
  39 #include <sstream>
  40 #include <map>
  41 #include <cfloat>
  42 #include <cstdio>
  43
  44 #include "libavoid/vpsc.h"
  45 #include "libavoid/assertions.h"
  46
  47
  48 using namespace std;
  49
  50 namespace Avoid {
  51
  52 static const double ZERO_UPPERBOUND=-1e-10;
  53 static const double LAGRANGIAN_TOLERANCE=-1e-4;
  54
  55 IncSolver::IncSolver(vector<Variable*> const &vs, vector<Constraint *> const &cs)
  56     : m(cs.size()),
  57       cs(cs),
  58       n(vs.size()),
  59       vs(vs)
  60 {
  61     for(unsigned i=0;i<n;++i) {
  62         vs[i]->in.clear();
  63         vs[i]->out.clear();
  64     }
  65     for(unsigned i=0;i<m;++i) {
  66         Constraint *c=cs[i];
  67         c->left->out.push_back(c);
  68         c->right->in.push_back(c);
  69     }
  70     bs=new Blocks(vs);
  71 #ifdef LIBVPSC_LOGGING
  72     printBlocks();
  73     //COLA_ASSERT(!constraintGraphIsCyclic(n,vs));
  74 #endif
  75
  76     inactive=cs;
  77     for(Constraints::iterator i=inactive.begin();i!=inactive.end();++i) {
  78         (*i)->active=false;
  79     }
  80 }
  81 IncSolver::~IncSolver() {
  82     delete bs;
  83 }
  84
  85 // useful in debugging
  86 void IncSolver::printBlocks() {
  87 #ifdef LIBVPSC_LOGGING
  88     ofstream f(LOGFILE,ios::app);
  89     for(set<Block*>::iterator i=bs->begin();i!=bs->end();++i) {
  90         Block *b=*i;
  91         f<<"  "<<*b<<endl;
  92     }
  93     for(unsigned i=0;i<m;i++) {
  94         f<<"  "<<*cs[i]<<endl;
  95     }
  96 #endif
  97 }
  98
  99 /*
 100  * Stores the relative positions of the variables in their finalPosition
 101  * field.
 102  */
 103 void IncSolver::copyResult() {
 104     for(Variables::const_iterator i=vs.begin();i!=vs.end();++i) {
 105         Variable* v=*i;
 106         v->finalPosition=v->position();
 107         COLA_ASSERT(v->finalPosition==v->finalPosition);
 108     }
 109 }
 110
 111 struct node {
 112     set<node*> in;
 113     set<node*> out;
 114 };
 115 // useful in debugging - cycles would be BAD
 116 bool IncSolver::constraintGraphIsCyclic(const unsigned n, Variable* const vs[]) {
 117     map<Variable*, node*> varmap;
 118     vector<node*> graph;
 119     for(unsigned i=0;i<n;i++) {
 120         node *u=new node;
 121         graph.push_back(u);
 122         varmap[vs[i]]=u;
 123     }
 124     for(unsigned i=0;i<n;i++) {
 125         for(vector<Constraint*>::iterator c=vs[i]->in.begin();c!=vs[i]->in.end();++c) {
 126             Variable *l=(*c)->left;
 127             varmap[vs[i]]->in.insert(varmap[l]);
 128         }
 129
 130         for(vector<Constraint*>::iterator c=vs[i]->out.begin();c!=vs[i]->out.end();++c) {
 131             Variable *r=(*c)->right;
 132             varmap[vs[i]]->out.insert(varmap[r]);
 133         }
 134     }
 135     while(graph.size()>0) {
 136         node *u=NULL;
 137         vector<node*>::iterator i=graph.begin();
 138         for(;i!=graph.end();++i) {
 139             u=*i;
 140             if(u->in.size()==0) {
 141                 break;
 142             }
 143         }
 144         if(i==graph.end() && graph.size()>0) {
 145             //cycle found!
 146             return true;
 147         } else {
 148             graph.erase(i);
 149             for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
 150                 node *v=*j;
 151                 v->in.erase(u);
 152             }
 153             delete u;
 154         }
 155     }
 156     for(unsigned i=0; i<graph.size(); ++i) {
 157         delete graph[i];
 158     }
 159     return false;
 160 }
 161
 162 // useful in debugging - cycles would be BAD
 163 bool IncSolver::blockGraphIsCyclic() {
 164     map<Block*, node*> bmap;
 165     vector<node*> graph;
 166     for(set<Block*>::const_iterator i=bs->begin();i!=bs->end();++i) {
 167         Block *b=*i;
 168         node *u=new node;
 169         graph.push_back(u);
 170         bmap[b]=u;
 171     }
 172     for(set<Block*>::const_iterator i=bs->begin();i!=bs->end();++i) {
 173         Block *b=*i;
 174         b->setUpInConstraints();
 175         Constraint *c=b->findMinInConstraint();
 176         while(c!=NULL) {
 177             Block *l=c->left->block;
 178             bmap[b]->in.insert(bmap[l]);
 179             b->deleteMinInConstraint();
 180             c=b->findMinInConstraint();
 181         }
 182
 183         b->setUpOutConstraints();
 184         c=b->findMinOutConstraint();
 185         while(c!=NULL) {
 186             Block *r=c->right->block;
 187             bmap[b]->out.insert(bmap[r]);
 188             b->deleteMinOutConstraint();
 189             c=b->findMinOutConstraint();
 190         }
 191     }
 192     while(graph.size()>0) {
 193         node *u=NULL;
 194         vector<node*>::iterator i=graph.begin();
 195         for(;i!=graph.end();++i) {
 196             u=*i;
 197             if(u->in.size()==0) {
 198                 break;
 199             }
 200         }
 201         if(i==graph.end() && graph.size()>0) {
 202             //cycle found!
 203             return true;
 204         } else {
 205             graph.erase(i);
 206             for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
 207                 node *v=*j;
 208                 v->in.erase(u);
 209             }
 210             delete u;
 211         }
 212     }
 213     for(unsigned i=0; i<graph.size(); i++) {
 214         delete graph[i];
 215     }
 216     return false;
 217 }
 218
 219 bool IncSolver::solve() {
 220 #ifdef LIBVPSC_LOGGING
 221     ofstream f(LOGFILE,ios::app);
 222     f<<"solve_inc()..."<<endl;
 223 #endif
 224     satisfy();
 225     double lastcost = DBL_MAX, cost = bs->cost();
 226     while(fabs(lastcost-cost)>0.0001) {
 227         satisfy();
 228         lastcost=cost;
 229         cost = bs->cost();
 230 #ifdef LIBVPSC_LOGGING
 231         f<<"  bs->size="<<bs->size()<<", cost="<<cost<<endl;
 232 #endif
 233     }
 234     copyResult();
 235     return bs->size()!=n;
 236 }
 237 /*
 238  * incremental version of satisfy that allows refinement after blocks are
 239  * moved.
 240  *
 241  *  - move blocks to new positions
 242  *  - repeatedly merge across most violated constraint until no more
 243  *    violated constraints exist
 244  *
 245  * Note: there is a special case to handle when the most violated constraint
 246  * is between two variables in the same block.  Then, we must split the block
 247  * over an active constraint between the two variables.  We choose the
 248  * constraint with the most negative lagrangian multiplier.
 249  */
 250 bool IncSolver::satisfy() {
 251 #ifdef LIBVPSC_LOGGING
 252     ofstream f(LOGFILE,ios::app);
 253     f<<"satisfy_inc()..."<<endl;
 254 #endif
 255     splitBlocks();
 256     //long splitCtr = 0;
 257     Constraint* v = NULL;
 258     //CBuffer buffer(inactive);
 259     while((v = mostViolated(inactive))
 260           && (v->equality || ((v->slack() < ZERO_UPPERBOUND) && !v->active)))
 261     {
 262         COLA_ASSERT(!v->active);
 263         Block *lb = v->left->block, *rb = v->right->block;
 264         if(lb != rb) {
 265             lb->merge(rb,v);
 266         } else {
 267             if(lb->isActiveDirectedPathBetween(v->right,v->left)) {
 268                 // cycle found, relax the violated, cyclic constraint
 269                 v->unsatisfiable=true;
 270                 continue;
 271                 //UnsatisfiableException e;
 272                 //lb->getActiveDirectedPathBetween(e.path,v->right,v->left);
 273                 //e.path.push_back(v);
 274                 //throw e;
 275             }
 276             //if(splitCtr++>10000) {
 277                 //throw "Cycle Error!";
 278             //}
 279             // constraint is within block, need to split first
 280             try {
 281                 Constraint* splitConstraint
 282                     =lb->splitBetween(v->left,v->right,lb,rb);
 283                 if(splitConstraint!=NULL) {
 284                     COLA_ASSERT(!splitConstraint->active);
 285                     inactive.push_back(splitConstraint);
 286                 } else {
 287                     v->unsatisfiable=true;
 288                     continue;
 289                 }
 290             } catch(UnsatisfiableException e) {
 291                 e.path.push_back(v);
 292                 std::cerr << "Unsatisfiable:" << std::endl;
 293                 for(std::vector<Constraint*>::iterator r=e.path.begin();
 294                         r!=e.path.end();++r)
 295                 {
 296                     std::cerr << **r <<std::endl;
 297                 }
 298                 v->unsatisfiable=true;
 299                 continue;
 300             }
 301             if(v->slack()>=0) {
 302                 COLA_ASSERT(!v->active);
 303                 // v was satisfied by the above split!
 304                 inactive.push_back(v);
 305                 bs->insert(lb);
 306                 bs->insert(rb);
 307             } else {
 308                 bs->insert(lb->merge(rb,v));
 309             }
 310         }
 311         bs->cleanup();
 312 #ifdef LIBVPSC_LOGGING
 313         f<<"...remaining blocks="<<bs->size()<<", cost="<<bs->cost()<<endl;
 314 #endif
 315     }
 316 #ifdef LIBVPSC_LOGGING
 317     f<<"  finished merges."<<endl;
 318 #endif
 319     bs->cleanup();
 320     bool activeConstraints=false;
 321     for(unsigned i=0;i<m;i++) {
 322         v=cs[i];
 323         if(v->active) activeConstraints=true;
 324         if(v->slack() < ZERO_UPPERBOUND) {
 325             ostringstream s;
 326             s<<"Unsatisfied constraint: "<<*v;
 327 #ifdef LIBVPSC_LOGGING
 328             ofstream f(LOGFILE,ios::app);
 329             f<<s.str()<<endl;
 330 #endif
 331             throw s.str().c_str();
 332         }
 333     }
 334 #ifdef LIBVPSC_LOGGING
 335     f<<"  finished cleanup."<<endl;
 336     printBlocks();
 337 #endif
 338     copyResult();
 339     return activeConstraints;
 340 }
 341 void IncSolver::moveBlocks() {
 342 #ifdef LIBVPSC_LOGGING
 343     ofstream f(LOGFILE,ios::app);
 344     f<<"moveBlocks()..."<<endl;
 345 #endif
 346     for(set<Block*>::const_iterator i(bs->begin());i!=bs->end();++i) {
 347         Block *b = *i;
 348         b->updateWeightedPosition();
 349         //b->posn = b->wposn / b->weight;
 350     }
 351 #ifdef LIBVPSC_LOGGING
 352     f<<"  moved blocks."<<endl;
 353 #endif
 354 }
 355 void IncSolver::splitBlocks() {
 356 #ifdef LIBVPSC_LOGGING
 357     ofstream f(LOGFILE,ios::app);
 358 #endif
 359     moveBlocks();
 360     splitCnt=0;
 361     // Split each block if necessary on min LM
 362     for(set<Block*>::const_iterator i(bs->begin());i!=bs->end();++i) {
 363         Block* b = *i;
 364         Constraint* v=b->findMinLM();
 365         if(v!=NULL && v->lm < LAGRANGIAN_TOLERANCE) {
 366             COLA_ASSERT(!v->equality);
 367 #ifdef LIBVPSC_LOGGING
 368             f<<"    found split point: "<<*v<<" lm="<<v->lm<<endl;
 369 #endif
 370             splitCnt++;
 371             Block *b = v->left->block, *l=NULL, *r=NULL;
 372             COLA_ASSERT(v->left->block == v->right->block);
 373             //double pos = b->posn;
 374             b->split(l,r,v);
 375             //l->posn=r->posn=pos;
 376             //l->wposn = l->posn * l->weight;
 377             //r->wposn = r->posn * r->weight;
 378             l->updateWeightedPosition();
 379             r->updateWeightedPosition();
 380             bs->insert(l);
 381             bs->insert(r);
 382             b->deleted=true;
 383             COLA_ASSERT(!v->active);
 384             inactive.push_back(v);
 385 #ifdef LIBVPSC_LOGGING
 386             f<<"  new blocks: "<<*l<<" and "<<*r<<endl;
 387 #endif
 388         }
 389     }
 390     //if(splitCnt>0) { std::cout<<"  splits: "<<splitCnt<<endl; }
 391 #ifdef LIBVPSC_LOGGING
 392     f<<"  finished splits."<<endl;
 393 #endif
 394     bs->cleanup();
 395 }
 396
 397 /*
 398  * Scan constraint list for the most violated constraint, or the first equality
 399  * constraint
 400  */
 401 Constraint* IncSolver::mostViolated(Constraints &l) {
 402     double minSlack = DBL_MAX;
 403     Constraint* v=NULL;
 404 #ifdef LIBVPSC_LOGGING
 405     ofstream f(LOGFILE,ios::app);
 406     f<<"Looking for most violated..."<<endl;
 407 #endif
 408     Constraints::iterator end = l.end();
 409     Constraints::iterator deletePoint = end;
 410     for(Constraints::iterator i=l.begin();i!=end;++i) {
 411         Constraint *c=*i;
 412         double slack = c->slack();
 413         if(c->equality || slack < minSlack) {
 414             minSlack=slack;
 415             v=c;
 416             deletePoint=i;
 417             if(c->equality) break;
 418         }
 419     }
 420     // Because the constraint list is not order dependent we just
 421     // move the last element over the deletePoint and resize
 422     // downwards.  There is always at least 1 element in the
 423     // vector because of search.
 424     // TODO check this logic and add parens:
 425     if((deletePoint != end) && ((minSlack < ZERO_UPPERBOUND) && !v->active || v->equality)) {
 426         *deletePoint = l[l.size()-1];
 427         l.resize(l.size()-1);
 428     }
 429 #ifdef LIBVPSC_LOGGING
 430     f<<"  most violated is: "<<*v<<endl;
 431 #endif
 432     return v;
 433 }
 434
 435
 436 using std::set;
 437 using std::vector;
 438 using std::iterator;
 439 using std::list;
 440 using std::copy;
 441 #define __NOTNAN(p) (p)==(p)
 442
 443 long blockTimeCtr;
 444
 445 Blocks::Blocks(vector<Variable*> const &vs) : vs(vs),nvs(vs.size()) {
 446     blockTimeCtr=0;
 447     for(int i=0;i<nvs;i++) {
 448         insert(new Block(vs[i]));
 449     }
 450 }
 451 Blocks::~Blocks(void)
 452 {
 453     blockTimeCtr=0;
 454     for(set<Block*>::iterator i=begin();i!=end();++i) {
 455         delete *i;
 456     }
 457     clear();
 458 }
 459
 460 /*
 461  * returns a list of variables with total ordering determined by the constraint
 462  * DAG
 463  */
 464 list<Variable*> *Blocks::totalOrder() {
 465     list<Variable*> *order = new list<Variable*>;
 466     for(int i=0;i<nvs;i++) {
 467         vs[i]->visited=false;
 468     }
 469     for(int i=0;i<nvs;i++) {
 470         if(vs[i]->in.size()==0) {
 471             dfsVisit(vs[i],order);
 472         }
 473     }
 474     return order;
 475 }
 476 // Recursive depth first search giving total order by pushing nodes in the DAG
 477 // onto the front of the list when we finish searching them
 478 void Blocks::dfsVisit(Variable *v, list<Variable*> *order) {
 479     v->visited=true;
 480     vector<Constraint*>::iterator it=v->out.begin();
 481     for(;it!=v->out.end();++it) {
 482         Constraint *c=*it;
 483         if(!c->right->visited) {
 484             dfsVisit(c->right, order);
 485         }
 486     }
 487 #ifdef LIBVPSC_LOGGING
 488     ofstream f(LOGFILE,ios::app);
 489     f<<"  order="<<*v<<endl;
 490 #endif
 491     order->push_front(v);
 492 }
 493 /*
 494  * Processes incoming constraints, most violated to least, merging with the
 495  * neighbouring (left) block until no more violated constraints are found
 496  */
 497 void Blocks::mergeLeft(Block *r) {
 498 #ifdef LIBVPSC_LOGGING
 499     ofstream f(LOGFILE,ios::app);
 500     f<<"mergeLeft called on "<<*r<<endl;
 501 #endif
 502     r->timeStamp=++blockTimeCtr;
 503     r->setUpInConstraints();
 504     Constraint *c=r->findMinInConstraint();
 505     while (c != NULL && c->slack()<0) {
 506 #ifdef LIBVPSC_LOGGING
 507         f<<"mergeLeft on constraint: "<<*c<<endl;
 508 #endif
 509         r->deleteMinInConstraint();
 510         Block *l = c->left->block;
 511         if (l->in==NULL) l->setUpInConstraints();
 512         double dist = c->right->offset - c->left->offset - c->gap;
 513         if (r->vars->size() < l->vars->size()) {
 514             dist=-dist;
 515             std::swap(l, r);
 516         }
 517         blockTimeCtr++;
 518         r->merge(l, c, dist);
 519         r->mergeIn(l);
 520         r->timeStamp=blockTimeCtr;
 521         removeBlock(l);
 522         c=r->findMinInConstraint();
 523     }
 524 #ifdef LIBVPSC_LOGGING
 525     f<<"merged "<<*r<<endl;
 526 #endif
 527 }
 528 /*
 529  * Symmetrical to mergeLeft
 530  */
 531 void Blocks::mergeRight(Block *l) {
 532 #ifdef LIBVPSC_LOGGING
 533     ofstream f(LOGFILE,ios::app);
 534     f<<"mergeRight called on "<<*l<<endl;
 535 #endif
 536     l->setUpOutConstraints();
 537     Constraint *c = l->findMinOutConstraint();
 538     while (c != NULL && c->slack()<0) {
 539 #ifdef LIBVPSC_LOGGING
 540         f<<"mergeRight on constraint: "<<*c<<endl;
 541 #endif
 542         l->deleteMinOutConstraint();
 543         Block *r = c->right->block;
 544         r->setUpOutConstraints();
 545         double dist = c->left->offset + c->gap - c->right->offset;
 546         if (l->vars->size() > r->vars->size()) {
 547             dist=-dist;
 548             std::swap(l, r);
 549         }
 550         l->merge(r, c, dist);
 551         l->mergeOut(r);
 552         removeBlock(r);
 553         c=l->findMinOutConstraint();
 554     }
 555 #ifdef LIBVPSC_LOGGING
 556     f<<"merged "<<*l<<endl;
 557 #endif
 558 }
 559 void Blocks::removeBlock(Block *doomed) {
 560     doomed->deleted=true;
 561     //erase(doomed);
 562 }
 563 void Blocks::cleanup() {
 564     vector<Block*> bcopy(begin(),end());
 565     for(vector<Block*>::iterator i=bcopy.begin();i!=bcopy.end();++i) {
 566         Block *b=*i;
 567         if(b->deleted) {
 568             erase(b);
 569             delete b;
 570         }
 571     }
 572 }
 573 /*
 574  * Splits block b across constraint c into two new blocks, l and r (c's left
 575  * and right sides respectively)
 576  */
 577 void Blocks::split(Block *b, Block *&l, Block *&r, Constraint *c) {
 578     b->split(l,r,c);
 579 #ifdef LIBVPSC_LOGGING
 580     ofstream f(LOGFILE,ios::app);
 581     f<<"Split left: "<<*l<<endl;
 582     f<<"Split right: "<<*r<<endl;
 583 #endif
 584     r->posn = b->posn;
 585     //COLA_ASSERT(r->weight!=0);
 586     //r->wposn = r->posn * r->weight;
 587     mergeLeft(l);
 588     // r may have been merged!
 589     r = c->right->block;
 590     r->updateWeightedPosition();
 591     //r->posn = r->wposn / r->weight;
 592     mergeRight(r);
 593     removeBlock(b);
 594
 595     insert(l);
 596     insert(r);
 597     COLA_ASSERT(__NOTNAN(l->posn));
 598     COLA_ASSERT(__NOTNAN(r->posn));
 599 }
 600 /*
 601  * returns the cost total squared distance of variables from their desired
 602  * positions
 603  */
 604 double Blocks::cost() {
 605     double c = 0;
 606     for(set<Block*>::iterator i=begin();i!=end();++i) {
 607         c += (*i)->cost();
 608     }
 609     return c;
 610 }
 611
 612 void PositionStats::addVariable(Variable* v) {
 613     double ai=scale/v->scale;
 614     double bi=v->offset/v->scale;
 615     double wi=v->weight;
 616     AB+=wi*ai*bi;
 617     AD+=wi*ai*v->desiredPosition;
 618     A2+=wi*ai*ai;
 619     /*
 620 #ifdef LIBVPSC_LOGGING
 621     ofstream f(LOGFILE,ios::app);
 622     f << "adding v[" << v->id << "], blockscale=" << scale << ", despos="
 623       << v->desiredPosition << ", ai=" << ai << ", bi=" << bi
 624       << ", AB=" << AB << ", AD=" << AD << ", A2=" << A2;
 625 #endif
 626 */
 627 }
 628 void Block::addVariable(Variable* v) {
 629     v->block=this;
 630     vars->push_back(v);
 631     if(ps.A2==0) ps.scale=v->scale;
 632     //weight+= v->weight;
 633     //wposn += v->weight * (v->desiredPosition - v->offset);
 634     //posn=wposn/weight;
 635     ps.addVariable(v);
 636     posn=(ps.AD - ps.AB) / ps.A2;
 637     COLA_ASSERT(__NOTNAN(posn));
 638     /*
 639 #ifdef LIBVPSC_LOGGING
 640     ofstream f(LOGFILE,ios::app);
 641     f << ", posn=" << posn << endl;
 642 #endif
 643 */
 644 }
 645 Block::Block(Variable* const v)
 646     : vars(new vector<Variable*>)
 647     , posn(0)
 648     //, weight(0)
 649     //, wposn(0)
 650     , deleted(false)
 651     , timeStamp(0)
 652     , in(NULL)
 653     , out(NULL)
 654 {
 655     if(v!=NULL) {
 656         v->offset=0;
 657         addVariable(v);
 658     }
 659 }
 660
 661 void Block::updateWeightedPosition() {
 662     //wposn=0;
 663     ps.AB=ps.AD=ps.A2=0;
 664     for (Vit v=vars->begin();v!=vars->end();++v) {
 665         //wposn += ((*v)->desiredPosition - (*v)->offset) * (*v)->weight;
 666         ps.addVariable(*v);
 667     }
 668     posn=(ps.AD - ps.AB) / ps.A2;
 669     COLA_ASSERT(__NOTNAN(posn));
 670 #ifdef LIBVPSC_LOGGING
 671     ofstream f(LOGFILE,ios::app);
 672     f << ", posn=" << posn << endl;
 673 #endif
 674 }
 675 Block::~Block(void)
 676 {
 677     delete vars;
 678     delete in;
 679     delete out;
 680 }
 681 void Block::setUpInConstraints() {
 682     setUpConstraintHeap(in,true);
 683 }
 684 void Block::setUpOutConstraints() {
 685     setUpConstraintHeap(out,false);
 686 }
 687 void Block::setUpConstraintHeap(Heap* &h,bool in) {
 688     delete h;
 689     h = new Heap();
 690     for (Vit i=vars->begin();i!=vars->end();++i) {
 691         Variable *v=*i;
 692         vector<Constraint*> *cs=in?&(v->in):&(v->out);
 693         for (Cit j=cs->begin();j!=cs->end();++j) {
 694             Constraint *c=*j;
 695             c->timeStamp=blockTimeCtr;
 696             if (((c->left->block != this) && in) || ((c->right->block != this) && !in)) {
 697                 h->push(c);
 698             }
 699         }
 700     }
 701 }
 702 Block* Block::merge(Block* b, Constraint* c) {
 703 #ifdef LIBVPSC_LOGGING
 704     ofstream f(LOGFILE,ios::app);
 705     f<<"  merging on: "<<*c<<",c->left->offset="<<c->left->offset<<",c->right->offset="<<c->right->offset<<endl;
 706 #endif
 707     double dist = c->right->offset - c->left->offset - c->gap;
 708     Block *l=c->left->block;
 709     Block *r=c->right->block;
 710     if (l->vars->size() < r->vars->size()) {
 711         r->merge(l,c,dist);
 712     } else {
 713                l->merge(r,c,-dist);
 714     }
 715     Block* mergeBlock=b->deleted?this:b;
 716 #ifdef LIBVPSC_LOGGING
 717     f<<"  merged block="<<*mergeBlock<<endl;
 718 #endif
 719     return mergeBlock;
 720 }
 721 /*
 722  * Merges b into this block across c.  Can be either a
 723  * right merge or a left merge
 724  * @param b block to merge into this
 725  * @param c constraint being merged
 726  * @param distance separation required to satisfy c
 727  */
 728 void Block::merge(Block *b, Constraint *c, double dist) {
 729 #ifdef LIBVPSC_LOGGING
 730     ofstream f(LOGFILE,ios::app);
 731     f<<"    merging: "<<*b<<"dist="<<dist<<endl;
 732 #endif
 733     c->active=true;
 734     //wposn+=b->wposn-dist*b->weight;
 735     //weight+=b->weight;
 736     for(Vit i=b->vars->begin();i!=b->vars->end();++i) {
 737         Variable *v=*i;
 738         //v->block=this;
 739         //vars->push_back(v);
 740         v->offset+=dist;
 741         addVariable(v);
 742     }
 743 #ifdef LIBVPSC_LOGGING
 744     for(Vit i=vars->begin();i!=vars->end();++i) {
 745         Variable *v=*i;
 746         f<<"    v["<<v->id<<"]: d="<<v->desiredPosition
 747             <<" a="<<v->scale<<" o="<<v->offset
 748             <<endl;
 749     }
 750     f<<"  AD="<<ps.AD<<" AB="<<ps.AB<<" A2="<<ps.A2<<endl;
 751 #endif
 752     //posn=wposn/weight;
 753     //COLA_ASSERT(wposn==ps.AD - ps.AB);
 754     posn=(ps.AD - ps.AB) / ps.A2;
 755     COLA_ASSERT(__NOTNAN(posn));
 756     b->deleted=true;
 757 }
 758
 759 void Block::mergeIn(Block *b) {
 760 #ifdef LIBVPSC_LOGGING
 761     ofstream f(LOGFILE,ios::app);
 762     f<<"  merging constraint heaps... "<<endl;
 763 #endif
 764     // We check the top of the heaps to remove possible internal constraints
 765     findMinInConstraint();
 766     b->findMinInConstraint();
 767     while (!b->in->empty())
 768     {
 769         in->push(b->in->top());
 770         b->in->pop();
 771     }
 772 #ifdef LIBVPSC_LOGGING
 773     f<<"  merged heap: "<<*in<<endl;
 774 #endif
 775 }
 776 void Block::mergeOut(Block *b) {
 777     findMinOutConstraint();
 778     b->findMinOutConstraint();
 779     while (!b->out->empty())
 780     {
 781         out->push(b->out->top());
 782         b->out->pop();
 783     }
 784 }
 785 Constraint *Block::findMinInConstraint() {
 786     Constraint *v = NULL;
 787     vector<Constraint*> outOfDate;
 788     while (!in->empty()) {
 789         v = in->top();
 790         Block *lb=v->left->block;
 791         Block *rb=v->right->block;
 792         // rb may not be this if called between merge and mergeIn
 793 #ifdef LIBVPSC_LOGGING
 794         ofstream f(LOGFILE,ios::app);
 795         f<<"  checking constraint ... "<<*v;
 796         f<<"    timestamps: left="<<lb->timeStamp<<" right="<<rb->timeStamp<<" constraint="<<v->timeStamp<<endl;
 797 #endif
 798         if(lb == rb) {
 799             // constraint has been merged into the same block
 800 #ifdef LIBVPSC_LOGGING
 801             if(v->slack()<0) {
 802                 f<<"  violated internal constraint found! "<<*v<<endl;
 803                 f<<"     lb="<<*lb<<endl;
 804                 f<<"     rb="<<*rb<<endl;
 805             }
 806 #endif
 807             in->pop();
 808 #ifdef LIBVPSC_LOGGING
 809             f<<" ... skipping internal constraint"<<endl;
 810 #endif
 811         } else if(v->timeStamp < lb->timeStamp) {
 812             // block at other end of constraint has been moved since this
 813             in->pop();
 814             outOfDate.push_back(v);
 815 #ifdef LIBVPSC_LOGGING
 816             f<<"    reinserting out of date (reinsert later)"<<endl;
 817 #endif
 818         } else {
 819             break;
 820         }
 821     }
 822     for(Cit i=outOfDate.begin();i!=outOfDate.end();++i) {
 823         v=*i;
 824         v->timeStamp=blockTimeCtr;
 825         in->push(v);
 826     }
 827     if(in->empty()) {
 828         v=NULL;
 829     } else {
 830         v=in->top();
 831     }
 832     return v;
 833 }
 834 Constraint *Block::findMinOutConstraint() {
 835     if(out->empty()) return NULL;
 836     Constraint *v = out->top();
 837     while (v->left->block == v->right->block) {
 838         out->pop();
 839         if(out->empty()) return NULL;
 840         v = out->top();
 841     }
 842     return v;
 843 }
 844 void Block::deleteMinInConstraint() {
 845     in->pop();
 846 #ifdef LIBVPSC_LOGGING
 847     ofstream f(LOGFILE,ios::app);
 848     f<<"deleteMinInConstraint... "<<endl;
 849     f<<"  result: "<<*in<<endl;
 850 #endif
 851 }
 852 void Block::deleteMinOutConstraint() {
 853     out->pop();
 854 }
 855 inline bool Block::canFollowLeft(Constraint const* c, Variable const* last) const {
 856     return c->left->block==this && c->active && last!=c->left;
 857 }
 858 inline bool Block::canFollowRight(Constraint const* c, Variable const* last) const {
 859     return c->right->block==this && c->active && last!=c->right;
 860 }
 861
 862 // computes the derivative of v and the lagrange multipliers
 863 // of v's out constraints (as the recursive sum of those below.
 864 // Does not backtrack over u.
 865 // also records the constraint with minimum lagrange multiplier
 866 // in min_lm
 867 double Block::compute_dfdv(Variable* const v, Variable* const u,
 868                Constraint *&min_lm) {
 869     double dfdv=v->dfdv();
 870     for(Cit it=v->out.begin();it!=v->out.end();++it) {
 871         Constraint *c=*it;
 872         if(canFollowRight(c,u)) {
 873             c->lm=compute_dfdv(c->right,v,min_lm);
 874             dfdv+=c->lm*c->left->scale;
 875             if(!c->equality&&(min_lm==NULL||c->lm<min_lm->lm)) min_lm=c;
 876         }
 877     }
 878     for(Cit it=v->in.begin();it!=v->in.end();++it) {
 879         Constraint *c=*it;
 880         if(canFollowLeft(c,u)) {
 881             c->lm=-compute_dfdv(c->left,v,min_lm);
 882             dfdv-=c->lm*c->right->scale;
 883             if(!c->equality&&(min_lm==NULL||c->lm<min_lm->lm)) min_lm=c;
 884         }
 885     }
 886     return dfdv/v->scale;
 887 }
 888 double Block::compute_dfdv(Variable* const v, Variable* const u) {
 889     double dfdv = v->dfdv();
 890     for(Cit it = v->out.begin(); it != v->out.end(); ++it) {
 891         Constraint *c = *it;
 892         if(canFollowRight(c,u)) {
 893             c->lm =   compute_dfdv(c->right,v);
 894             dfdv += c->lm * c->left->scale;
 895         }
 896     }
 897     for(Cit it=v->in.begin();it!=v->in.end();++it) {
 898         Constraint *c = *it;
 899         if(canFollowLeft(c,u)) {
 900             c->lm = - compute_dfdv(c->left,v);
 901             dfdv -= c->lm * c->right->scale;
 902         }
 903     }
 904     return dfdv/v->scale;
 905 }
 906
 907 // The top level v and r are variables between which we want to find the
 908 // constraint with the smallest lm.
 909 // Similarly, m is initially NULL and is only assigned a value if the next
 910 // variable to be visited is r or if a possible min constraint is returned from
 911 // a nested call (rather than NULL).
 912 // Then, the search for the m with minimum lm occurs as we return from
 913 // the recursion (checking only constraints traversed left-to-right
 914 // in order to avoid creating any new violations).
 915 // We also do not consider equality constraints as potential split points
 916 bool Block::split_path(
 917     Variable* r,
 918     Variable* const v,
 919     Variable* const u,
 920     Constraint* &m,
 921     bool desperation=false
 922     )
 923 {
 924     for(Cit it(v->in.begin());it!=v->in.end();++it) {
 925         Constraint *c=*it;
 926         if(canFollowLeft(c,u)) {
 927 #ifdef LIBVPSC_LOGGING
 928     ofstream f(LOGFILE,ios::app);
 929     f<<"  left split path: "<<*c<<endl;
 930 #endif
 931             if(c->left==r) {
 932                 if(desperation&&!c->equality) m=c;
 933                 return true;
 934             } else {
 935                 if(split_path(r,c->left,v,m)) {
 936                     if(desperation && !c->equality && (!m||c->lm<m->lm)) {
 937                                m=c;
 938                     }
 939                     return true;
 940                 }
 941             }
 942         }
 943     }
 944     for(Cit it(v->out.begin());it!=v->out.end();++it) {
 945         Constraint *c=*it;
 946         if(canFollowRight(c,u)) {
 947 #ifdef LIBVPSC_LOGGING
 948     ofstream f(LOGFILE,ios::app);
 949     f<<"  right split path: "<<*c<<endl;
 950 #endif
 951             if(c->right==r) {
 952                 if(!c->equality) m=c;
 953                 return true;
 954             } else {
 955                 if(split_path(r,c->right,v,m)) {
 956                     if(!c->equality && (!m||c->lm<m->lm))
 957                                m=c;
 958                     return true;
 959                 }
 960             }
 961         }
 962     }
 963     return false;
 964 }
 965 /*
 966 Block::Pair Block::compute_dfdv_between(
 967         Variable* r, Variable* const v, Variable* const u,
 968         const Direction dir = NONE, bool changedDirection = false) {
 969     double dfdv=v->weight*(v->position() - v->desiredPosition);
 970     Constraint *m=NULL;
 971     for(Cit it(v->in.begin());it!=v->in.end();++it) {
 972         Constraint *c=*it;
 973         if(canFollowLeft(c,u)) {
 974             if(dir==RIGHT) {
 975                 changedDirection = true;
 976             }
 977             if(c->left==r) {
 978                        r=NULL;
 979                     if(!c->equality) m=c;
 980             }
 981             Pair p=compute_dfdv_between(r,c->left,v,
 982                     LEFT,changedDirection);
 983             dfdv -= c->lm = -p.first;
 984             if(r && p.second)
 985                 m = p.second;
 986         }
 987     }
 988     for(Cit it(v->out.begin());it!=v->out.end();++it) {
 989         Constraint *c=*it;
 990         if(canFollowRight(c,u)) {
 991             if(dir==LEFT) {
 992                 changedDirection = true;
 993             }
 994             if(c->right==r) {
 995                        r=NULL;
 996                     if(!c->equality) m=c;
 997             }
 998             Pair p=compute_dfdv_between(r,c->right,v,
 999                     RIGHT,changedDirection);
1000             dfdv += c->lm = p.first;
1001             if(r && p.second)
1002                 m = changedDirection && !c->equality && c->lm < p.second->lm
1003                     ? c
1004                     : p.second;
1005         }
1006     }
1007     return Pair(dfdv,m);
1008 }
1009 */
1010
1011 // resets LMs for all active constraints to 0 by
1012 // traversing active constraint tree starting from v,
1013 // not back tracking over u
1014 void Block::reset_active_lm(Variable* const v, Variable* const u) {
1015     for(Cit it=v->out.begin();it!=v->out.end();++it) {
1016         Constraint *c=*it;
1017         if(canFollowRight(c,u)) {
1018             c->lm=0;
1019             reset_active_lm(c->right,v);
1020         }
1021     }
1022     for(Cit it=v->in.begin();it!=v->in.end();++it) {
1023         Constraint *c=*it;
1024         if(canFollowLeft(c,u)) {
1025             c->lm=0;
1026             reset_active_lm(c->left,v);
1027         }
1028     }
1029 }
1030 void Block::list_active(Variable* const v, Variable* const u) {
1031     for(Cit it=v->out.begin();it!=v->out.end();++it) {
1032         Constraint *c=*it;
1033         if(canFollowRight(c,u)) {
1034 #ifdef LIBVPSC_LOGGING
1035     ofstream f(LOGFILE,ios::app);
1036     f<<"  "<<*c<<endl;
1037 #endif
1038             list_active(c->right,v);
1039         }
1040     }
1041     for(Cit it=v->in.begin();it!=v->in.end();++it) {
1042         Constraint *c=*it;
1043         if(canFollowLeft(c,u)) {
1044 #ifdef LIBVPSC_LOGGING
1045     ofstream f(LOGFILE,ios::app);
1046     f<<"  "<<*c<<endl;
1047 #endif
1048             list_active(c->left,v);
1049         }
1050     }
1051 }
1052 /*
1053  * finds the constraint with the minimum lagrange multiplier, that is, the constraint
1054  * that most wants to split
1055  */
1056 Constraint *Block::findMinLM() {
1057     Constraint *min_lm=NULL;
1058     reset_active_lm(vars->front(),NULL);
1059     compute_dfdv(vars->front(),NULL,min_lm);
1060 #ifdef LIBVPSC_LOGGING
1061     ofstream f(LOGFILE,ios::app);
1062     f<<"  langrangians: "<<endl;
1063     list_active(vars->front(),NULL);
1064 #endif
1065     return min_lm;
1066 }
1067 Constraint *Block::findMinLMBetween(Variable* const lv, Variable* const rv) {
1068     reset_active_lm(vars->front(),NULL);
1069     compute_dfdv(vars->front(),NULL);
1070     Constraint *min_lm=NULL;
1071     split_path(rv,lv,NULL,min_lm);
1072 #if 0
1073     if(min_lm==NULL) {
1074         split_path(rv,lv,NULL,min_lm,true);
1075     }
1076 #else
1077     if(min_lm==NULL) {
1078         fprintf(stderr,"Couldn't find split point!\n");
1079         UnsatisfiableException e;
1080         getActivePathBetween(e.path,lv,rv,NULL);
1081         throw e;
1082     }
1083     COLA_ASSERT(min_lm!=NULL);
1084 #endif
1085     return min_lm;
1086 }
1087
1088 // populates block b by traversing the active constraint tree adding variables as they're
1089 // visited.  Starts from variable v and does not backtrack over variable u.
1090 void Block::populateSplitBlock(Block *b, Variable* v, Variable const* u) {
1091     b->addVariable(v);
1092     for (Cit c=v->in.begin();c!=v->in.end();++c) {
1093         if (canFollowLeft(*c,u))
1094             populateSplitBlock(b, (*c)->left, v);
1095     }
1096     for (Cit c=v->out.begin();c!=v->out.end();++c) {
1097         if (canFollowRight(*c,u))
1098             populateSplitBlock(b, (*c)->right, v);
1099     }
1100 }
1101 /*
1102  * Returns the active path between variables u and v... not back tracking over w
1103  */
1104 bool Block::getActivePathBetween(Constraints& path, Variable const* u,
1105                Variable const* v, Variable const *w) const {
1106     if(u==v) return true;
1107     for (Cit_const c=u->in.begin();c!=u->in.end();++c) {
1108         if (canFollowLeft(*c,w)) {
1109             if(getActivePathBetween(path, (*c)->left, v, u)) {
1110                 path.push_back(*c);
1111                 return true;
1112             }
1113         }
1114     }
1115     for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
1116         if (canFollowRight(*c,w)) {
1117             if(getActivePathBetween(path, (*c)->right, v, u)) {
1118                 path.push_back(*c);
1119                 return true;
1120             }
1121         }
1122     }
1123     return false;
1124 }
1125 // Search active constraint tree from u to see if there is a directed path to v.
1126 // Returns true if path is found with all constraints in path having their visited flag
1127 // set true.
1128 bool Block::isActiveDirectedPathBetween(Variable const* u, Variable const* v) const {
1129     if(u==v) return true;
1130     for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
1131         if(canFollowRight(*c,NULL)) {
1132             if(isActiveDirectedPathBetween((*c)->right,v)) {
1133                 return true;
1134             }
1135         }
1136     }
1137     return false;
1138 }
1139 bool Block::getActiveDirectedPathBetween(
1140         Constraints& path, Variable const* u, Variable const* v) const {
1141     if(u==v) return true;
1142     for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
1143         if(canFollowRight(*c,NULL)) {
1144             if(getActiveDirectedPathBetween(path,(*c)->right,v)) {
1145                 path.push_back(*c);
1146                 return true;
1147             }
1148         }
1149     }
1150     return false;
1151 }
1152 /*
1153  * Block needs to be split because of a violated constraint between vl and vr.
1154  * We need to search the active constraint tree between l and r and find the constraint
1155  * with min lagrangrian multiplier and split at that point.
1156  * Returns the split constraint
1157  */
1158 Constraint* Block::splitBetween(Variable* const vl, Variable* const vr,
1159                Block* &lb, Block* &rb) {
1160 #ifdef LIBVPSC_LOGGING
1161     ofstream f(LOGFILE,ios::app);
1162     f<<"  need to split between: "<<*vl<<" and "<<*vr<<endl;
1163 #endif
1164     Constraint *c=findMinLMBetween(vl, vr);
1165 #ifdef LIBVPSC_LOGGING
1166     f<<"  going to split on: "<<*c<<endl;
1167 #endif
1168     if(c!=NULL) {
1169         split(lb,rb,c);
1170         deleted = true;
1171     }
1172     return c;
1173 }
1174
1175 /*
1176  * Creates two new blocks, l and r, and splits this block across constraint c,
1177  * placing the left subtree of constraints (and associated variables) into l
1178  * and the right into r.
1179  */
1180 void Block::split(Block* &l, Block* &r, Constraint* c) {
1181     c->active=false;
1182     l=new Block();
1183     populateSplitBlock(l,c->left,c->right);
1184     //COLA_ASSERT(l->weight>0);
1185     r=new Block();
1186     populateSplitBlock(r,c->right,c->left);
1187     //COLA_ASSERT(r->weight>0);
1188 }
1189
1190 /*
1191  * Computes the cost (squared euclidean distance from desired positions) of the
1192  * current positions for variables in this block
1193  */
1194 double Block::cost() {
1195     double c = 0;
1196     for (Vit v=vars->begin();v!=vars->end();++v) {
1197         double diff = (*v)->position() - (*v)->desiredPosition;
1198         c += (*v)->weight * diff * diff;
1199     }
1200     return c;
1201 }
1202 ostream& operator <<(ostream &os, const Block& b)
1203 {
1204     os<<"Block(posn="<<b.posn<<"):";
1205     for(Block::Vit v=b.vars->begin();v!=b.vars->end();++v) {
1206         os<<" "<<**v;
1207     }
1208     if(b.deleted) {
1209         os<<" Deleted!";
1210     }
1211     return os;
1212 }
1213
1214 Constraint::Constraint(Variable *left, Variable *right, double gap, bool equality)
1215 : left(left),
1216   right(right),
1217   gap(gap),
1218   timeStamp(0),
1219   active(false),
1220   equality(equality),
1221   unsatisfiable(false)
1222 {
1223     // In hindsight I think it's probably better to build the constraint DAG
1224     // (by creating variable in/out lists) when needed, rather than in advance
1225     //left->out.push_back(this);
1226     //right->in.push_back(this);
1227 }
1228 Constraint::~Constraint() {
1229     // see constructor: the following is just way too slow.
1230     // Better to create a
1231     // new DAG on demand than maintain the lists dynamically.
1232     //Constraints::iterator i;
1233     //for(i=left->out.begin(); i!=left->out.end(); i++) {
1234         //if(*i==this) break;
1235     //}
1236     //left->out.erase(i);
1237     //for(i=right->in.begin(); i!=right->in.end(); i++) {
1238         //if(*i==this) break;
1239     //}
1240     //right->in.erase(i);
1241 }
1242 double Constraint::slack() const {
1243     return unsatisfiable ? DBL_MAX
1244            : right->scale * right->position()
1245          - gap - left->scale * left->position();
1246 }
1247 std::ostream& operator <<(std::ostream &os, const Constraint &c)
1248 {
1249     if(&c==NULL) {
1250         os<<"NULL";
1251     } else {
1252         const char *type=c.equality?"=":"<=";
1253         std::ostringstream lscale, rscale;
1254         if(c.left->scale!=1) {
1255             lscale << c.left->scale << "*";
1256         }
1257         if(c.right->scale!=1) {
1258             rscale << c.right->scale << "*";
1259         }
1260         os<<lscale.str()<<*c.left<<"+"<<c.gap<<type<<rscale.str()<<*c.right;
1261         if(c.left->block&&c.right->block)
1262             os<<"("<<c.slack()<<")"<<(c.active?"-active":"")
1263                 <<"(lm="<<c.lm<<")";
1264         else
1265             os<<"(vars have no position)";
1266     }
1267     return os;
1268 }
1269
1270 bool CompareConstraints::operator() (
1271     Constraint *const &l, Constraint *const &r
1272 ) const {
1273     double const sl =
1274         l->left->block->timeStamp > l->timeStamp
1275         ||l->left->block==l->right->block
1276         ?-DBL_MAX:l->slack();
1277     double const sr =
1278         r->left->block->timeStamp > r->timeStamp
1279         ||r->left->block==r->right->block
1280         ?-DBL_MAX:r->slack();
1281     if(sl==sr) {
1282         // arbitrary choice based on id
1283         if(l->left->id==r->left->id) {
1284             if(l->right->id<r->right->id) return true;
1285             return false;
1286         }
1287         if(l->left->id<r->left->id) return true;
1288         return false;
1289     }
1290     return sl > sr;
1291 }
1292
1293 std::ostream& operator <<(std::ostream &os, const Variable &v) {
1294     if(v.block)
1295         os << "(" << v.id << "=" << v.position() << ")";
1296     else
1297         os << "(" << v.id << "=" << v.desiredPosition << ")";
1298     return os;
1299 }
1300
1301 }