5 const Real TOL=1e-2; // roughly 1/10 mm
8 Active_constraints::status() const
10 String s("Active|Inactive [");
11 for (int i=0; i< active.sz(); i++) {
12 s += String(active[i]) + " ";
16 for (int i=0; i< inactive.sz(); i++) {
17 s += String(inactive[i]) + " ";
25 Active_constraints::OK() {
28 assert(active.sz() +inactive.sz() == opt->cons.sz());
29 assert(H.dim() == opt->dim());
30 assert(active.sz() == A.rows());
33 for (int i=0; i < opt->cons.sz(); i++)
35 for (int i=0; i < active.sz(); i++) {
39 for (int i=0; i < inactive.sz(); i++) {
43 for (int i=0; i < allcons.sz(); i++)
44 assert(allcons[i] == 1);
48 Active_constraints::get_lagrange(Vector gradient)
56 Active_constraints::add(int k)
62 inactive.swap(k,inactive.sz()-1);
65 Vector a( opt->cons[cidx] );
70 H -= Matrix(Ha , Ha)/(aHa);
72 Vector addrow(Ha/(aHa));
73 A -= Matrix(A*a, addrow);
74 A.insert_row(addrow,A.rows());
78 Active_constraints::drop(int k)
83 inactive.add(active[k]);
89 H += Matrix(a,a)/(a*opt->quad*a);
90 A -= A*opt->quad*Matrix(a,a)/(a*opt->quad*a);
92 Vector rem_row(A.row(q));
93 assert(rem_row.norm() < EPS);
98 Active_constraints::Active_constraints(Ineq_constrained_qp const *op)
103 for (int i=0; i < op->cons.sz(); i++)
105 Choleski_decomposition chol(op->quad);
109 /* Find the optimum which is in the planes generated by the active
113 Active_constraints::find_active_optimum(Vector g)
118 /****************************************************************/
121 min_elt_index(Vector v)
123 Real m=INFTY; int idx=-1;
124 for (int i = 0; i < v.dim(); i++)
132 ///the numerical solving
134 Ineq_constrained_qp::solve(Vector start) const
136 Active_constraints act(this);
143 Vector gradient=quad*x+lin;
146 Vector last_gradient(gradient);
149 while (iterations++ < MAXITER) {
150 Vector direction= - act.find_active_optimum(gradient);
152 // mtor << "gradient "<< gradient<< "\ndirection " << direction<<"\n";
154 if (direction.norm() > EPS) {
155 // mtor << act.status() << '\n';
159 Inactive_iter minidx(act);
163 we know the optimum on this "hyperplane". Check if we
164 bump into the edges of the simplex
167 for (Inactive_iter ia(act); ia.ok(); ia++) {
169 if (ia.vec() * direction >= 0)
171 Real alfa= - (ia.vec()*x - ia.rhs())/
172 (ia.vec()*direction);
179 Real unbounded_alfa = 1.0;
180 Real optimal_step = MIN(minalf, unbounded_alfa);
182 Vector deltax=direction * optimal_step;
184 gradient += optimal_step * (quad * deltax);
186 //mtor << "step = " << optimal_step<< " (|dx| = " <<
187 //deltax.norm() << ")\n";
189 if (minalf < unbounded_alfa) {
190 /* bumped into an edge. try again, in smaller space. */
191 act.add(minidx.idx());
194 /*ASSERT: we are at optimal solution for this "plane"*/
199 Vector lagrange_mult=act.get_lagrange(gradient);
200 int m= min_elt_index(lagrange_mult);
202 if (m>=0 && lagrange_mult(m) > 0) {
203 break; // optimal sol.
204 } else if (m<0 && gradient.norm() < EPS) {
208 mtor << "dropping cons " << m<<'\n';
211 if (iterations >= MAXITER)
212 WARN<<"didn't converge!\n";
214 // mtor << ": found " << x<<" in " << iterations <<" iterations\n";
219 /** Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
224 This is a "projected gradient" algorithm. Starting
225 from a point x the next point is found in a direction determined by
226 projecting the gradient onto the active constraints. */
229 thoroughly hacked to barely living tiny pieces by HW