Actual source code: tao_util.c

petsc-master 2020-11-29
Report Typos and Errors
  1: #include <petsc/private/petscimpl.h>
2: #include <petsctao.h>
3: #include <petscsys.h>

5: PETSC_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: }

14: /*@
15:    VecFischer - Evaluates the Fischer-Burmeister function for complementarity
16:    problems.

18:    Logically Collective on vectors

20:    Input Parameters:
21: +  X - current point
22: .  F - function evaluated at x
23: .  L - lower bounds
24: -  U - upper bounds

26:    Output Parameters:
27: .  FB - The Fischer-Burmeister function vector

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. 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] 42: Level: developer 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: PetscErrorCode ierr; 51: PetscInt low[5], high[5], n, i; 60: VecGetOwnershipRange(X, low, high); 61: VecGetOwnershipRange(F, low + 1, high + 1); 62: VecGetOwnershipRange(L, low + 2, high + 2); 63: VecGetOwnershipRange(U, low + 3, high + 3); 64: VecGetOwnershipRange(FB, low + 4, high + 4); 66: for (i = 1; i < 4; ++i) { 67: if (low[0] != low[i] || high[0] != high[i]) SETERRQ(PETSC_COMM_SELF,PETSC_ERR_ARG_WRONG,"Vectors must be identically loaded over processors"); 68: } 70: VecGetArrayRead(X, &x); 71: VecGetArrayRead(F, &f); 72: VecGetArrayRead(L, &l); 73: VecGetArrayRead(U, &u); 74: VecGetArray(FB, &fb); 76: VecGetLocalSize(X, &n); 78: for (i = 0; i < n; ++i) { 79: xval = PetscRealPart(x[i]); fval = PetscRealPart(f[i]); 80: lval = PetscRealPart(l[i]); uval = PetscRealPart(u[i]); 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: } 96: VecRestoreArrayRead(X, &x); 97: VecRestoreArrayRead(F, &f); 98: VecRestoreArrayRead(L, &l); 99: VecRestoreArrayRead(U, &u); 100: VecRestoreArray(FB, &fb); 101: return(0); 102: } 104: PETSC_STATIC_INLINE PetscReal SFischer(PetscReal a, PetscReal b, PetscReal c) 105: { 106: /* Method suggested by Bob Vanderbei */ 107: if (a + b <= 0) { 108: return PetscSqrtReal(a*a + b*b + 2.0*c*c) - (a + b); 109: } 110: return 2.0*(c*c - a*b) / (PetscSqrtReal(a*a + b*b + 2.0*c*c) + (a + b)); 111: } 113: /*@ 114: VecSFischer - Evaluates the Smoothed Fischer-Burmeister function for 115: complementarity problems. 117: Logically Collective on vectors 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 126: Output Parameters: 127: . FB - The Smoothed Fischer-Burmeister function vector 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.

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]

142:    Level: developer

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:   PetscErrorCode    ierr;
152:   PetscInt          low[5], high[5], n, i;

161:   VecGetOwnershipRange(X, low, high);
162:   VecGetOwnershipRange(F, low + 1, high + 1);
163:   VecGetOwnershipRange(L, low + 2, high + 2);
164:   VecGetOwnershipRange(U, low + 3, high + 3);
165:   VecGetOwnershipRange(FB, low + 4, high + 4);

167:   for (i = 1; i < 4; ++i) {
168:     if (low[0] != low[i] || high[0] != high[i]) SETERRQ(PETSC_COMM_SELF,PETSC_ERR_ARG_WRONG,"Vectors must be identically loaded over processors");
169:   }

175:   VecGetArray(FB, &fb);

177:   VecGetLocalSize(X, &n);

179:   for (i = 0; i < n; ++i) {
180:     xval = PetscRealPart(*x++); fval = PetscRealPart(*f++);
181:     lval = PetscRealPart(*l++); uval = PetscRealPart(*u++);

183:     if ((lval <= -PETSC_INFINITY) && (uval >= PETSC_INFINITY)) {
184:       (*fb++) = -fval - mu*xval;
185:     } else if (lval <= -PETSC_INFINITY) {
186:       (*fb++) = -SFischer(uval - xval, -fval, mu);
187:     } else if (uval >=  PETSC_INFINITY) {
188:       (*fb++) =  SFischer(xval - lval,  fval, mu);
189:     } else if (lval == uval) {
190:       (*fb++) = lval - xval;
191:     } else {
192:       fval    =  SFischer(uval - xval, -fval, mu);
193:       (*fb++) =  SFischer(xval - lval,  fval, mu);
194:     }
195:   }
196:   x -= n; f -= n; l -=n; u -= n; fb -= n;

202:   VecRestoreArray(FB, &fb);
203:   return(0);
204: }

206: PETSC_STATIC_INLINE PetscReal fischnorm(PetscReal a, PetscReal b)
207: {
208:   return PetscSqrtReal(a*a + b*b);
209: }

211: PETSC_STATIC_INLINE PetscReal fischsnorm(PetscReal a, PetscReal b, PetscReal c)
212: {
213:   return PetscSqrtReal(a*a + b*b + 2.0*c*c);
214: }

216: /*@
217:    MatDFischer - Calculates an element of the B-subdifferential of the
218:    Fischer-Burmeister function for complementarity problems.

220:    Collective on jac

222:    Input Parameters:
223: +  jac - the jacobian of f at X
224: .  X - current point
225: .  Con - constraints function evaluated at X
226: .  XL - lower bounds
227: .  XU - upper bounds
228: .  t1 - work vector
229: -  t2 - work vector

231:    Output Parameters:
232: +  Da - diagonal perturbation component of the result
233: -  Db - row scaling component of the result

235:    Level: developer

237: .seealso: VecFischer()
238: @*/
239: PetscErrorCode MatDFischer(Mat jac, Vec X, Vec Con, Vec XL, Vec XU, Vec T1, Vec T2, Vec Da, Vec Db)
240: {
241:   PetscErrorCode    ierr;
242:   PetscInt          i,nn;
243:   const PetscScalar *x,*f,*l,*u,*t2;
244:   PetscScalar       *da,*db,*t1;
245:   PetscReal          ai,bi,ci,di,ei;

248:   VecGetLocalSize(X,&nn);
253:   VecGetArray(Da,&da);
254:   VecGetArray(Db,&db);
255:   VecGetArray(T1,&t1);

258:   for (i = 0; i < nn; i++) {
259:     da[i] = 0.0;
260:     db[i] = 0.0;
261:     t1[i] = 0.0;

263:     if (PetscAbsScalar(f[i]) <= PETSC_MACHINE_EPSILON) {
264:       if (PetscRealPart(l[i]) > PETSC_NINFINITY && PetscAbsScalar(x[i] - l[i]) <= PETSC_MACHINE_EPSILON) {
265:         t1[i] = 1.0;
266:         da[i] = 1.0;
267:       }

269:       if (PetscRealPart(u[i]) <  PETSC_INFINITY && PetscAbsScalar(u[i] - x[i]) <= PETSC_MACHINE_EPSILON) {
270:         t1[i] = 1.0;
271:         db[i] = 1.0;
272:       }
273:     }
274:   }

276:   VecRestoreArray(T1,&t1);
278:   MatMult(jac,T1,T2);

281:   for (i = 0; i < nn; i++) {
282:     if ((PetscRealPart(l[i]) <= PETSC_NINFINITY) && (PetscRealPart(u[i]) >= PETSC_INFINITY)) {
283:       da[i] = 0.0;
284:       db[i] = -1.0;
285:     } else if (PetscRealPart(l[i]) <= PETSC_NINFINITY) {
286:       if (PetscRealPart(db[i]) >= 1) {
287:         ai = fischnorm(1.0, PetscRealPart(t2[i]));

289:         da[i] = -1.0 / ai - 1.0;
290:         db[i] = -t2[i] / ai - 1.0;
291:       } else {
292:         bi = PetscRealPart(u[i]) - PetscRealPart(x[i]);
293:         ai = fischnorm(bi, PetscRealPart(f[i]));
294:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

296:         da[i] = bi / ai - 1.0;
297:         db[i] = -f[i] / ai - 1.0;
298:       }
299:     } else if (PetscRealPart(u[i]) >=  PETSC_INFINITY) {
300:       if (PetscRealPart(da[i]) >= 1) {
301:         ai = fischnorm(1.0, PetscRealPart(t2[i]));

303:         da[i] = 1.0 / ai - 1.0;
304:         db[i] = t2[i] / ai - 1.0;
305:       } else {
306:         bi = PetscRealPart(x[i]) - PetscRealPart(l[i]);
307:         ai = fischnorm(bi, PetscRealPart(f[i]));
308:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

310:         da[i] = bi / ai - 1.0;
311:         db[i] = f[i] / ai - 1.0;
312:       }
313:     } else if (PetscRealPart(l[i]) == PetscRealPart(u[i])) {
314:       da[i] = -1.0;
315:       db[i] = 0.0;
316:     } else {
317:       if (PetscRealPart(db[i]) >= 1) {
318:         ai = fischnorm(1.0, PetscRealPart(t2[i]));

320:         ci = 1.0 / ai + 1.0;
321:         di = PetscRealPart(t2[i]) / ai + 1.0;
322:       } else {
323:         bi = PetscRealPart(x[i]) - PetscRealPart(u[i]);
324:         ai = fischnorm(bi, PetscRealPart(f[i]));
325:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

327:         ci = bi / ai + 1.0;
328:         di = PetscRealPart(f[i]) / ai + 1.0;
329:       }

331:       if (PetscRealPart(da[i]) >= 1) {
332:         bi = ci + di*PetscRealPart(t2[i]);
333:         ai = fischnorm(1.0, bi);

335:         bi = bi / ai - 1.0;
336:         ai = 1.0 / ai - 1.0;
337:       } else {
338:         ei = Fischer(PetscRealPart(u[i]) - PetscRealPart(x[i]), -PetscRealPart(f[i]));
339:         ai = fischnorm(PetscRealPart(x[i]) - PetscRealPart(l[i]), ei);
340:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

342:         bi = ei / ai - 1.0;
343:         ai = (PetscRealPart(x[i]) - PetscRealPart(l[i])) / ai - 1.0;
344:       }

346:       da[i] = ai + bi*ci;
347:       db[i] = bi*di;
348:     }
349:   }

351:   VecRestoreArray(Da,&da);
352:   VecRestoreArray(Db,&db);
358:   return(0);
359: }

