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     if(deletePoint != end && (minSlack < ZERO_UPPERBOUND && !v->active || v->equality)) {
 425         *deletePoint = l[l.size()-1];
 426         l.resize(l.size()-1);
 427     }
 428 #ifdef LIBVPSC_LOGGING
 429     f<<"  most violated is: "<<*v<<endl;
 430 #endif
 431     return v;
 432 }
 433
 434
 435 using std::set;
 436 using std::vector;
 437 using std::iterator;
 438 using std::list;
 439 using std::copy;
 440 #define __NOTNAN(p) (p)==(p)
 441
 442 long blockTimeCtr;
 443
 444 Blocks::Blocks(vector<Variable*> const &vs) : vs(vs),nvs(vs.size()) {
 445     blockTimeCtr=0;
 446     for(int i=0;i<nvs;i++) {
 447         insert(new Block(vs[i]));
 448     }
 449 }
 450 Blocks::~Blocks(void)
 451 {
 452     blockTimeCtr=0;
 453     for(set<Block*>::iterator i=begin();i!=end();++i) {
 454         delete *i;
 455     }
 456     clear();
 457 }
 458
 459 /*
 460  * returns a list of variables with total ordering determined by the constraint
 461  * DAG
 462  */
 463 list<Variable*> *Blocks::totalOrder() {
 464     list<Variable*> *order = new list<Variable*>;
 465     for(int i=0;i<nvs;i++) {
 466         vs[i]->visited=false;
 467     }
 468     for(int i=0;i<nvs;i++) {
 469         if(vs[i]->in.size()==0) {
 470             dfsVisit(vs[i],order);
 471         }
 472     }
 473     return order;
 474 }
 475 // Recursive depth first search giving total order by pushing nodes in the DAG
 476 // onto the front of the list when we finish searching them
 477 void Blocks::dfsVisit(Variable *v, list<Variable*> *order) {
 478     v->visited=true;
 479     vector<Constraint*>::iterator it=v->out.begin();
 480     for(;it!=v->out.end();++it) {
 481         Constraint *c=*it;
 482         if(!c->right->visited) {
 483             dfsVisit(c->right, order);
 484         }
 485     }
 486 #ifdef LIBVPSC_LOGGING
 487     ofstream f(LOGFILE,ios::app);
 488     f<<"  order="<<*v<<endl;
 489 #endif
 490     order->push_front(v);
 491 }
 492 /*
 493  * Processes incoming constraints, most violated to least, merging with the
 494  * neighbouring (left) block until no more violated constraints are found
 495  */
 496 void Blocks::mergeLeft(Block *r) {
 497 #ifdef LIBVPSC_LOGGING
 498     ofstream f(LOGFILE,ios::app);
 499     f<<"mergeLeft called on "<<*r<<endl;
 500 #endif
 501     r->timeStamp=++blockTimeCtr;
 502     r->setUpInConstraints();
 503     Constraint *c=r->findMinInConstraint();
 504     while (c != NULL && c->slack()<0) {
 505 #ifdef LIBVPSC_LOGGING
 506         f<<"mergeLeft on constraint: "<<*c<<endl;
 507 #endif
 508         r->deleteMinInConstraint();
 509         Block *l = c->left->block;
 510         if (l->in==NULL) l->setUpInConstraints();
 511         double dist = c->right->offset - c->left->offset - c->gap;
 512         if (r->vars->size() < l->vars->size()) {
 513             dist=-dist;
 514             std::swap(l, r);
 515         }
 516         blockTimeCtr++;
 517         r->merge(l, c, dist);
 518         r->mergeIn(l);
 519         r->timeStamp=blockTimeCtr;
 520         removeBlock(l);
 521         c=r->findMinInConstraint();
 522     }
 523 #ifdef LIBVPSC_LOGGING
 524     f<<"merged "<<*r<<endl;
 525 #endif
 526 }
 527 /*
 528  * Symmetrical to mergeLeft
 529  */
 530 void Blocks::mergeRight(Block *l) {
 531 #ifdef LIBVPSC_LOGGING
 532     ofstream f(LOGFILE,ios::app);
 533     f<<"mergeRight called on "<<*l<<endl;
 534 #endif
 535     l->setUpOutConstraints();
 536     Constraint *c = l->findMinOutConstraint();
 537     while (c != NULL && c->slack()<0) {
 538 #ifdef LIBVPSC_LOGGING
 539         f<<"mergeRight on constraint: "<<*c<<endl;
 540 #endif
 541         l->deleteMinOutConstraint();
 542         Block *r = c->right->block;
 543         r->setUpOutConstraints();
 544         double dist = c->left->offset + c->gap - c->right->offset;
 545         if (l->vars->size() > r->vars->size()) {
 546             dist=-dist;
 547             std::swap(l, r);
 548         }
 549         l->merge(r, c, dist);
 550         l->mergeOut(r);
 551         removeBlock(r);
 552         c=l->findMinOutConstraint();
 553     }
 554 #ifdef LIBVPSC_LOGGING
 555     f<<"merged "<<*l<<endl;
 556 #endif
 557 }
 558 void Blocks::removeBlock(Block *doomed) {
 559     doomed->deleted=true;
 560     //erase(doomed);
 561 }
 562 void Blocks::cleanup() {
 563     vector<Block*> bcopy(begin(),end());
 564     for(vector<Block*>::iterator i=bcopy.begin();i!=bcopy.end();++i) {
 565         Block *b=*i;
 566         if(b->deleted) {
 567             erase(b);
 568             delete b;
 569         }
 570     }
 571 }
 572 /*
 573  * Splits block b across constraint c into two new blocks, l and r (c's left
 574  * and right sides respectively)
 575  */
 576 void Blocks::split(Block *b, Block *&l, Block *&r, Constraint *c) {
 577     b->split(l,r,c);
 578 #ifdef LIBVPSC_LOGGING
 579     ofstream f(LOGFILE,ios::app);
 580     f<<"Split left: "<<*l<<endl;
 581     f<<"Split right: "<<*r<<endl;
 582 #endif
 583     r->posn = b->posn;
 584     //COLA_ASSERT(r->weight!=0);
 585     //r->wposn = r->posn * r->weight;
 586     mergeLeft(l);
 587     // r may have been merged!
 588     r = c->right->block;
 589     r->updateWeightedPosition();
 590     //r->posn = r->wposn / r->weight;
 591     mergeRight(r);
 592     removeBlock(b);
 593
 594     insert(l);
 595     insert(r);
 596     COLA_ASSERT(__NOTNAN(l->posn));
 597     COLA_ASSERT(__NOTNAN(r->posn));
 598 }
 599 /*
 600  * returns the cost total squared distance of variables from their desired
 601  * positions
 602  */
 603 double Blocks::cost() {
 604     double c = 0;
 605     for(set<Block*>::iterator i=begin();i!=end();++i) {
 606         c += (*i)->cost();
 607     }
 608     return c;
 609 }
 610
 611 void PositionStats::addVariable(Variable* v) {
 612     double ai=scale/v->scale;
 613     double bi=v->offset/v->scale;
 614     double wi=v->weight;
 615     AB+=wi*ai*bi;
 616     AD+=wi*ai*v->desiredPosition;
 617     A2+=wi*ai*ai;
 618     /*
 619 #ifdef LIBVPSC_LOGGING
 620     ofstream f(LOGFILE,ios::app);
 621     f << "adding v[" << v->id << "], blockscale=" << scale << ", despos="
 622       << v->desiredPosition << ", ai=" << ai << ", bi=" << bi
 623       << ", AB=" << AB << ", AD=" << AD << ", A2=" << A2;
 624 #endif
 625 */
 626 }
 627 void Block::addVariable(Variable* v) {
 628     v->block=this;
 629     vars->push_back(v);
 630     if(ps.A2==0) ps.scale=v->scale;
 631     //weight+= v->weight;
 632     //wposn += v->weight * (v->desiredPosition - v->offset);
 633     //posn=wposn/weight;
 634     ps.addVariable(v);
 635     posn=(ps.AD - ps.AB) / ps.A2;
 636     COLA_ASSERT(__NOTNAN(posn));
 637     /*
 638 #ifdef LIBVPSC_LOGGING
 639     ofstream f(LOGFILE,ios::app);
 640     f << ", posn=" << posn << endl;
 641 #endif
 642 */
 643 }
 644 Block::Block(Variable* const v)
 645     : vars(new vector<Variable*>)
 646     , posn(0)
 647     //, weight(0)
 648     //, wposn(0)
 649     , deleted(false)
 650     , timeStamp(0)
 651     , in(NULL)
 652     , out(NULL)
 653 {
 654     if(v!=NULL) {
 655         v->offset=0;
 656         addVariable(v);
 657     }
 658 }
 659
 660 void Block::updateWeightedPosition() {
 661     //wposn=0;
 662     ps.AB=ps.AD=ps.A2=0;
 663     for (Vit v=vars->begin();v!=vars->end();++v) {
 664         //wposn += ((*v)->desiredPosition - (*v)->offset) * (*v)->weight;
 665         ps.addVariable(*v);
 666     }
 667     posn=(ps.AD - ps.AB) / ps.A2;
 668     COLA_ASSERT(__NOTNAN(posn));
 669 #ifdef LIBVPSC_LOGGING
 670     ofstream f(LOGFILE,ios::app);
 671     f << ", posn=" << posn << endl;
 672 #endif
 673 }
 674 Block::~Block(void)
 675 {
 676     delete vars;
 677     delete in;
 678     delete out;
 679 }
 680 void Block::setUpInConstraints() {
 681     setUpConstraintHeap(in,true);
 682 }
 683 void Block::setUpOutConstraints() {
 684     setUpConstraintHeap(out,false);
 685 }
 686 void Block::setUpConstraintHeap(Heap* &h,bool in) {
 687     delete h;
 688     h = new Heap();
 689     for (Vit i=vars->begin();i!=vars->end();++i) {
 690         Variable *v=*i;
 691         vector<Constraint*> *cs=in?&(v->in):&(v->out);
 692         for (Cit j=cs->begin();j!=cs->end();++j) {
 693             Constraint *c=*j;
 694             c->timeStamp=blockTimeCtr;
 695             if (c->left->block != this && in || c->right->block != this && !in) {
 696                 h->push(c);
 697             }
 698         }
 699     }
 700 }
 701 Block* Block::merge(Block* b, Constraint* c) {
 702 #ifdef LIBVPSC_LOGGING
 703     ofstream f(LOGFILE,ios::app);
 704     f<<"  merging on: "<<*c<<",c->left->offset="<<c->left->offset<<",c->right->offset="<<c->right->offset<<endl;
 705 #endif
 706     double dist = c->right->offset - c->left->offset - c->gap;
 707     Block *l=c->left->block;
 708     Block *r=c->right->block;
 709     if (l->vars->size() < r->vars->size()) {
 710         r->merge(l,c,dist);
 711     } else {
 712                l->merge(r,c,-dist);
 713     }
 714     Block* mergeBlock=b->deleted?this:b;
 715 #ifdef LIBVPSC_LOGGING
 716     f<<"  merged block="<<*mergeBlock<<endl;
 717 #endif
 718     return mergeBlock;
 719 }
 720 /*
 721  * Merges b into this block across c.  Can be either a
 722  * right merge or a left merge
 723  * @param b block to merge into this
 724  * @param c constraint being merged
 725  * @param distance separation required to satisfy c
 726  */
 727 void Block::merge(Block *b, Constraint *c, double dist) {
 728 #ifdef LIBVPSC_LOGGING
 729     ofstream f(LOGFILE,ios::app);
 730     f<<"    merging: "<<*b<<"dist="<<dist<<endl;
 731 #endif
 732     c->active=true;
 733     //wposn+=b->wposn-dist*b->weight;
 734     //weight+=b->weight;
 735     for(Vit i=b->vars->begin();i!=b->vars->end();++i) {
 736         Variable *v=*i;
 737         //v->block=this;
 738         //vars->push_back(v);
 739         v->offset+=dist;
 740         addVariable(v);
 741     }
 742 #ifdef LIBVPSC_LOGGING
 743     for(Vit i=vars->begin();i!=vars->end();++i) {
 744         Variable *v=*i;
 745         f<<"    v["<<v->id<<"]: d="<<v->desiredPosition
 746             <<" a="<<v->scale<<" o="<<v->offset
 747             <<endl;
 748     }
 749     f<<"  AD="<<ps.AD<<" AB="<<ps.AB<<" A2="<<ps.A2<<endl;
 750 #endif
 751     //posn=wposn/weight;
 752     //COLA_ASSERT(wposn==ps.AD - ps.AB);
 753     posn=(ps.AD - ps.AB) / ps.A2;
 754     COLA_ASSERT(__NOTNAN(posn));
 755     b->deleted=true;
 756 }
 757
 758 void Block::mergeIn(Block *b) {
 759 #ifdef LIBVPSC_LOGGING
 760     ofstream f(LOGFILE,ios::app);
 761     f<<"  merging constraint heaps... "<<endl;
 762 #endif
 763     // We check the top of the heaps to remove possible internal constraints
 764     findMinInConstraint();
 765     b->findMinInConstraint();
 766     while (!b->in->empty())
 767     {
 768         in->push(b->in->top());
 769         b->in->pop();
 770     }
 771 #ifdef LIBVPSC_LOGGING
 772     f<<"  merged heap: "<<*in<<endl;
 773 #endif
 774 }
 775 void Block::mergeOut(Block *b) {
 776     findMinOutConstraint();
 777     b->findMinOutConstraint();
 778     while (!b->out->empty())
 779     {
 780         out->push(b->out->top());
 781         b->out->pop();
 782     }
 783 }
 784 Constraint *Block::findMinInConstraint() {
 785     Constraint *v = NULL;
 786     vector<Constraint*> outOfDate;
 787     while (!in->empty()) {
 788         v = in->top();
 789         Block *lb=v->left->block;
 790         Block *rb=v->right->block;
 791         // rb may not be this if called between merge and mergeIn
 792 #ifdef LIBVPSC_LOGGING
 793         ofstream f(LOGFILE,ios::app);
 794         f<<"  checking constraint ... "<<*v;
 795         f<<"    timestamps: left="<<lb->timeStamp<<" right="<<rb->timeStamp<<" constraint="<<v->timeStamp<<endl;
 796 #endif
 797         if(lb == rb) {
 798             // constraint has been merged into the same block
 799 #ifdef LIBVPSC_LOGGING
 800             if(v->slack()<0) {
 801                 f<<"  violated internal constraint found! "<<*v<<endl;
 802                 f<<"     lb="<<*lb<<endl;
 803                 f<<"     rb="<<*rb<<endl;
 804             }
 805 #endif
 806             in->pop();
 807 #ifdef LIBVPSC_LOGGING
 808             f<<" ... skipping internal constraint"<<endl;
 809 #endif
 810         } else if(v->timeStamp < lb->timeStamp) {
 811             // block at other end of constraint has been moved since this
 812             in->pop();
 813             outOfDate.push_back(v);
 814 #ifdef LIBVPSC_LOGGING
 815             f<<"    reinserting out of date (reinsert later)"<<endl;
 816 #endif
 817         } else {
 818             break;
 819         }
 820     }
 821     for(Cit i=outOfDate.begin();i!=outOfDate.end();++i) {
 822         v=*i;
 823         v->timeStamp=blockTimeCtr;
 824         in->push(v);
 825     }
 826     if(in->empty()) {
 827         v=NULL;
 828     } else {
 829         v=in->top();
 830     }
 831     return v;
 832 }
 833 Constraint *Block::findMinOutConstraint() {
 834     if(out->empty()) return NULL;
 835     Constraint *v = out->top();
 836     while (v->left->block == v->right->block) {
 837         out->pop();
 838         if(out->empty()) return NULL;
 839         v = out->top();
 840     }
 841     return v;
 842 }
 843 void Block::deleteMinInConstraint() {
 844     in->pop();
 845 #ifdef LIBVPSC_LOGGING
 846     ofstream f(LOGFILE,ios::app);
 847     f<<"deleteMinInConstraint... "<<endl;
 848     f<<"  result: "<<*in<<endl;
 849 #endif
 850 }
 851 void Block::deleteMinOutConstraint() {
 852     out->pop();
 853 }
 854 inline bool Block::canFollowLeft(Constraint const* c, Variable const* last) const {
 855     return c->left->block==this && c->active && last!=c->left;
 856 }
 857 inline bool Block::canFollowRight(Constraint const* c, Variable const* last) const {
 858     return c->right->block==this && c->active && last!=c->right;
 859 }
 860
 861 // computes the derivative of v and the lagrange multipliers
 862 // of v's out constraints (as the recursive sum of those below.
 863 // Does not backtrack over u.
 864 // also records the constraint with minimum lagrange multiplier
 865 // in min_lm
 866 double Block::compute_dfdv(Variable* const v, Variable* const u,
 867                Constraint *&min_lm) {
 868     double dfdv=v->dfdv();
 869     for(Cit it=v->out.begin();it!=v->out.end();++it) {
 870         Constraint *c=*it;
 871         if(canFollowRight(c,u)) {
 872             c->lm=compute_dfdv(c->right,v,min_lm);
 873             dfdv+=c->lm*c->left->scale;
 874             if(!c->equality&&(min_lm==NULL||c->lm<min_lm->lm)) min_lm=c;
 875         }
 876     }
 877     for(Cit it=v->in.begin();it!=v->in.end();++it) {
 878         Constraint *c=*it;
 879         if(canFollowLeft(c,u)) {
 880             c->lm=-compute_dfdv(c->left,v,min_lm);
 881             dfdv-=c->lm*c->right->scale;
 882             if(!c->equality&&(min_lm==NULL||c->lm<min_lm->lm)) min_lm=c;
 883         }
 884     }
 885     return dfdv/v->scale;
 886 }
 887 double Block::compute_dfdv(Variable* const v, Variable* const u) {
 888     double dfdv = v->dfdv();
 889     for(Cit it = v->out.begin(); it != v->out.end(); ++it) {
 890         Constraint *c = *it;
 891         if(canFollowRight(c,u)) {
 892             c->lm =   compute_dfdv(c->right,v);
 893             dfdv += c->lm * c->left->scale;
 894         }
 895     }
 896     for(Cit it=v->in.begin();it!=v->in.end();++it) {
 897         Constraint *c = *it;
 898         if(canFollowLeft(c,u)) {
 899             c->lm = - compute_dfdv(c->left,v);
 900             dfdv -= c->lm * c->right->scale;
 901         }
 902     }
 903     return dfdv/v->scale;
 904 }
 905
 906 // The top level v and r are variables between which we want to find the
 907 // constraint with the smallest lm.
 908 // Similarly, m is initially NULL and is only assigned a value if the next
 909 // variable to be visited is r or if a possible min constraint is returned from
 910 // a nested call (rather than NULL).
 911 // Then, the search for the m with minimum lm occurs as we return from
 912 // the recursion (checking only constraints traversed left-to-right
 913 // in order to avoid creating any new violations).
 914 // We also do not consider equality constraints as potential split points
 915 bool Block::split_path(
 916     Variable* r,
 917     Variable* const v,
 918     Variable* const u,
 919     Constraint* &m,
 920     bool desperation=false
 921     )
 922 {
 923     for(Cit it(v->in.begin());it!=v->in.end();++it) {
 924         Constraint *c=*it;
 925         if(canFollowLeft(c,u)) {
 926 #ifdef LIBVPSC_LOGGING
 927     ofstream f(LOGFILE,ios::app);
 928     f<<"  left split path: "<<*c<<endl;
 929 #endif
 930             if(c->left==r) {
 931                 if(desperation&&!c->equality) m=c;
 932                 return true;
 933             } else {
 934                 if(split_path(r,c->left,v,m)) {
 935                     if(desperation && !c->equality && (!m||c->lm<m->lm)) {
 936                                m=c;
 937                     }
 938                     return true;
 939                 }
 940             }
 941         }
 942     }
 943     for(Cit it(v->out.begin());it!=v->out.end();++it) {
 944         Constraint *c=*it;
 945         if(canFollowRight(c,u)) {
 946 #ifdef LIBVPSC_LOGGING
 947     ofstream f(LOGFILE,ios::app);
 948     f<<"  right split path: "<<*c<<endl;
 949 #endif
 950             if(c->right==r) {
 951                 if(!c->equality) m=c;
 952                 return true;
 953             } else {
 954                 if(split_path(r,c->right,v,m)) {
 955                     if(!c->equality && (!m||c->lm<m->lm))
 956                                m=c;
 957                     return true;
 958                 }
 959             }
 960         }
 961     }
 962     return false;
 963 }
 964 /*
 965 Block::Pair Block::compute_dfdv_between(
 966         Variable* r, Variable* const v, Variable* const u,
 967         const Direction dir = NONE, bool changedDirection = false) {
 968     double dfdv=v->weight*(v->position() - v->desiredPosition);
 969     Constraint *m=NULL;
 970     for(Cit it(v->in.begin());it!=v->in.end();++it) {
 971         Constraint *c=*it;
 972         if(canFollowLeft(c,u)) {
 973             if(dir==RIGHT) {
 974                 changedDirection = true;
 975             }
 976             if(c->left==r) {
 977                        r=NULL;
 978                     if(!c->equality) m=c;
 979             }
 980             Pair p=compute_dfdv_between(r,c->left,v,
 981                     LEFT,changedDirection);
 982             dfdv -= c->lm = -p.first;
 983             if(r && p.second)
 984                 m = p.second;
 985         }
 986     }
 987     for(Cit it(v->out.begin());it!=v->out.end();++it) {
 988         Constraint *c=*it;
 989         if(canFollowRight(c,u)) {
 990             if(dir==LEFT) {
 991                 changedDirection = true;
 992             }
 993             if(c->right==r) {
 994                        r=NULL;
 995                     if(!c->equality) m=c;
 996             }
 997             Pair p=compute_dfdv_between(r,c->right,v,
 998                     RIGHT,changedDirection);
 999             dfdv += c->lm = p.first;
1000             if(r && p.second)
1001                 m = changedDirection && !c->equality && c->lm < p.second->lm
1002                     ? c
1003                     : p.second;
1004         }
1005     }
1006     return Pair(dfdv,m);
1007 }
1008 */
1009
1010 // resets LMs for all active constraints to 0 by
1011 // traversing active constraint tree starting from v,
1012 // not back tracking over u
1013 void Block::reset_active_lm(Variable* const v, Variable* const u) {
1014     for(Cit it=v->out.begin();it!=v->out.end();++it) {
1015         Constraint *c=*it;
1016         if(canFollowRight(c,u)) {
1017             c->lm=0;
1018             reset_active_lm(c->right,v);
1019         }
1020     }
1021     for(Cit it=v->in.begin();it!=v->in.end();++it) {
1022         Constraint *c=*it;
1023         if(canFollowLeft(c,u)) {
1024             c->lm=0;
1025             reset_active_lm(c->left,v);
1026         }
1027     }
1028 }
1029 void Block::list_active(Variable* const v, Variable* const u) {
1030     for(Cit it=v->out.begin();it!=v->out.end();++it) {
1031         Constraint *c=*it;
1032         if(canFollowRight(c,u)) {
1033 #ifdef LIBVPSC_LOGGING
1034     ofstream f(LOGFILE,ios::app);
1035     f<<"  "<<*c<<endl;
1036 #endif
1037             list_active(c->right,v);
1038         }
1039     }
1040     for(Cit it=v->in.begin();it!=v->in.end();++it) {
1041         Constraint *c=*it;
1042         if(canFollowLeft(c,u)) {
1043 #ifdef LIBVPSC_LOGGING
1044     ofstream f(LOGFILE,ios::app);
1045     f<<"  "<<*c<<endl;
1046 #endif
1047             list_active(c->left,v);
1048         }
1049     }
1050 }
1051 /*
1052  * finds the constraint with the minimum lagrange multiplier, that is, the constraint
1053  * that most wants to split
1054  */
1055 Constraint *Block::findMinLM() {
1056     Constraint *min_lm=NULL;
1057     reset_active_lm(vars->front(),NULL);
1058     compute_dfdv(vars->front(),NULL,min_lm);
1059 #ifdef LIBVPSC_LOGGING
1060     ofstream f(LOGFILE,ios::app);
1061     f<<"  langrangians: "<<endl;
1062     list_active(vars->front(),NULL);
1063 #endif
1064     return min_lm;
1065 }
1066 Constraint *Block::findMinLMBetween(Variable* const lv, Variable* const rv) {
1067     reset_active_lm(vars->front(),NULL);
1068     compute_dfdv(vars->front(),NULL);
1069     Constraint *min_lm=NULL;
1070     split_path(rv,lv,NULL,min_lm);
1071 #if 0
1072     if(min_lm==NULL) {
1073         split_path(rv,lv,NULL,min_lm,true);
1074     }
1075 #else
1076     if(min_lm==NULL) {
1077         fprintf(stderr,"Couldn't find split point!\n");
1078         UnsatisfiableException e;
1079         getActivePathBetween(e.path,lv,rv,NULL);
1080         throw e;
1081     }
1082     COLA_ASSERT(min_lm!=NULL);
1083 #endif
1084     return min_lm;
1085 }
1086
1087 // populates block b by traversing the active constraint tree adding variables as they're
1088 // visited.  Starts from variable v and does not backtrack over variable u.
1089 void Block::populateSplitBlock(Block *b, Variable* v, Variable const* u) {
1090     b->addVariable(v);
1091     for (Cit c=v->in.begin();c!=v->in.end();++c) {
1092         if (canFollowLeft(*c,u))
1093             populateSplitBlock(b, (*c)->left, v);
1094     }
1095     for (Cit c=v->out.begin();c!=v->out.end();++c) {
1096         if (canFollowRight(*c,u))
1097             populateSplitBlock(b, (*c)->right, v);
1098     }
1099 }
1100 /*
1101  * Returns the active path between variables u and v... not back tracking over w
1102  */
1103 bool Block::getActivePathBetween(Constraints& path, Variable const* u,
1104                Variable const* v, Variable const *w) const {
1105     if(u==v) return true;
1106     for (Cit_const c=u->in.begin();c!=u->in.end();++c) {
1107         if (canFollowLeft(*c,w)) {
1108             if(getActivePathBetween(path, (*c)->left, v, u)) {
1109                 path.push_back(*c);
1110                 return true;
1111             }
1112         }
1113     }
1114     for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
1115         if (canFollowRight(*c,w)) {
1116             if(getActivePathBetween(path, (*c)->right, v, u)) {
1117                 path.push_back(*c);
1118                 return true;
1119             }
1120         }
1121     }
1122     return false;
1123 }
1124 // Search active constraint tree from u to see if there is a directed path to v.
1125 // Returns true if path is found with all constraints in path having their visited flag
1126 // set true.
1127 bool Block::isActiveDirectedPathBetween(Variable const* u, Variable const* v) const {
1128     if(u==v) return true;
1129     for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
1130         if(canFollowRight(*c,NULL)) {
1131             if(isActiveDirectedPathBetween((*c)->right,v)) {
1132                 return true;
1133             }
1134         }
1135     }
1136     return false;
1137 }
1138 bool Block::getActiveDirectedPathBetween(
1139         Constraints& path, Variable const* u, Variable const* v) const {
1140     if(u==v) return true;
1141     for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
1142         if(canFollowRight(*c,NULL)) {
1143             if(getActiveDirectedPathBetween(path,(*c)->right,v)) {
1144                 path.push_back(*c);
1145                 return true;
1146             }
1147         }
1148     }
1149     return false;
1150 }
1151 /*
1152  * Block needs to be split because of a violated constraint between vl and vr.
1153  * We need to search the active constraint tree between l and r and find the constraint
1154  * with min lagrangrian multiplier and split at that point.
1155  * Returns the split constraint
1156  */
1157 Constraint* Block::splitBetween(Variable* const vl, Variable* const vr,
1158                Block* &lb, Block* &rb) {
1159 #ifdef LIBVPSC_LOGGING
1160     ofstream f(LOGFILE,ios::app);
1161     f<<"  need to split between: "<<*vl<<" and "<<*vr<<endl;
1162 #endif
1163     Constraint *c=findMinLMBetween(vl, vr);
1164 #ifdef LIBVPSC_LOGGING
1165     f<<"  going to split on: "<<*c<<endl;
1166 #endif
1167     if(c!=NULL) {
1168         split(lb,rb,c);
1169         deleted = true;
1170     }
1171     return c;
1172 }
1173
1174 /*
1175  * Creates two new blocks, l and r, and splits this block across constraint c,
1176  * placing the left subtree of constraints (and associated variables) into l
1177  * and the right into r.
1178  */
1179 void Block::split(Block* &l, Block* &r, Constraint* c) {
1180     c->active=false;
1181     l=new Block();
1182     populateSplitBlock(l,c->left,c->right);
1183     //COLA_ASSERT(l->weight>0);
1184     r=new Block();
1185     populateSplitBlock(r,c->right,c->left);
1186     //COLA_ASSERT(r->weight>0);
1187 }
1188
1189 /*
1190  * Computes the cost (squared euclidean distance from desired positions) of the
1191  * current positions for variables in this block
1192  */
1193 double Block::cost() {
1194     double c = 0;
1195     for (Vit v=vars->begin();v!=vars->end();++v) {
1196         double diff = (*v)->position() - (*v)->desiredPosition;
1197         c += (*v)->weight * diff * diff;
1198     }
1199     return c;
1200 }
1201 ostream& operator <<(ostream &os, const Block& b)
1202 {
1203     os<<"Block(posn="<<b.posn<<"):";
1204     for(Block::Vit v=b.vars->begin();v!=b.vars->end();++v) {
1205         os<<" "<<**v;
1206     }
1207     if(b.deleted) {
1208         os<<" Deleted!";
1209     }
1210     return os;
1211 }
1212
1213 Constraint::Constraint(Variable *left, Variable *right, double gap, bool equality)
1214 : left(left),
1215   right(right),
1216   gap(gap),
1217   timeStamp(0),
1218   active(false),
1219   equality(equality),
1220   unsatisfiable(false)
1221 {
1222     // In hindsight I think it's probably better to build the constraint DAG
1223     // (by creating variable in/out lists) when needed, rather than in advance
1224     //left->out.push_back(this);
1225     //right->in.push_back(this);
1226 }
1227 Constraint::~Constraint() {
1228     // see constructor: the following is just way too slow.
1229     // Better to create a
1230     // new DAG on demand than maintain the lists dynamically.
1231     //Constraints::iterator i;
1232     //for(i=left->out.begin(); i!=left->out.end(); i++) {
1233         //if(*i==this) break;
1234     //}
1235     //left->out.erase(i);
1236     //for(i=right->in.begin(); i!=right->in.end(); i++) {
1237         //if(*i==this) break;
1238     //}
1239     //right->in.erase(i);
1240 }
1241 double Constraint::slack() const {
1242     return unsatisfiable ? DBL_MAX
1243            : right->scale * right->position()
1244          - gap - left->scale * left->position();
1245 }
1246 std::ostream& operator <<(std::ostream &os, const Constraint &c)
1247 {
1248     if(&c==NULL) {
1249         os<<"NULL";
1250     } else {
1251         const char *type=c.equality?"=":"<=";
1252         std::ostringstream lscale, rscale;
1253         if(c.left->scale!=1) {
1254             lscale << c.left->scale << "*";
1255         }
1256         if(c.right->scale!=1) {
1257             rscale << c.right->scale << "*";
1258         }
1259         os<<lscale.str()<<*c.left<<"+"<<c.gap<<type<<rscale.str()<<*c.right;
1260         if(c.left->block&&c.right->block)
1261             os<<"("<<c.slack()<<")"<<(c.active?"-active":"")
1262                 <<"(lm="<<c.lm<<")";
1263         else
1264             os<<"(vars have no position)";
1265     }
1266     return os;
1267 }
1268
1269 bool CompareConstraints::operator() (
1270     Constraint *const &l, Constraint *const &r
1271 ) const {
1272     double const sl =
1273         l->left->block->timeStamp > l->timeStamp
1274         ||l->left->block==l->right->block
1275         ?-DBL_MAX:l->slack();
1276     double const sr =
1277         r->left->block->timeStamp > r->timeStamp
1278         ||r->left->block==r->right->block
1279         ?-DBL_MAX:r->slack();
1280     if(sl==sr) {
1281         // arbitrary choice based on id
1282         if(l->left->id==r->left->id) {
1283             if(l->right->id<r->right->id) return true;
1284             return false;
1285         }
1286         if(l->left->id<r->left->id) return true;
1287         return false;
1288     }
1289     return sl > sr;
1290 }
1291
1292 std::ostream& operator <<(std::ostream &os, const Variable &v) {
1293     if(v.block)
1294         os << "(" << v.id << "=" << v.position() << ")";
1295     else
1296         os << "(" << v.id << "=" << v.desiredPosition << ")";
1297     return os;
1298 }
1299
1300 }