lily/qlpsolve.cc

   1 /*
   2   qlpsolve.cc -- implement Active_constraints, Inactive_iter
   3
   4   source file of the LilyPond music typesetter
   5
   6   (c) 1996, 1997 Han-Wen Nienhuys <hanwen@stack.nl>
   7
   8   TODO:
   9   try fixed point arithmetic, to speed up lily.
  10   */
  11
  12 #include "qlpsolve.hh"
  13 #include "const.hh"
  14 #include "debug.hh"
  15 #include "choleski.hh"
  16
  17 const Real TOL=1e-2;            // roughly 1/10 mm
  18
  19 String
  20 Active_constraints::status() const
  21 {
  22     String s("Active|Inactive [");
  23     for (int i=0; i< active.size(); i++) {
  24         s += String(active[i]) + " ";
  25     }
  26
  27     s+="| ";
  28     for (int i=0; i< inactive.size(); i++) {
  29         s += String(inactive[i]) + " ";
  30     }
  31     s+="]";
  32
  33     return s;
  34 }
  35
  36 void
  37 Active_constraints::OK()
  38 {
  39 #ifndef NDEBUG
  40     H.OK();
  41     A.OK();
  42     assert(active.size() +inactive.size() == opt->cons.size());
  43     assert(H.dim() == opt->dim());
  44     assert(active.size() == A.rows());
  45     Array<int> allcons;
  46
  47     for (int i=0; i < opt->cons.size(); i++)
  48         allcons.push(0);
  49     for (int i=0; i < active.size(); i++) {
  50         int j = active[i];
  51         allcons[j]++;
  52     }
  53     for (int i=0; i < inactive.size(); i++) {
  54         int j = inactive[i];
  55         allcons[j]++;
  56     }
  57     for (int i=0; i < allcons.size(); i++)
  58         assert(allcons[i] == 1);
  59 #endif
  60 }
  61
  62 Vector
  63 Active_constraints::get_lagrange(Vector gradient)
  64 {
  65     Vector l(A*gradient);
  66
  67     return l;
  68 }
  69
  70 void
  71 Active_constraints::add(int k)
  72 {
  73     // add indices
  74     int cidx=inactive[k];
  75     active.push(cidx);
  76
  77     inactive.swap(k,inactive.size()-1);
  78     inactive.pop();
  79
  80     Vector a( opt->cons[cidx] );
  81     // update of matrices
  82     Vector Ha = H*a;
  83     Real aHa = a*Ha;
  84     Vector addrow(Ha.dim());
  85     if (abs(aHa) > EPS) {
  86         /*
  87           a != 0, so if Ha = O(EPS), then
  88           Ha * aH / aHa = O(EPS^2/EPS)
  89
  90           if H*a == 0, the constraints are dependent.
  91           */
  92         H -= Matrix(Ha/aHa , Ha);
  93
  94
  95         /*
  96           sorry, don't know how to justify this. ..
  97           */
  98         addrow=Ha;
  99         addrow/= aHa;
 100         A -= Matrix(A*a, addrow);
 101         A.insert_row(addrow,A.rows());
 102     }else
 103         WARN << "degenerate constraints";
 104 }
 105
 106 void
 107 Active_constraints::drop(int k)
 108 {
 109     int q=active.size()-1;
 110
 111         // drop indices
 112     inactive.push(active[k]);
 113     active.swap(k,q);
 114     A.swap_rows(k,q);
 115     active.pop();
 116
 117     Vector a(A.row(q));
 118     if (a.norm() > EPS) {
 119         /*
 120
 121          */
 122         Real q = a*opt->quad*a;
 123         H += Matrix(a,a/q);
 124         A -= A*opt->quad*Matrix(a,a/q);
 125     }else
 126         WARN << "degenerate constraints";
 127 #ifndef NDEBUG
 128     Vector rem_row(A.row(q));
 129     assert(rem_row.norm() < EPS);
 130 #endif
 131
 132     A.delete_row(q);
 133 }
 134
 135
 136 Active_constraints::Active_constraints(Ineq_constrained_qp const *op)
 137     :       A(0,op->dim()),
 138             H(op->dim()),
 139             opt(op)
 140 {
 141     for (int i=0; i < op->cons.size(); i++)
 142         inactive.push(i);
 143     Choleski_decomposition chol(op->quad);
 144     H=chol.inverse();
 145 }
 146
 147 /** Find the optimum which is in the planes generated by the active
 148     constraints.
 149     */
 150 Vector
 151 Active_constraints::find_active_optimum(Vector g)
 152 {
 153     return H*g;
 154 }
 155
 156 /* *************************************************************** */
 157
 158 int
 159 min_elt_index(Vector v)
 160 {
 161     Real m=INFTY; int idx=-1;
 162     for (int i = 0; i < v.dim(); i++){
 163         if (v(i) < m) {
 164             idx = i;
 165             m = v(i);
 166         }
 167         assert(v(i) <= INFTY);
 168     }
 169     return idx;
 170 }
 171
 172
 173 /**the numerical solving. Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
 174     Prentice Hall.
 175
 176     Section 13.3
 177
 178     This is a "projected gradient" algorithm. Starting from a point x
 179     the next point is found in a direction determined by projecting
 180     the gradient onto the active constraints.  (well, not really the
 181     gradient. The optimal solution obeying the active constraints is
 182     tried. This is why H = Q^-1 in initialisation) )
 183
 184
 185     */
 186 Vector
 187 Ineq_constrained_qp::solve(Vector start) const
 188 {
 189     if (!dim())
 190         return Vector(0);
 191
 192     Active_constraints act(this);
 193
 194
 195     act.OK();
 196
 197
 198     Vector x(start);
 199     Vector gradient=quad*x+lin;
 200 //    Real fvalue = x*quad*x/2 + lin*x + const_term;
 201 // it's no use.
 202
 203     Vector last_gradient(gradient);
 204     int iterations=0;
 205
 206     while (iterations++ < MAXITER) {
 207         Vector direction= - act.find_active_optimum(gradient);
 208
 209         mtor << "gradient "<< gradient<< "\ndirection " << direction<<"\n";
 210
 211         if (direction.norm() > EPS) {
 212             mtor << act.status() << '\n';
 213
 214             Real minalf = INFTY;
 215
 216             Inactive_iter minidx(act);
 217
 218
 219             /*
 220     we know the optimum on this "hyperplane". Check if we
 221     bump into the edges of the simplex
 222     */
 223
 224             for (Inactive_iter ia(act); ia.ok(); ia++) {
 225
 226                 if (ia.vec() * direction >= 0)
 227                     continue;
 228                 Real alfa= - (ia.vec()*x - ia.rhs())/
 229                     (ia.vec()*direction);
 230
 231                 if (minalf > alfa) {
 232                     minidx = ia;
 233                     minalf = alfa;
 234                 }
 235             }
 236             Real unbounded_alfa = 1.0;
 237             Real optimal_step = min(minalf, unbounded_alfa);
 238
 239             Vector deltax=direction * optimal_step;
 240             x += deltax;
 241             gradient += optimal_step * (quad * deltax);
 242
 243             mtor << "step = " << optimal_step<< " (|dx| = " <<
 244                 deltax.norm() << ")\n";
 245
 246             if (minalf < unbounded_alfa) {
 247                 /* bumped into an edge. try again, in smaller space. */
 248                 act.add(minidx.idx());
 249                 mtor << "adding cons "<< minidx.idx()<<'\n';
 250                 continue;
 251             }
 252             /*ASSERT: we are at optimal solution for this "plane"*/
 253
 254
 255         }
 256
 257         Vector lagrange_mult=act.get_lagrange(gradient);
 258         int m= min_elt_index(lagrange_mult);
 259
 260         if (m>=0 && lagrange_mult(m) > 0) {
 261             break;              // optimal sol.
 262         } else if (m<0) {
 263             assert(gradient.norm() < EPS) ;
 264
 265             break;
 266         }
 267
 268         mtor << "dropping cons " << m<<'\n';
 269         act.drop(m);
 270     }
 271     if (iterations >= MAXITER)
 272         WARN<<"didn't converge!\n";
 273
 274     mtor <<  ": found " << x<<" in " << iterations <<" iterations\n";
 275     assert_solution(x);
 276     return x;
 277 }
 278
 279