Actual source code: tao_util.c

  1: #include "tao.h" /*I "tao.h" I*/
  2: #include "tao_util.h" /*I "tao_util.h" I*/

  6: /*@
  7:   VecPow - Replaces each component of a vector by x_i^p

  9:   Logically Collective on v

 11:   Input Parameter:
 12: + v - the vector
 13: - p - the exponent to use on each element

 15:   Output Parameter:
 16: . v - the vector

 18:   Level: intermediate

 20: @*/
 21: PetscErrorCode VecPow(Vec v, PetscReal p)
 22: {
 24:   PetscInt n,i;
 25:   PetscReal *v1;


 30:   VecGetArray(v, &v1); 
 31:   VecGetLocalSize(v, &n); 

 33:   if (1.0 == p) {
 34:   }
 35:   else if (-1.0 == p) {
 36:     for (i = 0; i < n; ++i){
 37:       v1[i] = 1.0 / v1[i];
 38:     }
 39:   }
 40:   else if (0.0 == p) {
 41:     for (i = 0; i < n; ++i){
 42:       /*  Not-a-number left alone
 43:           Infinity set to one  */
 44:       if (v1[i] == v1[i]) {
 45:         v1[i] = 1.0;
 46:       }
 47:     }
 48:   }
 49:   else if (0.5 == p) {
 50:     for (i = 0; i < n; ++i) {
 51:       if (v1[i] >= 0) {
 52:         v1[i] = PetscSqrtScalar(v1[i]);
 53:       }
 54:       else {
 55:         v1[i] = TAO_INFINITY;
 56:       }
 57:     }
 58:   }
 59:   else if (-0.5 == p) {
 60:     for (i = 0; i < n; ++i) {
 61:       if (v1[i] >= 0) {
 62:         v1[i] = 1.0 / PetscSqrtScalar(v1[i]);
 63:       }
 64:       else {
 65:         v1[i] = TAO_INFINITY;
 66:       }
 67:     }
 68:   }
 69:   else if (2.0 == p) {
 70:     for (i = 0; i < n; ++i){
 71:       v1[i] *= v1[i];
 72:     }
 73:   }
 74:   else if (-2.0 == p) {
 75:     for (i = 0; i < n; ++i){
 76:       v1[i] = 1.0 / (v1[i] * v1[i]);
 77:     }
 78:   }
 79:   else {
 80:     for (i = 0; i < n; ++i) {
 81:       if (v1[i] >= 0) {
 82:         v1[i] = PetscPowScalar(v1[i], p);
 83:       }
 84:       else {
 85:         v1[i] = TAO_INFINITY;
 86:       }
 87:     }
 88:   }

 90:   VecRestoreArray(v,&v1); 
 91:   return(0);
 92: }

 96: /*@
 97:   VecMedian - Computes the componentwise median of three vectors
 98:   and stores the result in this vector.  Used primarily for projecting
 99:   a vector within upper and lower bounds. 

101:   Logically Collective 

103:   Input Parameters:
104: . Vec1, Vec2, Vec3 - The three vectors

106:   Output Parameter:
107: . VMedian - The median vector

109:   Level: advanced
110: @*/
111: PetscErrorCode VecMedian(Vec Vec1, Vec Vec2, Vec Vec3, Vec VMedian)
112: {
114:   PetscInt i,n,low1,low2,low3,low4,high1,high2,high3,high4;
115:   PetscReal *v1,*v2,*v3,*vmed;


123:   if (Vec1==Vec2 || Vec1==Vec3){
124:     ierr=VecCopy(Vec1,VMedian);  
125:     return(0);
126:   }
127:   if (Vec2==Vec3){
128:     ierr=VecCopy(Vec2,VMedian);  
129:     return(0);
130:   }


138:   VecGetOwnershipRange(Vec1, &low1, &high1); 
139:   VecGetOwnershipRange(Vec2, &low2, &high2); 
140:   VecGetOwnershipRange(Vec3, &low3, &high3); 
141:   VecGetOwnershipRange(VMedian, &low4, &high4); 
142:   if ( low1!= low2 || low1!= low3 || low1!= low4 ||
143:        high1!= high2 || high1!= high3 || high1!= high4)
144:     SETERRQ(PETSC_COMM_SELF,1,"InCompatible vector local lengths");

146:   VecGetArray(Vec1,&v1); 
147:   VecGetArray(Vec2,&v2); 
148:   VecGetArray(Vec3,&v3); 

150:   if ( VMedian != Vec1 && VMedian != Vec2 && VMedian != Vec3){
151:     VecGetArray(VMedian,&vmed); 
152:   } else if ( VMedian==Vec1 ){
153:     vmed=v1;
154:   } else if ( VMedian==Vec2 ){
155:     vmed=v2;
156:   } else {
157:     vmed=v3;
158:   }

160:   ierr=VecGetLocalSize(Vec1,&n); 

162:   for (i=0;i<n;i++){
163:     vmed[i]=PetscMax(PetscMax(PetscMin(v1[i],v2[i]),PetscMin(v1[i],v3[i])),PetscMin(v2[i],v3[i]));
164:   }

166:   VecRestoreArray(Vec1,&v1); 
167:   VecRestoreArray(Vec2,&v2); 
168:   VecRestoreArray(Vec3,&v2); 
169:   
170:   if (VMedian!=Vec1 && VMedian != Vec2 && VMedian != Vec3){
171:     VecRestoreArray(VMedian,&vmed); 
172:   }

174:   return(0);
175: }