361: /*@
362:    MatDSFischer - Calculates an element of the B-subdifferential of the
363:    smoothed Fischer-Burmeister function for complementarity problems.

365:    Collective on jac

367:    Input Parameters:
368: +  jac - the jacobian of f at X
369: .  X - current point
370: .  F - constraint function evaluated at X
371: .  XL - lower bounds
372: .  XU - upper bounds
373: .  mu - smoothing parameter
374: .  T1 - work vector
375: -  T2 - work vector

377:    Output Parameter:
378: +  Da - diagonal perturbation component of the result
379: .  Db - row scaling component of the result
380: -  Dm - derivative with respect to scaling parameter

382:    Level: developer

384: .seealso MatDFischer()
385: @*/
386: PetscErrorCode MatDSFischer(Mat jac, Vec X, Vec Con,Vec XL, Vec XU, PetscReal mu,Vec T1, Vec T2,Vec Da, Vec Db, Vec Dm)
387: {
388:   PetscErrorCode    ierr;
389:   PetscInt          i,nn;
390:   const PetscScalar *x, *f, *l, *u;
391:   PetscScalar       *da, *db, *dm;
392:   PetscReal         ai, bi, ci, di, ei, fi;

395:   if (PetscAbsReal(mu) <= PETSC_MACHINE_EPSILON) {
396:     VecZeroEntries(Dm);
397:     MatDFischer(jac, X, Con, XL, XU, T1, T2, Da, Db);
398:   } else {
399:     VecGetLocalSize(X,&nn);
404:     VecGetArray(Da,&da);
405:     VecGetArray(Db,&db);
406:     VecGetArray(Dm,&dm);

408:     for (i = 0; i < nn; ++i) {
409:       if ((PetscRealPart(l[i]) <= PETSC_NINFINITY) && (PetscRealPart(u[i]) >= PETSC_INFINITY)) {
410:         da[i] = -mu;
411:         db[i] = -1.0;
412:         dm[i] = -x[i];
413:       } else if (PetscRealPart(l[i]) <= PETSC_NINFINITY) {
414:         bi = PetscRealPart(u[i]) - PetscRealPart(x[i]);
415:         ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
416:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

418:         da[i] = bi / ai - 1.0;
419:         db[i] = -PetscRealPart(f[i]) / ai - 1.0;
420:         dm[i] = 2.0 * mu / ai;
421:       } else if (PetscRealPart(u[i]) >=  PETSC_INFINITY) {
422:         bi = PetscRealPart(x[i]) - PetscRealPart(l[i]);
423:         ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
424:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

426:         da[i] = bi / ai - 1.0;
427:         db[i] = PetscRealPart(f[i]) / ai - 1.0;
428:         dm[i] = 2.0 * mu / ai;
429:       } else if (PetscRealPart(l[i]) == PetscRealPart(u[i])) {
430:         da[i] = -1.0;
431:         db[i] = 0.0;
432:         dm[i] = 0.0;
433:       } else {
434:         bi = PetscRealPart(x[i]) - PetscRealPart(u[i]);
435:         ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
436:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

438:         ci = bi / ai + 1.0;
439:         di = PetscRealPart(f[i]) / ai + 1.0;
440:         fi = 2.0 * mu / ai;

442:         ei = SFischer(PetscRealPart(u[i]) - PetscRealPart(x[i]), -PetscRealPart(f[i]), mu);
443:         ai = fischsnorm(PetscRealPart(x[i]) - PetscRealPart(l[i]), ei, mu);
444:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

446:         bi = ei / ai - 1.0;
447:         ei = 2.0 * mu / ei;
448:         ai = (PetscRealPart(x[i]) - PetscRealPart(l[i])) / ai - 1.0;

450:         da[i] = ai + bi*ci;
451:         db[i] = bi*di;
452:         dm[i] = ei + bi*fi;
453:       }
454:     }

460:     VecRestoreArray(Da,&da);
461:     VecRestoreArray(Db,&db);
462:     VecRestoreArray(Dm,&dm);
463:   }
464:   return(0);
465: }

