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) {
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 Parameter:
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: PetscInt low[5], high[5], n, i;
58: VecGetOwnershipRange(X, low, high);
59: VecGetOwnershipRange(F, low + 1, high + 1);
60: VecGetOwnershipRange(L, low + 2, high + 2);
61: VecGetOwnershipRange(U, low + 3, high + 3);
62: VecGetOwnershipRange(FB, low + 4, high + 4);
64: for (i = 1; i < 4; ++i) {
66: }
68: VecGetArrayRead(X, &x);
69: VecGetArrayRead(F, &f);
70: VecGetArrayRead(L, &l);
71: VecGetArrayRead(U, &u);
72: VecGetArray(FB, &fb);
74: VecGetLocalSize(X, &n);
76: for (i = 0; i < n; ++i) {
77: xval = PetscRealPart(x[i]); fval = PetscRealPart(f[i]);
78: lval = PetscRealPart(l[i]); uval = PetscRealPart(u[i]);
80: if ((lval <= -PETSC_INFINITY) && (uval >= PETSC_INFINITY)) {
81: fb[i] = -fval;
82: } else if (lval <= -PETSC_INFINITY) {
83: fb[i] = -Fischer(uval - xval, -fval);
84: } else if (uval >= PETSC_INFINITY) {
85: fb[i] = Fischer(xval - lval, fval);
86: } else if (lval == uval) {
87: fb[i] = lval - xval;
88: } else {
89: fval = Fischer(uval - xval, -fval);
90: fb[i] = Fischer(xval - lval, fval);
91: }
92: }
94: VecRestoreArrayRead(X, &x);
95: VecRestoreArrayRead(F, &f);
96: VecRestoreArrayRead(L, &l);
97: VecRestoreArrayRead(U, &u);
98: VecRestoreArray(FB, &fb);
99: return 0;
100: }
102: static inline PetscReal SFischer(PetscReal a, PetscReal b, PetscReal c)
103: {
104: /* Method suggested by Bob Vanderbei */
105: if (a + b <= 0) {
106: return PetscSqrtReal(a*a + b*b + 2.0*c*c) - (a + b);
107: }
108: return 2.0*(c*c - a*b) / (PetscSqrtReal(a*a + b*b + 2.0*c*c) + (a + b));
109: }
111: /*@
112: VecSFischer - Evaluates the Smoothed Fischer-Burmeister function for
113: complementarity problems.
115: Logically Collective on vectors
117: Input Parameters:
118: + X - current point
119: . F - function evaluated at x
120: . L - lower bounds
121: . U - upper bounds
122: - mu - smoothing parameter
124: Output Parameter:
125: . FB - The Smoothed Fischer-Burmeister function vector
127: Notes:
128: The Smoothed Fischer-Burmeister function is defined as
129: $ phi(a,b) := sqrt(a*a + b*b + 2*mu*mu) - a - b
130: and is used reformulate a complementarity problem as a semismooth
131: system of equations.
133: The result of this function is done by cases:
134: + l[i] == -infinity, u[i] == infinity -- fb[i] = -f[i] - 2*mu*x[i]
135: . l[i] == -infinity, u[i] finite -- fb[i] = phi(u[i]-x[i], -f[i], mu)
136: . l[i] finite, u[i] == infinity -- fb[i] = phi(x[i]-l[i], f[i], mu)
137: . l[i] finite < u[i] finite -- fb[i] = phi(x[i]-l[i], phi(u[i]-x[i], -f[u], mu), mu)
138: - otherwise l[i] == u[i] -- fb[i] = l[i] - x[i]
140: Level: developer
142: .seealso VecFischer()
143: @*/
144: PetscErrorCode VecSFischer(Vec X, Vec F, Vec L, Vec U, PetscReal mu, Vec FB)
145: {
146: const PetscScalar *x, *f, *l, *u;
147: PetscScalar *fb;
148: PetscReal xval, fval, lval, uval;
149: PetscInt low[5], high[5], n, i;
157: VecGetOwnershipRange(X, low, high);
158: VecGetOwnershipRange(F, low + 1, high + 1);
159: VecGetOwnershipRange(L, low + 2, high + 2);
160: VecGetOwnershipRange(U, low + 3, high + 3);
161: VecGetOwnershipRange(FB, low + 4, high + 4);
163: for (i = 1; i < 4; ++i) {
165: }
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++); fval = PetscRealPart(*f++);
177: lval = PetscRealPart(*l++); uval = PetscRealPart(*u++);
179: if ((lval <= -PETSC_INFINITY) && (uval >= PETSC_INFINITY)) {
180: (*fb++) = -fval - mu*xval;
181: } else if (lval <= -PETSC_INFINITY) {
182: (*fb++) = -SFischer(uval - xval, -fval, mu);
183: } else if (uval >= PETSC_INFINITY) {
184: (*fb++) = SFischer(xval - lval, fval, mu);
185: } else if (lval == uval) {
186: (*fb++) = lval - xval;
187: } else {
188: fval = SFischer(uval - xval, -fval, mu);
189: (*fb++) = SFischer(xval - lval, fval, mu);
190: }
191: }
192: x -= n; f -= n; l -=n; u -= n; fb -= n;
194: VecRestoreArrayRead(X, &x);
195: VecRestoreArrayRead(F, &f);
196: VecRestoreArrayRead(L, &l);
197: VecRestoreArrayRead(U, &u);
198: VecRestoreArray(FB, &fb);
199: return 0;
200: }
202: static inline PetscReal fischnorm(PetscReal a, PetscReal b)
203: {
204: return PetscSqrtReal(a*a + b*b);
205: }
207: static inline PetscReal fischsnorm(PetscReal a, PetscReal b, PetscReal c)
208: {
209: return PetscSqrtReal(a*a + b*b + 2.0*c*c);
210: }
212: /*@
213: MatDFischer - Calculates an element of the B-subdifferential of the
214: Fischer-Burmeister function for complementarity problems.
216: Collective on jac
218: Input Parameters:
219: + jac - the jacobian of f at X
220: . X - current point
221: . Con - constraints function evaluated at X
222: . XL - lower bounds
223: . XU - upper bounds
224: . t1 - work vector
225: - t2 - work vector
227: Output Parameters:
228: + Da - diagonal perturbation component of the result
229: - Db - row scaling component of the result
231: Level: developer
233: .seealso: VecFischer()
234: @*/
235: PetscErrorCode MatDFischer(Mat jac, Vec X, Vec Con, Vec XL, Vec XU, Vec T1, Vec T2, Vec Da, Vec Db)
236: {
237: PetscInt i,nn;
238: const PetscScalar *x,*f,*l,*u,*t2;
239: PetscScalar *da,*db,*t1;
240: PetscReal ai,bi,ci,di,ei;
242: VecGetLocalSize(X,&nn);
243: VecGetArrayRead(X,&x);
244: VecGetArrayRead(Con,&f);
245: VecGetArrayRead(XL,&l);
246: VecGetArrayRead(XU,&u);
247: VecGetArray(Da,&da);
248: VecGetArray(Db,&db);
249: VecGetArray(T1,&t1);
250: VecGetArrayRead(T2,&t2);
252: for (i = 0; i < nn; i++) {
253: da[i] = 0.0;
254: db[i] = 0.0;
255: t1[i] = 0.0;
257: if (PetscAbsScalar(f[i]) <= PETSC_MACHINE_EPSILON) {
258: if (PetscRealPart(l[i]) > PETSC_NINFINITY && PetscAbsScalar(x[i] - l[i]) <= PETSC_MACHINE_EPSILON) {
259: t1[i] = 1.0;
260: da[i] = 1.0;
261: }
263: if (PetscRealPart(u[i]) < PETSC_INFINITY && PetscAbsScalar(u[i] - x[i]) <= PETSC_MACHINE_EPSILON) {
264: t1[i] = 1.0;
265: db[i] = 1.0;
266: }
267: }
268: }
270: VecRestoreArray(T1,&t1);
271: VecRestoreArrayRead(T2,&t2);
272: MatMult(jac,T1,T2);
273: VecGetArrayRead(T2,&t2);
275: for (i = 0; i < nn; i++) {
276: if ((PetscRealPart(l[i]) <= PETSC_NINFINITY) && (PetscRealPart(u[i]) >= PETSC_INFINITY)) {
277: da[i] = 0.0;
278: db[i] = -1.0;
279: } else if (PetscRealPart(l[i]) <= PETSC_NINFINITY) {
280: if (PetscRealPart(db[i]) >= 1) {
281: ai = fischnorm(1.0, PetscRealPart(t2[i]));
283: da[i] = -1.0 / ai - 1.0;
284: db[i] = -t2[i] / ai - 1.0;
285: } else {
286: bi = PetscRealPart(u[i]) - PetscRealPart(x[i]);
287: ai = fischnorm(bi, PetscRealPart(f[i]));
288: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
290: da[i] = bi / ai - 1.0;
291: db[i] = -f[i] / ai - 1.0;
292: }
293: } else if (PetscRealPart(u[i]) >= PETSC_INFINITY) {
294: if (PetscRealPart(da[i]) >= 1) {
295: ai = fischnorm(1.0, PetscRealPart(t2[i]));
297: da[i] = 1.0 / ai - 1.0;
298: db[i] = t2[i] / ai - 1.0;
299: } else {
300: bi = PetscRealPart(x[i]) - PetscRealPart(l[i]);
301: ai = fischnorm(bi, PetscRealPart(f[i]));
302: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
304: da[i] = bi / ai - 1.0;
305: db[i] = f[i] / ai - 1.0;
306: }
307: } else if (PetscRealPart(l[i]) == PetscRealPart(u[i])) {
308: da[i] = -1.0;
309: db[i] = 0.0;
310: } else {
311: if (PetscRealPart(db[i]) >= 1) {
312: ai = fischnorm(1.0, PetscRealPart(t2[i]));
314: ci = 1.0 / ai + 1.0;
315: di = PetscRealPart(t2[i]) / ai + 1.0;
316: } else {
317: bi = PetscRealPart(x[i]) - PetscRealPart(u[i]);
318: ai = fischnorm(bi, PetscRealPart(f[i]));
319: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
321: ci = bi / ai + 1.0;
322: di = PetscRealPart(f[i]) / ai + 1.0;
323: }
325: if (PetscRealPart(da[i]) >= 1) {
326: bi = ci + di*PetscRealPart(t2[i]);
327: ai = fischnorm(1.0, bi);
329: bi = bi / ai - 1.0;
330: ai = 1.0 / ai - 1.0;
331: } else {
332: ei = Fischer(PetscRealPart(u[i]) - PetscRealPart(x[i]), -PetscRealPart(f[i]));
333: ai = fischnorm(PetscRealPart(x[i]) - PetscRealPart(l[i]), ei);
334: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
336: bi = ei / ai - 1.0;
337: ai = (PetscRealPart(x[i]) - PetscRealPart(l[i])) / ai - 1.0;
338: }
340: da[i] = ai + bi*ci;
341: db[i] = bi*di;
342: }
343: }
345: VecRestoreArray(Da,&da);
346: VecRestoreArray(Db,&db);
347: VecRestoreArrayRead(X,&x);
348: VecRestoreArrayRead(Con,&f);
349: VecRestoreArrayRead(XL,&l);
350: VecRestoreArrayRead(XU,&u);
351: VecRestoreArrayRead(T2,&t2);
352: return 0;
353: }
355: /*@
356: MatDSFischer - Calculates an element of the B-subdifferential of the
357: smoothed Fischer-Burmeister function for complementarity problems.
359: Collective on jac
361: Input Parameters:
362: + jac - the jacobian of f at X
363: . X - current point
364: . F - constraint function evaluated at X
365: . XL - lower bounds
366: . XU - upper bounds
367: . mu - smoothing parameter
368: . T1 - work vector
369: - T2 - work vector
371: Output Parameters:
372: + Da - diagonal perturbation component of the result
373: . Db - row scaling component of the result
374: - Dm - derivative with respect to scaling parameter
376: Level: developer
378: .seealso MatDFischer()
379: @*/
380: PetscErrorCode MatDSFischer(Mat jac, Vec X, Vec Con,Vec XL, Vec XU, PetscReal mu,Vec T1, Vec T2,Vec Da, Vec Db, Vec Dm)
381: {
382: PetscInt i,nn;
383: const PetscScalar *x, *f, *l, *u;
384: PetscScalar *da, *db, *dm;
385: PetscReal ai, bi, ci, di, ei, fi;
387: if (PetscAbsReal(mu) <= PETSC_MACHINE_EPSILON) {
388: VecZeroEntries(Dm);
389: MatDFischer(jac, X, Con, XL, XU, T1, T2, Da, Db);
390: } else {
391: VecGetLocalSize(X,&nn);
392: VecGetArrayRead(X,&x);
393: VecGetArrayRead(Con,&f);
394: VecGetArrayRead(XL,&l);
395: VecGetArrayRead(XU,&u);
396: VecGetArray(Da,&da);
397: VecGetArray(Db,&db);
398: VecGetArray(Dm,&dm);
400: for (i = 0; i < nn; ++i) {
401: if ((PetscRealPart(l[i]) <= PETSC_NINFINITY) && (PetscRealPart(u[i]) >= PETSC_INFINITY)) {
402: da[i] = -mu;
403: db[i] = -1.0;
404: dm[i] = -x[i];
405: } else if (PetscRealPart(l[i]) <= PETSC_NINFINITY) {
406: bi = PetscRealPart(u[i]) - PetscRealPart(x[i]);
407: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
408: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
410: da[i] = bi / ai - 1.0;
411: db[i] = -PetscRealPart(f[i]) / ai - 1.0;
412: dm[i] = 2.0 * mu / ai;
413: } else if (PetscRealPart(u[i]) >= PETSC_INFINITY) {
414: bi = PetscRealPart(x[i]) - PetscRealPart(l[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(l[i]) == PetscRealPart(u[i])) {
422: da[i] = -1.0;
423: db[i] = 0.0;
424: dm[i] = 0.0;
425: } else {
426: bi = PetscRealPart(x[i]) - PetscRealPart(u[i]);
427: ai = fischsnorm(bi, PetscRealPart(f[i]), mu);
428: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
430: ci = bi / ai + 1.0;
431: di = PetscRealPart(f[i]) / ai + 1.0;
432: fi = 2.0 * mu / ai;
434: ei = SFischer(PetscRealPart(u[i]) - PetscRealPart(x[i]), -PetscRealPart(f[i]), mu);
435: ai = fischsnorm(PetscRealPart(x[i]) - PetscRealPart(l[i]), ei, mu);
436: ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
438: bi = ei / ai - 1.0;
439: ei = 2.0 * mu / ei;
440: ai = (PetscRealPart(x[i]) - PetscRealPart(l[i])) / ai - 1.0;
442: da[i] = ai + bi*ci;
443: db[i] = bi*di;
444: dm[i] = ei + bi*fi;
445: }
446: }
448: VecRestoreArrayRead(X,&x);
449: VecRestoreArrayRead(Con,&f);
450: VecRestoreArrayRead(XL,&l);
451: VecRestoreArrayRead(XU,&u);
452: VecRestoreArray(Da,&da);
453: VecRestoreArray(Db,&db);
454: VecRestoreArray(Dm,&dm);
455: }
456: return 0;
457: }
459: static inline PetscReal ST_InternalPN(PetscScalar in, PetscReal lb, PetscReal ub)
460: {
461: return PetscMax(0,(PetscReal)PetscRealPart(in)-ub) - PetscMax(0,-(PetscReal)PetscRealPart(in)-PetscAbsReal(lb));
462: }
464: static inline PetscReal ST_InternalNN(PetscScalar in, PetscReal lb, PetscReal ub)
465: {
466: return PetscMax(0,(PetscReal)PetscRealPart(in) + PetscAbsReal(ub)) - PetscMax(0,-(PetscReal)PetscRealPart(in) - PetscAbsReal(lb));
467: }
469: static inline PetscReal ST_InternalPP(PetscScalar in, PetscReal lb, PetscReal ub)
470: {
471: return PetscMax(0, (PetscReal)PetscRealPart(in)-ub) + PetscMin(0, (PetscReal)PetscRealPart(in) - lb);
472: }
474: /*@
475: TaoSoftThreshold - Calculates soft thresholding routine with input vector
476: and given lower and upper bound and returns it to output vector.
478: Input Parameters:
479: + in - input vector to be thresholded
480: . lb - lower bound
481: - ub - upper bound
483: Output Parameter:
484: . out - Soft thresholded output vector
486: Notes:
487: Soft thresholding is defined as
488: \[ S(input,lb,ub) =
489: \begin{cases}
490: input - ub \text{input > ub} \\
491: 0 \text{lb =< input <= ub} \\
492: input + lb \text{input < lb} \\
493: \]
495: Level: developer
497: @*/
498: PetscErrorCode TaoSoftThreshold(Vec in, PetscReal lb, PetscReal ub, Vec out)
499: {
500: PetscInt i, nlocal, mlocal;
501: PetscScalar *inarray, *outarray;
503: VecGetArrayPair(in, out, &inarray, &outarray);
504: VecGetLocalSize(in, &nlocal);
505: VecGetLocalSize(in, &mlocal);
510: if (ub >= 0 && lb < 0) {
511: for (i=0; i<nlocal; i++) outarray[i] = ST_InternalPN(inarray[i], lb, ub);
512: } else if (ub < 0 && lb < 0) {
513: for (i=0; i<nlocal; i++) outarray[i] = ST_InternalNN(inarray[i], lb, ub);
514: } else {
515: for (i=0; i<nlocal; i++) outarray[i] = ST_InternalPP(inarray[i], lb, ub);
516: }
518: VecRestoreArrayPair(in, out, &inarray, &outarray);
519: return 0;
520: }