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