1 /* SLSQP: Sequentional Least Squares Programming (aka sequential quadratic programming SQP)
2 method for nonlinearly constrained nonlinear optimization, by Dieter Kraft (1991).
3 Fortran released under a free (BSD) license by ACM to the SciPy project and used there.
4 C translation via f2c + hand-cleanup and incorporation into NLopt by S. G. Johnson (2009). */
6 /* Table of constant values */
14 /* ALGORITHM 733, COLLECTED ALGORITHMS FROM ACM. */
15 /* TRANSACTIONS ON MATHEMATICAL SOFTWARE, */
16 /* VOL. 20, NO. 3, SEPTEMBER, 1994, PP. 262-281. */
17 /* http://doi.acm.org/10.1145/192115.192124 */
20 /* http://permalink.gmane.org/gmane.comp.python.scientific.devel/6725 */
22 /* From: Deborah Cotton <cotton@hq.acm.org> */
23 /* Date: Fri, 14 Sep 2007 12:35:55 -0500 */
24 /* Subject: RE: Algorithm License requested */
29 /* In that case, then because the author consents to [the ACM] releasing */
30 /* the code currently archived at http://www.netlib.org/toms/733 under the */
31 /* BSD license, the ACM hereby releases this code under the BSD license. */
35 /* Deborah Cotton, Copyright & Permissions */
36 /* ACM Publications */
37 /* 2 Penn Plaza, Suite 701** */
38 /* New York, NY 10121-0701 */
39 /* permissions@acm.org */
40 /* 212.869.7440 ext. 652 */
41 /* Fax. 212.869.0481 */
44 /********************************* BLAS1 routines *************************/
46 /* COPIES A VECTOR, X, TO A VECTOR, Y, with the given increments */
47 static void dcopy___(int *n_, const double *dx, int incx,
53 if (incx == 1 && incy == 1)
54 memcpy(dy, dx, sizeof(double) * ((unsigned) n));
55 else if (incx == 0 && incy == 1) {
57 for (i = 0; i < n; ++i) dy[i] = x;
60 for (i = 0; i < n; ++i) dy[i*incy] = dx[i*incx];
64 /* CONSTANT TIMES A VECTOR PLUS A VECTOR. */
65 static void daxpy_sl__(int *n_, const double *da_, const double *dx,
66 int incx, double *dy, int incy)
71 if (n <= 0 || da == 0) return;
72 for (i = 0; i < n; ++i) dy[i*incy] += da * dx[i*incx];
75 /* dot product dx dot dy. */
76 static double ddot_sl__(int *n_, double *dx, int incx, double *dy, int incy)
81 for (i = 0; i < n; ++i) sum += dx[i*incx] * dy[i*incy];
85 /* compute the L2 norm of array DX of length N, stride INCX */
86 static double dnrm2___(int *n_, double *dx, int incx)
89 double xmax = 0, scale;
91 for (i = 0; i < n; ++i) {
92 double xabs = fabs(dx[incx*i]);
93 if (xmax < xabs) xmax = xabs;
95 if (xmax == 0) return 0;
97 for (i = 0; i < n; ++i) {
98 double xs = scale * dx[incx*i];
101 return xmax * sqrt((double) sum);
104 /* apply Givens rotation */
105 static void dsrot_(int n, double *dx, int incx,
106 double *dy, int incy, double *c__, double *s_)
109 double c = *c__, s = *s_;
111 for (i = 0; i < n; ++i) {
112 double x = dx[incx*i], y = dy[incy*i];
113 dx[incx*i] = c * x + s * y;
114 dy[incy*i] = c * y - s * x;
118 /* construct Givens rotation */
119 static void dsrotg_(double *da, double *db, double *c, double *s)
121 double absa, absb, roe, scale;
123 absa = fabs(*da); absb = fabs(*db);
134 double r, iscale = 1 / scale;
135 double tmpa = (*da) * iscale, tmpb = (*db) * iscale;
136 r = (roe < 0 ? -scale : scale) * sqrt((tmpa * tmpa) + (tmpb * tmpb));
137 *c = *da / r; *s = *db / r;
139 if (*c != 0 && fabs(*c) <= *s) *db = 1 / *c;
148 /* scales vector X(n) by constant da */
149 static void dscal_sl__(int *n_, const double *da, double *dx, int incx)
153 for (i = 0; i < n; ++i) dx[i*incx] *= alpha;
156 /**************************************************************************/
158 static const int c__0 = 0;
159 static const int c__1 = 1;
160 static const int c__2 = 2;
162 #define MIN2(a,b) ((a) <= (b) ? (a) : (b))
163 #define MAX2(a,b) ((a) >= (b) ? (a) : (b))
165 static void h12_(const int *mode, int *lpivot, int *l1,
166 int *m, double *u, const int *iue, double *up,
167 double *c__, const int *ice, const int *icv, const int *ncv)
169 /* Initialized data */
171 const double one = 1.;
173 /* System generated locals */
174 int u_dim1, u_offset, i__1, i__2;
177 /* Local variables */
179 int i__, j, i2, i3, i4;
184 /* C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12 */
185 /* TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974 */
186 /* CONSTRUCTION AND/OR APPLICATION OF A SINGLE */
187 /* HOUSEHOLDER TRANSFORMATION Q = I + U*(U**T)/B */
188 /* MODE = 1 OR 2 TO SELECT ALGORITHM H1 OR H2 . */
189 /* LPIVOT IS THE INDEX OF THE PIVOT ELEMENT. */
190 /* L1,M IF L1 <= M THE TRANSFORMATION WILL BE CONSTRUCTED TO */
191 /* ZERO ELEMENTS INDEXED FROM L1 THROUGH M. */
192 /* IF L1 > M THE SUBROUTINE DOES AN IDENTITY TRANSFORMATION. */
194 /* ON ENTRY TO H1 U() STORES THE PIVOT VECTOR. */
195 /* IUE IS THE STORAGE INCREMENT BETWEEN ELEMENTS. */
196 /* ON EXIT FROM H1 U() AND UP STORE QUANTITIES DEFINING */
197 /* THE VECTOR U OF THE HOUSEHOLDER TRANSFORMATION. */
198 /* ON ENTRY TO H2 U() AND UP */
199 /* SHOULD STORE QUANTITIES PREVIOUSLY COMPUTED BY H1. */
200 /* THESE WILL NOT BE MODIFIED BY H2. */
201 /* C() ON ENTRY TO H1 OR H2 C() STORES A MATRIX WHICH WILL BE */
202 /* REGARDED AS A SET OF VECTORS TO WHICH THE HOUSEHOLDER */
203 /* TRANSFORMATION IS TO BE APPLIED. */
204 /* ON EXIT C() STORES THE SET OF TRANSFORMED VECTORS. */
205 /* ICE STORAGE INCREMENT BETWEEN ELEMENTS OF VECTORS IN C(). */
206 /* ICV STORAGE INCREMENT BETWEEN VECTORS IN C(). */
207 /* NCV NUMBER OF VECTORS IN C() TO BE TRANSFORMED. */
208 /* IF NCV <= 0 NO OPERATIONS WILL BE DONE ON C(). */
209 /* Parameter adjustments */
211 u_offset = 1 + u_dim1;
216 if (0 >= *lpivot || *lpivot >= *l1 || *l1 > *m) {
219 cl = (d__1 = u[*lpivot * u_dim1 + 1], fabs(d__1));
223 /* ****** CONSTRUCT THE TRANSFORMATION ****** */
225 for (j = *l1; j <= i__1; ++j) {
226 sm = (d__1 = u[j * u_dim1 + 1], fabs(d__1));
234 /* Computing 2nd power */
235 d__1 = u[*lpivot * u_dim1 + 1] * clinv;
238 for (j = *l1; j <= i__1; ++j) {
240 /* Computing 2nd power */
241 d__1 = u[j * u_dim1 + 1] * clinv;
245 if (u[*lpivot * u_dim1 + 1] > 0.0) {
248 *up = u[*lpivot * u_dim1 + 1] - cl;
249 u[*lpivot * u_dim1 + 1] = cl;
251 /* ****** APPLY THE TRANSFORMATION I+U*(U**T)/B TO C ****** */
260 b = *up * u[*lpivot * u_dim1 + 1];
265 i2 = 1 - *icv + *ice * (*lpivot - 1);
266 incr = *ice * (*l1 - *lpivot);
268 for (j = 1; j <= i__1; ++j) {
274 for (i__ = *l1; i__ <= i__2; ++i__) {
275 sm += c__[i3] * u[i__ * u_dim1 + 1];
285 for (i__ = *l1; i__ <= i__2; ++i__) {
286 c__[i4] += sm * u[i__ * u_dim1 + 1];
297 static void nnls_(double *a, int *mda, int *m, int *
298 n, double *b, double *x, double *rnorm, double *w,
299 double *z__, int *indx, int *mode)
301 /* Initialized data */
303 const double one = 1.;
304 const double factor = .01;
306 /* System generated locals */
307 int a_dim1, a_offset, i__1, i__2;
310 /* Local variables */
314 int ii, jj, ip, iz, jz;
316 int iz1, iz2, npp1, iter;
317 double wmax, alpha, asave;
318 int itmax, izmax = 0, nsetp;
321 /* C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY: */
322 /* 'SOLVING LEAST SQUARES PROBLEMS'. PRENTICE-HALL.1974 */
323 /* ********** NONNEGATIVE LEAST SQUARES ********** */
324 /* GIVEN AN M BY N MATRIX, A, AND AN M-VECTOR, B, COMPUTE AN */
325 /* N-VECTOR, X, WHICH SOLVES THE LEAST SQUARES PROBLEM */
326 /* A*X = B SUBJECT TO X >= 0 */
328 /* MDA IS THE FIRST DIMENSIONING PARAMETER FOR THE ARRAY,A(). */
329 /* ON ENTRY A() CONTAINS THE M BY N MATRIX,A. */
330 /* ON EXIT A() CONTAINS THE PRODUCT Q*A, */
331 /* WHERE Q IS AN M BY M ORTHOGONAL MATRIX GENERATED */
332 /* IMPLICITLY BY THIS SUBROUTINE. */
333 /* EITHER M>=N OR M<N IS PERMISSIBLE. */
334 /* THERE IS NO RESTRICTION ON THE RANK OF A. */
335 /* B() ON ENTRY B() CONTAINS THE M-VECTOR, B. */
336 /* ON EXIT B() CONTAINS Q*B. */
337 /* X() ON ENTRY X() NEED NOT BE INITIALIZED. */
338 /* ON EXIT X() WILL CONTAIN THE SOLUTION VECTOR. */
339 /* RNORM ON EXIT RNORM CONTAINS THE EUCLIDEAN NORM OF THE */
340 /* RESIDUAL VECTOR. */
341 /* W() AN N-ARRAY OF WORKING SPACE. */
342 /* ON EXIT W() WILL CONTAIN THE DUAL SOLUTION VECTOR. */
343 /* W WILL SATISFY W(I)=0 FOR ALL I IN SET P */
344 /* AND W(I)<=0 FOR ALL I IN SET Z */
345 /* Z() AN M-ARRAY OF WORKING SPACE. */
346 /* INDX()AN INT WORKING ARRAY OF LENGTH AT LEAST N. */
347 /* ON EXIT THE CONTENTS OF THIS ARRAY DEFINE THE SETS */
348 /* P AND Z AS FOLLOWS: */
349 /* INDX(1) THRU INDX(NSETP) = SET P. */
350 /* INDX(IZ1) THRU INDX (IZ2) = SET Z. */
351 /* IZ1=NSETP + 1 = NPP1, IZ2=N. */
352 /* MODE THIS IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANING: */
353 /* 1 THE SOLUTION HAS BEEN COMPUTED SUCCESSFULLY. */
354 /* 2 THE DIMENSIONS OF THE PROBLEM ARE WRONG, */
355 /* EITHER M <= 0 OR N <= 0. */
356 /* 3 ITERATION COUNT EXCEEDED, MORE THAN 3*N ITERATIONS. */
357 /* Parameter adjustments */
364 a_offset = 1 + a_dim1;
368 /* revised Dieter Kraft, March 1983 */
370 if (*m <= 0 || *n <= 0) {
376 /* STEP ONE (INITIALIZE) */
378 for (i__ = 1; i__ <= i__1; ++i__) {
387 dcopy___(n, &x[1], 0, &x[1], 1);
388 /* STEP TWO (COMPUTE DUAL VARIABLES) */
389 /* .....ENTRY LOOP A */
391 if (iz1 > iz2 || nsetp >= *m) {
395 for (iz = iz1; iz <= i__1; ++iz) {
399 w[j] = ddot_sl__(&i__2, &a[npp1 + j * a_dim1], 1, &b[npp1], 1)
402 /* STEP THREE (TEST DUAL VARIABLES) */
406 for (iz = iz1; iz <= i__2; ++iz) {
416 /* .....EXIT LOOP A */
422 /* STEP FOUR (TEST INDX J FOR LINEAR DEPENDENCY) */
423 asave = a[npp1 + j * a_dim1];
425 h12_(&c__1, &npp1, &i__2, m, &a[j * a_dim1 + 1], &c__1, &up, &z__[1], &
427 unorm = dnrm2___(&nsetp, &a[j * a_dim1 + 1], 1);
428 t = factor * (d__1 = a[npp1 + j * a_dim1], fabs(d__1));
430 if (d__1 - unorm <= 0.0) {
433 dcopy___(m, &b[1], 1, &z__[1], 1);
435 h12_(&c__2, &npp1, &i__2, m, &a[j * a_dim1 + 1], &c__1, &up, &z__[1], &
437 if (z__[npp1] / a[npp1 + j * a_dim1] > 0.0) {
441 a[npp1 + j * a_dim1] = asave;
444 /* STEP FIVE (ADD COLUMN) */
446 dcopy___(m, &z__[1], 1, &b[1], 1);
447 indx[iz] = indx[iz1];
453 for (jz = iz1; jz <= i__2; ++jz) {
456 h12_(&c__2, &nsetp, &npp1, m, &a[j * a_dim1 + 1], &c__1, &up, &a[jj *
457 a_dim1 + 1], &c__1, mda, &c__1);
462 dcopy___(&i__2, &w[j], 0, &a[k + j * a_dim1], 1);
463 /* STEP SIX (SOLVE LEAST SQUARES SUB-PROBLEM) */
464 /* .....ENTRY LOOP B */
466 for (ip = nsetp; ip >= 1; --ip) {
471 daxpy_sl__(&ip, &d__1, &a[jj * a_dim1 + 1], 1, &z__[1], 1);
475 z__[ip] /= a[ip + jj * a_dim1];
484 /* STEP SEVEN TO TEN (STEP LENGTH ALGORITHM) */
489 for (ip = 1; ip <= i__2; ++ip) {
494 t = -x[l] / (z__[ip] - x[l]);
504 for (ip = 1; ip <= i__2; ++ip) {
507 x[l] = (one - alpha) * x[l] + alpha * z__[ip];
509 /* .....EXIT LOOP B */
513 /* STEP ELEVEN (DELETE COLUMN) */
519 for (j = jj; j <= i__2; ++j) {
522 dsrotg_(&a[j - 1 + ii * a_dim1], &a[j + ii * a_dim1], &c__, &s);
523 t = a[j - 1 + ii * a_dim1];
524 dsrot_(*n, &a[j - 1 + a_dim1], *mda, &a[j + a_dim1], *mda, &c__, &s);
525 a[j - 1 + ii * a_dim1] = t;
526 a[j + ii * a_dim1] = 0.0;
528 dsrot_(1, &b[j - 1], 1, &b[j], 1, &c__, &s);
538 for (jj = 1; jj <= i__2; ++jj) {
545 dcopy___(m, &b[1], 1, &z__[1], 1);
547 /* STEP TWELVE (SOLUTION) */
551 *rnorm = dnrm2___(&i__2, &b[k], 1);
554 dcopy___(n, &w[1], 0, &w[1], 1);
556 /* END OF SUBROUTINE NNLS */
561 static void ldp_(double *g, int *mg, int *m, int *n,
562 double *h__, double *x, double *xnorm, double *w,
563 int *indx, int *mode)
565 /* Initialized data */
567 const double one = 1.;
569 /* System generated locals */
570 int g_dim1, g_offset, i__1, i__2;
573 /* Local variables */
574 int i__, j, n1, if__, iw, iy, iz;
580 /* MINIMIZE 1/2 X X SUBJECT TO G * X >= H. */
581 /* C.L. LAWSON, R.J. HANSON: 'SOLVING LEAST SQUARES PROBLEMS' */
582 /* PRENTICE HALL, ENGLEWOOD CLIFFS, NEW JERSEY, 1974. */
583 /* PARAMETER DESCRIPTION: */
584 /* G(),MG,M,N ON ENTRY G() STORES THE M BY N MATRIX OF */
585 /* LINEAR INEQUALITY CONSTRAINTS. G() HAS FIRST */
586 /* DIMENSIONING PARAMETER MG */
587 /* H() ON ENTRY H() STORES THE M VECTOR H REPRESENTING */
588 /* THE RIGHT SIDE OF THE INEQUALITY SYSTEM */
589 /* REMARK: G(),H() WILL NOT BE CHANGED DURING CALCULATIONS BY LDP */
590 /* X() ON ENTRY X() NEED NOT BE INITIALIZED. */
591 /* ON EXIT X() STORES THE SOLUTION VECTOR X IF MODE=1. */
592 /* XNORM ON EXIT XNORM STORES THE EUCLIDIAN NORM OF THE */
593 /* SOLUTION VECTOR IF COMPUTATION IS SUCCESSFUL */
594 /* W() W IS A ONE DIMENSIONAL WORKING SPACE, THE LENGTH */
595 /* OF WHICH SHOULD BE AT LEAST (M+2)*(N+1) + 2*M */
596 /* ON EXIT W() STORES THE LAGRANGE MULTIPLIERS */
597 /* ASSOCIATED WITH THE CONSTRAINTS */
598 /* AT THE SOLUTION OF PROBLEM LDP */
599 /* INDX() INDX() IS A ONE DIMENSIONAL INT WORKING SPACE */
600 /* OF LENGTH AT LEAST M */
601 /* MODE MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING */
603 /* MODE=1: SUCCESSFUL COMPUTATION */
604 /* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N.LE.0) */
605 /* 3: ITERATION COUNT EXCEEDED BY NNLS */
606 /* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
607 /* Parameter adjustments */
612 g_offset = 1 + g_dim1;
621 /* STATE DUAL PROBLEM */
624 dcopy___(n, &x[1], 0, &x[1], 1);
631 for (j = 1; j <= i__1; ++j) {
633 for (i__ = 1; i__ <= i__2; ++i__) {
636 w[iw] = g[j + i__ * g_dim1];
644 for (i__ = 1; i__ <= i__1; ++i__) {
654 /* SOLVE DUAL PROBLEM */
655 nnls_(&w[1], &n1, &n1, m, &w[if__], &w[iy], &rnorm, &w[iwdual], &w[iz], &
664 /* COMPUTE SOLUTION OF PRIMAL PROBLEM */
665 fac = one - ddot_sl__(m, &h__[1], 1, &w[iy], 1);
667 if (d__1 - one <= 0.0) {
673 for (j = 1; j <= i__1; ++j) {
675 x[j] = fac * ddot_sl__(m, &g[j * g_dim1 + 1], 1, &w[iy], 1);
677 *xnorm = dnrm2___(n, &x[1], 1);
678 /* COMPUTE LAGRANGE MULTIPLIERS FOR PRIMAL PROBLEM */
680 dcopy___(m, &w[1], 0, &w[1], 1);
681 daxpy_sl__(m, &fac, &w[iy], 1, &w[1], 1);
682 /* END OF SUBROUTINE LDP */
687 static void lsi_(double *e, double *f, double *g,
688 double *h__, int *le, int *me, int *lg, int *mg,
689 int *n, double *x, double *xnorm, double *w, int *
692 /* Initialized data */
694 const double epmach = 2.22e-16;
695 const double one = 1.;
697 /* System generated locals */
698 int e_dim1, e_offset, g_dim1, g_offset, i__1, i__2, i__3;
701 /* Local variables */
705 /* FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF */
706 /* INEQUALITY CONSTRAINED LINEAR LEAST SQUARES PROBLEM: */
710 /* THE ALGORITHM IS BASED ON QR DECOMPOSITION AS DESCRIBED IN */
711 /* CHAPTER 23.5 OF LAWSON & HANSON: SOLVING LEAST SQUARES PROBLEMS */
712 /* THE FOLLOWING DIMENSIONS OF THE ARRAYS DEFINING THE PROBLEM */
714 /* DIM(E) : FORMAL (LE,N), ACTUAL (ME,N) */
715 /* DIM(F) : FORMAL (LE ), ACTUAL (ME ) */
716 /* DIM(G) : FORMAL (LG,N), ACTUAL (MG,N) */
717 /* DIM(H) : FORMAL (LG ), ACTUAL (MG ) */
719 /* DIM(W) : (N+1)*(MG+2) + 2*MG */
721 /* ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS E, F, G, AND H. */
722 /* ON RETURN, ALL ARRAYS WILL BE CHANGED BY THE SUBROUTINE. */
723 /* X STORES THE SOLUTION VECTOR */
724 /* XNORM STORES THE RESIDUUM OF THE SOLUTION IN EUCLIDIAN NORM */
725 /* W STORES THE VECTOR OF LAGRANGE MULTIPLIERS IN ITS FIRST */
727 /* MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: */
728 /* MODE=1: SUCCESSFUL COMPUTATION */
729 /* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) */
730 /* 3: ITERATION COUNT EXCEEDED BY NNLS */
731 /* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
732 /* 5: MATRIX E IS NOT OF FULL RANK */
733 /* 03.01.1980, DIETER KRAFT: CODED */
734 /* 20.03.1987, DIETER KRAFT: REVISED TO FORTRAN 77 */
735 /* Parameter adjustments */
741 g_offset = 1 + g_dim1;
744 e_offset = 1 + e_dim1;
749 /* QR-FACTORS OF E AND APPLICATION TO F */
751 for (i__ = 1; i__ <= i__1; ++i__) {
757 h12_(&c__1, &i__, &i__2, me, &e[i__ * e_dim1 + 1], &c__1, &t, &e[j *
758 e_dim1 + 1], &c__1, le, &i__3);
761 h12_(&c__2, &i__, &i__2, me, &e[i__ * e_dim1 + 1], &c__1, &t, &f[1], &
764 /* TRANSFORM G AND H TO GET LEAST DISTANCE PROBLEM */
767 for (i__ = 1; i__ <= i__2; ++i__) {
769 for (j = 1; j <= i__1; ++j) {
770 if ((d__1 = e[j + j * e_dim1], fabs(d__1)) < epmach) {
775 g[i__ + j * g_dim1] = (g[i__ + j * g_dim1] - ddot_sl__(&i__3, &g[
776 i__ + g_dim1], *lg, &e[j * e_dim1 + 1], 1)) / e[j + j *
780 h__[i__] -= ddot_sl__(n, &g[i__ + g_dim1], *lg, &f[1], 1);
782 /* SOLVE LEAST DISTANCE PROBLEM */
783 ldp_(&g[g_offset], lg, mg, n, &h__[1], &x[1], xnorm, &w[1], &jw[1], mode);
787 /* SOLUTION OF ORIGINAL PROBLEM */
788 daxpy_sl__(n, &one, &f[1], 1, &x[1], 1);
789 for (i__ = *n; i__ >= 1; --i__) {
795 x[i__] = (x[i__] - ddot_sl__(&i__2, &e[i__ + j * e_dim1], *le, &x[j], 1))
796 / e[i__ + i__ * e_dim1];
802 t = dnrm2___(&i__2, &f[j], 1);
803 *xnorm = sqrt(*xnorm * *xnorm + t * t);
804 /* END OF SUBROUTINE LSI */
809 static void hfti_(double *a, int *mda, int *m, int *
810 n, double *b, int *mdb, const int *nb, double *tau, int
811 *krank, double *rnorm, double *h__, double *g, int *
814 /* Initialized data */
816 const double factor = .001;
818 /* System generated locals */
819 int a_dim1, a_offset, b_dim1, b_offset, i__1, i__2, i__3;
822 /* Local variables */
828 /* RANK-DEFICIENT LEAST SQUARES ALGORITHM AS DESCRIBED IN: */
829 /* C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12 */
830 /* TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974 */
831 /* A(*,*),MDA,M,N THE ARRAY A INITIALLY CONTAINS THE M x N MATRIX A */
832 /* OF THE LEAST SQUARES PROBLEM AX = B. */
833 /* THE FIRST DIMENSIONING PARAMETER MDA MUST SATISFY */
834 /* MDA >= M. EITHER M >= N OR M < N IS PERMITTED. */
835 /* THERE IS NO RESTRICTION ON THE RANK OF A. */
836 /* THE MATRIX A WILL BE MODIFIED BY THE SUBROUTINE. */
837 /* B(*,*),MDB,NB IF NB = 0 THE SUBROUTINE WILL MAKE NO REFERENCE */
838 /* TO THE ARRAY B. IF NB > 0 THE ARRAY B() MUST */
839 /* INITIALLY CONTAIN THE M x NB MATRIX B OF THE */
840 /* THE LEAST SQUARES PROBLEM AX = B AND ON RETURN */
841 /* THE ARRAY B() WILL CONTAIN THE N x NB SOLUTION X. */
842 /* IF NB>1 THE ARRAY B() MUST BE DOUBLE SUBSCRIPTED */
843 /* WITH FIRST DIMENSIONING PARAMETER MDB>=MAX(M,N), */
844 /* IF NB=1 THE ARRAY B() MAY BE EITHER SINGLE OR */
845 /* DOUBLE SUBSCRIPTED. */
846 /* TAU ABSOLUTE TOLERANCE PARAMETER FOR PSEUDORANK */
847 /* DETERMINATION, PROVIDED BY THE USER. */
848 /* KRANK PSEUDORANK OF A, SET BY THE SUBROUTINE. */
849 /* RNORM ON EXIT, RNORM(J) WILL CONTAIN THE EUCLIDIAN */
850 /* NORM OF THE RESIDUAL VECTOR FOR THE PROBLEM */
851 /* DEFINED BY THE J-TH COLUMN VECTOR OF THE ARRAY B. */
852 /* H(), G() ARRAYS OF WORKING SPACE OF LENGTH >= N. */
853 /* IP() INT ARRAY OF WORKING SPACE OF LENGTH >= N */
854 /* RECORDING PERMUTATION INDICES OF COLUMN VECTORS */
855 /* Parameter adjustments */
860 a_offset = 1 + a_dim1;
864 b_offset = 1 + b_dim1;
875 for (j = 1; j <= i__1; ++j) {
881 for (l = j; l <= i__2; ++l) {
882 /* Computing 2nd power */
883 d__1 = a[j - 1 + l * a_dim1];
884 h__[l] -= d__1 * d__1;
886 if (h__[l] > h__[lmax]) {
890 d__1 = hmax + factor * h__[lmax];
891 if (d__1 - hmax > 0.0) {
897 for (l = j; l <= i__2; ++l) {
900 for (i__ = j; i__ <= i__3; ++i__) {
902 /* Computing 2nd power */
903 d__1 = a[i__ + l * a_dim1];
904 h__[l] += d__1 * d__1;
907 if (h__[l] > h__[lmax]) {
912 /* COLUMN INTERCHANGES IF NEEDED */
919 for (i__ = 1; i__ <= i__2; ++i__) {
920 tmp = a[i__ + j * a_dim1];
921 a[i__ + j * a_dim1] = a[i__ + lmax * a_dim1];
923 a[i__ + lmax * a_dim1] = tmp;
926 /* J-TH TRANSFORMATION AND APPLICATION TO A AND B */
933 h12_(&c__1, &j, &i__2, m, &a[j * a_dim1 + 1], &c__1, &h__[j], &a[i__ *
934 a_dim1 + 1], &c__1, mda, &i__3);
937 h12_(&c__2, &j, &i__2, m, &a[j * a_dim1 + 1], &c__1, &h__[j], &b[
938 b_offset], &c__1, mdb, nb);
940 /* DETERMINE PSEUDORANK */
942 for (j = 1; j <= i__2; ++j) {
944 if ((d__1 = a[j + j * a_dim1], fabs(d__1)) <= *tau) {
954 /* NORM OF RESIDUALS */
956 for (jb = 1; jb <= i__2; ++jb) {
959 rnorm[jb] = dnrm2___(&i__1, &b[kp1 + jb * b_dim1], 1);
965 for (jb = 1; jb <= i__1; ++jb) {
967 for (i__ = 1; i__ <= i__2; ++i__) {
969 b[i__ + jb * b_dim1] = 0.0;
977 /* HOUSEHOLDER DECOMPOSITION OF FIRST K ROWS */
978 for (i__ = k; i__ >= 1; --i__) {
981 h12_(&c__1, &i__, &kp1, n, &a[i__ + a_dim1], mda, &g[i__], &a[
982 a_offset], mda, &c__1, &i__2);
986 for (jb = 1; jb <= i__2; ++jb) {
987 /* SOLVE K*K TRIANGULAR SYSTEM */
988 for (i__ = k; i__ >= 1; --i__) {
994 b[i__ + jb * b_dim1] = (b[i__ + jb * b_dim1] - ddot_sl__(&i__1, &
995 a[i__ + j * a_dim1], *mda, &b[j + jb * b_dim1], 1)) /
996 a[i__ + i__ * a_dim1];
998 /* COMPLETE SOLUTION VECTOR */
1003 for (j = kp1; j <= i__1; ++j) {
1005 b[j + jb * b_dim1] = 0.0;
1008 for (i__ = 1; i__ <= i__1; ++i__) {
1010 h12_(&c__2, &i__, &kp1, n, &a[i__ + a_dim1], mda, &g[i__], &b[jb *
1011 b_dim1 + 1], &c__1, mdb, &c__1);
1013 /* REORDER SOLUTION ACCORDING TO PREVIOUS COLUMN INTERCHANGES */
1015 for (j = ldiag; j >= 1; --j) {
1020 tmp = b[l + jb * b_dim1];
1021 b[l + jb * b_dim1] = b[j + jb * b_dim1];
1022 b[j + jb * b_dim1] = tmp;
1031 static void lsei_(double *c__, double *d__, double *e,
1032 double *f, double *g, double *h__, int *lc, int *
1033 mc, int *le, int *me, int *lg, int *mg, int *n,
1034 double *x, double *xnrm, double *w, int *jw, int *
1037 /* Initialized data */
1039 const double epmach = 2.22e-16;
1041 /* System generated locals */
1042 int c_dim1, c_offset, e_dim1, e_offset, g_dim1, g_offset, i__1, i__2,
1046 /* Local variables */
1049 int ie, if__, ig, iw, mc1;
1052 /* FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF */
1053 /* EQUALITY & INEQUALITY CONSTRAINED LEAST SQUARES PROBLEM LSEI : */
1054 /* MIN ||E*X - F|| */
1058 /* USING QR DECOMPOSITION & ORTHOGONAL BASIS OF NULLSPACE OF C */
1059 /* CHAPTER 23.6 OF LAWSON & HANSON: SOLVING LEAST SQUARES PROBLEMS. */
1060 /* THE FOLLOWING DIMENSIONS OF THE ARRAYS DEFINING THE PROBLEM */
1062 /* DIM(E) : FORMAL (LE,N), ACTUAL (ME,N) */
1063 /* DIM(F) : FORMAL (LE ), ACTUAL (ME ) */
1064 /* DIM(C) : FORMAL (LC,N), ACTUAL (MC,N) */
1065 /* DIM(D) : FORMAL (LC ), ACTUAL (MC ) */
1066 /* DIM(G) : FORMAL (LG,N), ACTUAL (MG,N) */
1067 /* DIM(H) : FORMAL (LG ), ACTUAL (MG ) */
1068 /* DIM(X) : FORMAL (N ), ACTUAL (N ) */
1069 /* DIM(W) : 2*MC+ME+(ME+MG)*(N-MC) for LSEI */
1070 /* +(N-MC+1)*(MG+2)+2*MG for LSI */
1071 /* DIM(JW): MAX(MG,L) */
1072 /* ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS C, D, E, F, G, AND H. */
1073 /* ON RETURN, ALL ARRAYS WILL BE CHANGED BY THE SUBROUTINE. */
1074 /* X STORES THE SOLUTION VECTOR */
1075 /* XNORM STORES THE RESIDUUM OF THE SOLUTION IN EUCLIDIAN NORM */
1076 /* W STORES THE VECTOR OF LAGRANGE MULTIPLIERS IN ITS FIRST */
1077 /* MC+MG ELEMENTS */
1078 /* MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: */
1079 /* MODE=1: SUCCESSFUL COMPUTATION */
1080 /* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) */
1081 /* 3: ITERATION COUNT EXCEEDED BY NNLS */
1082 /* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
1083 /* 5: MATRIX E IS NOT OF FULL RANK */
1084 /* 6: MATRIX C IS NOT OF FULL RANK */
1085 /* 7: RANK DEFECT IN HFTI */
1086 /* 18.5.1981, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN */
1087 /* 20.3.1987, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN */
1088 /* Parameter adjustments */
1094 g_offset = 1 + g_dim1;
1097 e_offset = 1 + e_dim1;
1100 c_offset = 1 + c_dim1;
1112 iw = (l + 1) * (*mg + 2) + (*mg << 1) + *mc;
1114 if__ = ie + *me * l;
1116 /* TRIANGULARIZE C AND APPLY FACTORS TO E AND G */
1118 for (i__ = 1; i__ <= i__1; ++i__) {
1124 h12_(&c__1, &i__, &i__2, n, &c__[i__ + c_dim1], lc, &w[iw + i__], &
1125 c__[j + c_dim1], lc, &c__1, &i__3);
1127 h12_(&c__2, &i__, &i__2, n, &c__[i__ + c_dim1], lc, &w[iw + i__], &e[
1128 e_offset], le, &c__1, me);
1131 h12_(&c__2, &i__, &i__2, n, &c__[i__ + c_dim1], lc, &w[iw + i__], &g[
1132 g_offset], lg, &c__1, mg);
1134 /* SOLVE C*X=D AND MODIFY F */
1137 for (i__ = 1; i__ <= i__2; ++i__) {
1138 if ((d__1 = c__[i__ + i__ * c_dim1], fabs(d__1)) < epmach) {
1142 x[i__] = (d__[i__] - ddot_sl__(&i__1, &c__[i__ + c_dim1], *lc, &x[1], 1))
1143 / c__[i__ + i__ * c_dim1];
1148 i__2 = *mg; /* BUGFIX for *mc == *n: changed from *mg - *mc, SGJ 2010 */
1149 dcopy___(&i__2, &w[mc1], 0, &w[mc1], 1);
1154 for (i__ = 1; i__ <= i__2; ++i__) {
1156 w[if__ - 1 + i__] = f[i__] - ddot_sl__(mc, &e[i__ + e_dim1], *le, &x[1], 1);
1158 /* STORE TRANSFORMED E & G */
1160 for (i__ = 1; i__ <= i__2; ++i__) {
1162 dcopy___(&l, &e[i__ + mc1 * e_dim1], *le, &w[ie - 1 + i__], *me);
1165 for (i__ = 1; i__ <= i__2; ++i__) {
1167 dcopy___(&l, &g[i__ + mc1 * g_dim1], *lg, &w[ig - 1 + i__], *mg);
1172 /* SOLVE LS WITHOUT INEQUALITY CONSTRAINTS */
1176 hfti_(&w[ie], me, me, &l, &w[if__], &k, &c__1, &t, &krank, xnrm, &w[1], &
1178 dcopy___(&l, &w[if__], 1, &x[mc1], 1);
1184 /* MODIFY H AND SOLVE INEQUALITY CONSTRAINED LS PROBLEM */
1187 for (i__ = 1; i__ <= i__2; ++i__) {
1189 h__[i__] -= ddot_sl__(mc, &g[i__ + g_dim1], *lg, &x[1], 1);
1191 lsi_(&w[ie], &w[if__], &w[ig], &h__[1], me, me, mg, mg, &l, &x[mc1], xnrm,
1192 &w[mc1], &jw[1], mode);
1196 t = dnrm2___(mc, &x[1], 1);
1197 *xnrm = sqrt(*xnrm * *xnrm + t * t);
1201 /* SOLUTION OF ORIGINAL PROBLEM AND LAGRANGE MULTIPLIERS */
1204 for (i__ = 1; i__ <= i__2; ++i__) {
1206 f[i__] = ddot_sl__(n, &e[i__ + e_dim1], *le, &x[1], 1) - f[i__];
1209 for (i__ = 1; i__ <= i__2; ++i__) {
1211 d__[i__] = ddot_sl__(me, &e[i__ * e_dim1 + 1], 1, &f[1], 1) -
1212 ddot_sl__(mg, &g[i__ * g_dim1 + 1], 1, &w[mc1], 1);
1214 for (i__ = *mc; i__ >= 1; --i__) {
1217 h12_(&c__2, &i__, &i__2, n, &c__[i__ + c_dim1], lc, &w[iw + i__], &x[
1218 1], &c__1, &c__1, &c__1);
1220 for (i__ = *mc; i__ >= 1; --i__) {
1225 w[i__] = (d__[i__] - ddot_sl__(&i__2, &c__[j + i__ * c_dim1], 1, &
1226 w[j], 1)) / c__[i__ + i__ * c_dim1];
1229 /* END OF SUBROUTINE LSEI */
1234 static void lsq_(int *m, int *meq, int *n, int *nl,
1235 int *la, double *l, double *g, double *a, double *
1236 b, const double *xl, const double *xu, double *x, double *y,
1237 double *w, int *jw, int *mode)
1239 /* Initialized data */
1241 const double one = 1.;
1243 /* System generated locals */
1244 int a_dim1, a_offset, i__1, i__2;
1247 /* Local variables */
1248 int i__, i1, i2, i3, i4, m1, n1, n2, n3, ic, id, ie, if__, ig, ih, il,
1254 /* MINIMIZE with respect to X */
1257 /* WITH UPPER TRIANGULAR MATRIX E = +D *L , */
1259 /* AND VECTOR F = -D *L *G, */
1260 /* WHERE THE UNIT LOWER TRIDIANGULAR MATRIX L IS STORED COLUMNWISE */
1261 /* DENSE IN THE N*(N+1)/2 ARRAY L WITH VECTOR D STORED IN ITS */
1262 /* 'DIAGONAL' THUS SUBSTITUTING THE ONE-ELEMENTS OF L */
1264 /* A(J)*X - B(J) = 0 , J=1,...,MEQ, */
1265 /* A(J)*X - B(J) >=0, J=MEQ+1,...,M, */
1266 /* XL(I) <= X(I) <= XU(I), I=1,...,N, */
1267 /* ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS L, G, A, B, XL, XU. */
1268 /* WITH DIMENSIONS: L(N*(N+1)/2), G(N), A(LA,N), B(M), XL(N), XU(N) */
1269 /* THE WORKING ARRAY W MUST HAVE AT LEAST THE FOLLOWING DIMENSION: */
1270 /* DIM(W) = (3*N+M)*(N+1) for LSQ */
1271 /* +(N-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI */
1272 /* +(N+MINEQ)*(N-MEQ) + 2*MEQ + N for LSEI */
1273 /* with MINEQ = M - MEQ + 2*N */
1274 /* ON RETURN, NO ARRAY WILL BE CHANGED BY THE SUBROUTINE. */
1275 /* X STORES THE N-DIMENSIONAL SOLUTION VECTOR */
1276 /* Y STORES THE VECTOR OF LAGRANGE MULTIPLIERS OF DIMENSION */
1277 /* M+N+N (CONSTRAINTS+LOWER+UPPER BOUNDS) */
1278 /* MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: */
1279 /* MODE=1: SUCCESSFUL COMPUTATION */
1280 /* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) */
1281 /* 3: ITERATION COUNT EXCEEDED BY NNLS */
1282 /* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
1283 /* 5: MATRIX E IS NOT OF FULL RANK */
1284 /* 6: MATRIX C IS NOT OF FULL RANK */
1285 /* 7: RANK DEFECT IN HFTI */
1286 /* coded Dieter Kraft, april 1987 */
1287 /* revised march 1989 */
1288 /* Parameter adjustments */
1297 a_offset = 1 + a_dim1;
1305 m1 = mineq + *n + *n;
1306 /* determine whether to solve problem */
1307 /* with inconsistent linerarization (n2=1) */
1309 n2 = n1 * *n / 2 + 1;
1316 /* RECOVER MATRIX E AND VECTOR F FROM L AND G */
1323 for (i__ = 1; i__ <= i__1; ++i__) {
1327 dcopy___(&i1, &w[i3], 0, &w[i3], 1);
1329 dcopy___(&i__2, &l[i2], 1, &w[i3], *n);
1331 dscal_sl__(&i__2, &diag, &w[i3], *n);
1334 w[if__ - 1 + i__] = (g[i__] - ddot_sl__(&i__2, &w[i4], 1, &w[if__]
1344 dcopy___(&n3, &w[i4], 0, &w[i4], 1);
1345 w[if__ - 1 + *n] = 0.0;
1348 dscal_sl__(n, &d__1, &w[if__], 1);
1350 id = ic + *meq * *n;
1352 /* RECOVER MATRIX C FROM UPPER PART OF A */
1354 for (i__ = 1; i__ <= i__1; ++i__) {
1355 dcopy___(n, &a[i__ + a_dim1], *la, &w[ic - 1 + i__], *meq);
1358 /* RECOVER VECTOR D FROM UPPER PART OF B */
1359 dcopy___(meq, &b[1], 1, &w[id], 1);
1361 dscal_sl__(meq, &d__1, &w[id], 1);
1365 /* RECOVER MATRIX G FROM LOWER PART OF A */
1367 for (i__ = 1; i__ <= i__1; ++i__) {
1368 dcopy___(n, &a[*meq + i__ + a_dim1], *la, &w[ig - 1 + i__], m1);
1372 /* AUGMENT MATRIX G BY +I AND -I */
1375 for (i__ = 1; i__ <= i__1; ++i__) {
1376 w[ip - 1 + i__] = 0.0;
1377 dcopy___(n, &w[ip - 1 + i__], 0, &w[ip - 1 + i__], m1);
1381 /* SGJ, 2010: skip constraints for infinite bounds */
1382 for (i__ = 1; i__ <= *n; ++i__)
1383 if (!nlopt_isinf(xl[i__])) w[(ip - i__1) + i__ * i__1] = +1.0;
1384 /* Old code: w[ip] = one; dcopy___(n, &w[ip], 0, &w[ip], i__1); */
1387 for (i__ = 1; i__ <= i__1; ++i__) {
1388 w[im - 1 + i__] = 0.0;
1389 dcopy___(n, &w[im - 1 + i__], 0, &w[im - 1 + i__], m1);
1393 /* SGJ, 2010: skip constraints for infinite bounds */
1394 for (i__ = 1; i__ <= *n; ++i__)
1395 if (!nlopt_isinf(xu[i__])) w[(im - i__1) + i__ * i__1] = -1.0;
1396 /* Old code: w[im] = -one; dcopy___(n, &w[im], 0, &w[im], i__1); */
1399 /* RECOVER H FROM LOWER PART OF B */
1400 dcopy___(&mineq, &b[*meq + 1], 1, &w[ih], 1);
1402 dscal_sl__(&mineq, &d__1, &w[ih], 1);
1404 /* AUGMENT VECTOR H BY XL AND XU */
1407 /* SGJ, 2010: skip constraints for infinite bounds */
1408 for (i__ = 1; i__ <= *n; ++i__) {
1409 w[(il-1) + i__] = nlopt_isinf(xl[i__]) ? 0 : xl[i__];
1410 w[(iu-1) + i__] = nlopt_isinf(xu[i__]) ? 0 : -xu[i__];
1412 /* Old code: dcopy___(n, &xl[1], 1, &w[il], 1);
1413 dcopy___(n, &xu[1], 1, &w[iu], 1);
1414 d__1 = -one; dscal_sl__(n, &d__1, &w[iu], 1); */
1416 i__1 = MAX2(1,*meq);
1417 lsei_(&w[ic], &w[id], &w[ie], &w[if__], &w[ig], &w[ih], &i__1, meq, n, n,
1418 &m1, &m1, n, &x[1], &xnorm, &w[iw], &jw[1], mode);
1420 /* restore Lagrange multipliers */
1421 dcopy___(m, &w[iw], 1, &y[1], 1);
1422 dcopy___(&n3, &w[iw + *m], 1, &y[*m + 1], 1);
1423 dcopy___(&n3, &w[iw + *m + *n], 1, &y[*m + n3 + 1], 1);
1425 /* SGJ, 2010: make sure bound constraints are satisfied, since
1426 roundoff error sometimes causes slight violations and
1427 NLopt guarantees that bounds are strictly obeyed */
1429 for (i__ = 1; i__ <= i__1; ++i__) {
1430 if (x[i__] < xl[i__]) x[i__] = xl[i__];
1431 else if (x[i__] > xu[i__]) x[i__] = xu[i__];
1434 /* END OF SUBROUTINE LSQ */
1437 static void ldl_(int *n, double *a, double *z__,
1438 double *sigma, double *w)
1440 /* Initialized data */
1442 const double one = 1.;
1443 const double four = 4.;
1444 const double epmach = 2.22e-16;
1446 /* System generated locals */
1449 /* Local variables */
1453 double tp, beta, gamma_, alpha, delta;
1455 /* LDL LDL' - RANK-ONE - UPDATE */
1457 /* UPDATES THE LDL' FACTORS OF MATRIX A BY RANK-ONE MATRIX */
1459 /* INPUT ARGUMENTS: (* MEANS PARAMETERS ARE CHANGED DURING EXECUTION) */
1460 /* N : ORDER OF THE COEFFICIENT MATRIX A */
1461 /* * A : POSITIVE DEFINITE MATRIX OF DIMENSION N; */
1462 /* ONLY THE LOWER TRIANGLE IS USED AND IS STORED COLUMN BY */
1463 /* COLUMN AS ONE DIMENSIONAL ARRAY OF DIMENSION N*(N+1)/2. */
1464 /* * Z : VECTOR OF DIMENSION N OF UPDATING ELEMENTS */
1465 /* SIGMA : SCALAR FACTOR BY WHICH THE MODIFYING DYADE Z*Z' IS */
1467 /* OUTPUT ARGUMENTS: */
1468 /* A : UPDATED LDL' FACTORS */
1469 /* WORKING ARRAY: */
1470 /* W : VECTOR OP DIMENSION N (USED ONLY IF SIGMA .LT. ZERO) */
1472 /* THAT OF FLETCHER AND POWELL AS DESCRIBED IN : */
1473 /* FLETCHER,R.,(1974) ON THE MODIFICATION OF LDL' FACTORIZATION. */
1474 /* POWELL,M.J.D. MATH.COMPUTATION 28, 1067-1078. */
1475 /* IMPLEMENTED BY: */
1476 /* KRAFT,D., DFVLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME */
1477 /* D-8031 OBERPFAFFENHOFEN */
1478 /* STATUS: 15. JANUARY 1980 */
1479 /* SUBROUTINES REQUIRED: NONE */
1480 /* Parameter adjustments */
1486 if (*sigma == 0.0) {
1494 /* PREPARE NEGATIVE UPDATE */
1496 for (i__ = 1; i__ <= i__1; ++i__) {
1501 for (i__ = 1; i__ <= i__1; ++i__) {
1505 for (j = i__ + 1; j <= i__2; ++j) {
1514 t = epmach / *sigma;
1517 for (i__ = 1; i__ <= i__1; ++i__) {
1526 /* HERE UPDATING BEGINS */
1528 for (i__ = 1; i__ <= i__1; ++i__) {
1534 else /* if (*sigma > 0.0), since *sigma != 0 from above */ {
1538 a[ij] = alpha * a[ij];
1547 for (j = i__ + 1; j <= i__2; ++j) {
1549 z__[j] -= v * a[ij];
1551 a[ij] += beta * z__[j];
1557 for (j = i__ + 1; j <= i__2; ++j) {
1560 a[ij] = gamma_ * u + beta * z__[j];
1575 /* we don't want to use this linmin function, for two reasons:
1576 1) it was apparently written assuming an old version of Fortran where all variables
1577 are saved by default, hence it was missing a "save" statement ... I would
1578 need to go through and figure out which variables need to be declared "static"
1579 (or, better yet, save them like I did in slsqp to make it re-entrant)
1580 2) it doesn't exploit gradient information, which is stupid since we have this info
1581 3) in the context of NLopt, it makes much more sence to use the local_opt algorithm
1582 to do the line minimization recursively by whatever algorithm the user wants
1583 (defaulting to a gradient-based method like LBFGS) */
1584 static double d_sign(double a, double s) { return s < 0 ? -a : a; }
1585 static double linmin_(int *mode, const double *ax, const double *bx, double *
1588 /* Initialized data */
1590 const double c__ = .381966011;
1591 const double eps = 1.5e-8;
1593 /* System generated locals */
1594 double ret_val, d__1;
1596 /* Local variables */
1597 double a, b, d__, e, m, p, q, r__, u, v, w, x, fu, fv, fw, fx, tol1,
1600 /* LINMIN LINESEARCH WITHOUT DERIVATIVES */
1602 /* TO FIND THE ARGUMENT LINMIN WHERE THE FUNCTION F TAKES ITS MINIMUM */
1603 /* ON THE INTERVAL AX, BX. */
1604 /* COMBINATION OF GOLDEN SECTION AND SUCCESSIVE QUADRATIC INTERPOLATION. */
1605 /* INPUT ARGUMENTS: (* MEANS PARAMETERS ARE CHANGED DURING EXECUTION) */
1606 /* *MODE SEE OUTPUT ARGUMENTS */
1607 /* AX LEFT ENDPOINT OF INITIAL INTERVAL */
1608 /* BX RIGHT ENDPOINT OF INITIAL INTERVAL */
1609 /* F FUNCTION VALUE AT LINMIN WHICH IS TO BE BROUGHT IN BY */
1610 /* REVERSE COMMUNICATION CONTROLLED BY MODE */
1611 /* TOL DESIRED LENGTH OF INTERVAL OF UNCERTAINTY OF FINAL RESULT */
1612 /* OUTPUT ARGUMENTS: */
1613 /* LINMIN ABSCISSA APPROXIMATING THE POINT WHERE F ATTAINS A MINIMUM */
1614 /* MODE CONTROLS REVERSE COMMUNICATION */
1615 /* MUST BE SET TO 0 INITIALLY, RETURNS WITH INTERMEDIATE */
1616 /* VALUES 1 AND 2 WHICH MUST NOT BE CHANGED BY THE USER, */
1617 /* ENDS WITH CONVERGENCE WITH VALUE 3. */
1618 /* WORKING ARRAY: */
1621 /* THIS FUNCTION SUBPROGRAM IS A SLIGHTLY MODIFIED VERSION OF THE */
1622 /* ALGOL 60 PROCEDURE LOCALMIN GIVEN IN */
1623 /* R.P. BRENT: ALGORITHMS FOR MINIMIZATION WITHOUT DERIVATIVES, */
1624 /* PRENTICE-HALL (1973). */
1625 /* IMPLEMENTED BY: */
1626 /* KRAFT, D., DFVLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME */
1627 /* D-8031 OBERPFAFFENHOFEN */
1628 /* STATUS: 31. AUGUST 1984 */
1629 /* SUBROUTINES REQUIRED: NONE */
1630 /* EPS = SQUARE - ROOT OF MACHINE PRECISION */
1631 /* C = GOLDEN SECTION RATIO = (3-SQRT(5))/2 */
1636 /* INITIALIZATION */
1640 v = a + c__ * (b - a);
1646 /* MAIN LOOP STARTS HERE */
1653 tol1 = eps * fabs(x) + *tol;
1655 /* TEST CONVERGENCE */
1656 if ((d__1 = x - m, fabs(d__1)) <= tol2 - (b - a) * .5) {
1662 if (fabs(e) <= tol1) {
1666 r__ = (x - w) * (fx - fv);
1667 q = (x - v) * (fx - fw);
1668 p = (x - v) * q - (x - w) * r__;
1679 /* IS PARABOLA ACCEPTABLE */
1681 if (fabs(p) >= (d__1 = q * r__, fabs(d__1)) * .5 || p <= q * (a - x) || p >=
1685 /* PARABOLIC INTERPOLATION STEP */
1687 /* F MUST NOT BE EVALUATED TOO CLOSE TO A OR B */
1690 d__ = d_sign(tol1, d__1);
1694 d__ = d_sign(tol1, d__1);
1697 /* GOLDEN SECTION STEP */
1706 /* F MUST NOT BE EVALUATED TOO CLOSE TO X */
1708 if (fabs(d__) < tol1) {
1709 d__ = d_sign(tol1, d__);
1717 /* UPDATE A, B, V, W, AND X */
1741 if (fu <= fw || w == x) {
1744 if (fu <= fv || v == x || v == w) {
1759 /* END OF MAIN LOOP */
1770 double t, f0, h1, h2, h3, h4;
1777 int incons, ireset, itermx;
1781 #define SS(var) state->var = var
1782 #define SAVE_STATE \
1783 SS(t); SS(f0); SS(h1); SS(h2); SS(h3); SS(h4); \
1784 SS(n1); SS(n2); SS(n3); \
1790 SS(incons); SS(ireset); SS(itermx)
1792 #define RS(var) var = state->var
1793 #define RESTORE_STATE \
1794 RS(t); RS(f0); RS(h1); RS(h2); RS(h3); RS(h4); \
1795 RS(n1); RS(n2); RS(n3); \
1801 RS(incons); RS(ireset); RS(itermx)
1803 static void slsqpb_(int *m, int *meq, int *la, int *
1804 n, double *x, const double *xl, const double *xu, double *f,
1805 double *c__, double *g, double *a, double *acc,
1806 int *iter, int *mode, double *r__, double *l,
1807 double *x0, double *mu, double *s, double *u,
1808 double *v, double *w, int *iw,
1809 slsqpb_state *state)
1811 /* Initialized data */
1813 const double one = 1.;
1814 const double alfmin = .1;
1815 const double hun = 100.;
1816 const double ten = 10.;
1817 const double two = 2.;
1819 /* System generated locals */
1820 int a_dim1, a_offset, i__1, i__2;
1823 /* Local variables */
1826 /* saved state from one call to the next;
1827 SGJ 2010: save/restore via state parameter, to make re-entrant. */
1828 double t, f0, h1, h2, h3, h4;
1835 int incons, ireset, itermx;
1838 /* NONLINEAR PROGRAMMING BY SOLVING SEQUENTIALLY QUADRATIC PROGRAMS */
1839 /* - L1 - LINE SEARCH, POSITIVE DEFINITE BFGS UPDATE - */
1840 /* BODY SUBROUTINE FOR SLSQP */
1841 /* dim(W) = N1*(N1+1) + MEQ*(N1+1) + MINEQ*(N1+1) for LSQ */
1842 /* +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ */
1843 /* +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI */
1844 /* with MINEQ = M - MEQ + 2*N1 & N1 = N+1 */
1845 /* Parameter adjustments */
1855 a_offset = 1 + a_dim1;
1867 } else if (*mode == 0) {
1888 dcopy___(n, &s[1], 0, &s[1], 1);
1889 dcopy___(m, &mu[1], 0, &mu[1], 1);
1890 /* RESET BFGS MATRIX */
1897 dcopy___(&n2, &l[1], 0, &l[1], 1);
1900 for (i__ = 1; i__ <= i__1; ++i__) {
1905 /* MAIN ITERATION : SEARCH DIRECTION, STEPLENGTH, LDL'-UPDATE */
1909 if (*iter > itermx && itermx > 0) { /* SGJ 2010: ignore if itermx <= 0 */
1912 /* SEARCH DIRECTION AS SOLUTION OF QP - SUBPROBLEM */
1913 dcopy___(n, &xl[1], 1, &u[1], 1);
1914 dcopy___(n, &xu[1], 1, &v[1], 1);
1916 daxpy_sl__(n, &d__1, &x[1], 1, &u[1], 1);
1918 daxpy_sl__(n, &d__1, &x[1], 1, &v[1], 1);
1920 lsq_(m, meq, n, &n3, la, &l[1], &g[1], &a[a_offset], &c__[1], &u[1], &v[1]
1921 , &s[1], &r__[1], &w[1], &iw[1], mode);
1923 /* AUGMENTED PROBLEM FOR INCONSISTENT LINEARIZATION */
1931 for (j = 1; j <= i__1; ++j) {
1933 a[j + n1 * a_dim1] = -c__[j];
1937 a[j + n1 * a_dim1] = MAX2(d__1,0.0);
1942 dcopy___(n, &s[1], 0, &s[1], 1);
1951 lsq_(m, meq, &n1, &n3, la, &l[1], &g[1], &a[a_offset], &c__[1], &u[1],
1952 &v[1], &s[1], &r__[1], &w[1], &iw[1], mode);
1955 l[n3] = ten * l[n3];
1961 } else if (*mode != 1) {
1964 } else if (*mode != 1) {
1967 /* UPDATE MULTIPLIERS FOR L1-TEST */
1969 for (i__ = 1; i__ <= i__1; ++i__) {
1970 v[i__] = g[i__] - ddot_sl__(m, &a[i__ * a_dim1 + 1], 1, &r__[1], 1);
1974 dcopy___(n, &x[1], 1, &x0[1], 1);
1975 gs = ddot_sl__(n, &g[1], 1, &s[1], 1);
1979 for (j = 1; j <= i__1; ++j) {
1987 h2 += MAX2(d__1,h3);
1988 h3 = (d__1 = r__[j], fabs(d__1));
1990 d__1 = h3, d__2 = (mu[j] + h3) / two;
1991 mu[j] = MAX2(d__1,d__2);
1992 h1 += h3 * (d__1 = c__[j], fabs(d__1));
1995 /* CHECK CONVERGENCE */
1997 if (h1 < *acc && h2 < *acc) {
2002 for (j = 1; j <= i__1; ++j) {
2010 h1 += mu[j] * MAX2(d__1,h3);
2019 /* LINE SEARCH WITH AN L1-TESTFUNCTION */
2025 /* INEXACT LINESEARCH */
2029 dscal_sl__(n, &alpha, &s[1], 1);
2030 dcopy___(n, &x0[1], 1, &x[1], 1);
2031 daxpy_sl__(n, &one, &s[1], 1, &x[1], 1);
2033 /* SGJ 2010: ensure roundoff doesn't push us past bound constraints */
2034 i__1 = *n; for (i__ = 1; i__ <= i__1; ++i__) {
2035 if (x[i__] < xl[i__]) x[i__] = xl[i__];
2036 else if (x[i__] > xu[i__]) x[i__] = xu[i__];
2039 /* SGJ 2010: optimizing for the common case where the inexact line
2040 search succeeds in one step, use special mode = -2 here to
2041 eliminate a a subsequent unnecessary mode = -1 call, at the
2042 expense of extra gradient evaluations when more than one inexact
2043 line-search step is required */
2044 *mode = line == 1 ? -2 : 1;
2047 if (nlopt_isfinite(h1)) {
2048 if (h1 <= h3 / ten || line > 10) {
2052 d__1 = h3 / (two * (h3 - h1));
2053 alpha = MAX2(d__1,alfmin);
2055 alpha = MAX2(alpha*.5,alfmin);
2058 /* EXACT LINESEARCH */
2061 /* SGJ: see comments by linmin_ above: if we want to do an exact linesearch
2062 (which usually we probably don't), we should call NLopt recursively */
2064 alpha = linmin_(&line, &alfmin, &one, &t, &tol);
2065 dcopy___(n, &x0[1], 1, &x[1], 1);
2066 daxpy_sl__(n, &alpha, &s[1], 1, &x[1], 1);
2071 *mode = 9 /* will yield nlopt_failure */; return;
2073 dscal_sl__(n, &alpha, &s[1], 1);
2075 /* CALL FUNCTIONS AT CURRENT X */
2079 for (j = 1; j <= i__1; ++j) {
2087 t += mu[j] * MAX2(d__1,h1);
2091 switch (iexact + 1) {
2095 /* CHECK CONVERGENCE */
2099 for (j = 1; j <= i__1; ++j) {
2107 h3 += MAX2(d__1,h1);
2110 if (((d__1 = *f - f0, fabs(d__1)) < *acc || dnrm2___(n, &s[1], 1) < *
2111 acc) && h3 < *acc) {
2117 /* CHECK relaxed CONVERGENCE in case of positive directional derivative */
2119 if (((d__1 = *f - f0, fabs(d__1)) < tol || dnrm2___(n, &s[1], 1) < tol)
2126 /* CALL JACOBIAN AT CURRENT X */
2127 /* UPDATE CHOLESKY-FACTORS OF HESSIAN MATRIX BY MODIFIED BFGS FORMULA */
2130 for (i__ = 1; i__ <= i__1; ++i__) {
2131 u[i__] = g[i__] - ddot_sl__(m, &a[i__ * a_dim1 + 1], 1, &r__[1], 1) - v[i__];
2137 for (i__ = 1; i__ <= i__1; ++i__) {
2141 for (j = i__ + 1; j <= i__2; ++j) {
2146 v[i__] = s[i__] + h1;
2152 for (i__ = 1; i__ <= i__1; ++i__) {
2153 v[i__] = l[k] * v[i__];
2158 for (i__ = *n; i__ >= 1; --i__) {
2162 for (j = 1; j <= i__1; ++j) {
2170 h1 = ddot_sl__(n, &s[1], 1, &u[1], 1);
2171 h2 = ddot_sl__(n, &s[1], 1, &v[1], 1);
2174 h4 = (h2 - h3) / (h2 - h1);
2176 dscal_sl__(n, &h4, &u[1], 1);
2178 daxpy_sl__(n, &d__1, &v[1], 1, &u[1], 1);
2181 ldl_(n, &l[1], &u[1], &d__1, &v[1]);
2183 ldl_(n, &l[1], &v[1], &d__1, &u[1]);
2184 /* END OF MAIN ITERATION */
2191 /* *********************************************************************** */
2193 /* *********************************************************************** */
2194 static void slsqp(int *m, int *meq, int *la, int *n,
2195 double *x, const double *xl, const double *xu, double *f,
2196 double *c__, double *g, double *a, double *acc,
2197 int *iter, int *mode, double *w, int *l_w__, int *
2198 jw, int *l_jw__, slsqpb_state *state)
2200 /* System generated locals */
2201 int a_dim1, a_offset, i__1, i__2;
2203 /* Local variables */
2204 int n1, il, im, ir, is, iu, iv, iw, ix, mineq;
2206 /* SLSQP S EQUENTIAL L EAST SQ UARES P ROGRAMMING */
2207 /* TO SOLVE GENERAL NONLINEAR OPTIMIZATION PROBLEMS */
2208 /* *********************************************************************** */
2211 /* * A NONLINEAR PROGRAMMING METHOD WITH * */
2212 /* * QUADRATIC PROGRAMMING SUBPROBLEMS * */
2215 /* * THIS SUBROUTINE SOLVES THE GENERAL NONLINEAR PROGRAMMING PROBLEM * */
2217 /* * MINIMIZE F(X) * */
2219 /* * SUBJECT TO C (X) .EQ. 0 , J = 1,...,MEQ * */
2222 /* * C (X) .GE. 0 , J = MEQ+1,...,M * */
2225 /* * XL .LE. X .LE. XU , I = 1,...,N. * */
2228 /* * THE ALGORITHM IMPLEMENTS THE METHOD OF HAN AND POWELL * */
2229 /* * WITH BFGS-UPDATE OF THE B-MATRIX AND L1-TEST FUNCTION * */
2230 /* * WITHIN THE STEPLENGTH ALGORITHM. * */
2232 /* * PARAMETER DESCRIPTION: * */
2233 /* * ( * MEANS THIS PARAMETER WILL BE CHANGED DURING CALCULATION ) * */
2235 /* * M IS THE TOTAL NUMBER OF CONSTRAINTS, M .GE. 0 * */
2236 /* * MEQ IS THE NUMBER OF EQUALITY CONSTRAINTS, MEQ .GE. 0 * */
2237 /* * LA SEE A, LA .GE. MAX(M,1) * */
2238 /* * N IS THE NUMBER OF VARIBLES, N .GE. 1 * */
2239 /* * * X() X() STORES THE CURRENT ITERATE OF THE N VECTOR X * */
2240 /* * ON ENTRY X() MUST BE INITIALIZED. ON EXIT X() * */
2241 /* * STORES THE SOLUTION VECTOR X IF MODE = 0. * */
2242 /* * XL() XL() STORES AN N VECTOR OF LOWER BOUNDS XL TO X. * */
2243 /* * XU() XU() STORES AN N VECTOR OF UPPER BOUNDS XU TO X. * */
2244 /* * F IS THE VALUE OF THE OBJECTIVE FUNCTION. * */
2245 /* * C() C() STORES THE M VECTOR C OF CONSTRAINTS, * */
2246 /* * EQUALITY CONSTRAINTS (IF ANY) FIRST. * */
2247 /* * DIMENSION OF C MUST BE GREATER OR EQUAL LA, * */
2248 /* * which must be GREATER OR EQUAL MAX(1,M). * */
2249 /* * G() G() STORES THE N VECTOR G OF PARTIALS OF THE * */
2250 /* * OBJECTIVE FUNCTION; DIMENSION OF G MUST BE * */
2251 /* * GREATER OR EQUAL N+1. * */
2252 /* * A(),LA,M,N THE LA BY N + 1 ARRAY A() STORES * */
2253 /* * THE M BY N MATRIX A OF CONSTRAINT NORMALS. * */
2254 /* * A() HAS FIRST DIMENSIONING PARAMETER LA, * */
2255 /* * WHICH MUST BE GREATER OR EQUAL MAX(1,M). * */
2256 /* * F,C,G,A MUST ALL BE SET BY THE USER BEFORE EACH CALL. * */
2257 /* * * ACC ABS(ACC) CONTROLS THE FINAL ACCURACY. * */
2258 /* * IF ACC .LT. ZERO AN EXACT LINESEARCH IS PERFORMED,* */
2259 /* * OTHERWISE AN ARMIJO-TYPE LINESEARCH IS USED. * */
2260 /* * * ITER PRESCRIBES THE MAXIMUM NUMBER OF ITERATIONS. * */
2261 /* * ON EXIT ITER INDICATES THE NUMBER OF ITERATIONS. * */
2262 /* * * MODE MODE CONTROLS CALCULATION: * */
2263 /* * REVERSE COMMUNICATION IS USED IN THE SENSE THAT * */
2264 /* * THE PROGRAM IS INITIALIZED BY MODE = 0; THEN IT IS* */
2265 /* * TO BE CALLED REPEATEDLY BY THE USER UNTIL A RETURN* */
2266 /* * WITH MODE .NE. IABS(1) TAKES PLACE. * */
2267 /* * IF MODE = -1 GRADIENTS HAVE TO BE CALCULATED, * */
2268 /* * WHILE WITH MODE = 1 FUNCTIONS HAVE TO BE CALCULATED */
2269 /* * MODE MUST NOT BE CHANGED BETWEEN SUBSEQUENT CALLS * */
2271 /* * EVALUATION MODES: * */
2272 /* * MODE = -2,-1: GRADIENT EVALUATION, (G&A) * */
2273 /* * 0: ON ENTRY: INITIALIZATION, (F,G,C&A) * */
2274 /* * ON EXIT : REQUIRED ACCURACY FOR SOLUTION OBTAINED * */
2275 /* * 1: FUNCTION EVALUATION, (F&C) * */
2277 /* * FAILURE MODES: * */
2278 /* * 2: NUMBER OF EQUALITY CONTRAINTS LARGER THAN N * */
2279 /* * 3: MORE THAN 3*N ITERATIONS IN LSQ SUBPROBLEM * */
2280 /* * 4: INEQUALITY CONSTRAINTS INCOMPATIBLE * */
2281 /* * 5: SINGULAR MATRIX E IN LSQ SUBPROBLEM * */
2282 /* * 6: SINGULAR MATRIX C IN LSQ SUBPROBLEM * */
2283 /* * 7: RANK-DEFICIENT EQUALITY CONSTRAINT SUBPROBLEM HFTI* */
2284 /* * 8: POSITIVE DIRECTIONAL DERIVATIVE FOR LINESEARCH * */
2285 /* * 9: MORE THAN ITER ITERATIONS IN SQP * */
2286 /* * >=10: WORKING SPACE W OR JW TOO SMALL, * */
2287 /* * W SHOULD BE ENLARGED TO L_W=MODE/1000 * */
2288 /* * JW SHOULD BE ENLARGED TO L_JW=MODE-1000*L_W * */
2289 /* * * W(), L_W W() IS A ONE DIMENSIONAL WORKING SPACE, * */
2290 /* * THE LENGTH L_W OF WHICH SHOULD BE AT LEAST * */
2291 /* * (3*N1+M)*(N1+1) for LSQ * */
2292 /* * +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI * */
2293 /* * +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI * */
2294 /* * + N1*N/2 + 2*M + 3*N + 3*N1 + 1 for SLSQPB * */
2295 /* * with MINEQ = M - MEQ + 2*N1 & N1 = N+1 * */
2296 /* * NOTICE: FOR PROPER DIMENSIONING OF W IT IS RECOMMENDED TO * */
2297 /* * COPY THE FOLLOWING STATEMENTS INTO THE HEAD OF * */
2298 /* * THE CALLING PROGRAM (AND REMOVE THE COMMENT C) * */
2299 /* ####################################################################### */
2300 /* INT LEN_W, LEN_JW, M, N, N1, MEQ, MINEQ */
2301 /* PARAMETER (M=... , MEQ=... , N=... ) */
2302 /* PARAMETER (N1= N+1, MINEQ= M-MEQ+N1+N1) */
2303 /* PARAMETER (LEN_W= */
2304 /* $ (3*N1+M)*(N1+1) */
2305 /* $ +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ */
2306 /* $ +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 */
2307 /* $ +(N+1)*N/2 + 2*M + 3*N + 3*N1 + 1, */
2308 /* $ LEN_JW=MINEQ) */
2309 /* DOUBLE PRECISION W(LEN_W) */
2310 /* INT JW(LEN_JW) */
2311 /* ####################################################################### */
2312 /* * THE FIRST M+N+N*N1/2 ELEMENTS OF W MUST NOT BE * */
2313 /* * CHANGED BETWEEN SUBSEQUENT CALLS OF SLSQP. * */
2314 /* * ON RETURN W(1) ... W(M) CONTAIN THE MULTIPLIERS * */
2315 /* * ASSOCIATED WITH THE GENERAL CONSTRAINTS, WHILE * */
2316 /* * W(M+1) ... W(M+N(N+1)/2) STORE THE CHOLESKY FACTOR* */
2317 /* * L*D*L(T) OF THE APPROXIMATE HESSIAN OF THE * */
2318 /* * LAGRANGIAN COLUMNWISE DENSE AS LOWER TRIANGULAR * */
2319 /* * UNIT MATRIX L WITH D IN ITS 'DIAGONAL' and * */
2320 /* * W(M+N(N+1)/2+N+2 ... W(M+N(N+1)/2+N+2+M+2N) * */
2321 /* * CONTAIN THE MULTIPLIERS ASSOCIATED WITH ALL * */
2322 /* * ALL CONSTRAINTS OF THE QUADRATIC PROGRAM FINDING * */
2323 /* * THE SEARCH DIRECTION TO THE SOLUTION X* * */
2324 /* * * JW(), L_JW JW() IS A ONE DIMENSIONAL INT WORKING SPACE * */
2325 /* * THE LENGTH L_JW OF WHICH SHOULD BE AT LEAST * */
2327 /* * with MINEQ = M - MEQ + 2*N1 & N1 = N+1 * */
2329 /* * THE USER HAS TO PROVIDE THE FOLLOWING SUBROUTINES: * */
2330 /* * LDL(N,A,Z,SIG,W) : UPDATE OF THE LDL'-FACTORIZATION. * */
2331 /* * LINMIN(A,B,F,TOL) : LINESEARCH ALGORITHM IF EXACT = 1 * */
2332 /* * LSQ(M,MEQ,LA,N,NC,C,D,A,B,XL,XU,X,LAMBDA,W,....) : * */
2334 /* * SOLUTION OF THE QUADRATIC PROGRAM * */
2335 /* * QPSOL IS RECOMMENDED: * */
2336 /* * PE GILL, W MURRAY, MA SAUNDERS, MH WRIGHT: * */
2337 /* * USER'S GUIDE FOR SOL/QPSOL: * */
2338 /* * A FORTRAN PACKAGE FOR QUADRATIC PROGRAMMING, * */
2339 /* * TECHNICAL REPORT SOL 83-7, JULY 1983 * */
2340 /* * DEPARTMENT OF OPERATIONS RESEARCH, STANFORD UNIVERSITY * */
2341 /* * STANFORD, CA 94305 * */
2342 /* * QPSOL IS THE MOST ROBUST AND EFFICIENT QP-SOLVER * */
2343 /* * AS IT ALLOWS WARM STARTS WITH PROPER WORKING SETS * */
2345 /* * IF IT IS NOT AVAILABLE USE LSEI, A CONSTRAINT LINEAR LEAST * */
2346 /* * SQUARES SOLVER IMPLEMENTED USING THE SOFTWARE HFTI, LDP, NNLS * */
2347 /* * FROM C.L. LAWSON, R.J.HANSON: SOLVING LEAST SQUARES PROBLEMS, * */
2348 /* * PRENTICE HALL, ENGLEWOOD CLIFFS, 1974. * */
2349 /* * LSEI COMES WITH THIS PACKAGE, together with all necessary SR's. * */
2351 /* * TOGETHER WITH A COUPLE OF SUBROUTINES FROM BLAS LEVEL 1 * */
2353 /* * SQP IS HEAD SUBROUTINE FOR BODY SUBROUTINE SQPBDY * */
2354 /* * IN WHICH THE ALGORITHM HAS BEEN IMPLEMENTED. * */
2356 /* * IMPLEMENTED BY: DIETER KRAFT, DFVLR OBERPFAFFENHOFEN * */
2357 /* * as described in Dieter Kraft: A Software Package for * */
2358 /* * Sequential Quadratic Programming * */
2359 /* * DFVLR-FB 88-28, 1988 * */
2360 /* * which should be referenced if the user publishes results of SLSQP * */
2362 /* * DATE: APRIL - OCTOBER, 1981. * */
2363 /* * STATUS: DECEMBER, 31-ST, 1984. * */
2364 /* * STATUS: MARCH , 21-ST, 1987, REVISED TO FORTAN 77 * */
2365 /* * STATUS: MARCH , 20-th, 1989, REVISED TO MS-FORTRAN * */
2366 /* * STATUS: APRIL , 14-th, 1989, HESSE in-line coded * */
2367 /* * STATUS: FEBRUARY, 28-th, 1991, FORTRAN/2 Version 1.04 * */
2368 /* * accepts Statement Functions * */
2369 /* * STATUS: MARCH , 1-st, 1991, tested with SALFORD * */
2370 /* * FTN77/386 COMPILER VERS 2.40* */
2371 /* * in protected mode * */
2373 /* *********************************************************************** */
2375 /* * Copyright 1991: Dieter Kraft, FHM * */
2377 /* *********************************************************************** */
2378 /* dim(W) = N1*(N1+1) + MEQ*(N1+1) + MINEQ*(N1+1) for LSQ */
2379 /* +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI */
2380 /* +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI */
2381 /* + N1*N/2 + 2*M + 3*N +3*N1 + 1 for SLSQPB */
2382 /* with MINEQ = M - MEQ + 2*N1 & N1 = N+1 */
2383 /* CHECK LENGTH OF WORKING ARRAYS */
2384 /* Parameter adjustments */
2387 a_offset = 1 + a_dim1;
2398 mineq = *m - *meq + n1 + n1;
2399 il = (n1 * 3 + *m) * (n1 + 1) + (n1 - *meq + 1) * (mineq + 2) + (mineq <<
2400 1) + (n1 + mineq) * (n1 - *meq) + (*meq << 1) + n1 * *n / 2 + (*m
2401 << 1) + *n * 3 + (n1 << 2) + 1;
2403 i__1 = mineq, i__2 = n1 - *meq;
2404 im = MAX2(i__1,i__2);
2405 if (*l_w__ < il || *l_jw__ < im) {
2406 *mode = MAX2(10,il) * 1000;
2407 *mode += MAX2(10,im);
2410 /* PREPARE DATA FOR CALLING SQPBDY - INITIAL ADDRESSES IN W */
2412 il = im + MAX2(1,*m);
2414 ix = il + n1 * *n / 2 + 1;
2416 is = ir + *n + *n + MAX2(1,*m);
2417 is = ir + *n + *n + *la;
2421 slsqpb_(m, meq, la, n, &x[1], &xl[1], &xu[1], f, &c__[1], &g[1], &a[
2422 a_offset], acc, iter, mode, &w[ir], &w[il], &w[ix], &w[im], &w[is]
2423 , &w[iu], &w[iv], &w[iw], &jw[1], state);
2428 static void length_work(int *LEN_W, int *LEN_JW, int M, int MEQ, int N)
2430 int N1 = N+1, MINEQ = M-MEQ+N1+N1;
2431 *LEN_W = (3*N1+M)*(N1+1)
2432 +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ
2433 +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1
2434 +(N+1)*N/2 + 2*M + 3*N + 3*N1 + 1;
2438 nlopt_result nlopt_slsqp(unsigned n, nlopt_func f, void *f_data,
2439 unsigned m, nlopt_constraint *fc,
2440 unsigned p, nlopt_constraint *h,
2441 const double *lb, const double *ub,
2442 double *x, double *minf,
2443 nlopt_stopping *stop)
2445 slsqpb_state state = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,NULL};
2446 unsigned mtot = nlopt_count_constraints(m, fc);
2447 unsigned ptot = nlopt_count_constraints(p, h);
2448 double *work, *cgrad, *c, *grad, *w,
2449 fcur, *xcur, fprev, *xprev, *cgradtmp;
2450 int mpi = (int) (mtot + ptot), pi = (int) ptot, ni = (int) n, mpi1 = mpi > 0 ? mpi : 1;
2451 int len_w, len_jw, *jw;
2452 int mode = 0, prev_mode = 0;
2453 double acc = 0; /* we do our own convergence tests below */
2454 int iter = 0; /* tell sqsqp to ignore this check, since we check evaluation counts ourselves */
2456 nlopt_result ret = NLOPT_SUCCESS;
2457 int feasible, feasible_cur;
2458 double infeasibility = HUGE_VAL, infeasibility_cur = HUGE_VAL;
2462 max_cdim = MAX2(nlopt_max_constraint_dim(m, fc),
2463 nlopt_max_constraint_dim(p, h));
2464 length_work(&len_w, &len_jw, mpi, pi, ni);
2466 #define U(n) ((unsigned) (n))
2467 work = (double *) malloc(sizeof(double) * (U(mpi1) * (n + 1)
2469 + n+1 + n + n + max_cdim*n
2471 + sizeof(int) * U(len_jw));
2472 if (!work) return NLOPT_OUT_OF_MEMORY;
2474 c = cgrad + U(mpi1) * (n + 1);
2478 cgradtmp = xprev + n;
2479 w = cgradtmp + max_cdim*n;
2480 jw = (int *) (w + len_w);
2482 memcpy(xcur, x, sizeof(double) * n);
2483 memcpy(xprev, x, sizeof(double) * n);
2484 fprev = fcur = *minf = HUGE_VAL;
2485 feasible = feasible_cur = 0;
2487 goto eval_f_and_grad; /* eval before calling slsqp the first time */
2490 slsqp(&mpi, &pi, &mpi1, &ni,
2491 xcur, lb, ub, &fcur,
2494 w, &len_w, jw, &len_jw,
2498 case -1: /* objective & gradient evaluation */
2499 if (prev_mode == -2 && !want_grad) break; /* just evaluated this point */
2503 case 1:{ /* don't need grad unless we don't have it yet */
2504 double *newgrad = 0;
2505 double *newcgrad = 0;
2508 newcgrad = cgradtmp;
2510 feasible_cur = 1; infeasibility_cur = 0;
2511 fcur = f(n, xcur, newgrad, f_data);
2513 if (nlopt_stop_forced(stop)) {
2514 fcur = HUGE_VAL; ret = NLOPT_FORCED_STOP; goto done; }
2515 if (nlopt_isfinite(fcur)) {
2518 for (i = 0; i < p; ++i) {
2520 nlopt_eval_constraint(c+ii, newcgrad, h+i, n, xcur);
2521 if (nlopt_stop_forced(stop)) {
2522 ret = NLOPT_FORCED_STOP; goto done; }
2523 for (k = 0; k < h[i].m; ++k, ++ii) {
2525 MAX2(infeasibility_cur, fabs(c[ii]));
2527 feasible_cur && fabs(c[ii]) <= h[i].tol[k];
2529 for (j = 0; j < n; ++ j)
2530 cgrad[j*U(mpi1) + ii] = cgradtmp[k*n + j];
2534 for (i = 0; i < m; ++i) {
2536 nlopt_eval_constraint(c+ii, newcgrad, fc+i, n, xcur);
2537 if (nlopt_stop_forced(stop)) {
2538 ret = NLOPT_FORCED_STOP; goto done; }
2539 for (k = 0; k < fc[i].m; ++k, ++ii) {
2541 MAX2(infeasibility_cur, c[ii]);
2543 feasible_cur && c[ii] <= fc[i].tol[k];
2545 for (j = 0; j < n; ++ j)
2546 cgrad[j*U(mpi1) + ii] = -cgradtmp[k*n + j];
2548 c[ii] = -c[ii]; /* slsqp sign convention */
2553 case 0: /* required accuracy for solution obtained */
2555 case 8: /* positive directional derivative for linesearch */
2556 /* relaxed convergence check for a feasible_cur point,
2557 as in the SLSQP code (except xtol as well as ftol) */
2558 ret = NLOPT_ROUNDOFF_LIMITED; /* usually why deriv>0 */
2560 double save_ftol_rel = stop->ftol_rel;
2561 double save_xtol_rel = stop->xtol_rel;
2562 double save_ftol_abs = stop->ftol_abs;
2563 stop->ftol_rel *= 10;
2564 stop->ftol_abs *= 10;
2565 stop->xtol_rel *= 10;
2566 if (nlopt_stop_ftol(stop, fcur, state.f0))
2567 ret = NLOPT_FTOL_REACHED;
2568 else if (nlopt_stop_x(stop, xcur, state.x0))
2569 ret = NLOPT_XTOL_REACHED;
2570 stop->ftol_rel = save_ftol_rel;
2571 stop->ftol_abs = save_ftol_abs;
2572 stop->xtol_rel = save_xtol_rel;
2575 case 5: /* singular matrix E in LSQ subproblem */
2576 case 6: /* singular matrix C in LSQ subproblem */
2577 case 7: /* rank-deficient equality constraint subproblem HFTI */
2578 ret = NLOPT_ROUNDOFF_LIMITED;
2580 case 4: /* inequality constraints incompatible */
2581 case 3: /* more than 3*n iterations in LSQ subproblem */
2582 case 9: /* more than iter iterations in SQP */
2583 nlopt_stop_msg(stop, "bug: more than iter SQP iterations");
2584 ret = NLOPT_FAILURE;
2586 case 2: /* number of equality constraints > n */
2587 default: /* >= 10: working space w or jw too small */
2588 nlopt_stop_msg(stop, "bug: workspace is too small");
2589 ret = NLOPT_INVALID_ARGS;
2594 /* update best point so far */
2595 if (nlopt_isfinite(fcur) && ((fcur < *minf && (feasible_cur || !feasible))
2596 || (!feasible && infeasibility_cur < infeasibility))) {
2598 feasible = feasible_cur;
2599 infeasibility = infeasibility_cur;
2600 memcpy(x, xcur, sizeof(double)*n);
2603 /* note: mode == -1 corresponds to the completion of a line search,
2604 and is the only time we should check convergence (as in original slsqp code) */
2606 if (!nlopt_isinf(fprev)) {
2607 if (nlopt_stop_ftol(stop, fcur, fprev))
2608 ret = NLOPT_FTOL_REACHED;
2609 else if (nlopt_stop_x(stop, xcur, xprev))
2610 ret = NLOPT_XTOL_REACHED;
2613 memcpy(xprev, xcur, sizeof(double)*n);
2616 /* do some additional termination tests */
2617 if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
2618 else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
2619 else if (feasible && *minf < stop->minf_max) ret = NLOPT_MINF_MAX_REACHED;
2620 } while (ret == NLOPT_SUCCESS);
2623 if (nlopt_isinf(*minf)) { /* didn't find any feasible points, just return last point evaluated */
2624 if (nlopt_isinf(fcur)) { /* invalid cur. point, use previous pt. */
2626 memcpy(x, xprev, sizeof(double)*n);
2630 memcpy(x, xcur, sizeof(double)*n);