1 #include <petsc-private/petscimpl.h> 2 #include <tao.h> /*I "tao.h" I*/ 3 #include <tao_util.h> /*I "tao_util.h" I*/ 4 5 6 PETSC_STATIC_INLINE PetscReal Fischer(PetscReal a, PetscReal b) 7 { 8 /* Method suggested by Bob Vanderbei */ 9 if (a + b <= 0) { 10 return PetscSqrtScalar(a*a + b*b) - (a + b); 11 } 12 return -2.0*a*b / (PetscSqrtScalar(a*a + b*b) + (a + b)); 13 } 14 15 #undef __FUNCT__ 16 #define __FUNCT__ "VecFischer" 17 /*@ 18 VecFischer - Evaluates the Fischer-Burmeister function for complementarity 19 problems. 20 21 Logically Collective on vectors 22 23 Input Parameters: 24 + X - current point 25 . F - function evaluated at x 26 . L - lower bounds 27 - U - upper bounds 28 29 Output Parameters: 30 . FB - The Fischer-Burmeister function vector 31 32 Notes: 33 The Fischer-Burmeister function is defined as 34 $ phi(a,b) := sqrt(a*a + b*b) - a - b 35 and is used reformulate a complementarity problem as a semismooth 36 system of equations. 37 38 The result of this function is done by cases: 39 + l[i] == -infinity, u[i] == infinity -- fb[i] = -f[i] 40 . l[i] == -infinity, u[i] finite -- fb[i] = phi(u[i]-x[i], -f[i]) 41 . l[i] finite, u[i] == infinity -- fb[i] = phi(x[i]-l[i], f[i]) 42 . l[i] finite < u[i] finite -- fb[i] = phi(x[i]-l[i], phi(u[i]-x[i], -f[u])) 43 - otherwise l[i] == u[i] -- fb[i] = l[i] - x[i] 44 45 Level: developer 46 47 @*/ 48 PetscErrorCode VecFischer(Vec X, Vec F, Vec L, Vec U, Vec FB) 49 { 50 PetscReal *x, *f, *l, *u, *fb; 51 PetscReal xval, fval, lval, uval; 52 PetscErrorCode ierr; 53 PetscInt low[5], high[5], n, i; 54 55 PetscFunctionBegin; 56 PetscValidHeaderSpecific(X, VEC_CLASSID,1); 57 PetscValidHeaderSpecific(F, VEC_CLASSID,2); 58 PetscValidHeaderSpecific(L, VEC_CLASSID,3); 59 PetscValidHeaderSpecific(U, VEC_CLASSID,4); 60 PetscValidHeaderSpecific(FB, VEC_CLASSID,4); 61 62 ierr = VecGetOwnershipRange(X, low, high);CHKERRQ(ierr); 63 ierr = VecGetOwnershipRange(F, low + 1, high + 1);CHKERRQ(ierr); 64 ierr = VecGetOwnershipRange(L, low + 2, high + 2);CHKERRQ(ierr); 65 ierr = VecGetOwnershipRange(U, low + 3, high + 3);CHKERRQ(ierr); 66 ierr = VecGetOwnershipRange(FB, low + 4, high + 4);CHKERRQ(ierr); 67 68 for (i = 1; i < 4; ++i) { 69 if (low[0] != low[i] || high[0] != high[i]) SETERRQ(PETSC_COMM_SELF,1,"Vectors must be identically loaded over processors"); 70 } 71 72 ierr = VecGetArray(X, &x);CHKERRQ(ierr); 73 ierr = VecGetArray(F, &f);CHKERRQ(ierr); 74 ierr = VecGetArray(L, &l);CHKERRQ(ierr); 75 ierr = VecGetArray(U, &u);CHKERRQ(ierr); 76 ierr = VecGetArray(FB, &fb);CHKERRQ(ierr); 77 78 ierr = VecGetLocalSize(X, &n);CHKERRQ(ierr); 79 80 for (i = 0; i < n; ++i) { 81 xval = x[i]; fval = f[i]; 82 lval = l[i]; uval = u[i]; 83 84 if ((lval <= -PETSC_INFINITY) && (uval >= PETSC_INFINITY)) { 85 fb[i] = -fval; 86 } else if (lval <= -PETSC_INFINITY) { 87 fb[i] = -Fischer(uval - xval, -fval); 88 } else if (uval >= PETSC_INFINITY) { 89 fb[i] = Fischer(xval - lval, fval); 90 } else if (lval == uval) { 91 fb[i] = lval - xval; 92 } else { 93 fval = Fischer(uval - xval, -fval); 94 fb[i] = Fischer(xval - lval, fval); 95 } 96 } 97 98 ierr = VecRestoreArray(X, &x);CHKERRQ(ierr); 99 ierr = VecRestoreArray(F, &f);CHKERRQ(ierr); 100 ierr = VecRestoreArray(L, &l);CHKERRQ(ierr); 101 ierr = VecRestoreArray(U, &u);CHKERRQ(ierr); 102 ierr = VecRestoreArray(FB, &fb);CHKERRQ(ierr); 103 PetscFunctionReturn(0); 104 } 105 106 PETSC_STATIC_INLINE PetscReal SFischer(PetscReal a, PetscReal b, PetscReal c) 107 { 108 /* Method suggested by Bob Vanderbei */ 109 if (a + b <= 0) { 110 return PetscSqrtScalar(a*a + b*b + 2.0*c*c) - (a + b); 111 } 112 return 2.0*(c*c - a*b) / (PetscSqrtScalar(a*a + b*b + 2.0*c*c) + (a + b)); 113 } 114 115 #undef __FUNCT__ 116 #define __FUNCT__ "VecSFischer" 117 /*@ 118 VecSFischer - Evaluates the Smoothed Fischer-Burmeister function for 119 complementarity problems. 120 121 Logically Collective on vectors 122 123 Input Parameters: 124 + X - current point 125 . F - function evaluated at x 126 . L - lower bounds 127 . U - upper bounds 128 - mu - smoothing parameter 129 130 Output Parameters: 131 . FB - The Smoothed Fischer-Burmeister function vector 132 133 Notes: 134 The Smoothed Fischer-Burmeister function is defined as 135 $ phi(a,b) := sqrt(a*a + b*b + 2*mu*mu) - a - b 136 and is used reformulate a complementarity problem as a semismooth 137 system of equations. 138 139 The result of this function is done by cases: 140 + l[i] == -infinity, u[i] == infinity -- fb[i] = -f[i] - 2*mu*x[i] 141 . l[i] == -infinity, u[i] finite -- fb[i] = phi(u[i]-x[i], -f[i], mu) 142 . l[i] finite, u[i] == infinity -- fb[i] = phi(x[i]-l[i], f[i], mu) 143 . l[i] finite < u[i] finite -- fb[i] = phi(x[i]-l[i], phi(u[i]-x[i], -f[u], mu), mu) 144 - otherwise l[i] == u[i] -- fb[i] = l[i] - x[i] 145 146 Level: developer 147 148 .seealso VecFischer() 149 @*/ 150 PetscErrorCode VecSFischer(Vec X, Vec F, Vec L, Vec U, PetscReal mu, Vec FB) 151 { 152 PetscReal *x, *f, *l, *u, *fb; 153 PetscReal xval, fval, lval, uval; 154 PetscErrorCode ierr; 155 PetscInt low[5], high[5], n, i; 156 157 PetscFunctionBegin; 158 PetscValidHeaderSpecific(X, VEC_CLASSID,1); 159 PetscValidHeaderSpecific(F, VEC_CLASSID,2); 160 PetscValidHeaderSpecific(L, VEC_CLASSID,3); 161 PetscValidHeaderSpecific(U, VEC_CLASSID,4); 162 PetscValidHeaderSpecific(FB, VEC_CLASSID,6); 163 164 ierr = VecGetOwnershipRange(X, low, high);CHKERRQ(ierr); 165 ierr = VecGetOwnershipRange(F, low + 1, high + 1);CHKERRQ(ierr); 166 ierr = VecGetOwnershipRange(L, low + 2, high + 2);CHKERRQ(ierr); 167 ierr = VecGetOwnershipRange(U, low + 3, high + 3);CHKERRQ(ierr); 168 ierr = VecGetOwnershipRange(FB, low + 4, high + 4);CHKERRQ(ierr); 169 170 for (i = 1; i < 4; ++i) { 171 if (low[0] != low[i] || high[0] != high[i]) SETERRQ(PETSC_COMM_SELF,1,"Vectors must be identically loaded over processors"); 172 } 173 174 ierr = VecGetArray(X, &x);CHKERRQ(ierr); 175 ierr = VecGetArray(F, &f);CHKERRQ(ierr); 176 ierr = VecGetArray(L, &l);CHKERRQ(ierr); 177 ierr = VecGetArray(U, &u);CHKERRQ(ierr); 178 ierr = VecGetArray(FB, &fb);CHKERRQ(ierr); 179 180 ierr = VecGetLocalSize(X, &n);CHKERRQ(ierr); 181 182 for (i = 0; i < n; ++i) { 183 xval = (*x++); fval = (*f++); 184 lval = (*l++); uval = (*u++); 185 186 if ((lval <= -PETSC_INFINITY) && (uval >= PETSC_INFINITY)) { 187 (*fb++) = -fval - mu*xval; 188 } else if (lval <= -PETSC_INFINITY) { 189 (*fb++) = -SFischer(uval - xval, -fval, mu); 190 } else if (uval >= PETSC_INFINITY) { 191 (*fb++) = SFischer(xval - lval, fval, mu); 192 } else if (lval == uval) { 193 (*fb++) = lval - xval; 194 } else { 195 fval = SFischer(uval - xval, -fval, mu); 196 (*fb++) = SFischer(xval - lval, fval, mu); 197 } 198 } 199 x -= n; f -= n; l -=n; u -= n; fb -= n; 200 201 ierr = VecRestoreArray(X, &x);CHKERRQ(ierr); 202 ierr = VecRestoreArray(F, &f);CHKERRQ(ierr); 203 ierr = VecRestoreArray(L, &l);CHKERRQ(ierr); 204 ierr = VecRestoreArray(U, &u);CHKERRQ(ierr); 205 ierr = VecRestoreArray(FB, &fb);CHKERRQ(ierr); 206 PetscFunctionReturn(0); 207 } 208 209 PETSC_STATIC_INLINE PetscReal fischnorm(PetscReal a, PetscReal b) 210 { 211 return PetscSqrtScalar(a*a + b*b); 212 } 213 214 PETSC_STATIC_INLINE PetscReal fischsnorm(PetscReal a, PetscReal b, PetscReal c) 215 { 216 return PetscSqrtScalar(a*a + b*b + 2.0*c*c); 217 } 218 219 #undef __FUNCT__ 220 #define __FUNCT__ "D_Fischer" 221 /*@ 222 D_Fischer - Calculates an element of the B-subdifferential of the 223 Fischer-Burmeister function for complementarity problems. 224 225 Collective on jac 226 227 Input Parameters: 228 + jac - the jacobian of f at X 229 . X - current point 230 . Con - constraints function evaluated at X 231 . XL - lower bounds 232 . XU - upper bounds 233 . t1 - work vector 234 - t2 - work vector 235 236 Output Parameters: 237 + Da - diagonal perturbation component of the result 238 - Db - row scaling component of the result 239 240 Level: developer 241 242 .seealso: VecFischer() 243 @*/ 244 PetscErrorCode D_Fischer(Mat jac, Vec X, Vec Con, Vec XL, Vec XU, Vec T1, Vec T2, Vec Da, Vec Db) 245 { 246 PetscErrorCode ierr; 247 PetscInt i,nn; 248 PetscReal *x,*f,*l,*u,*da,*db,*t1,*t2; 249 PetscReal ai,bi,ci,di,ei; 250 251 PetscFunctionBegin; 252 ierr = VecGetLocalSize(X,&nn);CHKERRQ(ierr); 253 ierr = VecGetArray(X,&x);CHKERRQ(ierr); 254 ierr = VecGetArray(Con,&f);CHKERRQ(ierr); 255 ierr = VecGetArray(XL,&l);CHKERRQ(ierr); 256 ierr = VecGetArray(XU,&u);CHKERRQ(ierr); 257 ierr = VecGetArray(Da,&da);CHKERRQ(ierr); 258 ierr = VecGetArray(Db,&db);CHKERRQ(ierr); 259 ierr = VecGetArray(T1,&t1);CHKERRQ(ierr); 260 ierr = VecGetArray(T2,&t2);CHKERRQ(ierr); 261 262 for (i = 0; i < nn; i++) { 263 da[i] = 0; 264 db[i] = 0; 265 t1[i] = 0; 266 267 if (PetscAbsReal(f[i]) <= PETSC_MACHINE_EPSILON) { 268 if (l[i] > PETSC_NINFINITY && PetscAbsReal(x[i] - l[i]) <= PETSC_MACHINE_EPSILON) { 269 t1[i] = 1; 270 da[i] = 1; 271 } 272 273 if (u[i] < PETSC_INFINITY && PetscAbsReal(u[i] - x[i]) <= PETSC_MACHINE_EPSILON) { 274 t1[i] = 1; 275 db[i] = 1; 276 } 277 } 278 } 279 280 ierr = VecRestoreArray(T1,&t1);CHKERRQ(ierr); 281 ierr = VecRestoreArray(T2,&t2);CHKERRQ(ierr); 282 ierr = MatMult(jac,T1,T2);CHKERRQ(ierr); 283 ierr = VecGetArray(T2,&t2);CHKERRQ(ierr); 284 285 for (i = 0; i < nn; i++) { 286 if ((l[i] <= PETSC_NINFINITY) && (u[i] >= PETSC_INFINITY)) { 287 da[i] = 0; 288 db[i] = -1; 289 } else if (l[i] <= PETSC_NINFINITY) { 290 if (db[i] >= 1) { 291 ai = fischnorm(1, t2[i]); 292 293 da[i] = -1/ai - 1; 294 db[i] = -t2[i]/ai - 1; 295 } else { 296 bi = u[i] - x[i]; 297 ai = fischnorm(bi, f[i]); 298 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 299 300 da[i] = bi / ai - 1; 301 db[i] = -f[i] / ai - 1; 302 } 303 } else if (u[i] >= PETSC_INFINITY) { 304 if (da[i] >= 1) { 305 ai = fischnorm(1, t2[i]); 306 307 da[i] = 1 / ai - 1; 308 db[i] = t2[i] / ai - 1; 309 } else { 310 bi = x[i] - l[i]; 311 ai = fischnorm(bi, f[i]); 312 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 313 314 da[i] = bi / ai - 1; 315 db[i] = f[i] / ai - 1; 316 } 317 } else if (l[i] == u[i]) { 318 da[i] = -1; 319 db[i] = 0; 320 } else { 321 if (db[i] >= 1) { 322 ai = fischnorm(1, t2[i]); 323 324 ci = 1 / ai + 1; 325 di = t2[i] / ai + 1; 326 } else { 327 bi = x[i] - u[i]; 328 ai = fischnorm(bi, f[i]); 329 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 330 331 ci = bi / ai + 1; 332 di = f[i] / ai + 1; 333 } 334 335 if (da[i] >= 1) { 336 bi = ci + di*t2[i]; 337 ai = fischnorm(1, bi); 338 339 bi = bi / ai - 1; 340 ai = 1 / ai - 1; 341 } else { 342 ei = Fischer(u[i] - x[i], -f[i]); 343 ai = fischnorm(x[i] - l[i], ei); 344 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 345 346 bi = ei / ai - 1; 347 ai = (x[i] - l[i]) / ai - 1; 348 } 349 350 da[i] = ai + bi*ci; 351 db[i] = bi*di; 352 } 353 } 354 355 ierr = VecRestoreArray(Da,&da);CHKERRQ(ierr); 356 ierr = VecRestoreArray(Db,&db);CHKERRQ(ierr); 357 ierr = VecRestoreArray(X,&x);CHKERRQ(ierr); 358 ierr = VecRestoreArray(Con,&f);CHKERRQ(ierr); 359 ierr = VecRestoreArray(XL,&l);CHKERRQ(ierr); 360 ierr = VecRestoreArray(XU,&u);CHKERRQ(ierr); 361 ierr = VecRestoreArray(T2,&t2);CHKERRQ(ierr); 362 PetscFunctionReturn(0); 363 } 364 365 #undef __FUNCT__ 366 #define __FUNCT__ "D_SFischer" 367 /*@ 368 D_SFischer - Calculates an element of the B-subdifferential of the 369 smoothed Fischer-Burmeister function for complementarity problems. 370 371 Collective on jac 372 373 Input Parameters: 374 + jac - the jacobian of f at X 375 . X - current point 376 . F - constraint function evaluated at X 377 . XL - lower bounds 378 . XU - upper bounds 379 . mu - smoothing parameter 380 . T1 - work vector 381 - T2 - work vector 382 383 Output Parameter: 384 + Da - diagonal perturbation component of the result 385 . Db - row scaling component of the result 386 - Dm - derivative with respect to scaling parameter 387 388 Level: developer 389 390 .seealso D_Fischer() 391 @*/ 392 PetscErrorCode D_SFischer(Mat jac, Vec X, Vec Con,Vec XL, Vec XU, PetscReal mu,Vec T1, Vec T2,Vec Da, Vec Db, Vec Dm) 393 { 394 PetscErrorCode ierr; 395 PetscInt i,nn; 396 PetscReal *x, *f, *l, *u, *da, *db, *dm; 397 PetscReal ai, bi, ci, di, ei, fi; 398 399 PetscFunctionBegin; 400 if (PetscAbsReal(mu) <= PETSC_MACHINE_EPSILON) { 401 ierr = VecZeroEntries(Dm);CHKERRQ(ierr); 402 ierr = D_Fischer(jac, X, Con, XL, XU, T1, T2, Da, Db);CHKERRQ(ierr); 403 } else { 404 ierr = VecGetLocalSize(X,&nn);CHKERRQ(ierr); 405 ierr = VecGetArray(X,&x);CHKERRQ(ierr); 406 ierr = VecGetArray(Con,&f);CHKERRQ(ierr); 407 ierr = VecGetArray(XL,&l);CHKERRQ(ierr); 408 ierr = VecGetArray(XU,&u);CHKERRQ(ierr); 409 ierr = VecGetArray(Da,&da);CHKERRQ(ierr); 410 ierr = VecGetArray(Db,&db);CHKERRQ(ierr); 411 ierr = VecGetArray(Dm,&dm);CHKERRQ(ierr); 412 413 for (i = 0; i < nn; ++i) { 414 if ((l[i] <= PETSC_NINFINITY) && (u[i] >= PETSC_INFINITY)) { 415 da[i] = -mu; 416 db[i] = -1; 417 dm[i] = -x[i]; 418 } else if (l[i] <= PETSC_NINFINITY) { 419 bi = u[i] - x[i]; 420 ai = fischsnorm(bi, f[i], mu); 421 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 422 423 da[i] = bi / ai - 1; 424 db[i] = -f[i] / ai - 1; 425 dm[i] = 2.0 * mu / ai; 426 } else if (u[i] >= PETSC_INFINITY) { 427 bi = x[i] - l[i]; 428 ai = fischsnorm(bi, f[i], mu); 429 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 430 431 da[i] = bi / ai - 1; 432 db[i] = f[i] / ai - 1; 433 dm[i] = 2.0 * mu / ai; 434 } else if (l[i] == u[i]) { 435 da[i] = -1; 436 db[i] = 0; 437 dm[i] = 0; 438 } else { 439 bi = x[i] - u[i]; 440 ai = fischsnorm(bi, f[i], mu); 441 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 442 443 ci = bi / ai + 1; 444 di = f[i] / ai + 1; 445 fi = 2.0 * mu / ai; 446 447 ei = SFischer(u[i] - x[i], -f[i], mu); 448 ai = fischsnorm(x[i] - l[i], ei, mu); 449 ai = PetscMax(PETSC_MACHINE_EPSILON, ai); 450 451 bi = ei / ai - 1; 452 ei = 2.0 * mu / ei; 453 ai = (x[i] - l[i]) / ai - 1; 454 455 da[i] = ai + bi*ci; 456 db[i] = bi*di; 457 dm[i] = ei + bi*fi; 458 } 459 } 460 461 ierr = VecRestoreArray(X,&x);CHKERRQ(ierr); 462 ierr = VecRestoreArray(Con,&f);CHKERRQ(ierr); 463 ierr = VecRestoreArray(XL,&l);CHKERRQ(ierr); 464 ierr = VecRestoreArray(XU,&u);CHKERRQ(ierr); 465 ierr = VecRestoreArray(Da,&da);CHKERRQ(ierr); 466 ierr = VecRestoreArray(Db,&db);CHKERRQ(ierr); 467 ierr = VecRestoreArray(Dm,&dm);CHKERRQ(ierr); 468 } 469 PetscFunctionReturn(0); 470 } 471 472