src/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     print();
  61     Ineq_constrained_qp pure(*this);
  62
  63     for  (int i= eq_cons.size()-1; i>=0; i--) {
  64         pure.eliminate_var(eq_cons[i], eq_consrhs[i]);
  65         start.del(eq_cons[i]);
  66     }
  67     Vector sol = pure.solve(start);
  68     for (int i= 0; i < eq_cons.size(); i++) {
  69         sol.insert( eq_consrhs[i],eq_cons[i]);
  70     }
  71     return sol;
  72 }
  73
  74 /*
  75     assume x(idx) == value, and adjust constraints, lin and quad accordingly
  76
  77     TODO: add const_term
  78     */
  79 void
  80 Ineq_constrained_qp::eliminate_var(int idx, Real value)
  81 {
  82     Vector row(quad.row(idx));
  83     row*= value;
  84
  85     quad.delete_row(idx);
  86
  87     quad.delete_column(idx);
  88
  89     lin.del(idx);
  90     row.del(idx);
  91     lin +=row ;
  92
  93    for (int i=0; i < cons.size(); i++) {
  94       consrhs[i] -= cons[i](idx) *value;
  95       cons[i].del(idx);
  96    }
  97 }
  98
  99
 100
 101 void
 102 Ineq_constrained_qp::assert_solution(Vector sol) const
 103 {
 104     Array<int> binding;
 105     for (int i=0; i < cons.size(); i++) {
 106         Real R=cons[i] * sol- consrhs[i];
 107         assert(R> -EPS);
 108         if (R < EPS)
 109             binding.push(i);
 110     }
 111     // KKT check...
 112     // todo
 113 }
 114
 115 void
 116 Ineq_constrained_qp::print() const
 117 {
 118 #ifndef NPRINT
 119     mtor << "Quad " << quad;
 120     mtor << "lin " << lin <<"\n"
 121         << "const " << const_term<<"\n";
 122     for (int i=0; i < cons.size(); i++) {
 123         mtor << "constraint["<<i<<"]: " << cons[i] << " >= " << consrhs[i];
 124         mtor << "\n";
 125     }
 126 #endif
 127 }
 128
 129 /* *************** */
 130
 131 Mixed_qp::Mixed_qp(int n)
 132     : Ineq_constrained_qp(n)
 133 {
 134 }
 135
 136 void
 137 Mixed_qp::OK() const
 138 {
 139 #ifndef NDEBUG
 140     Ineq_constrained_qp::OK();
 141     assert(eq_consrhs.size() == eq_cons.size());
 142 #endif
 143 }
 144
 145 void
 146 Mixed_qp::print() const
 147 {
 148 #ifndef NPRINT
 149     Ineq_constrained_qp::print();
 150     for (int i=0; i < eq_cons.size(); i++) {
 151         mtor << "eq cons "<<i<<": x["<<eq_cons[i]<<"] == " << eq_consrhs[i]<<"\n";
 152     }
 153 #endif
 154 }
 155