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