177: PETSC_STATIC_INLINE PetscReal Fischer(PetscReal a, PetscReal b)
178: {
179:   /* Method suggested by Bob Vanderbei */
180:    if (a + b <= 0) {
181:      return PetscSqrtScalar(a*a + b*b) - (a + b);
182:    }
183:    return -2.0*a*b / (PetscSqrtScalar(a*a + b*b) + (a + b));
184: }

188: /*@
189:    VecFischer - Evaluates the Fischer-Burmeister function for complementarity 
190:    problems.

192:    Logically Collective on vectors

194:    Input Parameters:
195: +  X - current point
196: .  F - function evaluated at x
197: .  L - lower bounds 
198: -  U - upper bounds

200:    Output Parameters:
201: .  FB - The Fischer-Burmeister function vector

203:    Notes: 
204:    The Fischer-Burmeister function is defined as
205: $        phi(a,b) := sqrt(a*a + b*b) - a - b
206:    and is used reformulate a complementarity problem as a semismooth
207:    system of equations.

209:    The result of this function is done by cases:
210: +  l[i] == -infinity, u[i] == infinity  -- fb[i] = -f[i]
211: .  l[i] == -infinity, u[i] finite       -- fb[i] = phi(u[i]-x[i], -f[i])
212: .  l[i] finite,       u[i] == infinity  -- fb[i] = phi(x[i]-l[i],  f[i])
213: .  l[i] finite < u[i] finite -- fb[i] = phi(x[i]-l[i], phi(u[i]-x[i], -f[u]))
214: -  otherwise l[i] == u[i] -- fb[i] = l[i] - x[i]

216:    Level: developer

218: @*/
219: PetscErrorCode VecFischer(Vec X, Vec F, Vec L, Vec U, Vec FB)
220: {
221:   PetscReal *x, *f, *l, *u, *fb;
222:   PetscReal xval, fval, lval, uval;
224:   PetscInt low[5], high[5], n, i;


233:   VecGetOwnershipRange(X, low, high); 
234:   VecGetOwnershipRange(F, low + 1, high + 1); 
235:   VecGetOwnershipRange(L, low + 2, high + 2); 
236:   VecGetOwnershipRange(U, low + 3, high + 3); 
237:   VecGetOwnershipRange(FB, low + 4, high + 4); 

239:   for (i = 1; i < 4; ++i) {
240:     if (low[0] != low[i] || high[0] != high[i])
241:       SETERRQ(PETSC_COMM_SELF,1,"Vectors must be identically loaded over processors");
242:   }

244:   VecGetArray(X, &x); 
245:   VecGetArray(F, &f); 
246:   VecGetArray(L, &l); 
247:   VecGetArray(U, &u); 
248:   VecGetArray(FB, &fb); 

250:   VecGetLocalSize(X, &n); 

252:   for (i = 0; i < n; ++i) {
253:     xval = x[i]; fval = f[i];
254:     lval = l[i]; uval = u[i];

256:     if ((lval <= -TAO_INFINITY) && (uval >= TAO_INFINITY)) {
257:       fb[i] = -fval;
258:     } 
259:     else if (lval <= -TAO_INFINITY) {
260:       fb[i] = -Fischer(uval - xval, -fval);
261:     } 
262:     else if (uval >=  TAO_INFINITY) {
263:       fb[i] =  Fischer(xval - lval,  fval);
264:     } 
265:     else if (lval == uval) {
266:       fb[i] = lval - xval;
267:     }
268:     else {
269:       fval  =  Fischer(uval - xval, -fval);
270:       fb[i] =  Fischer(xval - lval,  fval);
271:     }
272:   }
273:   
274:   VecRestoreArray(X, &x); 
275:   VecRestoreArray(F, &f); 
276:   VecRestoreArray(L, &l); 
277:   VecRestoreArray(U, &u); 
278:   VecRestoreArray(FB, &fb); 

280:   return(0);
281: }

