Actual source code: tao_util.c
1: #include <petsc/private/petscimpl.h>
2: #include <petsctao.h>
3: #include <petscsys.h>
5: static inline PetscReal Fischer(PetscReal a, PetscReal b)
6: {
7: /* Method suggested by Bob Vanderbei */
8: if (a + b <= 0) return PetscSqrtReal(a * a + b * b) - (a + b);
9: return -2.0 * a * b / (PetscSqrtReal(a * a + b * b) + (a + b));
10: }
12: /*@
13: VecFischer - Evaluates the Fischer-Burmeister function for complementarity
14: problems.
16: Logically Collective
18: Input Parameters:
19: + X - current point
20: . F - function evaluated at x
21: . L - lower bounds
22: - U - upper bounds
24: Output Parameter:
25: . FB - The Fischer-Burmeister function vector
27: Notes:
28: The Fischer-Burmeister function is defined as
29: $ phi(a,b) := sqrt(a*a + b*b) - a - b
30: and is used reformulate a complementarity problem as a semismooth
31: system of equations.
33: The result of this function is done by cases:
34: + l[i] == -infinity, u[i] == infinity -- fb[i] = -f[i]
35: . l[i] == -infinity, u[i] finite -- fb[i] = phi(u[i]-x[i], -f[i])
36: . l[i] finite, u[i] == infinity -- fb[i] = phi(x[i]-l[i], f[i])
37: . l[i] finite < u[i] finite -- fb[i] = phi(x[i]-l[i], phi(u[i]-x[i], -f[u]))
38: - otherwise l[i] == u[i] -- fb[i] = l[i] - x[i]
40: Level: developer
42: .seealso: `Vec`, `VecSFischer()`, `MatDFischer()`, `MatDSFischer()`
43: @*/
44: PetscErrorCode VecFischer(Vec X, Vec F, Vec L, Vec U, Vec FB)
45: {
46: const PetscScalar *x, *f, *l, *u;
47: PetscScalar *fb;
48: PetscReal xval, fval, lval, uval;
49: PetscInt low[5], high[5], n, i;
57: if (!L && !U) {
58: VecAXPBY(FB, -1.0, 0.0, F);
59: return 0;
60: }
62: VecGetOwnershipRange(X, low, high);
63: VecGetOwnershipRange(F, low + 1, high + 1);
64: VecGetOwnershipRange(L, low + 2, high + 2);
65: VecGetOwnershipRange(U, low + 3, high + 3);
66: VecGetOwnershipRange(FB, low + 4, high + 4);
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]);
80: fval = PetscRealPart(f[i]);
81: lval = PetscRealPart(l[i]);
82: uval = PetscRealPart(u[i]);
84: if (lval <= -PETSC_INFINITY && uval >= PETSC_INFINITY) {
85: fb[i] = -fval;
86: } else if (lval <= -PETSC_INFINITY) {
87: fb[i] = -Fischer(uval - xval, -fval);
88: } else if (uval >= PETSC_INFINITY) {
89: fb[i] = Fischer(xval - lval, fval);
90: } else if (lval == uval) {
91: fb[i] = lval - xval;
92: } else {
93: fval = Fischer(uval - xval, -fval);
94: fb[i] = Fischer(xval - lval, fval);
95: }
96: }
98: VecRestoreArrayRead(X, &x);
99: VecRestoreArrayRead(F, &f);
100: VecRestoreArrayRead(L, &l);
101: VecRestoreArrayRead(U, &u);
102: VecRestoreArray(FB, &fb);
103: return 0;
104: }
106: static inline PetscReal SFischer(PetscReal a, PetscReal b, PetscReal c)
107: {
108: /* Method suggested by Bob Vanderbei */
109: if (a + b <= 0) return PetscSqrtReal(a * a + b * b + 2.0 * c * c) - (a + b);
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
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 Parameter:
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: `Vec`, `VecFischer()`, `MatDFischer()`, `MatDSFischer()`
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: PetscInt low[5], high[5], n, i;
159: VecGetOwnershipRange(X, low, high);
160: VecGetOwnershipRange(F, low + 1, high + 1);
161: VecGetOwnershipRange(L, low + 2, high + 2);
162: VecGetOwnershipRange(U, low + 3, high + 3);
163: VecGetOwnershipRange(FB, low + 4, high + 4);
167: VecGetArrayRead(X, &x);
168: VecGetArrayRead(F, &f);
169: VecGetArrayRead(L, &l);
170: VecGetArrayRead(U, &u);
171: VecGetArray(FB, &fb);
173: VecGetLocalSize(X, &n);
175: for (i = 0; i < n; ++i) {
176: xval = PetscRealPart(*x++);
177: fval = PetscRealPart(*f++);
178: lval = PetscRealPart(*l++);
179: uval = PetscRealPart(*u++);
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;
195: f -= n;
196: l -= n;
197: u -= n;
198: fb -= n;
200: VecRestoreArrayRead(X, &x);
201: VecRestoreArrayRead(F, &f);
202: VecRestoreArrayRead(L, &l);
203: VecRestoreArrayRead(U, &u);
204: VecRestoreArray(FB, &fb);
205: return 0;
206: }
208: static inline PetscReal fischnorm(PetscReal a, PetscReal b)
209: {
210: return PetscSqrtReal(a * a + b * b);
211: }
213: static inline PetscReal fischsnorm(PetscReal a, PetscReal b, PetscReal c)
214: {
215: return PetscSqrtReal(a * a + b * b + 2.0 * c * c);
216: }
218: /*@
219: MatDFischer - Calculates an element of the B-subdifferential of the
220: Fischer-Burmeister function for complementarity problems.
222: Collective
224: Input Parameters:
225: + jac - the jacobian of f at X
226: . X - current point
227: . Con - constraints function evaluated at X
228: . XL - lower bounds
229: . XU - upper bounds
230: . t1 - work vector
231: - t2 - work vector
233: Output Parameters:
234: + Da - diagonal perturbation component of the result
235: - Db - row scaling component of the result
237: Level: developer
239: .seealso: `Mat`, `VecFischer()`, `VecSFischer()`, `MatDSFischer()`
240: @*/
241: PetscErrorCode MatDFischer(Mat jac, Vec X, Vec Con, Vec XL, Vec XU, Vec T1, Vec T2, Vec Da, Vec Db)
242: {
243: PetscInt i, nn;
244: const PetscScalar *x, *f, *l, *u, *t2;
245: PetscScalar *da, *db, *t1;
246: 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
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 Parameters:
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: `Mat`, `VecFischer()`, `VecDFischer()`, `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: PetscInt i, nn;
389: const PetscScalar *x, *f, *l, *u;
390: PetscScalar *da, *db, *dm;
391: PetscReal ai, bi, ci, di, ei, fi;
393: if (PetscAbsReal(mu) <= PETSC_MACHINE_EPSILON) {
394: VecZeroEntries(Dm);
395: MatDFischer(jac, X, Con, XL, XU, T1, T2, Da, Db);
396: } else {
397: VecGetLocalSize(X, &nn);
398: VecGetArrayRead(X, &x);
399: VecGetArrayRead(Con, &f);
400: VecGetArrayRead(XL, &l);
401: VecGetArrayRead(XU, &u);
402: VecGetArray(Da, &da);
403: VecGetArray(Db, &db);
404: VecGetArray(Dm, &dm);
406: for (i = 0; i < nn; ++i) {
407: if ((PetscRealPart(l[i]) <= PETSC_NINFINITY) && (PetscRealPart(u[i]) >= PETSC_INFINITY)) {
408: da[i] = -mu;
409: db[i] = -1.0;
410: dm[i] = -x[i];
411: } else if (PetscRealPart(l[i]) <= PETSC_NINFINITY) {
412: bi = PetscRealPart(u[i]) - PetscRealPart(x[i]);
413: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
414: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
416: da[i] = bi / ai - 1.0;
417: db[i] = -PetscRealPart(f[i]) / ai - 1.0;
418: dm[i] = 2.0 * mu / ai;
419: } else if (PetscRealPart(u[i]) >= PETSC_INFINITY) {
420: bi = PetscRealPart(x[i]) - PetscRealPart(l[i]);
421: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
422: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
424: da[i] = bi / ai - 1.0;
425: db[i] = PetscRealPart(f[i]) / ai - 1.0;
426: dm[i] = 2.0 * mu / ai;
427: } else if (PetscRealPart(l[i]) == PetscRealPart(u[i])) {
428: da[i] = -1.0;
429: db[i] = 0.0;
430: dm[i] = 0.0;
431: } else {
432: bi = PetscRealPart(x[i]) - PetscRealPart(u[i]);
433: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
434: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
436: ci = bi / ai + 1.0;
437: di = PetscRealPart(f[i]) / ai + 1.0;
438: fi = 2.0 * mu / ai;
440: ei = SFischer(PetscRealPart(u[i]) - PetscRealPart(x[i]), -PetscRealPart(f[i]), mu);
441: ai = fischsnorm(PetscRealPart(x[i]) - PetscRealPart(l[i]), ei, mu);
442: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
444: bi = ei / ai - 1.0;
445: ei = 2.0 * mu / ei;
446: ai = (PetscRealPart(x[i]) - PetscRealPart(l[i])) / ai - 1.0;
448: da[i] = ai + bi * ci;
449: db[i] = bi * di;
450: dm[i] = ei + bi * fi;
451: }
452: }
454: VecRestoreArrayRead(X, &x);
455: VecRestoreArrayRead(Con, &f);
456: VecRestoreArrayRead(XL, &l);
457: VecRestoreArrayRead(XU, &u);
458: VecRestoreArray(Da, &da);
459: VecRestoreArray(Db, &db);
460: VecRestoreArray(Dm, &dm);
461: }
462: return 0;
463: }
465: static inline PetscReal ST_InternalPN(PetscScalar in, PetscReal lb, PetscReal ub)
466: {
467: return PetscMax(0, (PetscReal)PetscRealPart(in) - ub) - PetscMax(0, -(PetscReal)PetscRealPart(in) - PetscAbsReal(lb));
468: }
470: static inline PetscReal ST_InternalNN(PetscScalar in, PetscReal lb, PetscReal ub)
471: {
472: return PetscMax(0, (PetscReal)PetscRealPart(in) + PetscAbsReal(ub)) - PetscMax(0, -(PetscReal)PetscRealPart(in) - PetscAbsReal(lb));
473: }
475: static inline PetscReal ST_InternalPP(PetscScalar in, PetscReal lb, PetscReal ub)
476: {
477: return PetscMax(0, (PetscReal)PetscRealPart(in) - ub) + PetscMin(0, (PetscReal)PetscRealPart(in) - lb);
478: }
480: /*@
481: TaoSoftThreshold - Calculates soft thresholding routine with input vector
482: and given lower and upper bound and returns it to output vector.
484: Input Parameters:
485: + in - input vector to be thresholded
486: . lb - lower bound
487: - ub - upper bound
489: Output Parameter:
490: . out - Soft thresholded output vector
492: Notes:
493: Soft thresholding is defined as
494: \[ S(input,lb,ub) =
495: \begin{cases}
496: input - ub \text{input > ub} \\
497: 0 \text{lb =< input <= ub} \\
498: input + lb \text{input < lb} \\
499: \]
501: Level: developer
503: .seealso: `Tao`, `Vec`
504: @*/
505: PetscErrorCode TaoSoftThreshold(Vec in, PetscReal lb, PetscReal ub, Vec out)
506: {
507: PetscInt i, nlocal, mlocal;
508: PetscScalar *inarray, *outarray;
510: VecGetArrayPair(in, out, &inarray, &outarray);
511: VecGetLocalSize(in, &nlocal);
512: VecGetLocalSize(in, &mlocal);
517: if (ub >= 0 && lb < 0) {
518: for (i = 0; i < nlocal; i++) outarray[i] = ST_InternalPN(inarray[i], lb, ub);
519: } else if (ub < 0 && lb < 0) {
520: for (i = 0; i < nlocal; i++) outarray[i] = ST_InternalNN(inarray[i], lb, ub);
521: } else {
522: for (i = 0; i < nlocal; i++) outarray[i] = ST_InternalPP(inarray[i], lb, ub);
523: }
525: VecRestoreArrayPair(in, out, &inarray, &outarray);
526: return 0;
527: }