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