467: PETSC_STATIC_INLINE PetscReal ST_InternalPN(PetscScalar in, PetscReal lb, PetscReal ub)
468: {
469:   return PetscMax(0,(PetscReal)PetscRealPart(in)-ub) - PetscMax(0,-(PetscReal)PetscRealPart(in)-PetscAbsReal(lb));
470: }

472: PETSC_STATIC_INLINE PetscReal ST_InternalNN(PetscScalar in, PetscReal lb, PetscReal ub)
473: {
474:   return PetscMax(0,(PetscReal)PetscRealPart(in) + PetscAbsReal(ub)) - PetscMax(0,-(PetscReal)PetscRealPart(in) - PetscAbsReal(lb));
475: }

477: PETSC_STATIC_INLINE PetscReal ST_InternalPP(PetscScalar in, PetscReal lb, PetscReal ub)
478: {
479:   return PetscMax(0, (PetscReal)PetscRealPart(in)-ub) + PetscMin(0, (PetscReal)PetscRealPart(in) - lb);
480: }

482: /*@
483:    TaoSoftThreshold - Calculates soft thresholding routine with input vector
484:    and given lower and upper bound and returns it to output vector.

486:    Input Parameters:
487: +  in - input vector to be thresholded
488: .  lb - lower bound
489: -  ub - upper bound

491:    Output Parameters:
492: .  out - Soft thresholded output vector

494:    Notes:
495:    Soft thresholding is defined as
496:    $S(input,lb,ub) = 497: \begin{cases} 498: input - ub \text{input > ub} \\ 499: 0 \text{lb =< input <= ub} \\ 500: input + lb \text{input < lb} \\ 501:$

503:    Level: developer

505: @*/
506: PetscErrorCode TaoSoftThreshold(Vec in, PetscReal lb, PetscReal ub, Vec out)
507: {
509:   PetscInt       i, nlocal, mlocal;
510:   PetscScalar   *inarray, *outarray;

513:   VecGetArrayPair(in, out, &inarray, &outarray);
514:   VecGetLocalSize(in, &nlocal);
515:   VecGetLocalSize(in, &mlocal);

517:   if (nlocal != mlocal) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_ARG_INCOMP, "Input and output vectors need to be of same size.");
518:   if (lb == ub) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_ARG_INCOMP, "Lower bound and upper bound need to be different.");
519:   if (lb > ub) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_ARG_INCOMP, "Lower bound needs to be lower than upper bound.");

521:   if (ub >= 0 && lb < 0){
522:     for (i=0; i<nlocal; i++) outarray[i] = ST_InternalPN(inarray[i], lb, ub);
523:   } else if (ub < 0 && lb < 0){
524:     for (i=0; i<nlocal; i++) outarray[i] = ST_InternalNN(inarray[i], lb, ub);
525:   } else {
526:     for (i=0; i<nlocal; i++) outarray[i] = ST_InternalPP(inarray[i], lb, ub);
527:   }

529:   VecRestoreArrayPair(in, out, &inarray, &outarray);
530:   return(0);
531: }