lily/ineq-constrained-qp.cc

   1 /*
   2   ineq-constrained-qp.cc -- implement Ineq_constrained_qp
   3
   4   source file of the GNU LilyPond music typesetter
   5
   6   (c) 1996,1997 Han-Wen Nienhuys <hanwen@stack.nl>
   7 */
   8 #include "ineq-constrained-qp.hh"
   9 #include "qlpsolve.hh"
  10 #include "const.hh"
  11 #include "debug.hh"
  12 /*
  13   MAy be this also should go into a library
  14  */
  15
  16 const int MAXITER=100;          // qlpsolve.hh
  17
  18 /*
  19     assume x(idx) == value, and adjust constraints, lin and quad accordingly
  20
  21     TODO: add const_term
  22     */
  23 void
  24 Ineq_constrained_qp::eliminate_var(int idx, Real value)
  25 {
  26     Vector row(quad.row(idx));
  27     row*= value;
  28
  29     quad.delete_row(idx);
  30
  31     quad.delete_column(idx);
  32
  33     lin.del(idx);
  34     row.del(idx);
  35     lin +=row ;
  36
  37    for (int i=0; i < cons.size(); i++) {
  38       consrhs[i] -= cons[i](idx) *value;
  39       cons[i].del(idx);
  40    }
  41 }
  42
  43 void
  44 Ineq_constrained_qp::add_inequality_cons(Vector c, double r)
  45 {
  46     cons.push(c);
  47     consrhs.push(r);
  48 }
  49
  50 Ineq_constrained_qp::Ineq_constrained_qp(int novars):
  51     quad(novars),
  52     lin(novars),
  53     const_term (0.0)
  54 {
  55 }
  56
  57 void
  58 Ineq_constrained_qp::OK() const
  59 {
  60 #if !defined(NDEBUG) && defined(PARANOID)
  61     assert(cons.size() == consrhs.size());
  62     Matrix Qdif= quad - quad.transposed();
  63     assert(Qdif.norm()/quad.norm() < EPS);
  64 #endif
  65 }
  66
  67
  68 Real
  69 Ineq_constrained_qp::eval (Vector v)
  70 {
  71     return v * quad * v + lin * v + const_term;
  72 }
  73
  74
  75 int
  76 min_elt_index(Vector v)
  77 {
  78     Real m=INFTY_f;
  79     int idx=-1;
  80     for (int i = 0; i < v.dim(); i++){
  81         if (v(i) < m) {
  82             idx = i;
  83             m = v(i);
  84         }
  85         assert(v(i) <= INFTY_f);
  86     }
  87     return idx;
  88 }
  89
  90
  91 /**the numerical solving. Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
  92     Prentice Hall.
  93
  94     Section 13.3
  95
  96     This is a "projected gradient" algorithm. Starting from a point x
  97     the next point is found in a direction determined by projecting
  98     the gradient onto the active constraints.  (well, not really the
  99     gradient. The optimal solution obeying the active constraints is
 100     tried. This is why H = Q^-1 in initialisation) )
 101
 102
 103     */
 104 Vector
 105 Ineq_constrained_qp::solve(Vector start) const
 106 {
 107     if (!dim())
 108         return Vector(0);
 109     // experimental
 110 //    quad.try_set_band();
 111
 112     Active_constraints act(this);
 113
 114
 115     act.OK();
 116
 117
 118     Vector x(start);
 119     Vector gradient=quad*x+lin;
 120 //    Real fvalue = x*quad*x/2 + lin*x + const_term;
 121 // it's no use.
 122
 123     Vector last_gradient(gradient);
 124     int iterations=0;
 125
 126     while (iterations++ < MAXITER) {
 127         Vector direction= - act.find_active_optimum(gradient);
 128
 129         mtor << "gradient "<< gradient<< "\ndirection " << direction<<"\n";
 130
 131         if (direction.norm() > EPS) {
 132             mtor << act.status() << '\n';
 133
 134             Real minalf = INFTY_f;
 135
 136             Inactive_iter minidx(act);
 137
 138
 139             /*
 140     we know the optimum on this "hyperplane". Check if we
 141     bump into the edges of the simplex
 142     */
 143
 144             for (Inactive_iter ia(act); ia.ok(); ia++) {
 145
 146                 if (ia.vec() * direction >= 0)
 147                     continue;
 148                 Real alfa= - (ia.vec()*x - ia.rhs())/
 149                     (ia.vec()*direction);
 150
 151                 if (minalf > alfa) {
 152                     minidx = ia;
 153                     minalf = alfa;
 154                 }
 155             }
 156             Real unbounded_alfa = 1.0;
 157             Real optimal_step = min(minalf, unbounded_alfa);
 158
 159             Vector deltax=direction * optimal_step;
 160             x += deltax;
 161             gradient += optimal_step * (quad * deltax);
 162
 163             mtor << "step = " << optimal_step<< " (|dx| = " <<
 164                 deltax.norm() << ")\n";
 165
 166             if (minalf < unbounded_alfa) {
 167                 /* bumped into an edge. try again, in smaller space. */
 168                 act.add(minidx.idx());
 169                 mtor << "adding cons "<< minidx.idx()<<'\n';
 170                 continue;
 171             }
 172             /*ASSERT: we are at optimal solution for this "plane"*/
 173
 174
 175         }
 176
 177         Vector lagrange_mult=act.get_lagrange(gradient);
 178         int m= min_elt_index(lagrange_mult);
 179
 180         if (m>=0 && lagrange_mult(m) > 0) {
 181             break;              // optimal sol.
 182         } else if (m<0) {
 183             assert(gradient.norm() < EPS) ;
 184
 185             break;
 186         }
 187
 188         mtor << "dropping cons " << m<<'\n';
 189         act.drop(m);
 190     }
 191     if (iterations >= MAXITER)
 192         WARN<<"didn't converge!\n";
 193
 194     mtor <<  ": found " << x<<" in " << iterations <<" iterations\n";
 195     assert_solution(x);
 196     return x;
 197 }
 198
 199