Actual source code: tao_util.c

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

    171:   VecGetArrayRead(X, &x);
    172:   VecGetArrayRead(F, &f);
    173:   VecGetArrayRead(L, &l);
    174:   VecGetArrayRead(U, &u);
    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;

    198:   VecRestoreArrayRead(X, &x);
    199:   VecRestoreArrayRead(F, &f);
    200:   VecRestoreArrayRead(L, &l);
    201:   VecRestoreArrayRead(U, &u);
    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);
    249:   VecGetArrayRead(X,&x);
    250:   VecGetArrayRead(Con,&f);
    251:   VecGetArrayRead(XL,&l);
    252:   VecGetArrayRead(XU,&u);
    253:   VecGetArray(Da,&da);
    254:   VecGetArray(Db,&db);
    255:   VecGetArray(T1,&t1);
    256:   VecGetArrayRead(T2,&t2);

    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);
    277:   VecRestoreArrayRead(T2,&t2);
    278:   MatMult(jac,T1,T2);
    279:   VecGetArrayRead(T2,&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);
    353:   VecRestoreArrayRead(X,&x);
    354:   VecRestoreArrayRead(Con,&f);
    355:   VecRestoreArrayRead(XL,&l);
    356:   VecRestoreArrayRead(XU,&u);
    357:   VecRestoreArrayRead(T2,&t2);
    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);
    400:     VecGetArrayRead(X,&x);
    401:     VecGetArrayRead(Con,&f);
    402:     VecGetArrayRead(XL,&l);
    403:     VecGetArrayRead(XU,&u);
    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:     }

    456:     VecRestoreArrayRead(X,&x);
    457:     VecRestoreArrayRead(Con,&f);
    458:     VecRestoreArrayRead(XL,&l);
    459:     VecRestoreArrayRead(XU,&u);
    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: }