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