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