Actual source code: tao_util.c
petsc-3.14.6 2021-03-30
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: }