qlp.cc

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