1 /*
2  * ECOS - Embedded Conic Solver.
3  * Copyright (C) 2012-2015 A. Domahidi [domahidi@embotech.com],
4  * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
5  *
6  * This program is free software: you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation, either version 3 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
18  */
19 
20 /* Equilibration module (c) Eric Chu, March 2014 */
21 
22 #include "ecos.h"
23 
24 #if defined EQUILIBRATE && EQUILIBRATE > 0
25 
26 #include <math.h>
27 
28 #include "equil.h"
29 #include "spla.h"
30 
31 void max_rows(pfloat *E, const spmat *mat)
32 {
33     /* assumes mat is not null */
34     idxint i, j, row;
35     for(i = 0; i < mat->n; i++) { /* cols */
36       for(j = mat->jc[i]; j < mat->jc[i+1]; j++) {
37         row = mat->ir[j];
38         E[row] = MAX(fabs(mat->pr[j]), E[row]);
39       }
40     }
41 }
42 
43 void max_cols(pfloat *E, const spmat *mat)
44 {
45     /* assumes mat is not null */
46     idxint i, j;
47     for(i = 0; i < mat->n; i++) { /* cols */
48       for(j = mat->jc[i]; j < mat->jc[i+1]; j++) {
49         E[i] = MAX(fabs(mat->pr[j]), E[i]);
50       }
51     }
52 }
53 
54 void sum_sq_rows(pfloat *E, const spmat *mat)
55 {
56     /* assumes mat is not null */
57     idxint i, j, row;
58     for(i = 0; i < mat->n; i++) { /* cols */
59       for(j = mat->jc[i]; j < mat->jc[i+1]; j++) {
60         row = mat->ir[j];
61         E[row] += (mat->pr[j] * mat->pr[j]);
62       }
63     }
64 }
65 
66 void sum_sq_cols(pfloat *E, const spmat *mat)
67 {
68     /* assumes mat is not null */
69     idxint i, j;
70     for(i = 0; i < mat->n; i++) { /* cols */
71       for(j = mat->jc[i]; j < mat->jc[i+1]; j++) {
72         E[i] += (mat->pr[j] * mat->pr[j]);
73       }
74     }
75 }
76 
77 void equilibrate_rows(const pfloat *E, spmat *mat)
78 {
79     idxint i, j, row;
80 
81     for(i = 0; i < mat->n; i++) {
82         /* equilibrate the rows of a matrix */
83         for(j = mat->jc[i]; j < mat->jc[i+1]; j++) {
84             row = mat->ir[j];
85             mat->pr[j] /= E[row];
86         }
87     }
88 }
89 
90 void equilibrate_cols(const pfloat *E, spmat *mat)
91 {
92     idxint i, j;
93 
94     for(i = 0; i < mat->n; i++) {
95         /* equilibrate the columns of a matrix */
96         for(j = mat->jc[i]; j < mat->jc[i+1]; j++) {
97             mat->pr[j] /= E[i];
98         }
99     }
100 }
101 
102 void restore(const pfloat *D, const pfloat *E, spmat *mat)
103 {
104     idxint i, j, row;
105 
106     for(i = 0; i < mat->n; i++) {
107         /* equilibrate the rows of a matrix */
108         for(j = mat->jc[i]; j < mat->jc[i+1]; j++) {
109             row = mat->ir[j];
110             mat->pr[j] *= (D[row] * E[i]);
111         }
112     }
113 }
114 
115 void use_alternating_norm_equilibration(pwork *w)
116 {
117     idxint i, j, ind;
118     idxint num_cols = w->A ? w->A->n : w->G->n;
119     idxint num_A_rows = w->A ? w->A->m : 0;
120     idxint num_G_rows = w->G->m;
121     pfloat sum;
122 
123     /* initialize equilibration vector to 0 */
124     for(i = 0; i < num_cols; i++) {
125         w->xequil[i] = 0.0;
126     }
127     for(i = 0; i < num_A_rows; i++) {
128         w->Aequil[i] = 0.0;
129     }
130     for(i = 0; i < num_G_rows; i++) {
131         w->Gequil[i] = 0.0;
132     }
133 
134     /* compute norm across rows of A */
135     if(w->A)
136         sum_sq_rows(w->Aequil, w->A);
137     /* compute norm across rows of G */
138     if(num_G_rows > 0)
139         sum_sq_rows(w->Gequil, w->G);
140 
141     /* now collapse cones together by taking average norm square */
142     ind = w->C->lpc->p;
143     for(i = 0; i < w->C->nsoc; i++) {
144       sum = 0.0;
145       for(j = 0; j < w->C->soc[i].p; j++) {
146         sum += w->Gequil[ind + j];
147       }
148       for(j = 0; j < w->C->soc[i].p; j++) {
149         w->Gequil[ind + j] = sum / w->C->soc[i].p;
150       }
151       ind += w->C->soc[i].p;
152     }
153 
154 #ifdef EXPCONE
155     for(i = 0; i < w->C->nexc; i++) {
156       sum = 0.0;
157       for(j = 0; j < 3; j++) {
158         sum += w->Gequil[ind + j];
159       }
160       for(j = 0; j < 3; j++) {
161         w->Gequil[ind + j] = sum / 3.0;
162       }
163       ind += 3;
164     }
165 #endif
166 
167     /* get the norm */
168     for(i = 0; i < num_A_rows; i++) {
169       w->Aequil[i] = fabs(w->Aequil[i]) < 1e-6 ? 1.0 : sqrt(w->Aequil[i]);
170     }
171     for(i = 0; i < num_G_rows; i++) {
172       w->Gequil[i] = fabs(w->Gequil[i]) < 1e-6 ? 1.0 : sqrt(w->Gequil[i]);
173     }
174 
175     /* now scale A */
176     if(w->A)
177         equilibrate_rows(w->Aequil, w->A);
178     if(num_G_rows > 0)
179         equilibrate_rows(w->Gequil, w->G);
180 
181     if(w->A)
182         sum_sq_cols(w->xequil, w->A);
183     if(num_G_rows > 0)
184         sum_sq_cols(w->xequil, w->G);
185 
186     /* get the norm */
187     for(i = 0; i < num_cols; i++) {
188         w->xequil[i] = fabs(w->xequil[i]) < 1e-6 ? 1.0 : sqrt(w->xequil[i]);
189     }
190     if(w->A)
191         equilibrate_cols(w->xequil, w->A);
192     if(num_G_rows > 0)
193         equilibrate_cols(w->xequil, w->G);
194 
195     /* the c vector is scaled in the ECOS_solve function
196     for(i = 0; i < num_cols; i++) {
197         w->c[i] /= w->xequil[i];
198     }  */
199 
200     /* equilibrate the b vector */
201     for(i = 0; i < num_A_rows; i++) {
202         w->b[i] /= w->Aequil[i];
203     }
204     /* equilibrate the h vector */
205     for(i = 0; i < num_G_rows; i++) {
206         w->h[i] /= w->Gequil[i];
207     }
208 }
209 
210 void use_ruiz_equilibration(pwork *w)
211 {
212     idxint i, j, ind, iter;
213     idxint num_cols = w->A ? w->A->n : w->G->n;
214     idxint num_A_rows = w->A ? w->A->m : 0;
215     idxint num_G_rows = w->G->m;
216     pfloat *xtmp = calloc(num_cols, sizeof(pfloat));
217     pfloat *Atmp = calloc(num_A_rows, sizeof(pfloat));
218     pfloat *Gtmp = calloc(num_G_rows, sizeof(pfloat));
219     pfloat total;
220 
221     /* initialize equilibration vector to 1 */
222     for(i = 0; i < num_cols; i++) {
223         w->xequil[i] = 1.0;
224     }
225     for(i = 0; i < num_A_rows; i++) {
226         w->Aequil[i] = 1.0;
227     }
228     for(i = 0; i < num_G_rows; i++) {
229         w->Gequil[i] = 1.0;
230     }
231 
232     /* iterative equilibration */
233     for(iter = 0; iter < EQUIL_ITERS; iter++) {
234         /* each iteration updates w->A and w->G */
235 
236         /* zero out the temp vectors */
237         for(i = 0; i < num_cols; i++) {
238             xtmp[i] = 0.0;
239         }
240         for(i = 0; i < num_A_rows; i++) {
241             Atmp[i] = 0.0;
242         }
243         for(i = 0; i < num_G_rows; i++) {
244             Gtmp[i] = 0.0;
245         }
246 
247         /* compute norm across columns of A, G */
248         if(w->A)
249             max_cols(xtmp, w->A);
250         if(num_G_rows > 0)
251             max_cols(xtmp, w->G);
252 
253         /* compute norm across rows of A */
254         if(w->A)
255             max_rows(Atmp, w->A);
256 
257         /* compute norm across rows of G */
258         if(num_G_rows > 0)
259             max_rows(Gtmp, w->G);
260 
261         /* now collapse cones together by using total over the group */
262         /* ECHU: not sure what the right thing to do here is */
263         ind = w->C->lpc->p;
264         for(i = 0; i < w->C->nsoc; i++) {
265           total = 0.0;
266           for(j = 0; j < w->C->soc[i].p; j++) {
267             total += Gtmp[ind + j];
268           }
269           for(j = 0; j < w->C->soc[i].p; j++) {
270             Gtmp[ind + j] = total;
271           }
272           ind += w->C->soc[i].p;
273         }
274 #ifdef EXPCONE
275        /*Do the same for the exponential cones*/
276        for(i = 0; i < w->C->nexc; i++) {
277          total = 0.0;
278          for(j = 0; j < 3; j++) {
279            total += Gtmp[ind + j];
280          }
281          for(j = 0; j < 3; j++) {
282            Gtmp[ind + j] = total;
283          }
284          ind += 3;
285        }
286 #endif
287 
288 
289         /* take the sqrt */
290         for(i = 0; i < num_cols; i++) {
291           xtmp[i] = fabs(xtmp[i]) < 1e-6 ? 1.0 : sqrt(xtmp[i]);
292         }
293         for(i = 0; i < num_A_rows; i++) {
294           Atmp[i] = fabs(Atmp[i]) < 1e-6 ? 1.0 : sqrt(Atmp[i]);
295         }
296         for(i = 0; i < num_G_rows; i++) {
297           Gtmp[i] = fabs(Gtmp[i]) < 1e-6 ? 1.0 : sqrt(Gtmp[i]);
298         }
299 
300         /* equilibrate the matrices */
301         if(w->A)
302             equilibrate_rows(Atmp, w->A);
303         if(num_G_rows > 0)
304             equilibrate_rows(Gtmp, w->G);
305 
306         if(w->A)
307             equilibrate_cols(xtmp, w->A);
308         if(num_G_rows > 0)
309             equilibrate_cols(xtmp, w->G);
310 
311         /* update the equilibration matrix */
312         for(i = 0; i < num_cols; i++) {
313           w->xequil[i] *= xtmp[i];
314         }
315         for(i = 0; i < num_A_rows; i++) {
316           w->Aequil[i] *= Atmp[i];
317         }
318         for(i = 0; i < num_G_rows; i++) {
319           w->Gequil[i] *= Gtmp[i];
320         }
321     }
322 
323     /* the c vector is scaled in the ECOS_solve function
324     for(i = 0; i < num_cols; i++) {
325         w->c[i] /= w->xequil[i];
326     } */
327 
328     /* equilibrate the b vector */
329     for(i = 0; i < num_A_rows; i++) {
330         w->b[i] /= w->Aequil[i];
331     }
332     /* equilibrate the h vector */
333     for(i = 0; i < num_G_rows; i++) {
334         w->h[i] /= w->Gequil[i];
335     }
336 
337     free(xtmp);
338     free(Atmp);
339     free(Gtmp);
340 }
341 
342 /* equilibrate */
343 void set_equilibration(pwork *w)
344 {
345 #if defined(RUIZ_EQUIL)
346     use_ruiz_equilibration(w);
347 #elif defined(ALTERNATING_EQUIL)
348     use_alternating_norm_equilibration(w);
349 #else
350     /* use identity equilibration */
351     idxint i;
352     idxint num_cols = w->A ? w->A->n : w->G->n;
353     idxint num_A_rows = w->A ? w->A->m : 0;
354     idxint num_G_rows = w->G->m;
355 
356     /* initialize equilibration vector to 1 */
357     for(i = 0; i < num_cols; i++) {
358         w->xequil[i] = 1.0;
359     }
360     for(i = 0; i < num_A_rows; i++) {
361         w->Aequil[i] = 1.0;
362     }
363     for(i = 0; i < num_G_rows; i++) {
364         w->Gequil[i] = 1.0;
365     }
366 #endif
367 }
368 /* invert the equilibration job */
369 void unset_equilibration(pwork *w)
370 {
371     idxint i;
372     /* idxint num_cols = w->A ? w->A->n : w->G->n; */
373     idxint num_A_rows = w->A ? w->A->m : 0;
374     idxint num_G_rows = w->G->m;
375 
376     if(w->A)
377         restore(w->Aequil, w->xequil, w->A);
378     if(num_G_rows > 0)
379         restore(w->Gequil, w->xequil, w->G);
380 
381     /* the c vector is unequilibrated in the ECOS_solve function
382         for(i = 0; i < num_cols; i++) {
383         w->c[i] *= w->xequil[i];
384     }*/
385 
386     /* unequilibrate the b vector */
387     for(i = 0; i < num_A_rows; i++) {
388         w->b[i] *= w->Aequil[i];
389     }
390     /* unequilibrate the h vector */
391     for(i = 0; i < num_G_rows; i++) {
392         w->h[i] *= w->Gequil[i];
393     }
394 }
395 
396 #endif
397