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