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