283: PETSC_STATIC_INLINE PetscReal SFischer(PetscReal a, PetscReal b, PetscReal c)
284: {
285:   /* Method suggested by Bob Vanderbei */
286:    if (a + b <= 0) {
287:      return PetscSqrtScalar(a*a + b*b + 2.0*c*c) - (a + b);
288:    }
289:    return 2.0*(c*c - a*b) / (PetscSqrtScalar(a*a + b*b + 2.0*c*c) + (a + b));
290: }

294: /*@
295:    VecSFischer - Evaluates the Smoothed Fischer-Burmeister function for
296:    complementarity problems.

298:    Logically Collective on vectors

300:    Input Parameters:
301: +  X - current point
302: .  F - function evaluated at x
303: .  L - lower bounds
304: .  U - upper bounds
305: -  mu - smoothing parameter

307:    Output Parameters:
308: .  FB - The Smoothed Fischer-Burmeister function vector

310:    Notes:
311:    The Smoothed Fischer-Burmeister function is defined as
312: $        phi(a,b) := sqrt(a*a + b*b + 2*mu*mu) - a - b
313:    and is used reformulate a complementarity problem as a semismooth
314:    system of equations.

316:    The result of this function is done by cases:
317: +  l[i] == -infinity, u[i] == infinity  -- fb[i] = -f[i] - 2*mu*x[i]
318: .  l[i] == -infinity, u[i] finite       -- fb[i] = phi(u[i]-x[i], -f[i], mu)
319: .  l[i] finite,       u[i] == infinity  -- fb[i] = phi(x[i]-l[i],  f[i], mu)
320: .  l[i] finite < u[i] finite -- fb[i] = phi(x[i]-l[i], phi(u[i]-x[i], -f[u], mu), mu)
321: -  otherwise l[i] == u[i] -- fb[i] = l[i] - x[i]

323:    Level: developer

325: .seealso  VecFischer()
326: @*/
327: PetscErrorCode VecSFischer(Vec X, Vec F, Vec L, Vec U, PetscReal mu, Vec FB)
328: {
329:   PetscReal *x, *f, *l, *u, *fb;
330:   PetscReal xval, fval, lval, uval;
332:   PetscInt low[5], high[5], n, i;


341:   VecGetOwnershipRange(X, low, high); 
342:   VecGetOwnershipRange(F, low + 1, high + 1); 
343:   VecGetOwnershipRange(L, low + 2, high + 2); 
344:   VecGetOwnershipRange(U, low + 3, high + 3); 
345:   VecGetOwnershipRange(FB, low + 4, high + 4); 

347:   for (i = 1; i < 4; ++i) {
348:     if (low[0] != low[i] || high[0] != high[i])
349:       SETERRQ(PETSC_COMM_SELF,1,"Vectors must be identically loaded over processors");
350:   }

352:   VecGetArray(X, &x); 
353:   VecGetArray(F, &f); 
354:   VecGetArray(L, &l); 
355:   VecGetArray(U, &u); 
356:   VecGetArray(FB, &fb); 

358:   VecGetLocalSize(X, &n); 
359:   
360:   for (i = 0; i < n; ++i) {
361:     xval = (*x++); fval = (*f++);
362:     lval = (*l++); uval = (*u++);

364:     if ((lval <= -TAO_INFINITY) && (uval >= TAO_INFINITY)) {
365:       (*fb++) = -fval - mu*xval;
366:     } 
367:     else if (lval <= -TAO_INFINITY) {
368:       (*fb++) = -SFischer(uval - xval, -fval, mu);
369:     } 
370:     else if (uval >=  TAO_INFINITY) {
371:       (*fb++) =  SFischer(xval - lval,  fval, mu);
372:     } 
373:     else if (lval == uval) {
374:       (*fb++) = lval - xval;
375:     } 
376:     else {
377:       fval    =  SFischer(uval - xval, -fval, mu);
378:       (*fb++) =  SFischer(xval - lval,  fval, mu);
379:     }
380:   }
381:   x -= n; f -= n; l -=n; u -= n; fb -= n;

383:   VecRestoreArray(X, &x); 
384:   VecRestoreArray(F, &f); 
385:   VecRestoreArray(L, &l); 
386:   VecRestoreArray(U, &u); 
387:   VecRestoreArray(FB, &fb); 
388:   return(0);
389: }

