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 on vectors
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: @*/
43: PetscErrorCode VecFischer(Vec X, Vec F, Vec L, Vec U, Vec FB)
44: {
45: const PetscScalar *x, *f, *l, *u;
46: PetscScalar *fb;
47: PetscReal xval, fval, lval, uval;
48: PetscInt low[5], high[5], n, i;
56: if (!L && !U) {
57: VecAXPBY(FB, -1.0, 0.0, F);
58: return 0;
59: }
61: VecGetOwnershipRange(X, low, high);
62: VecGetOwnershipRange(F, low + 1, high + 1);
63: VecGetOwnershipRange(L, low + 2, high + 2);
64: VecGetOwnershipRange(U, low + 3, high + 3);
65: VecGetOwnershipRange(FB, low + 4, high + 4);
69: VecGetArrayRead(X, &x);
70: VecGetArrayRead(F, &f);
71: VecGetArrayRead(L, &l);
72: VecGetArrayRead(U, &u);
73: VecGetArray(FB, &fb);
75: VecGetLocalSize(X, &n);
77: for (i = 0; i < n; ++i) {
78: xval = PetscRealPart(x[i]);
79: fval = PetscRealPart(f[i]);
80: lval = PetscRealPart(l[i]);
81: uval = PetscRealPart(u[i]);
83: if (lval <= -PETSC_INFINITY && uval >= PETSC_INFINITY) {
84: fb[i] = -fval;
85: } else if (lval <= -PETSC_INFINITY) {
86: fb[i] = -Fischer(uval - xval, -fval);
87: } else if (uval >= PETSC_INFINITY) {
88: fb[i] = Fischer(xval - lval, fval);
89: } else if (lval == uval) {
90: fb[i] = lval - xval;
91: } else {
92: fval = Fischer(uval - xval, -fval);
93: fb[i] = Fischer(xval - lval, fval);
94: }
95: }
97: VecRestoreArrayRead(X, &x);
98: VecRestoreArrayRead(F, &f);
99: VecRestoreArrayRead(L, &l);
100: VecRestoreArrayRead(U, &u);
101: VecRestoreArray(FB, &fb);
102: return 0;
103: }
105: static inline PetscReal SFischer(PetscReal a, PetscReal b, PetscReal c)
106: {
107: /* Method suggested by Bob Vanderbei */
108: if (a + b <= 0) return PetscSqrtReal(a * a + b * b + 2.0 * c * c) - (a + b);
109: return 2.0 * (c * c - a * b) / (PetscSqrtReal(a * a + b * b + 2.0 * c * c) + (a + b));
110: }
112: /*@
113: VecSFischer - Evaluates the Smoothed Fischer-Burmeister function for
114: complementarity problems.
116: Logically Collective on vectors
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
125: Output Parameter:
126: . FB - The Smoothed Fischer-Burmeister function vector
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.
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]
141: Level: developer
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;
158: VecGetOwnershipRange(X, low, high);
159: VecGetOwnershipRange(F, low + 1, high + 1);
160: VecGetOwnershipRange(L, low + 2, high + 2);
161: VecGetOwnershipRange(U, low + 3, high + 3);
162: VecGetOwnershipRange(FB, low + 4, high + 4);
166: VecGetArrayRead(X, &x);
167: VecGetArrayRead(F, &f);
168: VecGetArrayRead(L, &l);
169: VecGetArrayRead(U, &u);
170: VecGetArray(FB, &fb);
172: VecGetLocalSize(X, &n);
174: for (i = 0; i < n; ++i) {
175: xval = PetscRealPart(*x++);
176: fval = PetscRealPart(*f++);
177: lval = PetscRealPart(*l++);
178: uval = PetscRealPart(*u++);
180: if ((lval <= -PETSC_INFINITY) && (uval >= PETSC_INFINITY)) {
181: (*fb++) = -fval - mu * xval;
182: } else if (lval <= -PETSC_INFINITY) {
183: (*fb++) = -SFischer(uval - xval, -fval, mu);
184: } else if (uval >= PETSC_INFINITY) {
185: (*fb++) = SFischer(xval - lval, fval, mu);
186: } else if (lval == uval) {
187: (*fb++) = lval - xval;
188: } else {
189: fval = SFischer(uval - xval, -fval, mu);
190: (*fb++) = SFischer(xval - lval, fval, mu);
191: }
192: }
193: x -= n;
194: f -= n;
195: l -= n;
196: u -= n;
197: fb -= n;
199: VecRestoreArrayRead(X, &x);
200: VecRestoreArrayRead(F, &f);
201: VecRestoreArrayRead(L, &l);
202: VecRestoreArrayRead(U, &u);
203: VecRestoreArray(FB, &fb);
204: return 0;
205: }
207: static inline PetscReal fischnorm(PetscReal a, PetscReal b)
208: {
209: return PetscSqrtReal(a * a + b * b);
210: }
212: static inline PetscReal fischsnorm(PetscReal a, PetscReal b, PetscReal c)
213: {
214: return PetscSqrtReal(a * a + b * b + 2.0 * c * c);
215: }
217: /*@
218: MatDFischer - Calculates an element of the B-subdifferential of the
219: Fischer-Burmeister function for complementarity problems.
221: Collective on jac
223: Input Parameters:
224: + jac - the jacobian of f at X
225: . X - current point
226: . Con - constraints function evaluated at X
227: . XL - lower bounds
228: . XU - upper bounds
229: . t1 - work vector
230: - t2 - work vector
232: Output Parameters:
233: + Da - diagonal perturbation component of the result
234: - Db - row scaling component of the result
236: Level: developer
238: .seealso: `VecFischer()`
239: @*/
240: PetscErrorCode MatDFischer(Mat jac, Vec X, Vec Con, Vec XL, Vec XU, Vec T1, Vec T2, Vec Da, Vec Db)
241: {
242: PetscInt i, nn;
243: const PetscScalar *x, *f, *l, *u, *t2;
244: PetscScalar *da, *db, *t1;
245: PetscReal ai, bi, ci, di, ei;
247: VecGetLocalSize(X, &nn);
248: VecGetArrayRead(X, &x);
249: VecGetArrayRead(Con, &f);
250: VecGetArrayRead(XL, &l);
251: VecGetArrayRead(XU, &u);
252: VecGetArray(Da, &da);
253: VecGetArray(Db, &db);
254: VecGetArray(T1, &t1);
255: VecGetArrayRead(T2, &t2);
257: for (i = 0; i < nn; i++) {
258: da[i] = 0.0;
259: db[i] = 0.0;
260: t1[i] = 0.0;
262: if (PetscAbsScalar(f[i]) <= PETSC_MACHINE_EPSILON) {
263: if (PetscRealPart(l[i]) > PETSC_NINFINITY && PetscAbsScalar(x[i] - l[i]) <= PETSC_MACHINE_EPSILON) {
264: t1[i] = 1.0;
265: da[i] = 1.0;
266: }
268: if (PetscRealPart(u[i]) < PETSC_INFINITY && PetscAbsScalar(u[i] - x[i]) <= PETSC_MACHINE_EPSILON) {
269: t1[i] = 1.0;
270: db[i] = 1.0;
271: }
272: }
273: }
275: VecRestoreArray(T1, &t1);
276: VecRestoreArrayRead(T2, &t2);
277: MatMult(jac, T1, T2);
278: VecGetArrayRead(T2, &t2);
280: for (i = 0; i < nn; i++) {
281: if ((PetscRealPart(l[i]) <= PETSC_NINFINITY) && (PetscRealPart(u[i]) >= PETSC_INFINITY)) {
282: da[i] = 0.0;
283: db[i] = -1.0;
284: } else if (PetscRealPart(l[i]) <= PETSC_NINFINITY) {
285: if (PetscRealPart(db[i]) >= 1) {
286: ai = fischnorm(1.0, PetscRealPart(t2[i]));
288: da[i] = -1.0 / ai - 1.0;
289: db[i] = -t2[i] / ai - 1.0;
290: } else {
291: bi = PetscRealPart(u[i]) - PetscRealPart(x[i]);
292: ai = fischnorm(bi, PetscRealPart(f[i]));
293: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
295: da[i] = bi / ai - 1.0;
296: db[i] = -f[i] / ai - 1.0;
297: }
298: } else if (PetscRealPart(u[i]) >= PETSC_INFINITY) {
299: if (PetscRealPart(da[i]) >= 1) {
300: ai = fischnorm(1.0, PetscRealPart(t2[i]));
302: da[i] = 1.0 / ai - 1.0;
303: db[i] = t2[i] / ai - 1.0;
304: } else {
305: bi = PetscRealPart(x[i]) - PetscRealPart(l[i]);
306: ai = fischnorm(bi, PetscRealPart(f[i]));
307: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
309: da[i] = bi / ai - 1.0;
310: db[i] = f[i] / ai - 1.0;
311: }
312: } else if (PetscRealPart(l[i]) == PetscRealPart(u[i])) {
313: da[i] = -1.0;
314: db[i] = 0.0;
315: } else {
316: if (PetscRealPart(db[i]) >= 1) {
317: ai = fischnorm(1.0, PetscRealPart(t2[i]));
319: ci = 1.0 / ai + 1.0;
320: di = PetscRealPart(t2[i]) / ai + 1.0;
321: } else {
322: bi = PetscRealPart(x[i]) - PetscRealPart(u[i]);
323: ai = fischnorm(bi, PetscRealPart(f[i]));
324: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
326: ci = bi / ai + 1.0;
327: di = PetscRealPart(f[i]) / ai + 1.0;
328: }
330: if (PetscRealPart(da[i]) >= 1) {
331: bi = ci + di * PetscRealPart(t2[i]);
332: ai = fischnorm(1.0, bi);
334: bi = bi / ai - 1.0;
335: ai = 1.0 / ai - 1.0;
336: } else {
337: ei = Fischer(PetscRealPart(u[i]) - PetscRealPart(x[i]), -PetscRealPart(f[i]));
338: ai = fischnorm(PetscRealPart(x[i]) - PetscRealPart(l[i]), ei);
339: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
341: bi = ei / ai - 1.0;
342: ai = (PetscRealPart(x[i]) - PetscRealPart(l[i])) / ai - 1.0;
343: }
345: da[i] = ai + bi * ci;
346: db[i] = bi * di;
347: }
348: }
350: VecRestoreArray(Da, &da);
351: VecRestoreArray(Db, &db);
352: VecRestoreArrayRead(X, &x);
353: VecRestoreArrayRead(Con, &f);
354: VecRestoreArrayRead(XL, &l);
355: VecRestoreArrayRead(XU, &u);
356: VecRestoreArrayRead(T2, &t2);
357: return 0;
358: }
360: /*@
361: MatDSFischer - Calculates an element of the B-subdifferential of the
362: smoothed Fischer-Burmeister function for complementarity problems.
364: Collective on jac
366: Input Parameters:
367: + jac - the jacobian of f at X
368: . X - current point
369: . F - constraint function evaluated at X
370: . XL - lower bounds
371: . XU - upper bounds
372: . mu - smoothing parameter
373: . T1 - work vector
374: - T2 - work vector
376: Output Parameters:
377: + Da - diagonal perturbation component of the result
378: . Db - row scaling component of the result
379: - Dm - derivative with respect to scaling parameter
381: Level: developer
383: .seealso `MatDFischer()`
384: @*/
385: PetscErrorCode MatDSFischer(Mat jac, Vec X, Vec Con, Vec XL, Vec XU, PetscReal mu, Vec T1, Vec T2, Vec Da, Vec Db, Vec Dm)
386: {
387: PetscInt i, nn;
388: const PetscScalar *x, *f, *l, *u;
389: PetscScalar *da, *db, *dm;
390: PetscReal ai, bi, ci, di, ei, fi;
392: if (PetscAbsReal(mu) <= PETSC_MACHINE_EPSILON) {
393: VecZeroEntries(Dm);
394: MatDFischer(jac, X, Con, XL, XU, T1, T2, Da, Db);
395: } else {
396: VecGetLocalSize(X, &nn);
397: VecGetArrayRead(X, &x);
398: VecGetArrayRead(Con, &f);
399: VecGetArrayRead(XL, &l);
400: VecGetArrayRead(XU, &u);
401: VecGetArray(Da, &da);
402: VecGetArray(Db, &db);
403: VecGetArray(Dm, &dm);
405: for (i = 0; i < nn; ++i) {
406: if ((PetscRealPart(l[i]) <= PETSC_NINFINITY) && (PetscRealPart(u[i]) >= PETSC_INFINITY)) {
407: da[i] = -mu;
408: db[i] = -1.0;
409: dm[i] = -x[i];
410: } else if (PetscRealPart(l[i]) <= PETSC_NINFINITY) {
411: bi = PetscRealPart(u[i]) - PetscRealPart(x[i]);
412: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
413: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
415: da[i] = bi / ai - 1.0;
416: db[i] = -PetscRealPart(f[i]) / ai - 1.0;
417: dm[i] = 2.0 * mu / ai;
418: } else if (PetscRealPart(u[i]) >= PETSC_INFINITY) {
419: bi = PetscRealPart(x[i]) - PetscRealPart(l[i]);
420: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
421: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
423: da[i] = bi / ai - 1.0;
424: db[i] = PetscRealPart(f[i]) / ai - 1.0;
425: dm[i] = 2.0 * mu / ai;
426: } else if (PetscRealPart(l[i]) == PetscRealPart(u[i])) {
427: da[i] = -1.0;
428: db[i] = 0.0;
429: dm[i] = 0.0;
430: } else {
431: bi = PetscRealPart(x[i]) - PetscRealPart(u[i]);
432: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
433: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
435: ci = bi / ai + 1.0;
436: di = PetscRealPart(f[i]) / ai + 1.0;
437: fi = 2.0 * mu / ai;
439: ei = SFischer(PetscRealPart(u[i]) - PetscRealPart(x[i]), -PetscRealPart(f[i]), mu);
440: ai = fischsnorm(PetscRealPart(x[i]) - PetscRealPart(l[i]), ei, mu);
441: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
443: bi = ei / ai - 1.0;
444: ei = 2.0 * mu / ei;
445: ai = (PetscRealPart(x[i]) - PetscRealPart(l[i])) / ai - 1.0;
447: da[i] = ai + bi * ci;
448: db[i] = bi * di;
449: dm[i] = ei + bi * fi;
450: }
451: }
453: VecRestoreArrayRead(X, &x);
454: VecRestoreArrayRead(Con, &f);
455: VecRestoreArrayRead(XL, &l);
456: VecRestoreArrayRead(XU, &u);
457: VecRestoreArray(Da, &da);
458: VecRestoreArray(Db, &db);
459: VecRestoreArray(Dm, &dm);
460: }
461: return 0;
462: }
464: static inline PetscReal ST_InternalPN(PetscScalar in, PetscReal lb, PetscReal ub)
465: {
466: return PetscMax(0, (PetscReal)PetscRealPart(in) - ub) - PetscMax(0, -(PetscReal)PetscRealPart(in) - PetscAbsReal(lb));
467: }
469: static inline PetscReal ST_InternalNN(PetscScalar in, PetscReal lb, PetscReal ub)
470: {
471: return PetscMax(0, (PetscReal)PetscRealPart(in) + PetscAbsReal(ub)) - PetscMax(0, -(PetscReal)PetscRealPart(in) - PetscAbsReal(lb));
472: }
474: static inline PetscReal ST_InternalPP(PetscScalar in, PetscReal lb, PetscReal ub)
475: {
476: return PetscMax(0, (PetscReal)PetscRealPart(in) - ub) + PetscMin(0, (PetscReal)PetscRealPart(in) - lb);
477: }
479: /*@
480: TaoSoftThreshold - Calculates soft thresholding routine with input vector
481: and given lower and upper bound and returns it to output vector.
483: Input Parameters:
484: + in - input vector to be thresholded
485: . lb - lower bound
486: - ub - upper bound
488: Output Parameter:
489: . out - Soft thresholded output vector
491: Notes:
492: Soft thresholding is defined as
493: \[ S(input,lb,ub) =
494: \begin{cases}
495: input - ub \text{input > ub} \\
496: 0 \text{lb =< input <= ub} \\
497: input + lb \text{input < lb} \\
498: \]
500: Level: developer
502: @*/
503: PetscErrorCode TaoSoftThreshold(Vec in, PetscReal lb, PetscReal ub, Vec out)
504: {
505: PetscInt i, nlocal, mlocal;
506: PetscScalar *inarray, *outarray;
508: VecGetArrayPair(in, out, &inarray, &outarray);
509: VecGetLocalSize(in, &nlocal);
510: VecGetLocalSize(in, &mlocal);
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: }
523: VecRestoreArrayPair(in, out, &inarray, &outarray);
524: return 0;
525: }