Actual source code: bqpip.c

      1: /*$Id: bqpip.c 1.104 05/05/09 11:09:17-05:00 sarich@zorak.(none) $*/

     3:  #include bqpip.h
      4: static int TaoSetDown_BQPIP(TAO_SOLVER, void*);

      8: static int TaoSetUp_BQPIP(TAO_SOLVER tao, void *solver)
      9: {
     10:   TAO_BQPIP *qp =(TAO_BQPIP*)solver;
     11:   int       n, info;
     12:   TaoLinearSolver *ksp;

     14:   TaoFunctionBegin;

     16:   /* Set pointers to Data */
     17:   info = TaoGetHessian(tao,&qp->H);
     18:   info = TaoGetSolution(tao,&qp->XY);CHKERRQ(info);
     19:   info = qp->XY->GetDimension(&qp->n); CHKERRQ(info);

     21:   /* Allocate some arrays */
     22:   info = qp->XY->Clone(&qp->Work); CHKERRQ(info);
     23:   info = qp->XY->Clone(&qp->DXY); CHKERRQ(info);
     24:   info = qp->XY->Clone(&qp->HDiag); CHKERRQ(info);
     25:   info = qp->XY->Clone(&qp->DiagAxpy); CHKERRQ(info);
     26:   info = qp->XY->Clone(&qp->RHS); CHKERRQ(info);
     27:   info = qp->XY->Clone(&qp->RHS2); CHKERRQ(info);
     28:   info = qp->XY->Clone(&qp->C0); CHKERRQ(info);
     29:   info = qp->XY->Clone(&qp->R12); CHKERRQ(info);

     31:   info = qp->XY->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->XY->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 = TaoSetLagrangianGradientVector(tao,qp->R12);CHKERRQ(info);
     48:   info = TaoSetVariableBounds(tao,qp->XL,qp->XU);CHKERRQ(info);
     49:   info = TaoSetStepDirectionVector(tao,qp->DXY);CHKERRQ(info);

     51:   /* Register the Events */
     52:   info = qp->XY->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->H,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_BQPIP *qp)
     68: {
     69:   int       info;
     70:   double    two=2.0,p01=1;
     71:   double    gap1,gap2,fff,mu;

     73:   TaoFunctionBegin;
     74:   /* Compute function, Gradient R=Hx+b, and Hessian */
     75:   info = qp->XY->Median(qp->XL,qp->XY,qp->XU); CHKERRQ(info);
     76:   info = qp->H->Multiply(qp->XY,qp->R12); CHKERRQ(info);

     78:   info = qp->Work->Waxpby(0.5,qp->R12,1.0,qp->C0);CHKERRQ(info);
     79:   info = qp->R12->Axpy(1.0,qp->C0);CHKERRQ(info);
     80:   info = qp->Work->Dot(qp->XY,&fff);CHKERRQ(info);
     81:   qp->pobj = fff + qp->c;

     83:   /* Initialize Primal Vectors */

     85:   info = qp->T->Waxpby(1.0,qp->XU,-1.0,qp->XY);CHKERRQ(info);
     86:   info = qp->G->Waxpby(1.0,qp->XY,-1.0,qp->XL);CHKERRQ(info);

     88:   info = qp->GZwork->SetToConstant(p01);CHKERRQ(info);
     89:   info = qp->TSwork->SetToConstant(p01);CHKERRQ(info);

     91:   info = qp->G->PointwiseMaximum(qp->G,qp->GZwork); CHKERRQ(info);
     92:   info = qp->T->PointwiseMaximum(qp->T,qp->TSwork); CHKERRQ(info);

     94:   /* Initialize Dual Variable Vectors */

     96:   info = qp->Z->CopyFrom(qp->G); CHKERRQ(info);
     97:   info = qp->Z->Reciprocal(); CHKERRQ(info);

     99:   info = qp->S->CopyFrom(qp->T); CHKERRQ(info);
    100:   info = qp->S->Reciprocal(); CHKERRQ(info);

    102:   info = qp->H->Multiply(qp->Work,qp->RHS); CHKERRQ(info);
    103:   info = qp->RHS->AbsoluteValue(); CHKERRQ(info);
    104:   info = qp->Work->SetToConstant(p01);CHKERRQ(info);
    105:   info = qp->RHS->PointwiseMaximum(qp->RHS,qp->Work); CHKERRQ(info);

    107:   info = qp->RHS->PointwiseDivide(qp->R12,qp->RHS); CHKERRQ(info);
    108:   info = qp->RHS->Norm1(&gap1); CHKERRQ(info);
    109:   mu = TaoMin(10.0,(gap1+10.0)/qp->m);

    111:   info = qp->S->Scale(mu); CHKERRQ(info);
    112:   info = qp->Z->Scale(mu); CHKERRQ(info);

    114:   info = qp->TSwork->SetToConstant(p01); CHKERRQ(info);
    115:   info = qp->GZwork->SetToConstant(p01); CHKERRQ(info);
    116:   info = qp->S->PointwiseMaximum(qp->S,qp->TSwork); CHKERRQ(info);
    117:   info = qp->Z->PointwiseMaximum(qp->Z,qp->GZwork); CHKERRQ(info);

    119:   qp->mu=0;qp->dinfeas=1.0;qp->pinfeas=1.0;
    120:   while ( (qp->dinfeas+qp->pinfeas)/(qp->m+qp->n) >= qp->mu ){

    122:     info=qp->G->Scale(two); CHKERRQ(info);
    123:     info=qp->Z->Scale(two); CHKERRQ(info);
    124:     info=qp->S->Scale(two); CHKERRQ(info);
    125:     info=qp->T->Scale(two); CHKERRQ(info);

    127:     info = QPIPComputeResidual(qp); CHKERRQ(info);
    128:
    129:     info=qp->R3->Waxpby(1.0,qp->XY,-1.0,qp->G);CHKERRQ(info);
    130:     info=qp->R3->Axpy(-1.0,qp->XL);CHKERRQ(info);

    132:     info=qp->R5->Waxpby(1.0,qp->XY,1.0,qp->T);CHKERRQ(info);
    133:     info=qp->R5->Axpy(-1.0,qp->XU);CHKERRQ(info);
    134:
    135:     info=qp->R3->NormInfinity(&gap1);CHKERRQ(info);
    136:     info=qp->R5->NormInfinity(&gap2);CHKERRQ(info);
    137:     qp->pinfeas=TaoMax(gap1,gap2);
    138:
    139:     /* Compute the duality gap */
    140:     info=qp->G->Dot(qp->Z,&gap1);CHKERRQ(info);
    141:     info=qp->T->Dot(qp->S,&gap2);CHKERRQ(info);
    142:
    143:     qp->gap = (gap1+gap2);
    144:     qp->dobj = qp->pobj - qp->gap;
    145:     if (qp->m>0) qp->mu=qp->gap/(qp->m); else qp->mu=0.0;
    146:     qp->rgap=qp->gap/( TaoAbsScalar(qp->dobj) + TaoAbsScalar(qp->pobj) + 1.0 );
    147:   }

    149:   TaoFunctionReturn(0);
    150: }

    155: static int TaoSetDown_BQPIP(TAO_SOLVER tao, void*solver)
    156: {
    157:   TAO_BQPIP *qp = (TAO_BQPIP*)solver;
    158:   int info;

    160:   /* Free allocated memory in GPCG structure */
    161:   TaoFunctionBegin;

    163:   info=TaoVecDestroy(qp->G);CHKERRQ(info);
    164:   info=TaoVecDestroy(qp->DG);CHKERRQ(info);
    165:   info=TaoVecDestroy(qp->Z);CHKERRQ(info);
    166:   info=TaoVecDestroy(qp->DZ);CHKERRQ(info);
    167:   info=TaoVecDestroy(qp->GZwork);CHKERRQ(info);
    168:   info=TaoVecDestroy(qp->R3);CHKERRQ(info);
    169:   info=TaoVecDestroy(qp->XL);CHKERRQ(info);
    170:
    171:   info=TaoVecDestroy(qp->S);CHKERRQ(info);
    172:   info=TaoVecDestroy(qp->DS);CHKERRQ(info);
    173:   info=TaoVecDestroy(qp->T);CHKERRQ(info);
    174:   info=TaoVecDestroy(qp->DT);CHKERRQ(info);
    175:   info=TaoVecDestroy(qp->TSwork);CHKERRQ(info);
    176:   info=TaoVecDestroy(qp->R5);CHKERRQ(info);
    177:   info=TaoVecDestroy(qp->XU);CHKERRQ(info);
    178:
    179:   info=TaoVecDestroy(qp->HDiag);CHKERRQ(info);
    180:   info=TaoVecDestroy(qp->Work);CHKERRQ(info);
    181:   info=TaoVecDestroy(qp->DiagAxpy);CHKERRQ(info);
    182:   info=TaoVecDestroy(qp->RHS);CHKERRQ(info);
    183:   info=TaoVecDestroy(qp->RHS2);CHKERRQ(info);
    184:   info=TaoVecDestroy(qp->DXY);CHKERRQ(info);
    185:   info=TaoVecDestroy(qp->C0);CHKERRQ(info);
    186:   info=TaoVecDestroy(qp->R12);CHKERRQ(info);
    187:
    188:   info = TaoDestroyLinearSolver(tao);CHKERRQ(info);

    190:   TaoFunctionReturn(0);
    191: }

    195: static int TaoSolve_BQPIP(TAO_SOLVER tao, void *solver)
    196: {
    197:   TAO_BQPIP *qp = (TAO_BQPIP*)solver;
    198:   int       info,n,iter=0;
    199:   double    d1,d2,ksptol,sigma;
    200:   double    sigmamu;
    201:   double    dstep,pstep,step=0;
    202:   double    gap[4];
    203:   TaoTruth  kspsuccess;
    204:   TaoTerminateReason reason;
    205:   TaoLinearSolver *qpksp;
    206:

    208:   TaoFunctionBegin;

    210:   info = TaoGetSolution(tao,&qp->XY);CHKERRQ(info);
    211:   info = TaoEvaluateVariableBounds(tao,qp->XL,qp->XU); CHKERRQ(info);
    212:   info = qp->XY->GetDimension(&n);CHKERRQ(info);

    214:   info = TaoComputeFunctionGradient(tao,qp->XY,&qp->c,qp->C0); CHKERRQ(info);
    215:   info = TaoComputeHessian(tao,qp->XY,qp->H); CHKERRQ(info);
    216:   info = qp->H->Multiply(qp->XY,qp->Work); CHKERRQ(info);
    217:   info = qp->XY->Dot(qp->Work,&d1); CHKERRQ(info);
    218:   info = qp->C0->Axpy(-1.0,qp->Work); CHKERRQ(info);
    219:   info = qp->XY->Dot(qp->C0,&d2); CHKERRQ(info);
    220:   qp->c -= (d1/2.0+d2);

    222:   info = qp->H->GetDiagonal(qp->HDiag); CHKERRQ(info);

    224:   info = QPIPSetInitialPoint(tao,qp); CHKERRQ(info);
    225:   info = QPIPComputeResidual(qp); CHKERRQ(info);
    226:
    227:   /* Enter main loop */
    228:   while (1){

    230:     /* Check Stopping Condition      */
    231:     info=TaoMonitor(tao,iter++,qp->pobj,sqrt(qp->gap+qp->dinfeas),qp->pinfeas,
    232:                     step,&reason); CHKERRQ(info);
    233:     if (reason != TAO_CONTINUE_ITERATING) break;

    235:     /*
    236:        Dual Infeasibility Direction should already be in the right
    237:        hand side from computing the residuals
    238:     */

    240:     info = TAOComputeNormFromCentralPath_BQPIP(tao,&d1); CHKERRQ(info);

    242:     if (iter > 0 && (qp->rnorm>5*qp->mu || d1*d1>qp->m*qp->mu*qp->mu) ) {
    243:       sigma=1.0;sigmamu=qp->mu;
    244:       sigma=0.0;sigmamu=0;
    245:     } else {
    246:       sigma=0.0;sigmamu=0;
    247:     }
    248:     info = qp->DZ->SetToConstant(sigmamu); CHKERRQ(info);
    249:     info = qp->DS->SetToConstant(sigmamu); CHKERRQ(info);

    251:     if (sigmamu !=0){
    252:       info = qp->DZ->PointwiseDivide(qp->DZ,qp->G); CHKERRQ(info);
    253:       info = qp->DS->PointwiseDivide(qp->DS,qp->T); CHKERRQ(info);
    254:       info = qp->RHS2->Waxpby(1.0,qp->DZ,1.0,qp->DS); CHKERRQ(info);
    255:     } else {
    256:       info = qp->RHS2->SetToZero(); CHKERRQ(info);
    257:     }

    260:     /*
    261:        Compute the Primal Infeasiblitiy RHS and the
    262:        Diagonal Matrix to be added to H and store in Work
    263:     */
    264:     info = qp->DiagAxpy->PointwiseDivide(qp->Z,qp->G); CHKERRQ(info);
    265:     info = qp->GZwork->PointwiseMultiply(qp->DiagAxpy,qp->R3); CHKERRQ(info);
    266:     info = qp->RHS->Axpy(-1.0,qp->GZwork); CHKERRQ(info);

    268:     info = qp->TSwork->PointwiseDivide(qp->S,qp->T); CHKERRQ(info);
    269:     info = qp->DiagAxpy->Axpy(1.0,qp->TSwork); CHKERRQ(info);
    270:     info = qp->TSwork->PointwiseMultiply(qp->TSwork,qp->R5); CHKERRQ(info);
    271:     info = qp->RHS->Axpy(-1.0,qp->TSwork); CHKERRQ(info);

    273:     info = qp->RHS2->Axpy(1.0,qp->RHS); CHKERRQ(info);

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

    281:     info = qp->H->AddDiagonal(qp->DiagAxpy); CHKERRQ(info);

    283:     info = TaoPreLinearSolve(tao,qp->H);CHKERRQ(info);
    284:     info = TaoLinearSolve(tao,qp->H,qp->RHS,qp->DXY,&kspsuccess);CHKERRQ(info);

    286:     info = qp->DiagAxpy->Negate(); CHKERRQ(info);
    287:     info = qp->H->AddDiagonal(qp->DiagAxpy); CHKERRQ(info);
    288:     info = qp->DiagAxpy->Negate(); CHKERRQ(info);
    289:     info = QPComputeStepDirection(qp); CHKERRQ(info);
    290:     info = QPStepLength(qp);  CHKERRQ(info);

    292:     /* Calculate New Residual R1 in Work vector */
    293:     info = qp->H->Multiply(qp->DXY,qp->RHS2); CHKERRQ(info);
    294:     info = qp->RHS2->Axpy(1.0,qp->DS); CHKERRQ(info);
    295:     info = qp->RHS2->Axpy(-1.0,qp->DZ); CHKERRQ(info);
    296:     info = qp->RHS2->Aypx(qp->dsteplength,qp->R12); CHKERRQ(info);

    298:     qp->RHS2->Norm2(&qp->dinfeas); CHKERRQ(info);
    299:     qp->DG->Dot(qp->DZ, gap); CHKERRQ(info);
    300:     qp->DT->Dot(qp->DS, gap+1); CHKERRQ(info);
    301:
    302:     qp->rnorm=(qp->dinfeas+qp->psteplength*qp->pinfeas)/(qp->m+qp->n);
    303:     pstep = qp->psteplength; dstep = qp->dsteplength;
    304:     step = TaoMin(qp->psteplength,qp->dsteplength);
    305:     sigmamu= ( pstep*pstep*(gap[0]+gap[1]) +
    306:                (1 - pstep + pstep*sigma)*qp->gap  )/qp->m;

    308:     if (qp->predcorr && step < 0.9){
    309:       if (sigmamu < qp->mu){
    310:         sigmamu=sigmamu/qp->mu;
    311:         sigmamu=sigmamu*sigmamu*sigmamu;
    312:       } else {sigmamu = 1.0;}
    313:       sigmamu = sigmamu*qp->mu;
    314:
    315:       /* Compute Corrector Step */
    316:       info = qp->DZ->PointwiseMultiply(qp->DG,qp->DZ); CHKERRQ(info);
    317:       info = qp->DZ->Negate(); CHKERRQ(info);
    318:       info = qp->DZ->AddConstant(sigmamu); CHKERRQ(info);
    319:       info = qp->DZ->PointwiseDivide(qp->DZ,qp->G); CHKERRQ(info);

    321:       info = qp->DS->PointwiseMultiply(qp->DS,qp->DT); CHKERRQ(info);
    322:       info = qp->DS->Negate(); CHKERRQ(info);
    323:       info = qp->DS->AddConstant(sigmamu); CHKERRQ(info);
    324:       info = qp->DS->PointwiseDivide(qp->DS,qp->T); CHKERRQ(info);
    325:
    326:       info = qp->RHS2->Waxpby(1.0,qp->DZ,-1.0,qp->DS); CHKERRQ(info);
    327:       info = qp->RHS2->Axpy(1.0,qp->RHS); CHKERRQ(info);

    329:       /* Approximately solve the linear system */
    330:
    331:       info = qp->H->AddDiagonal(qp->DiagAxpy); CHKERRQ(info);
    332:
    333:       info = TaoLinearSolve(tao,qp->H,qp->RHS2,qp->DXY,&kspsuccess);CHKERRQ(info);
    334:
    335:       info = qp->H->SetDiagonal(qp->HDiag); CHKERRQ(info);
    336:       info = QPComputeStepDirection(qp); CHKERRQ(info);
    337:       info = QPStepLength(qp); CHKERRQ(info);

    339:     }  /* End Corrector step */

    342:     /* Take the step */
    343:     pstep = qp->psteplength; dstep = qp->dsteplength;

    345:     info = qp->Z->Axpy(dstep,qp->DZ); CHKERRQ(info);
    346:     info = qp->S->Axpy(dstep,qp->DS); CHKERRQ(info);
    347:     info = qp->XY->Axpy(dstep,qp->DXY); CHKERRQ(info);
    348:     info = qp->G->Axpy(pstep,qp->DG); CHKERRQ(info);
    349:     info = qp->T->Axpy(pstep,qp->DT); CHKERRQ(info);
    350:
    351:     /* Compute Residuals */
    352:     info = QPIPComputeResidual(qp); CHKERRQ(info);

    354:     /* Evaluate quadratic function */
    355:     info = qp->H->Multiply(qp->XY,qp->Work); CHKERRQ(info);

    357:     info = qp->XY->Dot(qp->Work,&d1); CHKERRQ(info);
    358:     info = qp->XY->Dot(qp->C0,&d2); CHKERRQ(info);
    359:     info = qp->G->Dot(qp->Z,gap); CHKERRQ(info);
    360:     info = qp->T->Dot(qp->S,gap+1); CHKERRQ(info);

    362:     qp->pobj=d1/2.0 + d2+qp->c;
    363:     /* Compute the duality gap */
    364:     qp->gap = (gap[0]+gap[1]);
    365:     qp->dobj = qp->pobj - qp->gap;
    366:     if (qp->m>0) qp->mu=qp->gap/(qp->m);
    367:     qp->rgap=qp->gap/( fabs(qp->dobj) + fabs(qp->pobj) + 1.0 );
    368:   }  /* END MAIN LOOP  */

    370:   TaoFunctionReturn(0);
    371: }

    375: static int QPComputeStepDirection(TAO_BQPIP *qp)
    376: {
    377:   int info;

    379:   TaoFunctionBegin;

    381:   /* Calculate DG */
    382:   info = qp->DG->Waxpby(1.0,qp->DXY,1.0,qp->R3);CHKERRQ(info);

    384:   /* Calculate DT */
    385:   info = qp->DT->Waxpby(-1.0,qp->DXY,-1.0,qp->R5);CHKERRQ(info);

    387:   /* Calculate DZ */
    388:
    389:   info = qp->DZ->Axpy(-1.0,qp->Z);CHKERRQ(info);
    390:   info = qp->GZwork->PointwiseDivide(qp->DG,qp->G); CHKERRQ(info);
    391:   info = qp->GZwork->PointwiseMultiply(qp->GZwork,qp->Z); CHKERRQ(info);
    392:   info = qp->DZ->Axpy(-1.0,qp->GZwork);CHKERRQ(info);

    394:   /* Calculate DS */

    396:   info = qp->DS->Axpy(-1.0,qp->S);CHKERRQ(info);
    397:   info = qp->TSwork->PointwiseDivide(qp->DT,qp->T); CHKERRQ(info);
    398:   info = qp->TSwork->PointwiseMultiply(qp->TSwork,qp->S); CHKERRQ(info);
    399:   info = qp->DS->Axpy(-1.0,qp->TSwork);CHKERRQ(info);

    401:   TaoFunctionReturn(0);
    402: }

    406: static int QPIPComputeResidual(TAO_BQPIP *qp)
    407: {
    408:   int info;
    409:   double gap1,gap2,dtmp = 1.0 - qp->psteplength;

    411:   TaoFunctionBegin;

    413:   /* Compute R3 and R5 */

    415:   if (1==1){

    417:     info = qp->R3->Scale(dtmp);
    418:     info = qp->R5->Scale(dtmp);
    419:     qp->pinfeas=dtmp*qp->pinfeas;

    421:   } else {

    423:     info = qp->R3->Waxpby(1.0,qp->XY,-1.0,qp->XL);CHKERRQ(info);
    424:     info = qp->R3->Axpy(-1.0,qp->G);CHKERRQ(info);

    426:     info = qp->R5->Waxpby(1.0,qp->XY,-1.0,qp->XU);CHKERRQ(info);
    427:     info = qp->R5->Axpy(1.0,qp->T);CHKERRQ(info);

    429:     info = qp->R3->NormInfinity(&gap1);CHKERRQ(info);
    430:     info = qp->R5->NormInfinity(&gap2);CHKERRQ(info);

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

    434:   }

    436:   qp->R12->Waxpby(1.0,qp->S,-1.0,qp->Z);CHKERRQ(info);

    438:   info = qp->H->Multiply(qp->XY,qp->RHS); CHKERRQ(info);
    439:   info = qp->RHS->Negate();CHKERRQ(info);
    440:   info = qp->RHS->Axpy(-1.0,qp->C0);CHKERRQ(info);
    441:   info = qp->R12->Axpy(-1.0,qp->RHS);CHKERRQ(info);

    443:   info = qp->R12->Norm1(&qp->dinfeas); CHKERRQ(info);
    444:   qp->rnorm=(qp->dinfeas+qp->pinfeas)/(qp->m+qp->n);
    445:
    446:   TaoFunctionReturn(0);
    447: }

    451: static int QPStepLength(TAO_BQPIP *qp)
    452: {
    453:   double tstep1,tstep2,tstep3,tstep4,tstep;
    454:   int info;

    456:   TaoFunctionBegin;
    457:   /* Compute stepsize to the boundary */

    459:   info = qp->G->StepMax(qp->DG,&tstep1); CHKERRQ(info);
    460:   info = qp->T->StepMax(qp->DT,&tstep2); CHKERRQ(info);
    461:   info = qp->S->StepMax(qp->DS,&tstep3); CHKERRQ(info);
    462:   info = qp->Z->StepMax(qp->DZ,&tstep4); CHKERRQ(info);

    464:   tstep = TaoMin(tstep1,tstep2);
    465:   qp->psteplength = TaoMin(0.95*tstep,1.0);

    467:   tstep = TaoMin(tstep3,tstep4);
    468:   qp->dsteplength = TaoMin(0.95*tstep,1.0);

    470:   qp->psteplength = TaoMin(qp->psteplength,qp->dsteplength);
    471:   qp->dsteplength = qp->psteplength;

    473:   TaoFunctionReturn(0);
    474: }

    479: int TaoGetDualVariables_BQPIP(TAO_SOLVER tao,TaoVec* DXL, TaoVec* DXU, void *solver)
    480: {
    481:   TAO_BQPIP *qp = (TAO_BQPIP*)solver;
    482:   int       info;

    484:   TaoFunctionBegin;
    485:   info = DXL->CopyFrom(qp->Z); CHKERRQ(info);
    486:   info = DXU->CopyFrom(qp->S); CHKERRQ(info);
    487:   info = DXU->Negate(); CHKERRQ(info);
    488:   TaoFunctionReturn(0);
    489: }

    494: int TAOComputeNormFromCentralPath_BQPIP(TAO_SOLVER tao, double *norm)
    495: {
    496:   TAO_BQPIP *qp;
    497:   int       info;
    498:   double    gap[2],mu[2], nmu;
    499:
    500:   TaoFunctionBegin;
    501:   info = TaoGetSolverContext(tao,"tao_bqpip",(void**)&qp); CHKERRQ(info);
    502:   info = qp->GZwork->PointwiseMultiply(qp->G,qp->Z); CHKERRQ(info);
    503:   info = qp->TSwork->PointwiseMultiply(qp->T,qp->S); CHKERRQ(info);
    504:   info = qp->TSwork->Norm1(&mu[0]); CHKERRQ(info);
    505:   info = qp->GZwork->Norm1(&mu[1]); CHKERRQ(info);

    507:   nmu=-(mu[0]+mu[1])/qp->m;

    509:   qp->GZwork->AddConstant(nmu); CHKERRQ(info);
    510:   qp->TSwork->AddConstant(nmu); CHKERRQ(info);

    512:   qp->GZwork->Norm2squared(&gap[0]); CHKERRQ(info);
    513:   qp->TSwork->Norm2squared(&gap[1]); CHKERRQ(info);

    515:   qp->pathnorm=sqrt( (gap[0]+gap[1]) );
    516:   *norm=qp->pathnorm;

    518:   TaoFunctionReturn(0);
    519: }

    524: static int TaoSetOptions_BQPIP(TAO_SOLVER tao, void *solver){

    526:   TAO_BQPIP *qp = (TAO_BQPIP*)solver;
    527:   int       info;
    528:   TaoTruth  flg;

    530:   TaoFunctionBegin;
    531:   info = TaoOptionsHead("Interior point method for bound constrained quadratic optimization");CHKERRQ(info);
    532:   info = TaoOptionInt("-predcorr","Use a predictor-corrector method","",qp->predcorr,&qp->predcorr,&flg);
    533:   CHKERRQ(info);
    534:   info = TaoOptionsTail();CHKERRQ(info);

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

    538:   TaoFunctionReturn(0);
    539: }

    544: static int TaoView_BQPIP(TAO_SOLVER tao,void *solver){
    545:   TaoFunctionBegin;
    546:   TaoFunctionReturn(0);
    547: }

    550: /* --------------------------------------------------------- */
    554: int TaoCreate_BQPIP(TAO_SOLVER tao)
    555: {
    556:   TAO_BQPIP *qp;
    557:   int       info;

    559:   TaoFunctionBegin;

    561:   info = TaoNew(TAO_BQPIP,&qp); CHKERRQ(info);
    562:   info = PetscLogObjectMemory(tao,sizeof(TAO_BQPIP)); CHKERRQ(info);

    564:   info=TaoSetTaoSolveRoutine(tao,TaoSolve_BQPIP,(void*)qp); CHKERRQ(info);
    565:   info=TaoSetTaoSetUpDownRoutines(tao,TaoSetUp_BQPIP,TaoSetDown_BQPIP); CHKERRQ(info);
    566:   info=TaoSetTaoOptionsRoutine(tao,TaoSetOptions_BQPIP); CHKERRQ(info);
    567:   info=TaoSetTaoViewRoutine(tao,TaoView_BQPIP); CHKERRQ(info);
    568:   info=TaoSetTaoDualVariablesRoutine(tao,TaoGetDualVariables_BQPIP); CHKERRQ(info);

    570:   info = TaoSetMaximumIterates(tao,100); CHKERRQ(info);
    571:   info = TaoSetMaximumFunctionEvaluations(tao,500); CHKERRQ(info);
    572:   info = TaoSetTolerances(tao,1e-12,1e-12,1e-12,0); CHKERRQ(info);
    573:   /*
    574:   tao->defaultmonitor     = TaoDefaultMonitor_BQPIP;
    575:   */
    576:   /* Initialize pointers and variables */
    577:   qp->n              = 0;
    578:   qp->m              = 0;
    579:   qp->ksp_tol       = 0.1;
    580:   qp->dobj           = 0.0;
    581:   qp->pobj           = 1.0;
    582:   qp->gap            = 10.0;
    583:   qp->rgap           = 1.0;
    584:   qp->mu             = 1.0;
    585:   qp->sigma          = 1.0;
    586:   qp->dinfeas        = 1.0;
    587:   qp->predcorr       = 1;
    588:   qp->psteplength    = 0.0;
    589:   qp->dsteplength    = 0.0;

    591:   TaoFunctionReturn(0);
    592: }