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