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--1999 Han-Wen Nienhuys <hanwen@cs.uu.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 const int MAXDEGEN=5;
  21
  22 /*
  23   assume x (idx) == value, and adjust constraints, lin and quad accordingly
  24
  25   TODO: add const_term
  26   */
  27 void
  28 Ineq_constrained_qp::eliminate_var (int idx, Real value)
  29 {
  30   Vector row (quad_.row (idx));
  31   row*= value;
  32
  33   quad_.delete_row (idx);
  34
  35   quad_.delete_column (idx);
  36
  37   lin_.del (idx);
  38   row.del (idx);
  39   lin_ +=row ;
  40
  41   for (int i=0; i < cons_.size(); i++)
  42     {
  43       consrhs_[i] -= cons_[i](idx) *value;
  44       cons_[i].del (idx);
  45     }
  46 }
  47
  48 void
  49 Ineq_constrained_qp::add_inequality_cons (Vector c, double r)
  50 {
  51   cons_.push (c);
  52   consrhs_.push (r);
  53 }
  54
  55 Ineq_constrained_qp::Ineq_constrained_qp (int novars):
  56   quad_ (novars),
  57   lin_ (novars),
  58   const_term_ (0.0)
  59 {
  60 }
  61
  62 void
  63 Ineq_constrained_qp::OK() const
  64 {
  65 #if !defined (NDEBUG) && defined (PARANOID)
  66   assert (cons_.size() == consrhs_.size ());
  67   Matrix Qdif= quad_ - quad_.transposed();
  68   assert (Qdif.norm()/quad_.norm () < EPS);
  69 #endif
  70 }
  71
  72
  73 Real
  74 Ineq_constrained_qp::eval (Vector v)
  75 {
  76   return v * quad_ * v + lin_ * v + const_term_;
  77 }
  78
  79
  80 int
  81 min_elt_index (Vector v)
  82 {
  83   Real m=infinity_f;
  84   int idx=-1;
  85   for (int i = 0; i < v.dim(); i++)
  86     {
  87       if (v (i) < m)
  88         {
  89           idx = i;
  90           m = v (i);
  91         }
  92       assert (v (i) <= infinity_f);
  93     }
  94   return idx;
  95 }
  96
  97
  98 /**the numerical solving. Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
  99   Prentice Hall.
 100
 101   Section 13.3
 102
 103   This is a "projected gradient" algorithm. Starting from a point x
 104   the next point is found in a direction determined by projecting
 105   the gradient onto the active constraints.  (well, not really the
 106   gradient. The optimal solution obeying the active constraints is
 107   tried. This is why H = Q^-1 in initialisation))
 108
 109
 110   */
 111 Vector
 112 Ineq_constrained_qp::constraint_solve (Vector start) const
 113 {
 114   if (!dim())
 115     return Vector (0);
 116
 117   Active_constraints act (this);
 118   act.OK();
 119
 120
 121   Vector x (start);
 122   Vector gradient=quad_*x+lin_;
 123
 124
 125   //    Real fvalue = x*quad_*x/2 + lin*x + const_term;// it's no use.
 126   Vector last_gradient (gradient);
 127   int iterations=0;
 128
 129   while (iterations++ < MAXITER && act.degenerate_count_i_ < MAXDEGEN)
 130     {
 131       if (experimental_features_global_b)
 132         assert_solution (x);
 133
 134       Vector direction= - act.find_active_optimum (gradient);
 135
 136       DOUT << "gradient "<< gradient<< "\ndirection " << direction<< '\n';
 137
 138       if (direction.norm() > EPS)
 139         {
 140           DOUT << act.status() << '\n';
 141
 142           Real unbounded_alfa = 1.0;
 143           Real minalf = unbounded_alfa;
 144
 145           Inactive_iter minidx (act);
 146
 147
 148           /*
 149             we know the optimum on this "hyperplane". Check if we
 150             bump into the edges of the simplex
 151             */
 152
 153           for (Inactive_iter ia (act); ia.ok(); ia++)
 154             {
 155               Real dot = ia.vec() * direction;
 156               Real mindot =  (experimental_features_global_b)
 157                 ? -EPS
 158                 : 0;
 159
 160               if (dot >= mindot)
 161                 continue;
 162
 163
 164               Real numerator = ia.rhs () - ia.vec()*x;
 165               if (numerator >= 0)
 166                 {
 167                   if (numerator > EPS)
 168                     {
 169                       warning (_f ("Ineq_constrained_qp::solve (): Constraint off by %f", numerator));
 170                       act.degenerate_count_i_ ++;
 171                     }
 172                   minalf = -numerator;
 173                   minidx = ia;
 174                   break;
 175                 }
 176
 177               Real alfa = numerator / dot;
 178
 179
 180               if (minalf > alfa)
 181                 {
 182                   minidx = ia;
 183                   minalf = alfa;
 184                 }
 185             }
 186
 187           Real optimal_step = minalf;
 188
 189           Vector deltax = direction * optimal_step;
 190           x += deltax;
 191           gradient += optimal_step * (quad_ * deltax);
 192
 193           DOUT << "step = " << optimal_step << " (|dx| = " <<
 194             to_str (deltax.norm()) << ")\n";
 195
 196           if (minalf < unbounded_alfa)
 197             {
 198               /* bumped into an edge. try again, in smaller space. */
 199               act.add_constraint (minidx.idx());
 200               DOUT << "adding cons "<< minidx.idx () << '\n';
 201               continue;
 202             }
 203           /*ASSERT: we are at the optimal solution for this "plane"*/
 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           Real n =gradient.norm();
 216           if (n >= EPS)
 217             {
 218               programming_error ("Huh? Gradient should be zero ");
 219               act.degenerate_count_i_ ++;
 220             }
 221           else if (n <0)
 222             {
 223               programming_error ("Huh? Norm should be positive");
 224               act.degenerate_count_i_++;
 225             }
 226           break;
 227         }
 228
 229       DOUT << "dropping cons " << m << '\n';
 230       act.drop_constraint (m);
 231     }
 232   if (iterations >= MAXITER)
 233     WARN << _ ("Didn't converge!") << '\n';
 234   if (act.degenerate_count_i_ >= MAXDEGEN)
 235     WARN << _ ("too much degeneracy") << '\n';
 236   DOUT <<  ": found " << x << " in " << iterations <<" iterations\n";
 237   assert_solution (x);
 238   return x;
 239 }
 240
 241
 242 Vector
 243 Ineq_constrained_qp::solve (Vector start) const
 244 {
 245   /* no hassle if no constraints*/
 246   if (! cons_.size())
 247     {
 248       Choleski_decomposition chol (quad_);
 249       return - chol.solve (lin_);
 250     }
 251   else
 252     {
 253       return constraint_solve (start);
 254     }
 255 }
 256
 257 int
 258 Ineq_constrained_qp::dim () const
 259 {
 260   return lin_.dim();
 261 }
 262
 263
 264
 265
 266 void
 267 Ineq_constrained_qp::assert_solution (Vector sol) const
 268 {
 269   bool sol_b=true;
 270
 271   for (int i=0; sol_b && i < cons_.size(); i++)
 272     {
 273       Real R=cons_[i] * sol- consrhs_[i];
 274       if (R> -EPS)
 275         sol_b = false;
 276     }
 277 }
 278
 279 void
 280 Ineq_constrained_qp::print() const
 281 {
 282 #ifndef NPRINT
 283   DOUT << "Ineq_constrained_qp { " << '\n';
 284   DOUT << "Quad " << quad_;
 285   DOUT << "lin " << lin_ << '\n'
 286        << "const " << const_term_<< '\n';
 287   for (int i=0; i < cons_.size(); i++)
 288     {
 289       DOUT << "constraint["<<i<<"]: " << cons_[i] << " >= " << consrhs_[i];
 290       DOUT << '\n';
 291     }
 292   DOUT << "}\n";
 293 #endif
 294 }