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