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 Vector
  50 Mixed_qp::solve(Vector start) const
  51 {
  52     print();
  53     Ineq_constrained_qp pure(*this);
  54
  55     for  (int i= eq_cons.size()-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.size(); 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.size(); i++) {
  84       consrhs[i] -= cons[i](idx) *value;
  85       cons[i].del(idx);
  86    }
  87 }
  88
  89
  90
  91 void
  92 Ineq_constrained_qp::assert_solution(Vector sol) const
  93 {
  94     Array<int> binding;
  95     for (int i=0; i < cons.size(); i++) {
  96         Real R=cons[i] * sol- consrhs[i];
  97         assert(R> -EPS);
  98         if (R < EPS)
  99             binding.push(i);
 100     }
 101     // KKT check...
 102     // todo
 103 }
 104
 105 void
 106 Ineq_constrained_qp::print() const
 107 {
 108 #ifndef NPRINT
 109     mtor << "Quad " << quad;
 110     mtor << "lin " << lin <<"\n"
 111         << "const " << const_term<<"\n";
 112     for (int i=0; i < cons.size(); i++) {
 113         mtor << "constraint["<<i<<"]: " << cons[i] << " >= " << consrhs[i];
 114         mtor << "\n";
 115     }
 116 #endif
 117 }
 118
 119 /****************/
 120
 121 /*
 122     eliminate appropriate variables, until we have a Ineq_constrained_qp
 123     then solve that.
 124
 125     PRE
 126     cons should be ascending
 127     */
 128
 129
 130 Mixed_qp::Mixed_qp(int n)
 131     : Ineq_constrained_qp(n)
 132 {
 133 }
 134
 135 void
 136 Mixed_qp::OK() const
 137 {
 138 #ifndef NDEBUG
 139     Ineq_constrained_qp::OK();
 140     assert(eq_consrhs.size() == eq_cons.size());
 141 #endif
 142 }
 143
 144 void
 145 Mixed_qp::print() const
 146 {
 147 #ifndef NPRINT
 148     Ineq_constrained_qp::print();
 149     for (int i=0; i < eq_cons.size(); i++) {
 150         mtor << "eq cons "<<i<<": x["<<eq_cons[i]<<"] == " << eq_consrhs[i]<<"\n";
 151     }
 152 #endif
 153 }
 154