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 }