xref: /petsc/src/tao/constrained/impls/ipm/ipm.c (revision 5b6bfdb9644f185dbf5e5a09b808ec241507e1e7) 
1 #include <petsctaolinesearch.h>
2 #include <../src/tao/constrained/impls/ipm/ipm.h> /*I "ipm.h" I*/
3 
4 /*
5    x,d in R^n
6    f in R
7    nb = mi + nlb+nub
8    s in R^nb is slack vector CI(x) / x-XL / -x+XU
9    bin in R^mi (tao->constraints_inequality)
10    beq in R^me (tao->constraints_equality)
11    lamdai in R^nb (ipmP->lamdai)
12    lamdae in R^me (ipmP->lamdae)
13    Jeq in R^(me x n) (tao->jacobian_equality)
14    Jin in R^(mi x n) (tao->jacobian_inequality)
15    Ai in  R^(nb x n) (ipmP->Ai)
16    H in R^(n x n) (tao->hessian)
17    min f=(1/2)*x'*H*x + d'*x
18    s.t.  CE(x) == 0
19          CI(x) >= 0
20          x >= tao->XL
21          -x >= -tao->XU
22 */
23 
24 static PetscErrorCode IPMComputeKKT(Tao tao);
25 static PetscErrorCode IPMPushInitialPoint(Tao tao);
26 static PetscErrorCode IPMEvaluate(Tao tao);
27 static PetscErrorCode IPMUpdateK(Tao tao);
28 static PetscErrorCode IPMUpdateAi(Tao tao);
29 static PetscErrorCode IPMGatherRHS(Tao tao,Vec,Vec,Vec,Vec,Vec);
30 static PetscErrorCode IPMScatterStep(Tao tao,Vec,Vec,Vec,Vec,Vec);
31 static PetscErrorCode IPMInitializeBounds(Tao tao);
32 
33 static PetscErrorCode TaoSolve_IPM(Tao tao)
34 {
35   PetscErrorCode     ierr;
36   TAO_IPM            *ipmP = (TAO_IPM*)tao->data;
37   PetscInt           its,i;
38   PetscScalar        stepsize=1.0;
39   PetscScalar        step_s,step_l,alpha,tau,sigma,phi_target;
40 
41   PetscFunctionBegin;
42   /* Push initial point away from bounds */
43   ierr = IPMInitializeBounds(tao);CHKERRQ(ierr);
44   ierr = IPMPushInitialPoint(tao);CHKERRQ(ierr);
45   ierr = VecCopy(tao->solution,ipmP->rhs_x);CHKERRQ(ierr);
46   ierr = IPMEvaluate(tao);CHKERRQ(ierr);
47   ierr = IPMComputeKKT(tao);CHKERRQ(ierr);
48 
49   tao->reason = TAO_CONTINUE_ITERATING;
50   ierr = TaoLogConvergenceHistory(tao,ipmP->kkt_f,ipmP->phi,0.0,tao->ksp_its);CHKERRQ(ierr);
51   ierr = TaoMonitor(tao,tao->niter,ipmP->kkt_f,ipmP->phi,0.0,1.0);CHKERRQ(ierr);
52   ierr = (*tao->ops->convergencetest)(tao,tao->cnvP);CHKERRQ(ierr);
53 
54   while (tao->reason == TAO_CONTINUE_ITERATING) {
55     tao->ksp_its=0;
56     ierr = IPMUpdateK(tao);CHKERRQ(ierr);
57     /*
58        rhs.x    = -rd
59        rhs.lame = -rpe
60        rhs.lami = -rpi
61        rhs.com  = -com
62     */
63 
64     ierr = VecCopy(ipmP->rd,ipmP->rhs_x);CHKERRQ(ierr);
65     if (ipmP->me > 0) {
66       ierr = VecCopy(ipmP->rpe,ipmP->rhs_lamdae);CHKERRQ(ierr);
67     }
68     if (ipmP->nb > 0) {
69       ierr = VecCopy(ipmP->rpi,ipmP->rhs_lamdai);CHKERRQ(ierr);
70       ierr = VecCopy(ipmP->complementarity,ipmP->rhs_s);CHKERRQ(ierr);
71     }
72     ierr = IPMGatherRHS(tao,ipmP->bigrhs,ipmP->rhs_x,ipmP->rhs_lamdae,ipmP->rhs_lamdai,ipmP->rhs_s);CHKERRQ(ierr);
73     ierr = VecScale(ipmP->bigrhs,-1.0);CHKERRQ(ierr);
74 
75     /* solve K * step = rhs */
76     ierr = KSPSetOperators(tao->ksp,ipmP->K,ipmP->K);CHKERRQ(ierr);
77     ierr = KSPSolve(tao->ksp,ipmP->bigrhs,ipmP->bigstep);CHKERRQ(ierr);
78 
79     ierr = IPMScatterStep(tao,ipmP->bigstep,tao->stepdirection,ipmP->ds,ipmP->dlamdae,ipmP->dlamdai);CHKERRQ(ierr);
80     ierr = KSPGetIterationNumber(tao->ksp,&its);CHKERRQ(ierr);
81     tao->ksp_its += its;
82     tao->ksp_tot_its+=its;
83      /* Find distance along step direction to closest bound */
84     if (ipmP->nb > 0) {
85       ierr = VecStepBoundInfo(ipmP->s,ipmP->ds,ipmP->Zero_nb,ipmP->Inf_nb,&step_s,NULL,NULL);CHKERRQ(ierr);
86       ierr = VecStepBoundInfo(ipmP->lamdai,ipmP->dlamdai,ipmP->Zero_nb,ipmP->Inf_nb,&step_l,NULL,NULL);CHKERRQ(ierr);
87       alpha = PetscMin(step_s,step_l);
88       alpha = PetscMin(alpha,1.0);
89       ipmP->alpha1 = alpha;
90     } else {
91       ipmP->alpha1 = alpha = 1.0;
92     }
93 
94     /* x_aff = x + alpha*d */
95     ierr = VecCopy(tao->solution,ipmP->save_x);CHKERRQ(ierr);
96     if (ipmP->me > 0) {
97       ierr = VecCopy(ipmP->lamdae,ipmP->save_lamdae);CHKERRQ(ierr);
98     }
99     if (ipmP->nb > 0) {
100       ierr = VecCopy(ipmP->lamdai,ipmP->save_lamdai);CHKERRQ(ierr);
101       ierr = VecCopy(ipmP->s,ipmP->save_s);CHKERRQ(ierr);
102     }
103 
104     ierr = VecAXPY(tao->solution,alpha,tao->stepdirection);CHKERRQ(ierr);
105     if (ipmP->me > 0) {
106       ierr = VecAXPY(ipmP->lamdae,alpha,ipmP->dlamdae);CHKERRQ(ierr);
107     }
108     if (ipmP->nb > 0) {
109       ierr = VecAXPY(ipmP->lamdai,alpha,ipmP->dlamdai);CHKERRQ(ierr);
110       ierr = VecAXPY(ipmP->s,alpha,ipmP->ds);CHKERRQ(ierr);
111     }
112 
113     /* Recompute kkt to find centering parameter sigma = (new_mu/old_mu)^3 */
114     if (ipmP->mu == 0.0) {
115       sigma = 0.0;
116     } else {
117       sigma = 1.0/ipmP->mu;
118     }
119     ierr = IPMComputeKKT(tao);CHKERRQ(ierr);
120     sigma *= ipmP->mu;
121     sigma*=sigma*sigma;
122 
123     /* revert kkt info */
124     ierr = VecCopy(ipmP->save_x,tao->solution);CHKERRQ(ierr);
125     if (ipmP->me > 0) {
126       ierr = VecCopy(ipmP->save_lamdae,ipmP->lamdae);CHKERRQ(ierr);
127     }
128     if (ipmP->nb > 0) {
129       ierr = VecCopy(ipmP->save_lamdai,ipmP->lamdai);CHKERRQ(ierr);
130       ierr = VecCopy(ipmP->save_s,ipmP->s);CHKERRQ(ierr);
131     }
132     ierr = IPMComputeKKT(tao);CHKERRQ(ierr);
133 
134     /* update rhs with new complementarity vector */
135     if (ipmP->nb > 0) {
136       ierr = VecCopy(ipmP->complementarity,ipmP->rhs_s);CHKERRQ(ierr);
137       ierr = VecScale(ipmP->rhs_s,-1.0);CHKERRQ(ierr);
138       ierr = VecShift(ipmP->rhs_s,sigma*ipmP->mu);CHKERRQ(ierr);
139     }
140     ierr = IPMGatherRHS(tao,ipmP->bigrhs,NULL,NULL,NULL,ipmP->rhs_s);CHKERRQ(ierr);
141 
142     /* solve K * step = rhs */
143     ierr = KSPSetOperators(tao->ksp,ipmP->K,ipmP->K);CHKERRQ(ierr);
144     ierr = KSPSolve(tao->ksp,ipmP->bigrhs,ipmP->bigstep);CHKERRQ(ierr);
145 
146     ierr = IPMScatterStep(tao,ipmP->bigstep,tao->stepdirection,ipmP->ds,ipmP->dlamdae,ipmP->dlamdai);CHKERRQ(ierr);
147     ierr = KSPGetIterationNumber(tao->ksp,&its);CHKERRQ(ierr);
148     tao->ksp_its += its;
149     tao->ksp_tot_its+=its;
150     if (ipmP->nb > 0) {
151       /* Get max step size and apply frac-to-boundary */
152       tau = PetscMax(ipmP->taumin,1.0-ipmP->mu);
153       tau = PetscMin(tau,1.0);
154       if (tau != 1.0) {
155         ierr = VecScale(ipmP->s,tau);CHKERRQ(ierr);
156         ierr = VecScale(ipmP->lamdai,tau);CHKERRQ(ierr);
157       }
158       ierr = VecStepBoundInfo(ipmP->s,ipmP->ds,ipmP->Zero_nb,ipmP->Inf_nb,&step_s,NULL,NULL);CHKERRQ(ierr);
159       ierr = VecStepBoundInfo(ipmP->lamdai,ipmP->dlamdai,ipmP->Zero_nb,ipmP->Inf_nb,&step_l,NULL,NULL);CHKERRQ(ierr);
160       if (tau != 1.0) {
161         ierr = VecCopy(ipmP->save_s,ipmP->s);CHKERRQ(ierr);
162         ierr = VecCopy(ipmP->save_lamdai,ipmP->lamdai);CHKERRQ(ierr);
163       }
164       alpha = PetscMin(step_s,step_l);
165       alpha = PetscMin(alpha,1.0);
166     } else {
167       alpha = 1.0;
168     }
169     ipmP->alpha2 = alpha;
170     /* TODO make phi_target meaningful */
171     phi_target = ipmP->dec * ipmP->phi;
172     for (i=0; i<11;i++) {
173       ierr = VecAXPY(tao->solution,alpha,tao->stepdirection);CHKERRQ(ierr);
174       if (ipmP->nb > 0) {
175         ierr = VecAXPY(ipmP->s,alpha,ipmP->ds);CHKERRQ(ierr);
176         ierr = VecAXPY(ipmP->lamdai,alpha,ipmP->dlamdai);CHKERRQ(ierr);
177       }
178       if (ipmP->me > 0) {
179         ierr = VecAXPY(ipmP->lamdae,alpha,ipmP->dlamdae);CHKERRQ(ierr);
180       }
181 
182       /* update dual variables */
183       if (ipmP->me > 0) {
184         ierr = VecCopy(ipmP->lamdae,tao->DE);CHKERRQ(ierr);
185       }
186 
187       ierr = IPMEvaluate(tao);CHKERRQ(ierr);
188       ierr = IPMComputeKKT(tao);CHKERRQ(ierr);
189       if (ipmP->phi <= phi_target) break;
190       alpha /= 2.0;
191     }
192 
193     ierr = TaoLogConvergenceHistory(tao,ipmP->kkt_f,ipmP->phi,0.0,tao->ksp_its);CHKERRQ(ierr);
194     ierr = TaoMonitor(tao,tao->niter,ipmP->kkt_f,ipmP->phi,0.0,stepsize);CHKERRQ(ierr);
195     ierr = (*tao->ops->convergencetest)(tao,tao->cnvP);CHKERRQ(ierr);
196     tao->niter++;
197   }
198   PetscFunctionReturn(0);
199 }
200 
201 static PetscErrorCode TaoSetup_IPM(Tao tao)
202 {
203   TAO_IPM        *ipmP = (TAO_IPM*)tao->data;
204   PetscErrorCode ierr;
205 
206   PetscFunctionBegin;
207   ipmP->nb = ipmP->mi = ipmP->me = 0;
208   ipmP->K=0;
209   ierr = VecGetSize(tao->solution,&ipmP->n);CHKERRQ(ierr);
210   if (!tao->gradient) {
211     ierr = VecDuplicate(tao->solution, &tao->gradient);CHKERRQ(ierr);
212     ierr = VecDuplicate(tao->solution, &tao->stepdirection);CHKERRQ(ierr);
213     ierr = VecDuplicate(tao->solution, &ipmP->rd);CHKERRQ(ierr);
214     ierr = VecDuplicate(tao->solution, &ipmP->rhs_x);CHKERRQ(ierr);
215     ierr = VecDuplicate(tao->solution, &ipmP->work);CHKERRQ(ierr);
216     ierr = VecDuplicate(tao->solution, &ipmP->save_x);CHKERRQ(ierr);
217   }
218   if (tao->constraints_equality) {
219     ierr = VecGetSize(tao->constraints_equality,&ipmP->me);CHKERRQ(ierr);
220     ierr = VecDuplicate(tao->constraints_equality,&ipmP->lamdae);CHKERRQ(ierr);
221     ierr = VecDuplicate(tao->constraints_equality,&ipmP->dlamdae);CHKERRQ(ierr);
222     ierr = VecDuplicate(tao->constraints_equality,&ipmP->rhs_lamdae);CHKERRQ(ierr);
223     ierr = VecDuplicate(tao->constraints_equality,&ipmP->save_lamdae);CHKERRQ(ierr);
224     ierr = VecDuplicate(tao->constraints_equality,&ipmP->rpe);CHKERRQ(ierr);
225     ierr = VecDuplicate(tao->constraints_equality,&tao->DE);CHKERRQ(ierr);
226   }
227   if (tao->constraints_inequality) {
228     ierr = VecDuplicate(tao->constraints_inequality,&tao->DI);CHKERRQ(ierr);
229   }
230   PetscFunctionReturn(0);
231 }
232 
233 static PetscErrorCode IPMInitializeBounds(Tao tao)
234 {
235   TAO_IPM        *ipmP = (TAO_IPM*)tao->data;
236   Vec            xtmp;
237   PetscInt       xstart,xend;
238   PetscInt       ucstart,ucend; /* user ci */
239   PetscInt       ucestart,uceend; /* user ce */
240   PetscInt       sstart,send;
241   PetscInt       bigsize;
242   PetscInt       i,counter,nloc;
243   PetscInt       *cind,*xind,*ucind,*uceind,*stepind;
244   VecType        vtype;
245   const PetscInt *xli,*xui;
246   PetscInt       xl_offset,xu_offset;
247   IS             bigxl,bigxu,isuc,isc,isx,sis,is1;
248   PetscErrorCode ierr;
249   MPI_Comm       comm;
250 
251   PetscFunctionBegin;
252   cind=xind=ucind=uceind=stepind=0;
253   ipmP->mi=0;
254   ipmP->nxlb=0;
255   ipmP->nxub=0;
256   ipmP->nb=0;
257   ipmP->nslack=0;
258 
259   ierr = VecDuplicate(tao->solution,&xtmp);CHKERRQ(ierr);
260   if (!tao->XL && !tao->XU && tao->ops->computebounds) {
261     ierr = TaoComputeVariableBounds(tao);CHKERRQ(ierr);
262   }
263   if (tao->XL) {
264     ierr = VecSet(xtmp,PETSC_NINFINITY);CHKERRQ(ierr);
265     ierr = VecWhichGreaterThan(tao->XL,xtmp,&ipmP->isxl);CHKERRQ(ierr);
266     ierr = ISGetSize(ipmP->isxl,&ipmP->nxlb);CHKERRQ(ierr);
267   } else {
268     ipmP->nxlb=0;
269   }
270   if (tao->XU) {
271     ierr = VecSet(xtmp,PETSC_INFINITY);CHKERRQ(ierr);
272     ierr = VecWhichLessThan(tao->XU,xtmp,&ipmP->isxu);CHKERRQ(ierr);
273     ierr = ISGetSize(ipmP->isxu,&ipmP->nxub);CHKERRQ(ierr);
274   } else {
275     ipmP->nxub=0;
276   }
277   ierr = VecDestroy(&xtmp);CHKERRQ(ierr);
278   if (tao->constraints_inequality) {
279     ierr = VecGetSize(tao->constraints_inequality,&ipmP->mi);CHKERRQ(ierr);
280   } else {
281     ipmP->mi = 0;
282   }
283   ipmP->nb = ipmP->nxlb + ipmP->nxub + ipmP->mi;
284 
285   ierr = PetscObjectGetComm((PetscObject)tao->solution,&comm);CHKERRQ(ierr);
286 
287   bigsize = ipmP->n+2*ipmP->nb+ipmP->me;
288   ierr = PetscMalloc1(bigsize,&stepind);CHKERRQ(ierr);
289   ierr = PetscMalloc1(ipmP->n,&xind);CHKERRQ(ierr);
290   ierr = PetscMalloc1(ipmP->me,&uceind);CHKERRQ(ierr);
291   ierr = VecGetOwnershipRange(tao->solution,&xstart,&xend);CHKERRQ(ierr);
292 
293   if (ipmP->nb > 0) {
294     ierr = VecCreate(comm,&ipmP->s);CHKERRQ(ierr);
295     ierr = VecSetSizes(ipmP->s,PETSC_DECIDE,ipmP->nb);CHKERRQ(ierr);
296     ierr = VecSetFromOptions(ipmP->s);CHKERRQ(ierr);
297     ierr = VecDuplicate(ipmP->s,&ipmP->ds);CHKERRQ(ierr);
298     ierr = VecDuplicate(ipmP->s,&ipmP->rhs_s);CHKERRQ(ierr);
299     ierr = VecDuplicate(ipmP->s,&ipmP->complementarity);CHKERRQ(ierr);
300     ierr = VecDuplicate(ipmP->s,&ipmP->ci);CHKERRQ(ierr);
301 
302     ierr = VecDuplicate(ipmP->s,&ipmP->lamdai);CHKERRQ(ierr);
303     ierr = VecDuplicate(ipmP->s,&ipmP->dlamdai);CHKERRQ(ierr);
304     ierr = VecDuplicate(ipmP->s,&ipmP->rhs_lamdai);CHKERRQ(ierr);
305     ierr = VecDuplicate(ipmP->s,&ipmP->save_lamdai);CHKERRQ(ierr);
306 
307     ierr = VecDuplicate(ipmP->s,&ipmP->save_s);CHKERRQ(ierr);
308     ierr = VecDuplicate(ipmP->s,&ipmP->rpi);CHKERRQ(ierr);
309     ierr = VecDuplicate(ipmP->s,&ipmP->Zero_nb);CHKERRQ(ierr);
310     ierr = VecSet(ipmP->Zero_nb,0.0);CHKERRQ(ierr);
311     ierr = VecDuplicate(ipmP->s,&ipmP->One_nb);CHKERRQ(ierr);
312     ierr = VecSet(ipmP->One_nb,1.0);CHKERRQ(ierr);
313     ierr = VecDuplicate(ipmP->s,&ipmP->Inf_nb);CHKERRQ(ierr);
314     ierr = VecSet(ipmP->Inf_nb,PETSC_INFINITY);CHKERRQ(ierr);
315 
316     ierr = PetscMalloc1(ipmP->nb,&cind);CHKERRQ(ierr);
317     ierr = PetscMalloc1(ipmP->mi,&ucind);CHKERRQ(ierr);
318     ierr = VecGetOwnershipRange(ipmP->s,&sstart,&send);CHKERRQ(ierr);
319 
320     if (ipmP->mi > 0) {
321       ierr = VecGetOwnershipRange(tao->constraints_inequality,&ucstart,&ucend);CHKERRQ(ierr);
322       counter=0;
323       for (i=ucstart;i<ucend;i++) {
324         cind[counter++] = i;
325       }
326       ierr = ISCreateGeneral(comm,counter,cind,PETSC_COPY_VALUES,&isuc);CHKERRQ(ierr);
327       ierr = ISCreateGeneral(comm,counter,cind,PETSC_COPY_VALUES,&isc);CHKERRQ(ierr);
328       ierr = VecScatterCreate(tao->constraints_inequality,isuc,ipmP->ci,isc,&ipmP->ci_scat);CHKERRQ(ierr);
329 
330       ierr = ISDestroy(&isuc);CHKERRQ(ierr);
331       ierr = ISDestroy(&isc);CHKERRQ(ierr);
332     }
333     /* need to know how may xbound indices are on each process */
334     /* TODO better way */
335     if (ipmP->nxlb) {
336       ierr = ISAllGather(ipmP->isxl,&bigxl);CHKERRQ(ierr);
337       ierr = ISGetIndices(bigxl,&xli);CHKERRQ(ierr);
338       /* find offsets for this processor */
339       xl_offset = ipmP->mi;
340       for (i=0;i<ipmP->nxlb;i++) {
341         if (xli[i] < xstart) {
342           xl_offset++;
343         } else break;
344       }
345       ierr = ISRestoreIndices(bigxl,&xli);CHKERRQ(ierr);
346 
347       ierr = ISGetIndices(ipmP->isxl,&xli);CHKERRQ(ierr);
348       ierr = ISGetLocalSize(ipmP->isxl,&nloc);CHKERRQ(ierr);
349       for (i=0;i<nloc;i++) {
350         xind[i] = xli[i];
351         cind[i] = xl_offset+i;
352       }
353 
354       ierr = ISCreateGeneral(comm,nloc,xind,PETSC_COPY_VALUES,&isx);CHKERRQ(ierr);
355       ierr = ISCreateGeneral(comm,nloc,cind,PETSC_COPY_VALUES,&isc);CHKERRQ(ierr);
356       ierr = VecScatterCreate(tao->XL,isx,ipmP->ci,isc,&ipmP->xl_scat);CHKERRQ(ierr);
357       ierr = ISDestroy(&isx);CHKERRQ(ierr);
358       ierr = ISDestroy(&isc);CHKERRQ(ierr);
359       ierr = ISDestroy(&bigxl);CHKERRQ(ierr);
360     }
361 
362     if (ipmP->nxub) {
363       ierr = ISAllGather(ipmP->isxu,&bigxu);CHKERRQ(ierr);
364       ierr = ISGetIndices(bigxu,&xui);CHKERRQ(ierr);
365       /* find offsets for this processor */
366       xu_offset = ipmP->mi + ipmP->nxlb;
367       for (i=0;i<ipmP->nxub;i++) {
368         if (xui[i] < xstart) {
369           xu_offset++;
370         } else break;
371       }
372       ierr = ISRestoreIndices(bigxu,&xui);CHKERRQ(ierr);
373 
374       ierr = ISGetIndices(ipmP->isxu,&xui);CHKERRQ(ierr);
375       ierr = ISGetLocalSize(ipmP->isxu,&nloc);CHKERRQ(ierr);
376       for (i=0;i<nloc;i++) {
377         xind[i] = xui[i];
378         cind[i] = xu_offset+i;
379       }
380 
381       ierr = ISCreateGeneral(comm,nloc,xind,PETSC_COPY_VALUES,&isx);CHKERRQ(ierr);
382       ierr = ISCreateGeneral(comm,nloc,cind,PETSC_COPY_VALUES,&isc);CHKERRQ(ierr);
383       ierr = VecScatterCreate(tao->XU,isx,ipmP->ci,isc,&ipmP->xu_scat);CHKERRQ(ierr);
384       ierr = ISDestroy(&isx);CHKERRQ(ierr);
385       ierr = ISDestroy(&isc);CHKERRQ(ierr);
386       ierr = ISDestroy(&bigxu);CHKERRQ(ierr);
387     }
388   }
389   ierr = VecCreate(comm,&ipmP->bigrhs);CHKERRQ(ierr);
390   ierr = VecGetType(tao->solution,&vtype);CHKERRQ(ierr);
391   ierr = VecSetType(ipmP->bigrhs,vtype);CHKERRQ(ierr);
392   ierr = VecSetSizes(ipmP->bigrhs,PETSC_DECIDE,bigsize);CHKERRQ(ierr);
393   ierr = VecSetFromOptions(ipmP->bigrhs);CHKERRQ(ierr);
394   ierr = VecDuplicate(ipmP->bigrhs,&ipmP->bigstep);CHKERRQ(ierr);
395 
396   /* create scatters for step->x and x->rhs */
397   for (i=xstart;i<xend;i++) {
398     stepind[i-xstart] = i;
399     xind[i-xstart] = i;
400   }
401   ierr = ISCreateGeneral(comm,xend-xstart,stepind,PETSC_COPY_VALUES,&sis);CHKERRQ(ierr);
402   ierr = ISCreateGeneral(comm,xend-xstart,xind,PETSC_COPY_VALUES,&is1);CHKERRQ(ierr);
403   ierr = VecScatterCreate(ipmP->bigstep,sis,tao->solution,is1,&ipmP->step1);CHKERRQ(ierr);
404   ierr = VecScatterCreate(tao->solution,is1,ipmP->bigrhs,sis,&ipmP->rhs1);CHKERRQ(ierr);
405   ierr = ISDestroy(&sis);CHKERRQ(ierr);
406   ierr = ISDestroy(&is1);CHKERRQ(ierr);
407 
408   if (ipmP->nb > 0) {
409     for (i=sstart;i<send;i++) {
410       stepind[i-sstart] = i+ipmP->n;
411       cind[i-sstart] = i;
412     }
413     ierr = ISCreateGeneral(comm,send-sstart,stepind,PETSC_COPY_VALUES,&sis);CHKERRQ(ierr);
414     ierr = ISCreateGeneral(comm,send-sstart,cind,PETSC_COPY_VALUES,&is1);CHKERRQ(ierr);
415     ierr = VecScatterCreate(ipmP->bigstep,sis,ipmP->s,is1,&ipmP->step2);CHKERRQ(ierr);
416     ierr = ISDestroy(&sis);CHKERRQ(ierr);
417 
418     for (i=sstart;i<send;i++) {
419       stepind[i-sstart] = i+ipmP->n+ipmP->me;
420       cind[i-sstart] = i;
421     }
422     ierr = ISCreateGeneral(comm,send-sstart,stepind,PETSC_COPY_VALUES,&sis);CHKERRQ(ierr);
423     ierr = VecScatterCreate(ipmP->s,is1,ipmP->bigrhs,sis,&ipmP->rhs3);CHKERRQ(ierr);
424     ierr = ISDestroy(&sis);CHKERRQ(ierr);
425     ierr = ISDestroy(&is1);CHKERRQ(ierr);
426   }
427 
428   if (ipmP->me > 0) {
429     ierr = VecGetOwnershipRange(tao->constraints_equality,&ucestart,&uceend);CHKERRQ(ierr);
430     for (i=ucestart;i<uceend;i++) {
431       stepind[i-ucestart] = i + ipmP->n+ipmP->nb;
432       uceind[i-ucestart] = i;
433     }
434 
435     ierr = ISCreateGeneral(comm,uceend-ucestart,stepind,PETSC_COPY_VALUES,&sis);CHKERRQ(ierr);
436     ierr = ISCreateGeneral(comm,uceend-ucestart,uceind,PETSC_COPY_VALUES,&is1);CHKERRQ(ierr);
437     ierr = VecScatterCreate(ipmP->bigstep,sis,tao->constraints_equality,is1,&ipmP->step3);CHKERRQ(ierr);
438     ierr = ISDestroy(&sis);CHKERRQ(ierr);
439 
440     for (i=ucestart;i<uceend;i++) {
441       stepind[i-ucestart] = i + ipmP->n;
442     }
443 
444     ierr = ISCreateGeneral(comm,uceend-ucestart,stepind,PETSC_COPY_VALUES,&sis);CHKERRQ(ierr);
445     ierr = VecScatterCreate(tao->constraints_equality,is1,ipmP->bigrhs,sis,&ipmP->rhs2);CHKERRQ(ierr);
446     ierr = ISDestroy(&sis);CHKERRQ(ierr);
447     ierr = ISDestroy(&is1);CHKERRQ(ierr);
448   }
449 
450   if (ipmP->nb > 0) {
451     for (i=sstart;i<send;i++) {
452       stepind[i-sstart] = i + ipmP->n + ipmP->nb + ipmP->me;
453       cind[i-sstart] = i;
454     }
455     ierr = ISCreateGeneral(comm,send-sstart,cind,PETSC_COPY_VALUES,&is1);CHKERRQ(ierr);
456     ierr = ISCreateGeneral(comm,send-sstart,stepind,PETSC_COPY_VALUES,&sis);CHKERRQ(ierr);
457     ierr = VecScatterCreate(ipmP->bigstep,sis,ipmP->s,is1,&ipmP->step4);CHKERRQ(ierr);
458     ierr = VecScatterCreate(ipmP->s,is1,ipmP->bigrhs,sis,&ipmP->rhs4);CHKERRQ(ierr);
459     ierr = ISDestroy(&sis);CHKERRQ(ierr);
460     ierr = ISDestroy(&is1);CHKERRQ(ierr);
461   }
462 
463   ierr = PetscFree(stepind);CHKERRQ(ierr);
464   ierr = PetscFree(cind);CHKERRQ(ierr);
465   ierr = PetscFree(ucind);CHKERRQ(ierr);
466   ierr = PetscFree(uceind);CHKERRQ(ierr);
467   ierr = PetscFree(xind);CHKERRQ(ierr);
468   PetscFunctionReturn(0);
469 }
470 
471 static PetscErrorCode TaoDestroy_IPM(Tao tao)
472 {
473   TAO_IPM        *ipmP = (TAO_IPM*)tao->data;
474   PetscErrorCode ierr;
475 
476   PetscFunctionBegin;
477   ierr = VecDestroy(&ipmP->rd);CHKERRQ(ierr);
478   ierr = VecDestroy(&ipmP->rpe);CHKERRQ(ierr);
479   ierr = VecDestroy(&ipmP->rpi);CHKERRQ(ierr);
480   ierr = VecDestroy(&ipmP->work);CHKERRQ(ierr);
481   ierr = VecDestroy(&ipmP->lamdae);CHKERRQ(ierr);
482   ierr = VecDestroy(&ipmP->lamdai);CHKERRQ(ierr);
483   ierr = VecDestroy(&ipmP->s);CHKERRQ(ierr);
484   ierr = VecDestroy(&ipmP->ds);CHKERRQ(ierr);
485   ierr = VecDestroy(&ipmP->ci);CHKERRQ(ierr);
486 
487   ierr = VecDestroy(&ipmP->rhs_x);CHKERRQ(ierr);
488   ierr = VecDestroy(&ipmP->rhs_lamdae);CHKERRQ(ierr);
489   ierr = VecDestroy(&ipmP->rhs_lamdai);CHKERRQ(ierr);
490   ierr = VecDestroy(&ipmP->rhs_s);CHKERRQ(ierr);
491 
492   ierr = VecDestroy(&ipmP->save_x);CHKERRQ(ierr);
493   ierr = VecDestroy(&ipmP->save_lamdae);CHKERRQ(ierr);
494   ierr = VecDestroy(&ipmP->save_lamdai);CHKERRQ(ierr);
495   ierr = VecDestroy(&ipmP->save_s);CHKERRQ(ierr);
496 
497   ierr = VecScatterDestroy(&ipmP->step1);CHKERRQ(ierr);
498   ierr = VecScatterDestroy(&ipmP->step2);CHKERRQ(ierr);
499   ierr = VecScatterDestroy(&ipmP->step3);CHKERRQ(ierr);
500   ierr = VecScatterDestroy(&ipmP->step4);CHKERRQ(ierr);
501 
502   ierr = VecScatterDestroy(&ipmP->rhs1);CHKERRQ(ierr);
503   ierr = VecScatterDestroy(&ipmP->rhs2);CHKERRQ(ierr);
504   ierr = VecScatterDestroy(&ipmP->rhs3);CHKERRQ(ierr);
505   ierr = VecScatterDestroy(&ipmP->rhs4);CHKERRQ(ierr);
506 
507   ierr = VecScatterDestroy(&ipmP->ci_scat);CHKERRQ(ierr);
508   ierr = VecScatterDestroy(&ipmP->xl_scat);CHKERRQ(ierr);
509   ierr = VecScatterDestroy(&ipmP->xu_scat);CHKERRQ(ierr);
510 
511   ierr = VecDestroy(&ipmP->dlamdai);CHKERRQ(ierr);
512   ierr = VecDestroy(&ipmP->dlamdae);CHKERRQ(ierr);
513   ierr = VecDestroy(&ipmP->Zero_nb);CHKERRQ(ierr);
514   ierr = VecDestroy(&ipmP->One_nb);CHKERRQ(ierr);
515   ierr = VecDestroy(&ipmP->Inf_nb);CHKERRQ(ierr);
516   ierr = VecDestroy(&ipmP->complementarity);CHKERRQ(ierr);
517 
518   ierr = VecDestroy(&ipmP->bigrhs);CHKERRQ(ierr);
519   ierr = VecDestroy(&ipmP->bigstep);CHKERRQ(ierr);
520   ierr = MatDestroy(&ipmP->Ai);CHKERRQ(ierr);
521   ierr = MatDestroy(&ipmP->K);CHKERRQ(ierr);
522   ierr = ISDestroy(&ipmP->isxu);CHKERRQ(ierr);
523   ierr = ISDestroy(&ipmP->isxl);CHKERRQ(ierr);
524   ierr = PetscFree(tao->data);CHKERRQ(ierr);
525   PetscFunctionReturn(0);
526 }
527 
528 static PetscErrorCode TaoSetFromOptions_IPM(PetscOptionItems *PetscOptionsObject,Tao tao)
529 {
530   TAO_IPM        *ipmP = (TAO_IPM*)tao->data;
531   PetscErrorCode ierr;
532 
533   PetscFunctionBegin;
534   ierr = PetscOptionsHead(PetscOptionsObject,"IPM method for constrained optimization");CHKERRQ(ierr);
535   ierr = PetscOptionsBool("-tao_ipm_monitorkkt","monitor kkt status",NULL,ipmP->monitorkkt,&ipmP->monitorkkt,NULL);CHKERRQ(ierr);
536   ierr = PetscOptionsReal("-tao_ipm_pushs","parameter to push initial slack variables away from bounds",NULL,ipmP->pushs,&ipmP->pushs,NULL);CHKERRQ(ierr);
537   ierr = PetscOptionsReal("-tao_ipm_pushnu","parameter to push initial (inequality) dual variables away from bounds",NULL,ipmP->pushnu,&ipmP->pushnu,NULL);CHKERRQ(ierr);
538   ierr = PetscOptionsTail();CHKERRQ(ierr);
539   ierr = KSPSetFromOptions(tao->ksp);CHKERRQ(ierr);
540   PetscFunctionReturn(0);
541 }
542 
543 static PetscErrorCode TaoView_IPM(Tao tao, PetscViewer viewer)
544 {
545   return 0;
546 }
547 
548 /* IPMObjectiveAndGradient()
549    f = d'x + 0.5 * x' * H * x
550    rd = H*x + d + Ae'*lame - Ai'*lami
551    rpe = Ae*x - be
552    rpi = Ai*x - yi - bi
553    mu = yi' * lami/mi;
554    com = yi.*lami
555 
556    phi = ||rd|| + ||rpe|| + ||rpi|| + ||com||
557 */
558 /*
559 static PetscErrorCode IPMObjective(TaoLineSearch ls, Vec X, PetscReal *f, void *tptr)
560 {
561   Tao tao = (Tao)tptr;
562   TAO_IPM *ipmP = (TAO_IPM*)tao->data;
563   PetscErrorCode ierr;
564   PetscFunctionBegin;
565   ierr = IPMComputeKKT(tao);CHKERRQ(ierr);
566   *f = ipmP->phi;
567   PetscFunctionReturn(0);
568 }
569 */
570 
571 /*
572    f = d'x + 0.5 * x' * H * x
573    rd = H*x + d + Ae'*lame - Ai'*lami
574        Ai =   jac_ineq
575                I (w/lb)
576               -I (w/ub)
577 
578    rpe = ce
579    rpi = ci - s;
580    com = s.*lami
581    mu = yi' * lami/mi;
582 
583    phi = ||rd|| + ||rpe|| + ||rpi|| + ||com||
584 */
585 static PetscErrorCode IPMComputeKKT(Tao tao)
586 {
587   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
588   PetscScalar    norm;
589   PetscErrorCode ierr;
590 
591   PetscFunctionBegin;
592   ierr = VecCopy(tao->gradient,ipmP->rd);CHKERRQ(ierr);
593 
594   if (ipmP->me > 0) {
595     /* rd = gradient + Ae'*lamdae */
596     ierr = MatMultTranspose(tao->jacobian_equality,ipmP->lamdae,ipmP->work);CHKERRQ(ierr);
597     ierr = VecAXPY(ipmP->rd, 1.0, ipmP->work);CHKERRQ(ierr);
598 
599     /* rpe = ce(x) */
600     ierr = VecCopy(tao->constraints_equality,ipmP->rpe);CHKERRQ(ierr);
601   }
602   if (ipmP->nb > 0) {
603     /* rd = rd - Ai'*lamdai */
604     ierr = MatMultTranspose(ipmP->Ai,ipmP->lamdai,ipmP->work);CHKERRQ(ierr);
605     ierr = VecAXPY(ipmP->rd, -1.0, ipmP->work);CHKERRQ(ierr);
606 
607     /* rpi = cin - s */
608     ierr = VecCopy(ipmP->ci,ipmP->rpi);CHKERRQ(ierr);
609     ierr = VecAXPY(ipmP->rpi, -1.0, ipmP->s);CHKERRQ(ierr);
610 
611     /* com = s .* lami */
612     ierr = VecPointwiseMult(ipmP->complementarity, ipmP->s,ipmP->lamdai);CHKERRQ(ierr);
613   }
614   /* phi = ||rd; rpe; rpi; com|| */
615   ierr = VecDot(ipmP->rd,ipmP->rd,&norm);CHKERRQ(ierr);
616   ipmP->phi = norm;
617   if (ipmP->me > 0 ) {
618     ierr = VecDot(ipmP->rpe,ipmP->rpe,&norm);CHKERRQ(ierr);
619     ipmP->phi += norm;
620   }
621   if (ipmP->nb > 0) {
622     ierr = VecDot(ipmP->rpi,ipmP->rpi,&norm);CHKERRQ(ierr);
623     ipmP->phi += norm;
624     ierr = VecDot(ipmP->complementarity,ipmP->complementarity,&norm);CHKERRQ(ierr);
625     ipmP->phi += norm;
626     /* mu = s'*lami/nb */
627     ierr = VecDot(ipmP->s,ipmP->lamdai,&ipmP->mu);CHKERRQ(ierr);
628     ipmP->mu /= ipmP->nb;
629   } else {
630     ipmP->mu = 1.0;
631   }
632 
633   ipmP->phi = PetscSqrtScalar(ipmP->phi);
634   PetscFunctionReturn(0);
635 }
636 
637 /* evaluate user info at current point */
638 PetscErrorCode IPMEvaluate(Tao tao)
639 {
640   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
641   PetscErrorCode ierr;
642 
643   PetscFunctionBegin;
644   ierr = TaoComputeObjectiveAndGradient(tao,tao->solution,&ipmP->kkt_f,tao->gradient);CHKERRQ(ierr);
645   ierr = TaoComputeHessian(tao,tao->solution,tao->hessian,tao->hessian_pre);CHKERRQ(ierr);
646   if (ipmP->me > 0) {
647     ierr = TaoComputeEqualityConstraints(tao,tao->solution,tao->constraints_equality);CHKERRQ(ierr);
648     ierr = TaoComputeJacobianEquality(tao,tao->solution,tao->jacobian_equality,tao->jacobian_equality_pre);CHKERRQ(ierr);
649   }
650   if (ipmP->mi > 0) {
651     ierr = TaoComputeInequalityConstraints(tao,tao->solution,tao->constraints_inequality);CHKERRQ(ierr);
652     ierr = TaoComputeJacobianInequality(tao,tao->solution,tao->jacobian_inequality,tao->jacobian_inequality_pre);CHKERRQ(ierr);
653   }
654   if (ipmP->nb > 0) {
655     /* Ai' =   jac_ineq | I (w/lb) | -I (w/ub)  */
656     ierr = IPMUpdateAi(tao);CHKERRQ(ierr);
657   }
658   PetscFunctionReturn(0);
659 }
660 
661 /* Push initial point away from bounds */
662 PetscErrorCode IPMPushInitialPoint(Tao tao)
663 {
664   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
665   PetscErrorCode ierr;
666 
667   PetscFunctionBegin;
668   ierr = TaoComputeVariableBounds(tao);CHKERRQ(ierr);
669   if (tao->XL && tao->XU) {
670     ierr = VecMedian(tao->XL, tao->solution, tao->XU, tao->solution);CHKERRQ(ierr);
671   }
672   if (ipmP->nb > 0) {
673     ierr = VecSet(ipmP->s,ipmP->pushs);CHKERRQ(ierr);
674     ierr = VecSet(ipmP->lamdai,ipmP->pushnu);CHKERRQ(ierr);
675     if (ipmP->mi > 0) {
676       ierr = VecSet(tao->DI,ipmP->pushnu);CHKERRQ(ierr);
677     }
678   }
679   if (ipmP->me > 0) {
680     ierr = VecSet(tao->DE,1.0);CHKERRQ(ierr);
681     ierr = VecSet(ipmP->lamdae,1.0);CHKERRQ(ierr);
682   }
683   PetscFunctionReturn(0);
684 }
685 
686 PetscErrorCode IPMUpdateAi(Tao tao)
687 {
688   /* Ai =     Ji
689               I (w/lb)
690              -I (w/ub) */
691 
692   /* Ci =    user->ci
693              Xi - lb (w/lb)
694              -Xi + ub (w/ub)  */
695 
696   TAO_IPM           *ipmP = (TAO_IPM *)tao->data;
697   MPI_Comm          comm;
698   PetscInt          i;
699   PetscScalar       newval;
700   PetscInt          newrow,newcol,ncols;
701   const PetscScalar *vals;
702   const PetscInt    *cols;
703   PetscInt          astart,aend,jstart,jend;
704   PetscInt          *nonzeros;
705   PetscInt          r2,r3,r4;
706   PetscMPIInt       size;
707   PetscErrorCode    ierr;
708 
709   PetscFunctionBegin;
710   r2 = ipmP->mi;
711   r3 = r2 + ipmP->nxlb;
712   r4 = r3 + ipmP->nxub;
713 
714   if (!ipmP->nb) PetscFunctionReturn(0);
715 
716   /* Create Ai matrix if it doesn't exist yet */
717   if (!ipmP->Ai) {
718     comm = ((PetscObject)(tao->solution))->comm;
719     ierr = PetscMalloc1(ipmP->nb,&nonzeros);CHKERRQ(ierr);
720     ierr = MPI_Comm_size(comm,&size);CHKERRQ(ierr);
721     if (size == 1) {
722       for (i=0;i<ipmP->mi;i++) {
723         ierr = MatGetRow(tao->jacobian_inequality,i,&ncols,NULL,NULL);CHKERRQ(ierr);
724         nonzeros[i] = ncols;
725         ierr = MatRestoreRow(tao->jacobian_inequality,i,&ncols,NULL,NULL);CHKERRQ(ierr);
726       }
727       for (i=r2;i<r4;i++) {
728         nonzeros[i] = 1;
729       }
730     }
731     ierr = MatCreate(comm,&ipmP->Ai);CHKERRQ(ierr);
732     ierr = MatSetType(ipmP->Ai,MATAIJ);CHKERRQ(ierr);
733     ierr = MatSetSizes(ipmP->Ai,PETSC_DECIDE,PETSC_DECIDE,ipmP->nb,ipmP->n);CHKERRQ(ierr);
734     ierr = MatSetFromOptions(ipmP->Ai);CHKERRQ(ierr);
735     ierr = MatMPIAIJSetPreallocation(ipmP->Ai,ipmP->nb,NULL,ipmP->nb,NULL);CHKERRQ(ierr);
736     ierr = MatSeqAIJSetPreallocation(ipmP->Ai,PETSC_DEFAULT,nonzeros);CHKERRQ(ierr);
737     if (size ==1) {
738       ierr = PetscFree(nonzeros);CHKERRQ(ierr);
739     }
740   }
741 
742   /* Copy values from user jacobian to Ai */
743   ierr = MatGetOwnershipRange(ipmP->Ai,&astart,&aend);CHKERRQ(ierr);
744 
745   /* Ai w/lb */
746   if (ipmP->mi) {
747     ierr = MatZeroEntries(ipmP->Ai);CHKERRQ(ierr);
748     ierr = MatGetOwnershipRange(tao->jacobian_inequality,&jstart,&jend);CHKERRQ(ierr);
749     for (i=jstart;i<jend;i++) {
750       ierr = MatGetRow(tao->jacobian_inequality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
751       newrow = i;
752       ierr = MatSetValues(ipmP->Ai,1,&newrow,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
753       ierr = MatRestoreRow(tao->jacobian_inequality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
754     }
755   }
756 
757   /* I w/ xlb */
758   if (ipmP->nxlb) {
759     for (i=0;i<ipmP->nxlb;i++) {
760       if (i>=astart && i<aend) {
761         newrow = i+r2;
762         newcol = i;
763         newval = 1.0;
764         ierr = MatSetValues(ipmP->Ai,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
765       }
766     }
767   }
768   if (ipmP->nxub) {
769     /* I w/ xub */
770     for (i=0;i<ipmP->nxub;i++) {
771       if (i>=astart && i<aend) {
772       newrow = i+r3;
773       newcol = i;
774       newval = -1.0;
775       ierr = MatSetValues(ipmP->Ai,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
776       }
777     }
778   }
779 
780   ierr = MatAssemblyBegin(ipmP->Ai,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
781   ierr = MatAssemblyEnd(ipmP->Ai,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
782   CHKMEMQ;
783 
784   ierr = VecSet(ipmP->ci,0.0);CHKERRQ(ierr);
785 
786   /* user ci */
787   if (ipmP->mi > 0) {
788     ierr = VecScatterBegin(ipmP->ci_scat,tao->constraints_inequality,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
789     ierr = VecScatterEnd(ipmP->ci_scat,tao->constraints_inequality,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
790   }
791   if (!ipmP->work){
792     VecDuplicate(tao->solution,&ipmP->work);
793   }
794   ierr = VecCopy(tao->solution,ipmP->work);CHKERRQ(ierr);
795   if (tao->XL) {
796     ierr = VecAXPY(ipmP->work,-1.0,tao->XL);CHKERRQ(ierr);
797 
798     /* lower bounds on variables */
799     if (ipmP->nxlb > 0) {
800       ierr = VecScatterBegin(ipmP->xl_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
801       ierr = VecScatterEnd(ipmP->xl_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
802     }
803   }
804   if (tao->XU) {
805     /* upper bounds on variables */
806     ierr = VecCopy(tao->solution,ipmP->work);CHKERRQ(ierr);
807     ierr = VecScale(ipmP->work,-1.0);CHKERRQ(ierr);
808     ierr = VecAXPY(ipmP->work,1.0,tao->XU);CHKERRQ(ierr);
809     if (ipmP->nxub > 0) {
810       ierr = VecScatterBegin(ipmP->xu_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
811       ierr = VecScatterEnd(ipmP->xu_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
812     }
813   }
814   PetscFunctionReturn(0);
815 }
816 
817 /* create K = [ Hlag , 0 , Ae', -Ai'];
818               [Ae , 0,   0  , 0];
819               [Ai ,-I,   0 ,  0];
820               [ 0 , S ,  0,   Y ];  */
821 PetscErrorCode IPMUpdateK(Tao tao)
822 {
823   TAO_IPM         *ipmP = (TAO_IPM *)tao->data;
824   MPI_Comm        comm;
825   PetscMPIInt     size;
826   PetscErrorCode  ierr;
827   PetscInt        i,j,row;
828   PetscInt        ncols,newcol,newcols[2],newrow;
829   const PetscInt  *cols;
830   const PetscReal *vals;
831   const PetscReal *l,*y;
832   PetscReal       *newvals;
833   PetscReal       newval;
834   PetscInt        subsize;
835   const PetscInt  *indices;
836   PetscInt        *nonzeros,*d_nonzeros,*o_nonzeros;
837   PetscInt        bigsize;
838   PetscInt        r1,r2,r3;
839   PetscInt        c1,c2,c3;
840   PetscInt        klocalsize;
841   PetscInt        hstart,hend,kstart,kend;
842   PetscInt        aistart,aiend,aestart,aeend;
843   PetscInt        sstart,send;
844 
845   PetscFunctionBegin;
846   comm = ((PetscObject)(tao->solution))->comm;
847   ierr = MPI_Comm_size(comm,&size);CHKERRQ(ierr);
848   ierr = IPMUpdateAi(tao);CHKERRQ(ierr);
849 
850   /* allocate workspace */
851   subsize = PetscMax(ipmP->n,ipmP->nb);
852   subsize = PetscMax(ipmP->me,subsize);
853   subsize = PetscMax(2,subsize);
854   ierr = PetscMalloc1(subsize,(PetscInt**)&indices);CHKERRQ(ierr);
855   ierr = PetscMalloc1(subsize,&newvals);CHKERRQ(ierr);
856 
857   r1 = c1 = ipmP->n;
858   r2 = r1 + ipmP->me;  c2 = c1 + ipmP->nb;
859   r3 = c3 = r2 + ipmP->nb;
860 
861   bigsize = ipmP->n+2*ipmP->nb+ipmP->me;
862   ierr = VecGetOwnershipRange(ipmP->bigrhs,&kstart,&kend);CHKERRQ(ierr);
863   ierr = MatGetOwnershipRange(tao->hessian,&hstart,&hend);CHKERRQ(ierr);
864   klocalsize = kend-kstart;
865   if (!ipmP->K) {
866     if (size == 1) {
867       ierr = PetscMalloc1(kend-kstart,&nonzeros);CHKERRQ(ierr);
868       for (i=0;i<bigsize;i++) {
869         if (i<r1) {
870           ierr = MatGetRow(tao->hessian,i,&ncols,NULL,NULL);CHKERRQ(ierr);
871           nonzeros[i] = ncols;
872           ierr = MatRestoreRow(tao->hessian,i,&ncols,NULL,NULL);CHKERRQ(ierr);
873           nonzeros[i] += ipmP->me+ipmP->nb;
874         } else if (i<r2) {
875           nonzeros[i-kstart] = ipmP->n;
876         } else if (i<r3) {
877           nonzeros[i-kstart] = ipmP->n+1;
878         } else if (i<bigsize) {
879           nonzeros[i-kstart] = 2;
880         }
881       }
882       ierr = MatCreate(comm,&ipmP->K);CHKERRQ(ierr);
883       ierr = MatSetType(ipmP->K,MATSEQAIJ);CHKERRQ(ierr);
884       ierr = MatSetSizes(ipmP->K,klocalsize,klocalsize,PETSC_DETERMINE,PETSC_DETERMINE);CHKERRQ(ierr);
885       ierr = MatSeqAIJSetPreallocation(ipmP->K,0,nonzeros);CHKERRQ(ierr);
886       ierr = MatSetFromOptions(ipmP->K);CHKERRQ(ierr);
887       ierr = PetscFree(nonzeros);CHKERRQ(ierr);
888     } else {
889       ierr = PetscMalloc1(kend-kstart,&d_nonzeros);CHKERRQ(ierr);
890       ierr = PetscMalloc1(kend-kstart,&o_nonzeros);CHKERRQ(ierr);
891       for (i=kstart;i<kend;i++) {
892         if (i<r1) {
893           /* TODO fix preallocation for mpi mats */
894           d_nonzeros[i-kstart] = PetscMin(ipmP->n+ipmP->me+ipmP->nb,kend-kstart);
895           o_nonzeros[i-kstart] = PetscMin(ipmP->n+ipmP->me+ipmP->nb,bigsize-(kend-kstart));
896         } else if (i<r2) {
897           d_nonzeros[i-kstart] = PetscMin(ipmP->n,kend-kstart);
898           o_nonzeros[i-kstart] = PetscMin(ipmP->n,bigsize-(kend-kstart));
899         } else if (i<r3) {
900           d_nonzeros[i-kstart] = PetscMin(ipmP->n+2,kend-kstart);
901           o_nonzeros[i-kstart] = PetscMin(ipmP->n+2,bigsize-(kend-kstart));
902         } else {
903           d_nonzeros[i-kstart] = PetscMin(2,kend-kstart);
904           o_nonzeros[i-kstart] = PetscMin(2,bigsize-(kend-kstart));
905         }
906       }
907       ierr = MatCreate(comm,&ipmP->K);CHKERRQ(ierr);
908       ierr = MatSetType(ipmP->K,MATMPIAIJ);CHKERRQ(ierr);
909       ierr = MatSetSizes(ipmP->K,klocalsize,klocalsize,PETSC_DETERMINE,PETSC_DETERMINE);CHKERRQ(ierr);
910       ierr = MatMPIAIJSetPreallocation(ipmP->K,0,d_nonzeros,0,o_nonzeros);CHKERRQ(ierr);
911       ierr = PetscFree(d_nonzeros);CHKERRQ(ierr);
912       ierr = PetscFree(o_nonzeros);CHKERRQ(ierr);
913       ierr = MatSetFromOptions(ipmP->K);CHKERRQ(ierr);
914     }
915   }
916 
917   ierr = MatZeroEntries(ipmP->K);CHKERRQ(ierr);
918   /* Copy H */
919   for (i=hstart;i<hend;i++) {
920     ierr = MatGetRow(tao->hessian,i,&ncols,&cols,&vals);CHKERRQ(ierr);
921     if (ncols > 0) {
922       ierr = MatSetValues(ipmP->K,1,&i,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
923     }
924     ierr = MatRestoreRow(tao->hessian,i,&ncols,&cols,&vals);CHKERRQ(ierr);
925   }
926 
927   /* Copy Ae and Ae' */
928   if (ipmP->me > 0) {
929     ierr = MatGetOwnershipRange(tao->jacobian_equality,&aestart,&aeend);CHKERRQ(ierr);
930     for (i=aestart;i<aeend;i++) {
931       ierr = MatGetRow(tao->jacobian_equality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
932       if (ncols > 0) {
933         /*Ae*/
934         row = i+r1;
935         ierr = MatSetValues(ipmP->K,1,&row,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
936         /*Ae'*/
937         for (j=0;j<ncols;j++) {
938           newcol = i + c2;
939           newrow = cols[j];
940           newval = vals[j];
941           ierr = MatSetValues(ipmP->K,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
942         }
943       }
944       ierr = MatRestoreRow(tao->jacobian_equality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
945     }
946   }
947 
948   if (ipmP->nb > 0) {
949     ierr = MatGetOwnershipRange(ipmP->Ai,&aistart,&aiend);CHKERRQ(ierr);
950     /* Copy Ai,and Ai' */
951     for (i=aistart;i<aiend;i++) {
952       row = i+r2;
953       ierr = MatGetRow(ipmP->Ai,i,&ncols,&cols,&vals);CHKERRQ(ierr);
954       if (ncols > 0) {
955         /*Ai*/
956         ierr = MatSetValues(ipmP->K,1,&row,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
957         /*-Ai'*/
958         for (j=0;j<ncols;j++) {
959           newcol = i + c3;
960           newrow = cols[j];
961           newval = -vals[j];
962           ierr = MatSetValues(ipmP->K,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
963         }
964       }
965       ierr = MatRestoreRow(ipmP->Ai,i,&ncols,&cols,&vals);CHKERRQ(ierr);
966     }
967 
968     /* -I */
969     for (i=kstart;i<kend;i++) {
970       if (i>=r2 && i<r3) {
971         newrow = i;
972         newcol = i-r2+c1;
973         newval = -1.0;
974         MatSetValues(ipmP->K,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
975       }
976     }
977 
978     /* Copy L,Y */
979     ierr = VecGetOwnershipRange(ipmP->s,&sstart,&send);CHKERRQ(ierr);
980     ierr = VecGetArrayRead(ipmP->lamdai,&l);CHKERRQ(ierr);
981     ierr = VecGetArrayRead(ipmP->s,&y);CHKERRQ(ierr);
982 
983     for (i=sstart;i<send;i++) {
984       newcols[0] = c1+i;
985       newcols[1] = c3+i;
986       newvals[0] = l[i-sstart];
987       newvals[1] = y[i-sstart];
988       newrow = r3+i;
989       ierr = MatSetValues(ipmP->K,1,&newrow,2,newcols,newvals,INSERT_VALUES);CHKERRQ(ierr);
990     }
991 
992     ierr = VecRestoreArrayRead(ipmP->lamdai,&l);CHKERRQ(ierr);
993     ierr = VecRestoreArrayRead(ipmP->s,&y);CHKERRQ(ierr);
994   }
995 
996   ierr = PetscFree(indices);CHKERRQ(ierr);
997   ierr = PetscFree(newvals);CHKERRQ(ierr);
998   ierr = MatAssemblyBegin(ipmP->K,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
999   ierr = MatAssemblyEnd(ipmP->K,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
1000   PetscFunctionReturn(0);
1001 }
1002 
1003 PetscErrorCode IPMGatherRHS(Tao tao,Vec RHS,Vec X1,Vec X2,Vec X3,Vec X4)
1004 {
1005   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
1006   PetscErrorCode ierr;
1007 
1008   PetscFunctionBegin;
1009   /* rhs = [x1      (n)
1010             x2     (me)
1011             x3     (nb)
1012             x4     (nb)] */
1013   if (X1) {
1014     ierr = VecScatterBegin(ipmP->rhs1,X1,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1015     ierr = VecScatterEnd(ipmP->rhs1,X1,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1016   }
1017   if (ipmP->me > 0 && X2) {
1018     ierr = VecScatterBegin(ipmP->rhs2,X2,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1019     ierr = VecScatterEnd(ipmP->rhs2,X2,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1020   }
1021   if (ipmP->nb > 0) {
1022     if (X3) {
1023       ierr = VecScatterBegin(ipmP->rhs3,X3,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1024       ierr = VecScatterEnd(ipmP->rhs3,X3,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1025     }
1026     if (X4) {
1027       ierr = VecScatterBegin(ipmP->rhs4,X4,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1028       ierr = VecScatterEnd(ipmP->rhs4,X4,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1029     }
1030   }
1031   PetscFunctionReturn(0);
1032 }
1033 
1034 PetscErrorCode IPMScatterStep(Tao tao, Vec STEP, Vec X1, Vec X2, Vec X3, Vec X4)
1035 {
1036   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
1037   PetscErrorCode ierr;
1038 
1039   PetscFunctionBegin;
1040   CHKMEMQ;
1041   /*        [x1    (n)
1042              x2    (nb) may be 0
1043              x3    (me) may be 0
1044              x4    (nb) may be 0 */
1045   if (X1) {
1046     ierr = VecScatterBegin(ipmP->step1,STEP,X1,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1047     ierr = VecScatterEnd(ipmP->step1,STEP,X1,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1048   }
1049   if (X2 && ipmP->nb > 0) {
1050     ierr = VecScatterBegin(ipmP->step2,STEP,X2,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1051     ierr = VecScatterEnd(ipmP->step2,STEP,X2,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1052   }
1053   if (X3 && ipmP->me > 0) {
1054     ierr = VecScatterBegin(ipmP->step3,STEP,X3,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1055     ierr = VecScatterEnd(ipmP->step3,STEP,X3,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1056   }
1057   if (X4 && ipmP->nb > 0) {
1058     ierr = VecScatterBegin(ipmP->step4,STEP,X4,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1059     ierr = VecScatterEnd(ipmP->step4,STEP,X4,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1060   }
1061   CHKMEMQ;
1062   PetscFunctionReturn(0);
1063 }
1064 
1065 /*MC
1066   TAOIPM - Interior point algorithm for generally constrained optimization.
1067 
1068   Option Database Keys:
1069 +   -tao_ipm_pushnu - parameter to push initial dual variables away from bounds
1070 .   -tao_ipm_pushs - parameter to push initial slack variables away from bounds
1071 
1072   Notes: This algorithm is more of a place-holder for future constrained optimization algorithms and should not yet be used for large problems or production code.
1073   Level: beginner
1074 
1075 M*/
1076 
1077 PETSC_EXTERN PetscErrorCode TaoCreate_IPM(Tao tao)
1078 {
1079   TAO_IPM        *ipmP;
1080   PetscErrorCode ierr;
1081 
1082   PetscFunctionBegin;
1083   tao->ops->setup = TaoSetup_IPM;
1084   tao->ops->solve = TaoSolve_IPM;
1085   tao->ops->view = TaoView_IPM;
1086   tao->ops->setfromoptions = TaoSetFromOptions_IPM;
1087   tao->ops->destroy = TaoDestroy_IPM;
1088   /* tao->ops->computedual = TaoComputeDual_IPM; */
1089 
1090   ierr = PetscNewLog(tao,&ipmP);CHKERRQ(ierr);
1091   tao->data = (void*)ipmP;
1092 
1093   /* Override default settings (unless already changed) */
1094   if (!tao->max_it_changed) tao->max_it = 200;
1095   if (!tao->max_funcs_changed) tao->max_funcs = 500;
1096 
1097   ipmP->dec = 10000; /* line search critera */
1098   ipmP->taumin = 0.995;
1099   ipmP->monitorkkt = PETSC_FALSE;
1100   ipmP->pushs = 100;
1101   ipmP->pushnu = 100;
1102   ierr = KSPCreate(((PetscObject)tao)->comm, &tao->ksp);CHKERRQ(ierr);
1103   ierr = PetscObjectIncrementTabLevel((PetscObject)tao->ksp, (PetscObject)tao, 1);CHKERRQ(ierr);
1104   ierr = KSPSetOptionsPrefix(tao->ksp, tao->hdr.prefix);CHKERRQ(ierr);
1105   PetscFunctionReturn(0);
1106 }
1107