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       info;
 11:   TaoInt    n;

 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);

 60:   TaoFunctionReturn(0);
 61: }

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

 77:   TaoFunctionBegin;

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

 83:   info = gzwork->SetToConstant(p01);CHKERRQ(info);
 84:   info = tswork->SetToConstant(p01);CHKERRQ(info);

 86:   info = g->PointwiseMaximum(g,gzwork); CHKERRQ(info);
 87:   info = t->PointwiseMaximum(t,tswork); CHKERRQ(info);

 89:   /* Initialize Dual Variable Vectors */

 91:   info = z->CopyFrom(g); CHKERRQ(info);
 92:   info = z->Reciprocal(); CHKERRQ(info);

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

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

132:   TaoFunctionReturn(0);
133: }


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

143:   /* Free allocated memory in GPCG structure */
144:   TaoFunctionBegin;

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

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

178:   TaoFunctionReturn(0);
179: }

183: static int TaoSolve_ICP(TAO_SOLVER tao, void *solver)
184: {
185:   TAO_ICP   *qp = (TAO_ICP*)solver;
186:   TaoVec    *x = qp->x, *xl=qp->xl, *xu=qp->xu, *f=qp->f;
187:   TaoVec    *g = qp->g, *z=qp->z, *s=qp->s, *t=qp->t;
188:   TaoVec    *dg=qp->dg, *dz=qp->dz, *ds=qp->ds, *dt=qp->dt;
189:   TaoVec    *gzwork=qp->gzwork, *tswork=qp->tswork;
190:   TaoVec    *rhs=qp->rhs, *rhs2=qp->rhs2;
191:   TaoVec    *diagaxpy=qp->diagaxpy, *dx=qp->dx;
192:   TaoVec    *r3=qp->r3,*r5=qp->r5;
193:   TaoVec    *JDiag=qp->JDiag;
194:   TaoMat    *J;
195:   int       info;
196:   TaoInt    iter=0;
197:   TaoInt    n;
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:     /*
280:     info = qpksp->SetTolerances(ksptol,1e-30,1e30,2*n); CHKERRQ(info);
281:     */

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

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

290:     info = QPComputeStepDirection(qp); CHKERRQ(info);
291:     info = QPStepLength(qp);  CHKERRQ(info);

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

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

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

338:     }  /* End Corrector step */


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

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

352:   }  /* END MAIN LOOP  */

354:   TaoFunctionReturn(0);
355: }

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

369:   TaoFunctionBegin;

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

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

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

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

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

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

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

426:   TaoFunctionBegin;

428:   /* Compute R3 and R5 */

430:   if (0==1){

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

436:   } else {

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

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

444:     info = r3->NormInfinity(&gap1);CHKERRQ(info);
445:     info = r5->NormInfinity(&gap2);CHKERRQ(info);

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

449:   }

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

453:   info = rhs->CopyFrom(f);CHKERRQ(info);
454:   info = rhs->Negate();CHKERRQ(info);

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

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

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

475:   TaoFunctionBegin;
476:   /* Compute stepsize to the boundary */

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

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

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

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

493:   TaoFunctionReturn(0);
494: }


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

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

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

519:   info = gzwork->AddConstant(nmu); CHKERRQ(info);
520:   info = tswork->AddConstant(nmu); CHKERRQ(info);

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

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

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


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

538:   TAO_ICP *qp = (TAO_ICP*)solver;
539:   int       info;
540:   TaoTruth  flg;

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

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

550:   TaoFunctionReturn(0);
551: }


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


562: /* --------------------------------------------------------- */
566: int TaoCreate_ICP(TAO_SOLVER tao)
567: {
568:   TAO_ICP *qp;
569:   int       info;

571:   TaoFunctionBegin;

573:   info = TaoNew(TAO_ICP,&qp); CHKERRQ(info);
574:   info = PetscLogObjectMemory(tao,sizeof(TAO_ICP)); CHKERRQ(info);

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

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

602:   TaoFunctionReturn(0);
603: }