391: PETSC_STATIC_INLINE PetscReal fischnorm(PetscReal a, PetscReal b)
392: {
393:   return PetscSqrtScalar(a*a + b*b);
394: }

396: PETSC_STATIC_INLINE PetscReal fischsnorm(PetscReal a, PetscReal b, PetscReal c)
397: {
398:   return PetscSqrtScalar(a*a + b*b + 2.0*c*c);
399: }

403: /*@
404:    D_Fischer - Calculates an element of the B-subdifferential of the 
405:    Fischer-Burmeister function for complementarity problems.

407:    Collective on jac

409:    Input Parameters:   
410: +  jac - the jacobian of f at X
411: .  X - current point
412: .  Con - constraints function evaluated at X
413: .  XL - lower bounds
414: .  XU - upper bounds
415: .  t1 - work vector
416: -  t2 - work vector

418:    Output Parameters:
419: +  Da - diagonal perturbation component of the result
420: -  Db - row scaling component of the result

422:    Level: developer

424: .seealso: VecFischer()
425: @*/
426: PetscErrorCode D_Fischer(Mat jac, Vec X, Vec Con, Vec XL, Vec XU, 
427:                       Vec T1, Vec T2, Vec Da, Vec Db)
428: {
430:   PetscInt i,nn;
431:   PetscReal *x,*f,*l,*u,*da,*db,*t1,*t2;
432:   PetscReal ai,bi,ci,di,ei;


436:   VecGetLocalSize(X,&nn); 
437:   
438:   VecGetArray(X,&x);
439:   VecGetArray(Con,&f);
440:   VecGetArray(XL,&l);
441:   VecGetArray(XU,&u);
442:   VecGetArray(Da,&da);
443:   VecGetArray(Db,&db);
444:   VecGetArray(T1,&t1);
445:   VecGetArray(T2,&t2);

447:   for (i = 0; i < nn; i++) {
448:     da[i] = 0;
449:     db[i] = 0;
450:     t1[i] = 0;

452:     if (PetscAbsReal(f[i]) <= PETSC_MACHINE_EPSILON) {
453:       if (l[i] > TAO_NINFINITY && PetscAbsReal(x[i] - l[i]) <= PETSC_MACHINE_EPSILON) {
454:         t1[i] = 1;
455:         da[i] = 1;
456:       }

458:       if (u[i] <  TAO_INFINITY && PetscAbsReal(u[i] - x[i]) <= PETSC_MACHINE_EPSILON) {
459:         t1[i] = 1;
460:         db[i] = 1;
461:       }
462:     }
463:   }

465:   VecRestoreArray(T1,&t1); 
466:   VecRestoreArray(T2,&t2); 
467:   MatMult(jac,T1,T2); 
468:   VecGetArray(T2,&t2); 

470:   for (i = 0; i < nn; i++) {
471:     if ((l[i] <= TAO_NINFINITY) && (u[i] >= TAO_INFINITY)) {
472:       da[i] = 0;
473:       db[i] = -1;
474:     } 
475:     else if (l[i] <= TAO_NINFINITY) {
476:       if (db[i] >= 1) {
477:         ai = fischnorm(1, t2[i]);

479:         da[i] = -1/ai - 1;
480:         db[i] = -t2[i]/ai - 1;
481:       } 
482:       else {
483:         bi = u[i] - x[i];
484:         ai = fischnorm(bi, f[i]);
485:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

487:         da[i] = bi / ai - 1;
488:         db[i] = -f[i] / ai - 1;
489:       }
490:     } 
491:     else if (u[i] >=  TAO_INFINITY) {
492:       if (da[i] >= 1) {
493:         ai = fischnorm(1, t2[i]);

495:         da[i] = 1 / ai - 1;
496:         db[i] = t2[i] / ai - 1;
497:       } 
498:       else {
499:         bi = x[i] - l[i];
500:         ai = fischnorm(bi, f[i]);
501:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

503:         da[i] = bi / ai - 1;
504:         db[i] = f[i] / ai - 1;
505:       }
506:     } 
507:     else if (l[i] == u[i]) {
508:       da[i] = -1;
509:       db[i] = 0;
510:     } 
511:     else {
512:       if (db[i] >= 1) {
513:         ai = fischnorm(1, t2[i]);

515:         ci = 1 / ai + 1;
516:         di = t2[i] / ai + 1;
517:       } 
518:       else {
519:         bi = x[i] - u[i];
520:         ai = fischnorm(bi, f[i]);
521:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

523:         ci = bi / ai + 1;
524:         di = f[i] / ai + 1;
525:       }

527:       if (da[i] >= 1) {
528:         bi = ci + di*t2[i];
529:         ai = fischnorm(1, bi);

531:         bi = bi / ai - 1;
532:         ai = 1 / ai - 1;
533:       } 
534:       else {
535:         ei = Fischer(u[i] - x[i], -f[i]);
536:         ai = fischnorm(x[i] - l[i], ei);
537:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

539:         bi = ei / ai - 1;
540:         ai = (x[i] - l[i]) / ai - 1;
541:       }

543:       da[i] = ai + bi*ci;
544:       db[i] = bi*di;
545:     }
546:   }

548:   VecRestoreArray(Da,&da); 
549:   VecRestoreArray(Db,&db); 
550:   VecRestoreArray(X,&x); 
551:   VecRestoreArray(Con,&f); 
552:   VecRestoreArray(XL,&l); 
553:   VecRestoreArray(XU,&u); 
554:   VecRestoreArray(T2,&t2); 
555:   return(0);
556: };

