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