qlp.cc

   1 #include "debug.hh"
   2 #include "qlp.hh"
   3 #include "choleski.hh"
   4 void
   5 Mixed_qp::add_equality_cons(Vector v, double r)
   6 {
   7     assert(false);
   8 }
   9 void
  10 Mixed_qp::add_fixed_var(int i, Real r)
  11 {
  12     eq_cons.add(i);
  13     eq_consrhs.add(r);
  14 }
  15    void
  16 Ineq_constrained_qp::add_inequality_cons(Vector c, double r)
  17 {
  18     cons.add(c);
  19     consrhs.add(r);
  20 }
  21
  22 Ineq_constrained_qp::Ineq_constrained_qp(int novars):
  23     quad(novars),
  24     lin(novars)
  25 {
  26 }
  27
  28 void
  29 Ineq_constrained_qp::OK()const
  30 {
  31     assert(cons.sz() == consrhs.sz());
  32     Matrix Qdif= quad - quad.transposed();
  33     assert(Qdif.norm() < EPS);
  34 }
  35
  36
  37 Real
  38 Ineq_constrained_qp::eval (Vector v)
  39 {
  40     return v * quad * v + lin * v + const_term;
  41 }
  42 /*
  43     eliminate appropriate variables, until we have a Ineq_constrained_qp
  44     then solve that.
  45
  46     PRE
  47     cons should be ascending
  48     */
  49 Vector
  50 Mixed_qp::solve(Vector start) const
  51 {
  52     print();
  53     Ineq_constrained_qp pure(*this);
  54
  55     for  (int i= eq_cons.sz()-1; i>=0; i--) {
  56         pure.eliminate_var(eq_cons[i], eq_consrhs[i]);
  57         start.del(eq_cons[i]);
  58     }
  59     Vector sol = pure.solve(start);
  60     for (int i= 0; i < eq_cons.sz(); i++) {
  61         sol.insert( eq_consrhs[i],eq_cons[i]);
  62     }
  63     return sol;
  64 }
  65
  66 /*
  67     assume x(idx) == value, and adjust constraints, lin and quad accordingly
  68     */
  69 void
  70 Ineq_constrained_qp::eliminate_var(int idx, Real value)
  71 {
  72     Vector row(quad.row(idx));
  73     row*= value;
  74
  75     quad.delete_row(idx);
  76
  77     quad.delete_column(idx);
  78
  79     lin.del(idx);
  80     row.del(idx);
  81     lin +=row ;
  82
  83    for (int i=0; i < cons.sz(); i++) {
  84       consrhs[i] -= cons[i](idx) *value;
  85       cons[i].del(idx);
  86    }
  87 }
  88
  89
  90
  91
  92 Mixed_qp::Mixed_qp(int n)
  93     : Ineq_constrained_qp(n)
  94 {
  95 }
  96
  97 void
  98 Mixed_qp::OK()const
  99 {
 100     Ineq_constrained_qp::OK();
 101     assert(eq_consrhs.sz() == eq_cons.sz());
 102 }
 103 void
 104 Ineq_constrained_qp::print() const
 105 {
 106
 107     mtor << "Quad " << quad;
 108     mtor << "lin " << lin <<"\n";
 109     for (int i=0; i < cons.sz(); i++) {
 110         mtor << "constraint["<<i<<"]: " << cons[i] << " >= " << consrhs[i];
 111         mtor << "\n";
 112     }
 113 }
 114 void
 115 Mixed_qp::print() const
 116 {
 117     Ineq_constrained_qp::print();
 118     for (int i=0; i < eq_cons.sz(); i++) {
 119         mtor << "eq cons "<<i<<": x["<<eq_cons[i]<<"] == " << eq_consrhs[i]<<"\n";
 120     }
 121 }
 122
 123
 124 void
 125 Ineq_constrained_qp::assert_solution(Vector sol) const
 126 {
 127     svec<int> binding;
 128     for (int i=0; i < cons.sz(); i++) {
 129         Real R=cons[i] * sol- consrhs[i];
 130         assert(R> -EPS);
 131         if (R < EPS)
 132             binding.add(i);
 133     }
 134     // KKT check...
 135     // todo
 136 }