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