source file of the GNU LilyPond music typesetter
- (c) 1996,1997 Han-Wen Nienhuys <hanwen@stack.nl>
+ (c) 1996, 1997--1998 Han-Wen Nienhuys <hanwen@cs.uu.nl>
*/
#include "ineq-constrained-qp.hh"
#include "qlpsolve.hh"
-#include "const.hh"
#include "debug.hh"
+#include "choleski.hh"
+#include "main.hh"
+
/*
- MAy be this also should go into a library
+ May be this also should go into a library
*/
const int MAXITER=100; // qlpsolve.hh
+const int MAXDEGEN=5;
+
/*
- assume x(idx) == value, and adjust constraints, lin and quad accordingly
+ assume x (idx) == value, and adjust constraints, lin and quad accordingly
- TODO: add const_term
- */
+ TODO: add const_term
+ */
void
-Ineq_constrained_qp::eliminate_var(int idx, Real value)
+Ineq_constrained_qp::eliminate_var (int idx, Real value)
{
- Vector row(quad.row(idx));
- row*= value;
+ Vector row (quad_.row (idx));
+ row*= value;
- quad.delete_row(idx);
+ quad_.delete_row (idx);
- quad.delete_column(idx);
+ quad_.delete_column (idx);
- lin.del(idx);
- row.del(idx);
- lin +=row ;
+ lin_.del (idx);
+ row.del (idx);
+ lin_ +=row ;
- for (int i=0; i < cons.size(); i++) {
- consrhs[i] -= cons[i](idx) *value;
- cons[i].del(idx);
- }
+ for (int i=0; i < cons_.size(); i++)
+ {
+ consrhs_[i] -= cons_[i](idx) *value;
+ cons_[i].del (idx);
+ }
}
void
-Ineq_constrained_qp::add_inequality_cons(Vector c, double r)
+Ineq_constrained_qp::add_inequality_cons (Vector c, double r)
{
- cons.push(c);
- consrhs.push(r);
+ cons_.push (c);
+ consrhs_.push (r);
}
-Ineq_constrained_qp::Ineq_constrained_qp(int novars):
- quad(novars),
- lin(novars),
- const_term (0.0)
+Ineq_constrained_qp::Ineq_constrained_qp (int novars):
+ quad_ (novars),
+ lin_ (novars),
+ const_term_ (0.0)
{
}
void
Ineq_constrained_qp::OK() const
{
-#if !defined(NDEBUG) && defined(PARANOID)
- assert(cons.size() == consrhs.size());
- Matrix Qdif= quad - quad.transposed();
- assert(Qdif.norm()/quad.norm() < EPS);
-#endif
+#if !defined (NDEBUG) && defined (PARANOID)
+ assert (cons_.size() == consrhs_.size ());
+ Matrix Qdif= quad_ - quad_.transposed();
+ assert (Qdif.norm()/quad_.norm () < EPS);
+#endif
}
-
+
Real
Ineq_constrained_qp::eval (Vector v)
{
- return v * quad * v + lin * v + const_term;
+ return v * quad_ * v + lin_ * v + const_term_;
}
int
-min_elt_index(Vector v)
+min_elt_index (Vector v)
{
- Real m=INFTY; int idx=-1;
- for (int i = 0; i < v.dim(); i++){
- if (v(i) < m) {
- idx = i;
- m = v(i);
+ Real m=infinity_f;
+ int idx=-1;
+ for (int i = 0; i < v.dim(); i++)
+ {
+ if (v (i) < m)
+ {
+ idx = i;
+ m = v (i);
}
- assert(v(i) <= INFTY);
+ assert (v (i) <= infinity_f);
}
- return idx;
+ return idx;
}
/**the numerical solving. Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
- Prentice Hall.
+ Prentice Hall.
- Section 13.3
+ Section 13.3
- This is a "projected gradient" algorithm. Starting from a point x
- the next point is found in a direction determined by projecting
- the gradient onto the active constraints. (well, not really the
- gradient. The optimal solution obeying the active constraints is
- tried. This is why H = Q^-1 in initialisation) )
+ This is a "projected gradient" algorithm. Starting from a point x
+ the next point is found in a direction determined by projecting
+ the gradient onto the active constraints. (well, not really the
+ gradient. The optimal solution obeying the active constraints is
+ tried. This is why H = Q^-1 in initialisation))
- */
+ */
Vector
-Ineq_constrained_qp::solve(Vector start) const
-{
- if (!dim())
- return Vector(0);
- // experimental
-// quad.try_set_band();
-
- Active_constraints act(this);
-
-
- act.OK();
-
-
- Vector x(start);
- Vector gradient=quad*x+lin;
-// Real fvalue = x*quad*x/2 + lin*x + const_term;
-// it's no use.
-
- Vector last_gradient(gradient);
- int iterations=0;
-
- while (iterations++ < MAXITER) {
- Vector direction= - act.find_active_optimum(gradient);
-
- mtor << "gradient "<< gradient<< "\ndirection " << direction<<"\n";
-
- if (direction.norm() > EPS) {
- mtor << act.status() << '\n';
-
- Real minalf = INFTY;
-
- Inactive_iter minidx(act);
-
-
- /*
- we know the optimum on this "hyperplane". Check if we
- bump into the edges of the simplex
- */
-
- for (Inactive_iter ia(act); ia.ok(); ia++) {
-
- if (ia.vec() * direction >= 0)
- continue;
- Real alfa= - (ia.vec()*x - ia.rhs())/
- (ia.vec()*direction);
-
- if (minalf > alfa) {
- minidx = ia;
- minalf = alfa;
+Ineq_constrained_qp::constraint_solve (Vector start) const
+{
+ if (!dim())
+ return Vector (0);
+
+ Active_constraints act (this);
+ act.OK();
+
+
+ Vector x (start);
+ Vector gradient=quad_*x+lin_;
+
+
+ // Real fvalue = x*quad_*x/2 + lin*x + const_term;// it's no use.
+ Vector last_gradient (gradient);
+ int iterations=0;
+
+ while (iterations++ < MAXITER && act.degenerate_count_i_ < MAXDEGEN)
+ {
+ if (experimental_features_global_b)
+ assert_solution (x);
+
+ Vector direction= - act.find_active_optimum (gradient);
+
+ DOUT << "gradient "<< gradient<< "\ndirection " << direction<< '\n';
+
+ if (direction.norm() > EPS)
+ {
+ DOUT << act.status() << '\n';
+
+ Real unbounded_alfa = 1.0;
+ Real minalf = unbounded_alfa;
+
+ Inactive_iter minidx (act);
+
+
+ /*
+ we know the optimum on this "hyperplane". Check if we
+ bump into the edges of the simplex
+ */
+
+ for (Inactive_iter ia (act); ia.ok(); ia++)
+ {
+ Real dot = ia.vec() * direction;
+ Real mindot = (experimental_features_global_b)
+ ? -EPS
+ : 0;
+
+ if (dot >= mindot)
+ continue;
+
+
+ Real numerator = ia.rhs () - ia.vec()*x;
+ if (numerator >= 0)
+ {
+ if (numerator > EPS)
+ {
+ warning (_f ("Ineq_constrained_qp::solve (): Constraint off by %f", numerator));
+ act.degenerate_count_i_ ++;
+ }
+ minalf = -numerator;
+ minidx = ia;
+ break;
+ }
+
+ Real alfa = numerator / dot;
+
+
+ if (minalf > alfa)
+ {
+ minidx = ia;
+ minalf = alfa;
}
}
- Real unbounded_alfa = 1.0;
- Real optimal_step = min(minalf, unbounded_alfa);
-
- Vector deltax=direction * optimal_step;
- x += deltax;
- gradient += optimal_step * (quad * deltax);
-
- mtor << "step = " << optimal_step<< " (|dx| = " <<
- deltax.norm() << ")\n";
-
- if (minalf < unbounded_alfa) {
- /* bumped into an edge. try again, in smaller space. */
- act.add(minidx.idx());
- mtor << "adding cons "<< minidx.idx()<<'\n';
- continue;
+
+ Real optimal_step = minalf;
+
+ Vector deltax = direction * optimal_step;
+ x += deltax;
+ gradient += optimal_step * (quad_ * deltax);
+
+ DOUT << "step = " << optimal_step << " (|dx| = " <<
+ to_str (deltax.norm()) << ")\n";
+
+ if (minalf < unbounded_alfa)
+ {
+ /* bumped into an edge. try again, in smaller space. */
+ act.add_constraint (minidx.idx());
+ DOUT << "adding cons "<< minidx.idx () << '\n';
+ continue;
}
- /*ASSERT: we are at optimal solution for this "plane"*/
-
-
+ /*ASSERT: we are at the optimal solution for this "plane"*/
+ }
+
+ Vector lagrange_mult=act.get_lagrange (gradient);
+ int m= min_elt_index (lagrange_mult);
+
+ if (m>=0 && lagrange_mult (m) > 0)
+ {
+ break; // optimal sol.
}
-
- Vector lagrange_mult=act.get_lagrange(gradient);
- int m= min_elt_index(lagrange_mult);
-
- if (m>=0 && lagrange_mult(m) > 0) {
- break; // optimal sol.
- } else if (m<0) {
- assert(gradient.norm() < EPS) ;
-
- break;
+ else if (m<0)
+ {
+ assert (gradient.norm() < EPS) ;
+
+ break;
}
-
- mtor << "dropping cons " << m<<'\n';
- act.drop(m);
+
+ DOUT << "dropping cons " << m << '\n';
+ act.drop_constraint (m);
}
- if (iterations >= MAXITER)
- WARN<<"didn't converge!\n";
-
- mtor << ": found " << x<<" in " << iterations <<" iterations\n";
- assert_solution(x);
- return x;
-}
-
-
+ if (iterations >= MAXITER)
+ WARN << _ ("didn't converge!") << '\n';
+ if (act.degenerate_count_i_ >= MAXDEGEN)
+ WARN << _ ("Too much degeneracy. ") << '\n';
+ DOUT << ": found " << x << " in " << iterations <<" iterations\n";
+ assert_solution (x);
+ return x;
+}
+
+
+Vector
+Ineq_constrained_qp::solve (Vector start) const
+{
+ /* no hassle if no constraints*/
+ if (! cons_.size())
+ {
+ Choleski_decomposition chol (quad_);
+ return - chol.solve (lin_);
+ }
+ else
+ {
+ return constraint_solve (start);
+ }
+}
+
+int
+Ineq_constrained_qp::dim () const
+{
+ return lin_.dim();
+}
+
+
+
+
+void
+Ineq_constrained_qp::assert_solution (Vector sol) const
+{
+ bool sol_b=true;
+
+ for (int i=0; sol_b && i < cons_.size(); i++)
+ {
+ Real R=cons_[i] * sol- consrhs_[i];
+ if (R> -EPS)
+ sol_b = false;
+ }
+}
+
+void
+Ineq_constrained_qp::print() const
+{
+#ifndef NPRINT
+ DOUT << "Quad " << quad_;
+ DOUT << "lin " << lin_ << '\n'
+ << "const " << const_term_<< '\n';
+ for (int i=0; i < cons_.size(); i++)
+ {
+ DOUT << "constraint["<<i<<"]: " << cons_[i] << " >= " << consrhs_[i];
+ DOUT << '\n';
+ }
+#endif
+}