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