560: /*@
561:    D_SFischer - Calculates an element of the B-subdifferential of the
562:    smoothed Fischer-Burmeister function for complementarity problems.
563:  
564:    Collective on jac

566:    Input Parameters: 
567: +  jac - the jacobian of f at X
568: .  X - current point
569: .  F - constraint function evaluated at X
570: .  XL - lower bounds
571: .  XU - upper bounds
572: .  mu - smoothing parameter
573: .  T1 - work vector
574: -  T2 - work vector

576:    Output Parameter: 
577: +  Da - diagonal perturbation component of the result
578: .  Db - row scaling component of the result
579: -  Dm - derivative with respect to scaling parameter

581:    Level: developer

583: .seealso D_Fischer()
584: @*/
585: PetscErrorCode D_SFischer(Mat jac, Vec X, Vec Con, 
586:                        Vec XL, Vec XU, PetscReal mu, 
587:                        Vec T1, Vec T2, 
588:                        Vec Da, Vec Db, Vec Dm)
589: {
591:   PetscInt i,nn;
592:   PetscReal *x, *f, *l, *u, *da, *db, *dm;
593:   PetscReal ai, bi, ci, di, ei, fi;


597:   if (PetscAbsReal(mu) <= PETSC_MACHINE_EPSILON) {
598:     VecZeroEntries(Dm); 
599:     D_Fischer(jac, X, Con, XL, XU, T1, T2, Da, Db); 
600:   } 
601:   else {
602:     VecGetLocalSize(X,&nn); 
603:     VecGetArray(X,&x); 
604:     VecGetArray(Con,&f); 
605:     VecGetArray(XL,&l); 
606:     VecGetArray(XU,&u); 
607:     VecGetArray(Da,&da); 
608:     VecGetArray(Db,&db); 
609:     VecGetArray(Dm,&dm); 

611:     for (i = 0; i < nn; ++i) {
612:       if ((l[i] <= TAO_NINFINITY) && (u[i] >= TAO_INFINITY)) {
613:         da[i] = -mu;
614:         db[i] = -1;
615:         dm[i] = -x[i];
616:       } 
617:       else if (l[i] <= TAO_NINFINITY) {
618:         bi = u[i] - x[i];
619:         ai = fischsnorm(bi, f[i], mu);
620:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

622:         da[i] = bi / ai - 1;
623:         db[i] = -f[i] / ai - 1;
624:         dm[i] = 2.0 * mu / ai;
625:       } 
626:       else if (u[i] >=  TAO_INFINITY) {
627:         bi = x[i] - l[i];
628:         ai = fischsnorm(bi, f[i], mu);
629:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);

631:         da[i] = bi / ai - 1;
632:         db[i] = f[i] / ai - 1;
633:         dm[i] = 2.0 * mu / ai;
634:       } 
635:       else if (l[i] == u[i]) {
636:         da[i] = -1;
637:         db[i] = 0;
638:         dm[i] = 0;
639:       } 
640:       else {
641:         bi = x[i] - u[i];
642:         ai = fischsnorm(bi, f[i], mu);
643:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
644:   
645:         ci = bi / ai + 1;
646:         di = f[i] / ai + 1;
647:         fi = 2.0 * mu / ai;

649:         ei = SFischer(u[i] - x[i], -f[i], mu);
650:         ai = fischsnorm(x[i] - l[i], ei, mu);
651:         ai = PetscMax(PETSC_MACHINE_EPSILON, ai);
652:   
653:         bi = ei / ai - 1;
654:         ei = 2.0 * mu / ei;
655:         ai = (x[i] - l[i]) / ai - 1;
656:   
657:         da[i] = ai + bi*ci;
658:         db[i] = bi*di;
659:         dm[i] = ei + bi*fi;
660:       }
661:     }
662:     
663:     VecRestoreArray(X,&x); 
664:     VecRestoreArray(Con,&f); 
665:     VecRestoreArray(XL,&l); 
666:     VecRestoreArray(XU,&u); 
667:     VecRestoreArray(Da,&da); 
668:     VecRestoreArray(Db,&db); 
669:     VecRestoreArray(Dm,&dm); 

671:   }
672:   return(0);
673: }