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