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