lilypond-0.0.9

diff --git a/src/qlpsolve.cc b/src/qlpsolve.cc
new file mode 100644 (file)
index 0000000..13bd30e
--- /dev/null
+++ b/src/qlpsolve.cc
@@ -0,0 +1,255 @@
+#include "qlpsolve.hh"
+#include "const.hh"
+#include "debug.hh"
+#include "choleski.hh"
+
+const Real TOL=1e-2;           // roughly 1/10 mm
+
+String
+Active_constraints::status() const
+{
+    String s("Active|Inactive [");
+    for (int i=0; i< active.sz(); i++) {
+       s += String(active[i]) + " ";
+    }
+
+    s+="| ";
+    for (int i=0; i< inactive.sz(); i++) {
+       s += String(inactive[i]) + " ";
+    }
+    s+="]";
+
+    return s;
+}
+
+void
+Active_constraints::OK() {
+    H.OK();
+    A.OK();
+    assert(active.sz() +inactive.sz() == opt->cons.sz());
+    assert(H.dim() == opt->dim());
+    assert(active.sz() == A.rows());
+    svec<int> allcons;
+
+    for (int i=0; i < opt->cons.sz(); i++)
+       allcons.add(0);
+    for (int i=0; i < active.sz(); i++) {
+       int j = active[i];
+       allcons[j]++;
+    }
+    for (int i=0; i < inactive.sz(); i++) {
+       int j = inactive[i];
+       allcons[j]++;
+    }
+    for (int i=0; i < allcons.sz(); i++)
+       assert(allcons[i] == 1);
+}
+
+Vector
+Active_constraints::get_lagrange(Vector gradient)
+{
+    Vector l(A*gradient);
+
+    return l;
+}
+
+void
+Active_constraints::add(int k)
+{
+    // add indices
+    int cidx=inactive[k];
+    active.add(cidx);
+
+    inactive.swap(k,inactive.sz()-1);
+    inactive.pop();
+
+    Vector a( opt->cons[cidx] );
+    // update of matrices
+    Vector Ha = H*a;
+    Real aHa = a*Ha;
+    if (ABS(aHa) > EPS) {
+       /*
+         a != 0, so if Ha = O(EPS), then
+         Ha * aH / aHa = O(EPS^2/EPS)
+
+         if H*a == 0, the constraints are dependent.
+         */
+       H -= Matrix(Ha , Ha)/(aHa);
+    
+
+       /*
+         sorry, don't know how to justify this. ..
+         */
+       Vector addrow(Ha/(aHa));
+       A -= Matrix(A*a, addrow);
+       A.insert_row(addrow,A.rows());
+    }else
+       WARN << "degenerate constraints";
+}
+
+void
+Active_constraints::drop(int k)
+{
+    int q=active.sz()-1;
+
+        // drop indices
+    inactive.add(active[k]);
+    active.swap(k,q);
+    A.swap_rows(k,q);
+    active.pop();
+
+    Vector a(A.row(q));
+    if (a.norm() > EPS) {
+       /*
+        
+        */
+       H += Matrix(a,a)/(a*opt->quad*a);
+       A -= A*opt->quad*Matrix(a,a)/(a*opt->quad*a);
+    }else
+       WARN << "degenerate constraints";
+    Vector rem_row(A.row(q));
+    assert(rem_row.norm() < EPS);    
+    A.delete_row(q);
+}
+
+
+Active_constraints::Active_constraints(Ineq_constrained_qp const *op)
+    :       A(0,op->dim()),
+           H(op->dim()),
+           opt(op)
+{
+    for (int i=0; i < op->cons.sz(); i++)
+       inactive.add(i);
+    Choleski_decomposition chol(op->quad);
+    H=chol.inverse();
+}
+
+/* Find the optimum which is in the planes generated by the active
+    constraints.        
+    */
+Vector
+Active_constraints::find_active_optimum(Vector g)
+{
+    return H*g;
+}
+
+/****************************************************************/
+
+int
+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);
+       }
+       assert(v(i) <= INFTY);
+    }
+    return idx;
+}
+
+///the numerical solving
+Vector
+Ineq_constrained_qp::solve(Vector start) const 
+{    
+    Active_constraints act(this);
+
+
+    act.OK();    
+
+    
+    Vector x(start);
+    Vector gradient=quad*x+lin;
+
+
+    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;
+               }
+           }
+           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;
+           }
+           /*ASSERT: we are at 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.
+       } else if (m<0) {
+           assert(gradient.norm() < EPS) ;
+           
+           break;
+       }
+       
+       mtor << "dropping cons " << m<<'\n';
+       act.drop(m);
+    }
+    if (iterations >= MAXITER)
+       WARN<<"didn't converge!\n";
+    
+    mtor <<  ": found " << x<<" in " << iterations <<" iterations\n";
+    assert_solution(x);
+    return x;
+} 
+
+/** Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
+    Prentice Hall.
+
+    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) )
+
+
+    */
+