Actual source code: icp.c

     2:  #include icp.h
      3: static int TaoSetDown_ICP(TAO_SOLVER, void*);

      7: static int TaoSetUp_ICP(TAO_SOLVER tao, void *solver)
      8: {
      9:   TAO_ICP *qp =(TAO_ICP*)solver;
     10:   int       n, info;
     11:   TaoLinearSolver *ksp;

     13:   TaoFunctionBegin;

     15:   /* Set pointers to Data */
     16:   info = TaoGetJacobian(tao,&qp->J);CHKERRQ(info);
     17:   info = TaoGetSolution(tao,&qp->x);CHKERRQ(info);
     18:   info = qp->x->GetDimension(&qp->n); CHKERRQ(info);

     20:   /* Allocate some arrays */
     21:   info = qp->x->Clone(&qp->f); CHKERRQ(info);
     22:   info = qp->x->Clone(&qp->g2); CHKERRQ(info);
     23:   info = qp->x->Clone(&qp->work); CHKERRQ(info);
     24:   info = qp->x->Clone(&qp->dx); CHKERRQ(info);
     25:   info = qp->x->Clone(&qp->JDiag); CHKERRQ(info);
     26:   info = qp->x->Clone(&qp->diagaxpy); CHKERRQ(info);
     27:   info = qp->x->Clone(&qp->rhs); CHKERRQ(info);
     28:   info = qp->x->Clone(&qp->rhs2); CHKERRQ(info);
     29:   info = qp->x->Clone(&qp->r12); CHKERRQ(info);

     31:   info = qp->x->Clone(&qp->xl); CHKERRQ(info);
     32:   info = qp->xl->Clone(&qp->g); CHKERRQ(info);
     33:   info = qp->xl->Clone(&qp->dg); CHKERRQ(info);
     34:   info = qp->xl->Clone(&qp->z); CHKERRQ(info);
     35:   info = qp->xl->Clone(&qp->dz); CHKERRQ(info);
     36:   info = qp->xl->Clone(&qp->gzwork); CHKERRQ(info);
     37:   info = qp->xl->Clone(&qp->r3); CHKERRQ(info);

     39:   info = qp->x->Clone(&qp->xu); CHKERRQ(info);
     40:   info = qp->xu->Clone(&qp->t); CHKERRQ(info);
     41:   info = qp->xu->Clone(&qp->dt); CHKERRQ(info);
     42:   info = qp->xu->Clone(&qp->s); CHKERRQ(info);
     43:   info = qp->xu->Clone(&qp->ds); CHKERRQ(info);
     44:   info = qp->xu->Clone(&qp->tswork); CHKERRQ(info);
     45:   info = qp->xu->Clone(&qp->r5); CHKERRQ(info);

     47:   info = TaoSetVariableBounds(tao,qp->xl,qp->xu);CHKERRQ(info);
     48:   info = TaoSetStepDirectionVector(tao,qp->dx);CHKERRQ(info);
     49:   info = TaoSetLagrangianGradientVector(tao,qp->g2);CHKERRQ(info);

     51:   /* Register the Events */
     52:   info = qp->x->GetDimension(&qp->n); CHKERRQ(info);

     54:   qp->m=0;
     55:   info=qp->g->GetDimension(&n); CHKERRQ(info); qp->m+=n;
     56:   info=qp->t->GetDimension(&n); CHKERRQ(info); qp->m+=n;

     58:   info = TaoCreateLinearSolver(tao,qp->J,300,0); CHKERRQ(info);
     59:   info = TaoGetLinearSolver(tao,&ksp); CHKERRQ(info);
     60:   info = ksp->SetTolerances(1e-14,1e-30,1e30,qp->n);CHKERRQ(info);

     62:   TaoFunctionReturn(0);
     63: }

     67: static int  QPIPSetInitialPoint(TAO_SOLVER tao, TAO_ICP *qp)
     68: {
     69:   int       info;
     70:   double    p01=1.0e-2;
     71:   double    mu;
     72:   double    dd1,dd2;
     73:   TaoVec    *g = qp->g, *z=qp->z, *s=qp->s, *t=qp->t;
     74:   TaoVec    *gzwork=qp->gzwork, *tswork=qp->tswork;
     75:   TaoVec    *rhs=qp->rhs, *r12=qp->r12;
     76:   TaoVec    *x = qp->x, *xl=qp->xl, *xu=qp->xu, *f=qp->f;
     77:   TaoVec    *work=qp->work;

     79:   TaoFunctionBegin;

     81:   /* Initialize Primal Vectors */
     82:   info = t->Waxpby(1.0,xu,-1.0,x);CHKERRQ(info);
     83:   info = g->Waxpby(1.0,x,-1.0,xl);CHKERRQ(info);

     85:   info = gzwork->SetToConstant(p01);CHKERRQ(info);
     86:   info = tswork->SetToConstant(p01);CHKERRQ(info);

     88:   info = g->PointwiseMaximum(g,gzwork); CHKERRQ(info);
     89:   info = t->PointwiseMaximum(t,tswork); CHKERRQ(info);

     91:   /* Initialize Dual Variable Vectors */

     93:   info = z->CopyFrom(g); CHKERRQ(info);
     94:   info = z->Reciprocal(); CHKERRQ(info);

     96:   info = s->CopyFrom(t); CHKERRQ(info);
     97:   info = s->Reciprocal(); CHKERRQ(info);
     98:
     99:   info = r12->Waxpby(1.0,s,-1.0,z);CHKERRQ(info);
    100:   info = rhs->CopyFrom(f);CHKERRQ(info);
    101:
    102:   info = rhs->AbsoluteValue(); CHKERRQ(info);
    103:   info = r12->AbsoluteValue(); CHKERRQ(info);
    104:   info = work->SetToConstant(p01);CHKERRQ(info);
    105:   info = rhs->PointwiseMaximum(rhs,work); CHKERRQ(info);
    106:   info = r12->PointwiseMaximum(r12,work); CHKERRQ(info);
    107:
    108:   info = rhs->PointwiseDivide(rhs,r12); CHKERRQ(info);
    109:
    110:   info = r12->Norm1(&qp->dinfeas); CHKERRQ(info);
    111:   mu = (qp->dinfeas)/(qp->n);
    112:
    113:   info = s->Scale(mu); CHKERRQ(info);
    114:   info = z->Scale(mu); CHKERRQ(info);
    115:
    116:   info = QPIPComputeResidual(qp); CHKERRQ(info);
    117:   info = TAOComputeNormFromCentralPath_ICP(tao,&dd2); CHKERRQ(info);
    118:   dd1=TaoMax(qp->pathnorm,1.0e-2);
    119:   while ( (qp->dinfeas+qp->pinfeas)/(qp->m+qp->n) >= qp->mu ){
    120:
    121:     info=g->AddConstant(dd1); CHKERRQ(info);
    122:     info=z->AddConstant(dd1); CHKERRQ(info);
    123:     info=s->AddConstant(dd1); CHKERRQ(info);
    124:     info=t->AddConstant(dd1); CHKERRQ(info);

    126:     info = QPIPComputeResidual(qp); CHKERRQ(info);
    127:     info = TAOComputeNormFromCentralPath_ICP(tao,&dd2); CHKERRQ(info);
    128:
    129:     qp->dobj = qp->pobj - qp->gap;
    130:     qp->rgap=qp->gap/( TaoAbsScalar(qp->dobj) + TaoAbsScalar(qp->pobj) + 1.0 );
    131:     dd1=dd1*2;
    132:   }

    134:   TaoFunctionReturn(0);
    135: }

    140: static int TaoSetDown_ICP(TAO_SOLVER tao, void*solver)
    141: {
    142:   TAO_ICP *qp = (TAO_ICP*)solver;
    143:   int info;

    145:   /* Free allocated memory in GPCG structure */
    146:   TaoFunctionBegin;

    148:   info=TaoVecDestroy(qp->g2);CHKERRQ(info);
    149:   info=TaoVecDestroy(qp->g);CHKERRQ(info);
    150:   info=TaoVecDestroy(qp->dg);CHKERRQ(info);
    151:   info=TaoVecDestroy(qp->z);CHKERRQ(info);
    152:   info=TaoVecDestroy(qp->dz);CHKERRQ(info);
    153:   info=TaoVecDestroy(qp->gzwork);CHKERRQ(info);
    154:   info=TaoVecDestroy(qp->r3);CHKERRQ(info);
    155:   info=TaoVecDestroy(qp->xl);CHKERRQ(info);
    156:
    157:   info=TaoVecDestroy(qp->s);CHKERRQ(info);
    158:   info=TaoVecDestroy(qp->ds);CHKERRQ(info);
    159:   info=TaoVecDestroy(qp->t);CHKERRQ(info);
    160:   info=TaoVecDestroy(qp->dt);CHKERRQ(info);
    161:   info=TaoVecDestroy(qp->tswork);CHKERRQ(info);
    162:   info=TaoVecDestroy(qp->r5);CHKERRQ(info);
    163:   info=TaoVecDestroy(qp->xu);CHKERRQ(info);
    164:
    165:   info=TaoVecDestroy(qp->f);CHKERRQ(info);
    166:   info=TaoVecDestroy(qp->JDiag);CHKERRQ(info);
    167:   info=TaoVecDestroy(qp->work);CHKERRQ(info);
    168:   info=TaoVecDestroy(qp->diagaxpy);CHKERRQ(info);
    169:   info=TaoVecDestroy(qp->rhs);CHKERRQ(info);
    170:   info=TaoVecDestroy(qp->rhs2);CHKERRQ(info);
    171:   info=TaoVecDestroy(qp->dx);CHKERRQ(info);
    172:   info=TaoVecDestroy(qp->r12);CHKERRQ(info);
    173:
    174:   info = TaoDestroyLinearSolver(tao);CHKERRQ(info);

    176:   info = TaoSetVariableBounds(tao,0,0);CHKERRQ(info);
    177:   info = TaoSetStepDirectionVector(tao,0);CHKERRQ(info);
    178:   info = TaoSetLagrangianGradientVector(tao,qp->g2);CHKERRQ(info);

    180:   TaoFunctionReturn(0);
    181: }

    185: static int TaoSolve_ICP(TAO_SOLVER tao, void *solver)
    186: {
    187:   TAO_ICP   *qp = (TAO_ICP*)solver;
    188:   TaoVec    *x = qp->x, *xl=qp->xl, *xu=qp->xu, *f=qp->f;
    189:   TaoVec    *g = qp->g, *z=qp->z, *s=qp->s, *t=qp->t;
    190:   TaoVec    *dg=qp->dg, *dz=qp->dz, *ds=qp->ds, *dt=qp->dt;
    191:   TaoVec    *gzwork=qp->gzwork, *tswork=qp->tswork;
    192:   TaoVec    *rhs=qp->rhs, *rhs2=qp->rhs2;
    193:   TaoVec    *diagaxpy=qp->diagaxpy, *dx=qp->dx;
    194:   TaoVec    *r3=qp->r3,*r5=qp->r5;
    195:   TaoVec    *JDiag=qp->JDiag;
    196:   TaoMat    *J;
    197:   int       info,n,iter=0;
    198:   double    d1,ksptol;
    199:   double    sigmamu;// sigma;
    200:   double    dstep,pstep,step=0;
    201:   double    gap[4];
    202:   TaoTruth  kspsuccess;
    203:   TaoTerminateReason reason;
    204:   TaoLinearSolver *qpksp;

    206:   TaoFunctionBegin;

    208:   info = TaoGetSolution(tao,&x);CHKERRQ(info); qp->x=x;
    209:   info = TaoGetJacobian(tao,&J);CHKERRQ(info); qp->J=J;

    211:   info = x->Median(xl,x,xu); CHKERRQ(info);
    212:   info = TaoEvaluateVariableBounds(tao,xl,xu); CHKERRQ(info);
    213:   info = TaoComputeConstraints(tao, x, f); CHKERRQ(info);
    214:
    215:   info = x->GetDimension(&n);CHKERRQ(info);

    217:   info = QPIPSetInitialPoint(tao,qp); CHKERRQ(info);
    218:
    219:   /* Enter main loop */
    220:   while (1){

    222:     info = QPIPComputeResidual(qp); CHKERRQ(info);
    223:     info = TAOComputeNormFromCentralPath_ICP(tao,&d1); CHKERRQ(info);
    224:
    225:     /* Check Stopping Condition      */
    226:     info=TaoMonitor(tao,iter++,qp->gap,(qp->gap+qp->dinfeas),qp->pinfeas,
    227:                     step,&reason); CHKERRQ(info);
    228:     if (reason != TAO_CONTINUE_ITERATING) break;

    230:     info = rhs->ScaleCopyFrom(-1.0,f); CHKERRQ(info);
    231:
    232:     info = diagaxpy->SetToZero(); CHKERRQ(info);
    233:
    234:     info = gzwork->PointwiseDivide(z,g); CHKERRQ(info);
    235:     info = diagaxpy->Axpy(1.0,gzwork); CHKERRQ(info);
    236:     info = gzwork->PointwiseMultiply(gzwork,r3); CHKERRQ(info);
    237:     info = rhs->Axpy(-1.0,gzwork); CHKERRQ(info);
    238:
    239:     info = tswork->PointwiseDivide(s,t); CHKERRQ(info);
    240:     info = diagaxpy->Axpy(1.0,tswork); CHKERRQ(info);
    241:     info = tswork->PointwiseMultiply(tswork,r5); CHKERRQ(info);
    242:     info = rhs->Axpy(-1.0,tswork); CHKERRQ(info);
    243:     /*
    244:     info = dz->SetToZero(); CHKERRQ(info);
    245:     info = dg->SetToZero(); CHKERRQ(info);
    246:     info = ds->SetToZero(); CHKERRQ(info);
    247:     info = dt->SetToZero(); CHKERRQ(info);
    248:     */
    249:     /* Add centrality part */
    250:     if (iter > 0 && (qp->rnorm>5*qp->mu || d1*d1>qp->m*qp->mu*qp->mu) ) {
    251:       sigmamu=qp->mu;//sigma=1.0;
    252:       sigmamu=0;//sigma=0.0;
    253:     } else {
    254:       sigmamu=0;//sigma=0.0;
    255:     }
    256:     sigmamu=0.1*qp->mu;//sigma=0.1;
    257:     sigmamu=0.0*qp->mu;//sigma=0.0;
    258:     info = dz->SetToConstant(sigmamu); CHKERRQ(info);
    259:     info = ds->SetToConstant(sigmamu); CHKERRQ(info);
    260:     //    printf("TargetMu: %4.2e\n",sigmamu);
    261:     if (sigmamu !=0){
    262:       info = dz->PointwiseDivide(dz,g); CHKERRQ(info);
    263:       info = ds->PointwiseDivide(ds,t); CHKERRQ(info);
    264:       info = rhs->Axpy(1.0,dz); CHKERRQ(info);
    265:       info = rhs->Axpy(-1.0,ds); CHKERRQ(info);
    266:     } else {
    267:       //      info = qp->RHS2->SetToZero(); CHKERRQ(info);
    268:     }

    270:     qp->usedcorrector=TAO_FALSE;
    271:     qp->sigmamu=sigmamu;

    275:     /*  Determine the solving tolerance */
    276:     ksptol = qp->mu/10.0;
    277:     ksptol = TaoMin(ksptol,0.00000000001);
    278:     info = TaoGetLinearSolver(tao,&qpksp); CHKERRQ(info);
    279:     info = qpksp->SetTolerances(ksptol,1e-30,1e30,2*n); CHKERRQ(info);

    281:     info = TaoComputeJacobian(tao,x,J); CHKERRQ(info);
    282:     info = J->GetDiagonal(JDiag); CHKERRQ(info);
    283:     info = J->AddDiagonal(diagaxpy); CHKERRQ(info);

    285:     info = TaoPreLinearSolve(tao,J);CHKERRQ(info);
    286:     info = TaoLinearSolve(tao,J,rhs,dx,&kspsuccess);CHKERRQ(info);

    288:     info = QPComputeStepDirection(qp); CHKERRQ(info);
    289:     info = QPStepLength(qp);  CHKERRQ(info);

    291:     /* Calculate New Residual R1 in Work vector */
    292:     info=dg->Dot(dz, gap); CHKERRQ(info);
    293:     info=dt->Dot(ds, gap+1); CHKERRQ(info);
    294:
    295:     qp->rnorm=(qp->dinfeas+qp->psteplength*qp->pinfeas)/(qp->m+qp->n);
    296:     pstep = qp->psteplength; dstep = qp->dsteplength;
    297:     step = TaoMin(qp->psteplength,qp->dsteplength);
    298:     //    printf("Step: %4.2f\n",step);
    299:     step=1-step;
    300:     sigmamu = step*step*step*qp->mu;
    301:     //    sigmamu = 0;
    302:     qp->sigmamu=sigmamu;
    303:     /*
    304:     ( pstep*pstep*(gap[0]+gap[1]) +
    305:                (1 - pstep + pstep*sigma)*qp->gap  )/qp->m;
    306:     */
    307:     if (1==1 && /* && qp->predcorr &&*/ step < 1.99){
    308:
    309:       /*
    310:       if (sigmamu < qp->mu){
    311:         sigmamu=sigmamu/qp->mu;
    312:         sigmamu=sigmamu*sigmamu*sigmamu;
    313:       } else {sigmamu = 1.0;}
    314:       sigmamu = 0.0*qp->mu;
    315:       */
    316:       qp->usedcorrector=TAO_TRUE;
    317:       /* Compute Corrector Step */
    318:       info = gzwork->PointwiseMultiply(dg,dz); CHKERRQ(info);
    319:       info = gzwork->Negate(); CHKERRQ(info);
    320:       info = gzwork->AddConstant(sigmamu); CHKERRQ(info);
    321:       info = gzwork->PointwiseDivide(gzwork,g); CHKERRQ(info);

    323:       info = tswork->PointwiseMultiply(ds,dt); CHKERRQ(info);
    324:       info = tswork->Negate(); CHKERRQ(info);
    325:       info = tswork->AddConstant(sigmamu); CHKERRQ(info);
    326:       info = tswork->PointwiseDivide(tswork,t); CHKERRQ(info);
    327:
    328:       info = rhs2->Waxpby(1.0,gzwork,-1.0,tswork); CHKERRQ(info);
    329:       info = rhs2->Axpy(1.0,rhs); CHKERRQ(info);

    331:       info = TaoLinearSolve(tao,J,rhs2,dx,&kspsuccess);CHKERRQ(info);
    332:
    333:       info = QPComputeStepDirection(qp); CHKERRQ(info);
    334:       info = QPStepLength(qp); CHKERRQ(info);

    336:     }  /* End Corrector step */

    339:     /* Take the step */
    340:     pstep = qp->psteplength; dstep = qp->dsteplength;

    342:     info = z->Axpy(dstep,dz); CHKERRQ(info);
    343:     info = s->Axpy(dstep,ds); CHKERRQ(info);
    344:     info = x->Axpy(dstep,dx); CHKERRQ(info);
    345:     info = g->Axpy(pstep,dg); CHKERRQ(info);
    346:     info = t->Axpy(pstep,dt); CHKERRQ(info);
    347:
    348:     info = TaoComputeConstraints(tao, x, f); CHKERRQ(info);

    350:   }  /* END MAIN LOOP  */

    352:   TaoFunctionReturn(0);
    353: }

    357: static int QPComputeStepDirection(TAO_ICP *qp)
    358: {
    359:   int info;
    360:   double gap1,gap2;
    361:   TaoVec    *g = qp->g, *z=qp->z, *s=qp->s, *t=qp->t;
    362:   TaoVec    *dg=qp->dg, *dz=qp->dz, *ds=qp->ds, *dt=qp->dt;
    363:   TaoVec    *gzwork=qp->gzwork, *tswork=qp->tswork;
    364:   TaoVec    *dx=qp->dx;
    365:   TaoVec    *r3=qp->r3,*r5=qp->r5;

    367:   TaoFunctionBegin;

    369:   if (qp->usedcorrector==TAO_TRUE){
    370:     info = dz->PointwiseMultiply(dz,dg); CHKERRQ(info);
    371:     info = dz->PointwiseDivide(dz,g); CHKERRQ(info);
    372:     info = dz->Negate(); CHKERRQ(info);
    373:
    374:     info = ds->PointwiseMultiply(ds,dt); CHKERRQ(info);
    375:     info = ds->PointwiseDivide(ds,t); CHKERRQ(info);
    376:     info = ds->Negate(); CHKERRQ(info);
    377:   }
    378:   /*
    379:     info = dz->SetToZero(); CHKERRQ(info);
    380:     info = ds->SetToZero(); CHKERRQ(info);
    381:   */
    382:    /* Calculate DG */
    383:   info = dg->Waxpby(1.0,dx,1.0,r3);CHKERRQ(info);

    385:   /* Calculate DT */
    386:   info = dt->Waxpby(-1.0,dx,-1.0,r5);CHKERRQ(info);

    388:   /* Calculate DZ */
    389:   info = gzwork->PointwiseMultiply(dg,z); CHKERRQ(info);
    390:   info = gzwork->Negate(); CHKERRQ(info);
    391:   info = gzwork->AddConstant(qp->sigmamu); CHKERRQ(info);
    392:   info = gzwork->PointwiseDivide(gzwork,g); CHKERRQ(info);
    393:   info = gzwork->Axpy(-1.0,z);CHKERRQ(info);

    395:   info = dz->Axpy(1.0,gzwork);CHKERRQ(info);
    396:   //  info = qp->DZ->CopyFrom(qp->GZwork);CHKERRQ(info);

    398:   /* Calculate DS */
    399:   info = tswork->PointwiseMultiply(dt,s); CHKERRQ(info);
    400:   info = tswork->Negate(); CHKERRQ(info);
    401:   info = tswork->AddConstant(qp->sigmamu); CHKERRQ(info);
    402:   info = tswork->PointwiseDivide(tswork,t); CHKERRQ(info);
    403:   info = tswork->Axpy(-1.0,s);CHKERRQ(info);
    404:   info = ds->Axpy(1.0,tswork);CHKERRQ(info);
    405:   //  info = qp->DS->CopyFrom(qp->TSwork);CHKERRQ(info);

    407:   info = ds->Dot(dt,&gap1); CHKERRQ(info);
    408:   info = dg->Dot(dz,&gap2); CHKERRQ(info);
    409:   //  printf("Gap Reduce: %4.2e \n",gap1+gap2);
    410:   TaoFunctionReturn(0);
    411: }

    415: static int QPIPComputeResidual(TAO_ICP *qp)
    416: {
    417:   int info;
    418:   double gap1,gap2,dtmp = 1.0 - qp->psteplength;
    419:   TaoVec    *x = qp->x, *xl=qp->xl, *xu=qp->xu, *f=qp->f;
    420:   TaoVec    *g = qp->g, *z=qp->z, *s=qp->s, *t=qp->t;
    421:   TaoVec    *rhs=qp->rhs;
    422:   TaoVec    *r12=qp->r12,*r3=qp->r3,*r5=qp->r5;

    424:   TaoFunctionBegin;

    426:   /* Compute R3 and R5 */

    428:   if (0==1){

    430:     info = r3->Scale(dtmp);
    431:     info = r5->Scale(dtmp);
    432:     qp->pinfeas=dtmp*qp->pinfeas;

    434:   } else {

    436:     info = r3->Waxpby(1.0,x,-1.0,xl);CHKERRQ(info);
    437:     info = r3->Axpy(-1.0,g);CHKERRQ(info);

    439:     info = r5->Waxpby(1.0,x,-1.0,xu);CHKERRQ(info);
    440:     info = r5->Axpy(1.0,t);CHKERRQ(info);

    442:     info = r3->NormInfinity(&gap1);CHKERRQ(info);
    443:     info = r5->NormInfinity(&gap2);CHKERRQ(info);

    445:     qp->pinfeas=TaoMax(gap1,gap2);

    447:   }

    449:   info = r12->Waxpby(1.0,s,-1.0,z);CHKERRQ(info);

    451:   info = rhs->CopyFrom(f);CHKERRQ(info);
    452:   info = rhs->Negate();CHKERRQ(info);

    454:   info = r12->Axpy(-1.0,rhs);CHKERRQ(info);

    456:   info = r12->NormInfinity(&qp->dinfeas); CHKERRQ(info);
    457:   qp->rnorm=(qp->dinfeas+qp->pinfeas)/(qp->m+qp->n);
    458:   qp->rnorm=TaoMax(qp->dinfeas,qp->pinfeas);
    459:   //  printf("Dual Infeas: %4.2e,  Primal Infeas: %4.2e\n",qp->dinfeas,qp->pinfeas);
    460:
    461:   TaoFunctionReturn(0);
    462: }

    466: static int QPStepLength(TAO_ICP *qp)
    467: {
    468:   double tstep1,tstep2,tstep3,tstep4,tstep;
    469:   int info;
    470:   TaoVec    *g = qp->g, *z=qp->z, *s=qp->s, *t=qp->t;
    471:   TaoVec    *dg=qp->dg, *dz=qp->dz, *ds=qp->ds, *dt=qp->dt;

    473:   TaoFunctionBegin;
    474:   /* Compute stepsize to the boundary */

    476:   info = g->StepMax(dg,&tstep1); CHKERRQ(info);
    477:   info = t->StepMax(dt,&tstep2); CHKERRQ(info);
    478:   info = s->StepMax(ds,&tstep3); CHKERRQ(info);
    479:   info = z->StepMax(dz,&tstep4); CHKERRQ(info);

    481:   tstep = TaoMin(tstep1,tstep2);
    482:   qp->psteplength = TaoMin(0.999*tstep,1.0);

    484:   tstep = TaoMin(tstep3,tstep4);
    485:   qp->dsteplength = TaoMin(0.999*tstep,1.0);

    487:   qp->psteplength = TaoMin(qp->psteplength,qp->dsteplength);
    488:   qp->dsteplength = qp->psteplength;
    489:   //  printf("StepSize: %4.2e\n",qp->dsteplength);

    491:   TaoFunctionReturn(0);
    492: }

    497: int TAOComputeNormFromCentralPath_ICP(TAO_SOLVER tao, double *norm)
    498: {
    499:   TAO_ICP *qp;
    500:   int       info;
    501:   double    gap[2],mu[2], nmu;
    502:
    503:   TaoFunctionBegin;
    504:   info = TaoGetSolverContext(tao,"tao_icp",(void**)&qp); CHKERRQ(info);
    505:   TaoVec    *g = qp->g, *z=qp->z, *s=qp->s, *t=qp->t;
    506:   TaoVec    *gzwork=qp->gzwork, *tswork=qp->tswork;

    508:   info = gzwork->PointwiseMultiply(g,z); CHKERRQ(info);
    509:   info = tswork->PointwiseMultiply(t,s); CHKERRQ(info);
    510:   info = tswork->Norm1(&mu[0]); CHKERRQ(info);
    511:   info = gzwork->Norm1(&mu[1]); CHKERRQ(info);

    513:   nmu=-(mu[0]+mu[1])/qp->m;
    514:   qp->gap=mu[0]+mu[1];
    515:   qp->mu=-nmu;

    517:   info = gzwork->AddConstant(nmu); CHKERRQ(info);
    518:   info = tswork->AddConstant(nmu); CHKERRQ(info);

    520:   info = gzwork->NormInfinity(&gap[0]); CHKERRQ(info);
    521:   info = tswork->NormInfinity(&gap[1]); CHKERRQ(info);

    523:   qp->pathnorm=TaoMax(gap[0],gap[1]);
    524:   *norm=qp->pathnorm;
    525:   //  printf("Gap: %4.2e,  Mu: %4.2e,  Norm: %4.2e\n",qp->gap,-nmu,qp->pathnorm);

    527:   //  printf("Gap: %4.2e,  Mu: %4.2e,  Norm: %4.2e\n",qp->gap,qp->mu,qp->pathnorm);
    528:   TaoFunctionReturn(0);
    529: }

    534: static int TaoSetOptions_ICP(TAO_SOLVER tao, void *solver){

    536:   TAO_ICP *qp = (TAO_ICP*)solver;
    537:   int       info;
    538:   TaoTruth  flg;

    540:   TaoFunctionBegin;
    541:   info = TaoOptionsHead("Interior point method for bound constrained quadratic optimization");CHKERRQ(info);
    542:   info = TaoOptionInt("-predcorr","Use a predictor-corrector method","",qp->predcorr,&qp->predcorr,&flg);
    543:   CHKERRQ(info);
    544:   info = TaoOptionsTail();CHKERRQ(info);

    546:   /*   info = TaoSetLinearSolverOptions(tao);CHKERRQ(info); */

    548:   TaoFunctionReturn(0);
    549: }

    554: static int TaoView_ICP(TAO_SOLVER tao,void *solver){
    555:   TaoFunctionBegin;
    556:   TaoFunctionReturn(0);
    557: }

    560: /* --------------------------------------------------------- */
    564: int TaoCreate_ICP(TAO_SOLVER tao)
    565: {
    566:   TAO_ICP *qp;
    567:   int       info;

    569:   TaoFunctionBegin;

    571:   info = TaoNew(TAO_ICP,&qp); CHKERRQ(info);
    572:   info = PetscLogObjectMemory(tao,sizeof(TAO_ICP)); CHKERRQ(info);

    574:   info=TaoSetTaoSolveRoutine(tao,TaoSolve_ICP,(void*)qp); CHKERRQ(info);
    575:   info=TaoSetTaoSetUpDownRoutines(tao,TaoSetUp_ICP,TaoSetDown_ICP); CHKERRQ(info);
    576:   info=TaoSetTaoOptionsRoutine(tao,TaoSetOptions_ICP); CHKERRQ(info);
    577:   info=TaoSetTaoViewRoutine(tao,TaoView_ICP); CHKERRQ(info);

    579:   info = TaoSetMaximumIterates(tao,100); CHKERRQ(info);
    580:   info = TaoSetMaximumFunctionEvaluations(tao,500); CHKERRQ(info);
    581:   info = TaoSetTolerances(tao,1e-10,1e-10,1e-12,0); CHKERRQ(info);
    582:   /*
    583:   tao->defaultmonitor     = TaoDefaultMonitor_ICP;
    584:   */
    585:   /* Initialize pointers and variables */
    586:   qp->n              = 0;
    587:   qp->m              = 0;
    588:   qp->ksp_tol       = 0.1;
    589:   qp->dobj           = 0.0;
    590:   qp->pobj           = 1.0;
    591:   qp->gap            = 10.0;
    592:   qp->rgap           = 1.0;
    593:   qp->mu             = 1.0;
    594:   qp->sigma          = 1.0;
    595:   qp->dinfeas        = 1.0;
    596:   qp->predcorr       = 1;
    597:   qp->psteplength    = 0.0;
    598:   qp->dsteplength    = 0.0;

    600:   TaoFunctionReturn(0);
    601: }