xref: /petsc/src/tao/constrained/impls/ipm/ipm.c (revision 94ae4db594e046293ec15c24a9f6581a7ec3c4e6) 
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 
536   PetscFunctionBegin;
537   ierr = PetscOptionsHead("IPM method for constrained optimization");CHKERRQ(ierr);
538   ierr = PetscOptionsBool("-tao_ipm_monitorkkt","monitor kkt status",NULL,ipmP->monitorkkt,&ipmP->monitorkkt,NULL);CHKERRQ(ierr);
539   ierr = PetscOptionsReal("-tao_ipm_pushs","parameter to push initial slack variables away from bounds",NULL,ipmP->pushs,&ipmP->pushs,NULL);CHKERRQ(ierr);
540   ierr = PetscOptionsReal("-tao_ipm_pushnu","parameter to push initial (inequality) dual variables away from bounds",NULL,ipmP->pushnu,&ipmP->pushnu,NULL);CHKERRQ(ierr);
541   ierr = PetscOptionsTail();CHKERRQ(ierr);
542   ierr = KSPSetFromOptions(tao->ksp);CHKERRQ(ierr);
543   PetscFunctionReturn(0);
544 }
545 
546 #undef __FUNCT__
547 #define __FUNCT__ "TaoView_IPM"
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 #undef __FUNCT__
565 #define __FUNCT__ "IPMObjective"
566 static PetscErrorCode IPMObjective(TaoLineSearch ls, Vec X, PetscReal *f, void *tptr)
567 {
568   Tao tao = (Tao)tptr;
569   TAO_IPM *ipmP = (TAO_IPM*)tao->data;
570   PetscErrorCode ierr;
571   PetscFunctionBegin;
572   ierr = IPMComputeKKT(tao);CHKERRQ(ierr);
573   *f = ipmP->phi;
574   PetscFunctionReturn(0);
575 }
576 */
577 
578 /*
579    f = d'x + 0.5 * x' * H * x
580    rd = H*x + d + Ae'*lame - Ai'*lami
581        Ai =   jac_ineq
582                I (w/lb)
583               -I (w/ub)
584 
585    rpe = ce
586    rpi = ci - s;
587    com = s.*lami
588    mu = yi' * lami/mi;
589 
590    phi = ||rd|| + ||rpe|| + ||rpi|| + ||com||
591 */
592 #undef __FUNCT__
593 #define __FUNCT__ "IPMComputeKKT"
594 static PetscErrorCode IPMComputeKKT(Tao tao)
595 {
596   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
597   PetscScalar    norm;
598   PetscErrorCode ierr;
599 
600   PetscFunctionBegin;
601   ierr = VecCopy(tao->gradient,ipmP->rd);CHKERRQ(ierr);
602 
603   if (ipmP->me > 0) {
604     /* rd = gradient + Ae'*lamdae */
605     ierr = MatMultTranspose(tao->jacobian_equality,ipmP->lamdae,ipmP->work);CHKERRQ(ierr);
606     ierr = VecAXPY(ipmP->rd, 1.0, ipmP->work);CHKERRQ(ierr);
607 
608     /* rpe = ce(x) */
609     ierr = VecCopy(tao->constraints_equality,ipmP->rpe);CHKERRQ(ierr);
610   }
611   if (ipmP->nb > 0) {
612     /* rd = rd - Ai'*lamdai */
613     ierr = MatMultTranspose(ipmP->Ai,ipmP->lamdai,ipmP->work);CHKERRQ(ierr);
614     ierr = VecAXPY(ipmP->rd, -1.0, ipmP->work);CHKERRQ(ierr);
615 
616     /* rpi = cin - s */
617     ierr = VecCopy(ipmP->ci,ipmP->rpi);CHKERRQ(ierr);
618     ierr = VecAXPY(ipmP->rpi, -1.0, ipmP->s);CHKERRQ(ierr);
619 
620     /* com = s .* lami */
621     ierr = VecPointwiseMult(ipmP->complementarity, ipmP->s,ipmP->lamdai);CHKERRQ(ierr);
622   }
623   /* phi = ||rd; rpe; rpi; com|| */
624   ierr = VecDot(ipmP->rd,ipmP->rd,&norm);CHKERRQ(ierr);
625   ipmP->phi = norm;
626   if (ipmP->me > 0 ) {
627     ierr = VecDot(ipmP->rpe,ipmP->rpe,&norm);CHKERRQ(ierr);
628     ipmP->phi += norm;
629   }
630   if (ipmP->nb > 0) {
631     ierr = VecDot(ipmP->rpi,ipmP->rpi,&norm);CHKERRQ(ierr);
632     ipmP->phi += norm;
633     ierr = VecDot(ipmP->complementarity,ipmP->complementarity,&norm);CHKERRQ(ierr);
634     ipmP->phi += norm;
635     /* mu = s'*lami/nb */
636     ierr = VecDot(ipmP->s,ipmP->lamdai,&ipmP->mu);CHKERRQ(ierr);
637     ipmP->mu /= ipmP->nb;
638   } else {
639     ipmP->mu = 1.0;
640   }
641 
642   ipmP->phi = PetscSqrtScalar(ipmP->phi);
643   PetscFunctionReturn(0);
644 }
645 
646 #undef __FUNCT__
647 #define __FUNCT__ "IPMEvaluate"
648 /* evaluate user info at current point */
649 PetscErrorCode IPMEvaluate(Tao tao)
650 {
651   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
652   PetscErrorCode ierr;
653 
654   PetscFunctionBegin;
655   ierr = TaoComputeObjectiveAndGradient(tao,tao->solution,&ipmP->kkt_f,tao->gradient);CHKERRQ(ierr);
656   ierr = TaoComputeHessian(tao,tao->solution,tao->hessian,tao->hessian_pre);CHKERRQ(ierr);
657   if (ipmP->me > 0) {
658     ierr = TaoComputeEqualityConstraints(tao,tao->solution,tao->constraints_equality);
659     ierr = TaoComputeJacobianEquality(tao,tao->solution,tao->jacobian_equality,tao->jacobian_equality_pre);CHKERRQ(ierr);
660   }
661   if (ipmP->mi > 0) {
662     ierr = TaoComputeInequalityConstraints(tao,tao->solution,tao->constraints_inequality);
663     ierr = TaoComputeJacobianInequality(tao,tao->solution,tao->jacobian_inequality,tao->jacobian_inequality_pre);CHKERRQ(ierr);
664   }
665   if (ipmP->nb > 0) {
666     /* Ai' =   jac_ineq | I (w/lb) | -I (w/ub)  */
667     ierr = IPMUpdateAi(tao);CHKERRQ(ierr);
668   }
669   PetscFunctionReturn(0);
670 }
671 
672 #undef __FUNCT__
673 #define __FUNCT__ "IPMPushInitialPoint"
674 /* Push initial point away from bounds */
675 PetscErrorCode IPMPushInitialPoint(Tao tao)
676 {
677   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
678   PetscErrorCode ierr;
679 
680   PetscFunctionBegin;
681   ierr = TaoComputeVariableBounds(tao);CHKERRQ(ierr);
682   if (tao->XL && tao->XU) {
683     ierr = VecMedian(tao->XL, tao->solution, tao->XU, tao->solution);CHKERRQ(ierr);
684   }
685   if (ipmP->nb > 0) {
686     ierr = VecSet(ipmP->s,ipmP->pushs);CHKERRQ(ierr);
687     ierr = VecSet(ipmP->lamdai,ipmP->pushnu);CHKERRQ(ierr);
688     if (ipmP->mi > 0) {
689       ierr = VecSet(tao->DI,ipmP->pushnu);CHKERRQ(ierr);
690     }
691   }
692   if (ipmP->me > 0) {
693     ierr = VecSet(tao->DE,1.0);CHKERRQ(ierr);
694     ierr = VecSet(ipmP->lamdae,1.0);CHKERRQ(ierr);
695   }
696   PetscFunctionReturn(0);
697 }
698 
699 #undef __FUNCT__
700 #define __FUNCT__ "IPMUpdateAi"
701 PetscErrorCode IPMUpdateAi(Tao tao)
702 {
703   /* Ai =     Ji
704               I (w/lb)
705              -I (w/ub) */
706 
707   /* Ci =    user->ci
708              Xi - lb (w/lb)
709              -Xi + ub (w/ub)  */
710 
711   TAO_IPM           *ipmP = (TAO_IPM *)tao->data;
712   MPI_Comm          comm;
713   PetscInt          i;
714   PetscScalar       newval;
715   PetscInt          newrow,newcol,ncols;
716   const PetscScalar *vals;
717   const PetscInt    *cols;
718   PetscInt          astart,aend,jstart,jend;
719   PetscInt          *nonzeros;
720   PetscInt          r2,r3,r4;
721   PetscMPIInt       size;
722   PetscErrorCode    ierr;
723 
724   PetscFunctionBegin;
725   r2 = ipmP->mi;
726   r3 = r2 + ipmP->nxlb;
727   r4 = r3 + ipmP->nxub;
728 
729   if (!ipmP->nb) PetscFunctionReturn(0);
730 
731   /* Create Ai matrix if it doesn't exist yet */
732   if (!ipmP->Ai) {
733     comm = ((PetscObject)(tao->solution))->comm;
734     ierr = PetscMalloc1(ipmP->nb,&nonzeros);CHKERRQ(ierr);
735     ierr = MPI_Comm_size(comm,&size);
736     if (size == 1) {
737       for (i=0;i<ipmP->mi;i++) {
738         ierr = MatGetRow(tao->jacobian_inequality,i,&ncols,NULL,NULL);CHKERRQ(ierr);
739         nonzeros[i] = ncols;
740         ierr = MatRestoreRow(tao->jacobian_inequality,i,&ncols,NULL,NULL);CHKERRQ(ierr);
741       }
742       for (i=r2;i<r4;i++) {
743         nonzeros[i] = 1;
744       }
745     }
746     ierr = MatCreate(comm,&ipmP->Ai);CHKERRQ(ierr);
747     ierr = MatSetType(ipmP->Ai,MATAIJ);CHKERRQ(ierr);
748     ierr = MatSetSizes(ipmP->Ai,PETSC_DECIDE,PETSC_DECIDE,ipmP->nb,ipmP->n);CHKERRQ(ierr);
749     ierr = MatSetFromOptions(ipmP->Ai);CHKERRQ(ierr);
750     ierr = MatMPIAIJSetPreallocation(ipmP->Ai,ipmP->nb,NULL,ipmP->nb,NULL);
751     ierr = MatSeqAIJSetPreallocation(ipmP->Ai,PETSC_DEFAULT,nonzeros);CHKERRQ(ierr);
752     if (size ==1) {
753       ierr = PetscFree(nonzeros);CHKERRQ(ierr);
754     }
755   }
756 
757   /* Copy values from user jacobian to Ai */
758   ierr = MatGetOwnershipRange(ipmP->Ai,&astart,&aend);CHKERRQ(ierr);
759 
760   /* Ai w/lb */
761   if (ipmP->mi) {
762     ierr = MatZeroEntries(ipmP->Ai);CHKERRQ(ierr);
763     ierr = MatGetOwnershipRange(tao->jacobian_inequality,&jstart,&jend);CHKERRQ(ierr);
764     for (i=jstart;i<jend;i++) {
765       ierr = MatGetRow(tao->jacobian_inequality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
766       newrow = i;
767       ierr = MatSetValues(ipmP->Ai,1,&newrow,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
768       ierr = MatRestoreRow(tao->jacobian_inequality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
769     }
770   }
771 
772   /* I w/ xlb */
773   if (ipmP->nxlb) {
774     for (i=0;i<ipmP->nxlb;i++) {
775       if (i>=astart && i<aend) {
776         newrow = i+r2;
777         newcol = i;
778         newval = 1.0;
779         ierr = MatSetValues(ipmP->Ai,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
780       }
781     }
782   }
783   if (ipmP->nxub) {
784     /* I w/ xub */
785     for (i=0;i<ipmP->nxub;i++) {
786       if (i>=astart && i<aend) {
787       newrow = i+r3;
788       newcol = i;
789       newval = -1.0;
790       ierr = MatSetValues(ipmP->Ai,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
791       }
792     }
793   }
794 
795   ierr = MatAssemblyBegin(ipmP->Ai,MAT_FINAL_ASSEMBLY);
796   ierr = MatAssemblyEnd(ipmP->Ai,MAT_FINAL_ASSEMBLY);
797   CHKMEMQ;
798 
799   ierr = VecSet(ipmP->ci,0.0);CHKERRQ(ierr);
800 
801   /* user ci */
802   if (ipmP->mi > 0) {
803     ierr = VecScatterBegin(ipmP->ci_scat,tao->constraints_inequality,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
804     ierr = VecScatterEnd(ipmP->ci_scat,tao->constraints_inequality,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
805   }
806   if (!ipmP->work){
807     VecDuplicate(tao->solution,&ipmP->work);
808   }
809   ierr = VecCopy(tao->solution,ipmP->work);CHKERRQ(ierr);
810   if (tao->XL) {
811     ierr = VecAXPY(ipmP->work,-1.0,tao->XL);CHKERRQ(ierr);
812 
813     /* lower bounds on variables */
814     if (ipmP->nxlb > 0) {
815       ierr = VecScatterBegin(ipmP->xl_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
816       ierr = VecScatterEnd(ipmP->xl_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
817     }
818   }
819   if (tao->XU) {
820     /* upper bounds on variables */
821     ierr = VecCopy(tao->solution,ipmP->work);CHKERRQ(ierr);
822     ierr = VecScale(ipmP->work,-1.0);CHKERRQ(ierr);
823     ierr = VecAXPY(ipmP->work,1.0,tao->XU);CHKERRQ(ierr);
824     if (ipmP->nxub > 0) {
825       ierr = VecScatterBegin(ipmP->xu_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
826       ierr = VecScatterEnd(ipmP->xu_scat,ipmP->work,ipmP->ci,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
827     }
828   }
829   PetscFunctionReturn(0);
830 }
831 
832 #undef __FUNCT__
833 #define __FUNCT__ "IPMUpdateK"
834 /* create K = [ Hlag , 0 , Ae', -Ai'];
835               [Ae , 0,   0  , 0];
836               [Ai ,-I,   0 ,  0];
837               [ 0 , S ,  0,   Y ];  */
838 PetscErrorCode IPMUpdateK(Tao tao)
839 {
840   TAO_IPM         *ipmP = (TAO_IPM *)tao->data;
841   MPI_Comm        comm;
842   PetscMPIInt     size;
843   PetscErrorCode  ierr;
844   PetscInt        i,j,row;
845   PetscInt        ncols,newcol,newcols[2],newrow;
846   const PetscInt  *cols;
847   const PetscReal *vals;
848   const PetscReal *l,*y;
849   PetscReal       *newvals;
850   PetscReal       newval;
851   PetscInt        subsize;
852   const PetscInt  *indices;
853   PetscInt        *nonzeros,*d_nonzeros,*o_nonzeros;
854   PetscInt        bigsize;
855   PetscInt        r1,r2,r3;
856   PetscInt        c1,c2,c3;
857   PetscInt        klocalsize;
858   PetscInt        hstart,hend,kstart,kend;
859   PetscInt        aistart,aiend,aestart,aeend;
860   PetscInt        sstart,send;
861 
862   PetscFunctionBegin;
863   comm = ((PetscObject)(tao->solution))->comm;
864   ierr = MPI_Comm_size(comm,&size);CHKERRQ(ierr);
865   ierr = IPMUpdateAi(tao);CHKERRQ(ierr);
866 
867   /* allocate workspace */
868   subsize = PetscMax(ipmP->n,ipmP->nb);
869   subsize = PetscMax(ipmP->me,subsize);
870   subsize = PetscMax(2,subsize);
871   ierr = PetscMalloc1(subsize,&indices);CHKERRQ(ierr);
872   ierr = PetscMalloc1(subsize,&newvals);CHKERRQ(ierr);
873 
874   r1 = c1 = ipmP->n;
875   r2 = r1 + ipmP->me;  c2 = c1 + ipmP->nb;
876   r3 = c3 = r2 + ipmP->nb;
877 
878   bigsize = ipmP->n+2*ipmP->nb+ipmP->me;
879   ierr = VecGetOwnershipRange(ipmP->bigrhs,&kstart,&kend);CHKERRQ(ierr);
880   ierr = MatGetOwnershipRange(tao->hessian,&hstart,&hend);CHKERRQ(ierr);
881   klocalsize = kend-kstart;
882   if (!ipmP->K) {
883     if (size == 1) {
884       ierr = PetscMalloc1((kend-kstart),&nonzeros);CHKERRQ(ierr);
885       for (i=0;i<bigsize;i++) {
886         if (i<r1) {
887           ierr = MatGetRow(tao->hessian,i,&ncols,NULL,NULL);CHKERRQ(ierr);
888           nonzeros[i] = ncols;
889           ierr = MatRestoreRow(tao->hessian,i,&ncols,NULL,NULL);CHKERRQ(ierr);
890           nonzeros[i] += ipmP->me+ipmP->nb;
891         } else if (i<r2) {
892           nonzeros[i-kstart] = ipmP->n;
893         } else if (i<r3) {
894           nonzeros[i-kstart] = ipmP->n+1;
895         } else if (i<bigsize) {
896           nonzeros[i-kstart] = 2;
897         }
898       }
899       ierr = MatCreate(comm,&ipmP->K);CHKERRQ(ierr);
900       ierr = MatSetType(ipmP->K,MATSEQAIJ);CHKERRQ(ierr);
901       ierr = MatSetSizes(ipmP->K,klocalsize,klocalsize,PETSC_DETERMINE,PETSC_DETERMINE);CHKERRQ(ierr);
902       ierr = MatSeqAIJSetPreallocation(ipmP->K,0,nonzeros);CHKERRQ(ierr);
903       ierr = MatSetFromOptions(ipmP->K);CHKERRQ(ierr);
904       ierr = PetscFree(nonzeros);CHKERRQ(ierr);
905     } else {
906       ierr = PetscMalloc1((kend-kstart),&d_nonzeros);CHKERRQ(ierr);
907       ierr = PetscMalloc1((kend-kstart),&o_nonzeros);CHKERRQ(ierr);
908       for (i=kstart;i<kend;i++) {
909         if (i<r1) {
910           /* TODO fix preallocation for mpi mats */
911           d_nonzeros[i-kstart] = PetscMin(ipmP->n+ipmP->me+ipmP->nb,kend-kstart);
912           o_nonzeros[i-kstart] = PetscMin(ipmP->n+ipmP->me+ipmP->nb,bigsize-(kend-kstart));
913         } else if (i<r2) {
914           d_nonzeros[i-kstart] = PetscMin(ipmP->n,kend-kstart);
915           o_nonzeros[i-kstart] = PetscMin(ipmP->n,bigsize-(kend-kstart));
916         } else if (i<r3) {
917           d_nonzeros[i-kstart] = PetscMin(ipmP->n+2,kend-kstart);
918           o_nonzeros[i-kstart] = PetscMin(ipmP->n+2,bigsize-(kend-kstart));
919         } else {
920           d_nonzeros[i-kstart] = PetscMin(2,kend-kstart);
921           o_nonzeros[i-kstart] = PetscMin(2,bigsize-(kend-kstart));
922         }
923       }
924       ierr = MatCreate(comm,&ipmP->K);CHKERRQ(ierr);
925       ierr = MatSetType(ipmP->K,MATMPIAIJ);CHKERRQ(ierr);
926       ierr = MatSetSizes(ipmP->K,klocalsize,klocalsize,PETSC_DETERMINE,PETSC_DETERMINE);CHKERRQ(ierr);
927       ierr = MatMPIAIJSetPreallocation(ipmP->K,0,d_nonzeros,0,o_nonzeros);CHKERRQ(ierr);
928       ierr = PetscFree(d_nonzeros);CHKERRQ(ierr);
929       ierr = PetscFree(o_nonzeros);CHKERRQ(ierr);
930       ierr = MatSetFromOptions(ipmP->K);CHKERRQ(ierr);
931     }
932   }
933 
934   ierr = MatZeroEntries(ipmP->K);CHKERRQ(ierr);
935   /* Copy H */
936   for (i=hstart;i<hend;i++) {
937     ierr = MatGetRow(tao->hessian,i,&ncols,&cols,&vals);CHKERRQ(ierr);
938     if (ncols > 0) {
939       ierr = MatSetValues(ipmP->K,1,&i,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
940     }
941     ierr = MatRestoreRow(tao->hessian,i,&ncols,&cols,&vals);CHKERRQ(ierr);
942   }
943 
944   /* Copy Ae and Ae' */
945   if (ipmP->me > 0) {
946     ierr = MatGetOwnershipRange(tao->jacobian_equality,&aestart,&aeend);CHKERRQ(ierr);
947     for (i=aestart;i<aeend;i++) {
948       ierr = MatGetRow(tao->jacobian_equality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
949       if (ncols > 0) {
950         /*Ae*/
951         row = i+r1;
952         ierr = MatSetValues(ipmP->K,1,&row,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
953         /*Ae'*/
954         for (j=0;j<ncols;j++) {
955           newcol = i + c2;
956           newrow = cols[j];
957           newval = vals[j];
958           ierr = MatSetValues(ipmP->K,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
959         }
960       }
961       ierr = MatRestoreRow(tao->jacobian_equality,i,&ncols,&cols,&vals);CHKERRQ(ierr);
962     }
963   }
964 
965   if (ipmP->nb > 0) {
966     ierr = MatGetOwnershipRange(ipmP->Ai,&aistart,&aiend);CHKERRQ(ierr);
967     /* Copy Ai,and Ai' */
968     for (i=aistart;i<aiend;i++) {
969       row = i+r2;
970       ierr = MatGetRow(ipmP->Ai,i,&ncols,&cols,&vals);CHKERRQ(ierr);
971       if (ncols > 0) {
972         /*Ai*/
973         ierr = MatSetValues(ipmP->K,1,&row,ncols,cols,vals,INSERT_VALUES);CHKERRQ(ierr);
974         /*-Ai'*/
975         for (j=0;j<ncols;j++) {
976           newcol = i + c3;
977           newrow = cols[j];
978           newval = -vals[j];
979           ierr = MatSetValues(ipmP->K,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
980         }
981       }
982       ierr = MatRestoreRow(ipmP->Ai,i,&ncols,&cols,&vals);CHKERRQ(ierr);
983     }
984 
985     /* -I */
986     for (i=kstart;i<kend;i++) {
987       if (i>=r2 && i<r3) {
988         newrow = i;
989         newcol = i-r2+c1;
990         newval = -1.0;
991         MatSetValues(ipmP->K,1,&newrow,1,&newcol,&newval,INSERT_VALUES);CHKERRQ(ierr);
992       }
993     }
994 
995     /* Copy L,Y */
996     ierr = VecGetOwnershipRange(ipmP->s,&sstart,&send);CHKERRQ(ierr);
997     ierr = VecGetArrayRead(ipmP->lamdai,&l);CHKERRQ(ierr);
998     ierr = VecGetArrayRead(ipmP->s,&y);CHKERRQ(ierr);
999 
1000     for (i=sstart;i<send;i++) {
1001       newcols[0] = c1+i;
1002       newcols[1] = c3+i;
1003       newvals[0] = l[i-sstart];
1004       newvals[1] = y[i-sstart];
1005       newrow = r3+i;
1006       ierr = MatSetValues(ipmP->K,1,&newrow,2,newcols,newvals,INSERT_VALUES);CHKERRQ(ierr);
1007     }
1008 
1009     ierr = VecRestoreArrayRead(ipmP->lamdai,&l);CHKERRQ(ierr);
1010     ierr = VecRestoreArrayRead(ipmP->s,&y);CHKERRQ(ierr);
1011   }
1012 
1013   ierr = PetscFree(indices);CHKERRQ(ierr);
1014   ierr = PetscFree(newvals);CHKERRQ(ierr);
1015   ierr = MatAssemblyBegin(ipmP->K,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
1016   ierr = MatAssemblyEnd(ipmP->K,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
1017   PetscFunctionReturn(0);
1018 }
1019 
1020 #undef __FUNCT__
1021 #define __FUNCT__ "IPMGatherRHS"
1022 PetscErrorCode IPMGatherRHS(Tao tao,Vec RHS,Vec X1,Vec X2,Vec X3,Vec X4)
1023 {
1024   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
1025   PetscErrorCode ierr;
1026 
1027   PetscFunctionBegin;
1028   /* rhs = [x1      (n)
1029             x2     (me)
1030             x3     (nb)
1031             x4     (nb)] */
1032   if (X1) {
1033     ierr = VecScatterBegin(ipmP->rhs1,X1,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1034     ierr = VecScatterEnd(ipmP->rhs1,X1,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1035   }
1036   if (ipmP->me > 0 && X2) {
1037     ierr = VecScatterBegin(ipmP->rhs2,X2,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1038     ierr = VecScatterEnd(ipmP->rhs2,X2,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1039   }
1040   if (ipmP->nb > 0) {
1041     if (X3) {
1042       ierr = VecScatterBegin(ipmP->rhs3,X3,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1043       ierr = VecScatterEnd(ipmP->rhs3,X3,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1044     }
1045     if (X4) {
1046       ierr = VecScatterBegin(ipmP->rhs4,X4,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1047       ierr = VecScatterEnd(ipmP->rhs4,X4,RHS,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1048     }
1049   }
1050   PetscFunctionReturn(0);
1051 }
1052 
1053 #undef __FUNCT__
1054 #define __FUNCT__ "IPMScatterStep"
1055 PetscErrorCode IPMScatterStep(Tao tao, Vec STEP, Vec X1, Vec X2, Vec X3, Vec X4)
1056 {
1057   TAO_IPM        *ipmP = (TAO_IPM *)tao->data;
1058   PetscErrorCode ierr;
1059 
1060   PetscFunctionBegin;
1061   CHKMEMQ;
1062   /*        [x1    (n)
1063              x2    (nb) may be 0
1064              x3    (me) may be 0
1065              x4    (nb) may be 0 */
1066   if (X1) {
1067     ierr = VecScatterBegin(ipmP->step1,STEP,X1,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1068     ierr = VecScatterEnd(ipmP->step1,STEP,X1,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1069   }
1070   if (X2 && ipmP->nb > 0) {
1071     ierr = VecScatterBegin(ipmP->step2,STEP,X2,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1072     ierr = VecScatterEnd(ipmP->step2,STEP,X2,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1073   }
1074   if (X3 && ipmP->me > 0) {
1075     ierr = VecScatterBegin(ipmP->step3,STEP,X3,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1076     ierr = VecScatterEnd(ipmP->step3,STEP,X3,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1077   }
1078   if (X4 && ipmP->nb > 0) {
1079     ierr = VecScatterBegin(ipmP->step4,STEP,X4,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1080     ierr = VecScatterEnd(ipmP->step4,STEP,X4,INSERT_VALUES,SCATTER_FORWARD);CHKERRQ(ierr);
1081   }
1082   CHKMEMQ;
1083   PetscFunctionReturn(0);
1084 }
1085 
1086 /*MC
1087   TAOIPM - Interior point algorithm for generally constrained optimization.
1088 
1089   Option Database Keys:
1090 +   -tao_ipm_pushnu - parameter to push initial dual variables away from bounds
1091 .   -tao_ipm_pushs - parameter to push initial slack variables away from bounds
1092 
1093   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.
1094   Level: beginner
1095 
1096 M*/
1097 
1098 EXTERN_C_BEGIN
1099 #undef __FUNCT__
1100 #define __FUNCT__ "TaoCreate_IPM"
1101 PetscErrorCode TaoCreate_IPM(Tao tao)
1102 {
1103   TAO_IPM        *ipmP;
1104   PetscErrorCode ierr;
1105 
1106   PetscFunctionBegin;
1107   tao->ops->setup = TaoSetup_IPM;
1108   tao->ops->solve = TaoSolve_IPM;
1109   tao->ops->view = TaoView_IPM;
1110   tao->ops->setfromoptions = TaoSetFromOptions_IPM;
1111   tao->ops->destroy = TaoDestroy_IPM;
1112   /* tao->ops->computedual = TaoComputeDual_IPM; */
1113 
1114   ierr = PetscNewLog(tao,&ipmP);CHKERRQ(ierr);
1115   tao->data = (void*)ipmP;
1116   tao->max_it = 200;
1117   tao->max_funcs = 500;
1118   tao->fatol = 1e-4;
1119   tao->frtol = 1e-4;
1120   ipmP->dec = 10000; /* line search critera */
1121   ipmP->taumin = 0.995;
1122   ipmP->monitorkkt = PETSC_FALSE;
1123   ipmP->pushs = 100;
1124   ipmP->pushnu = 100;
1125   ierr = KSPCreate(((PetscObject)tao)->comm, &tao->ksp);CHKERRQ(ierr);
1126   PetscFunctionReturn(0);
1127 }
1128 EXTERN_C_END
1129 
1130