src/algs/slsqp/slsqp.c

   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). */
   5
   6 /* Table of constant values */
   7
   8 #include <math.h>
   9 #include <stdlib.h>
  10 #include <string.h>
  11
  12 #include "slsqp.h"
  13
  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 */
  18
  19
  20 /*      http://permalink.gmane.org/gmane.comp.python.scientific.devel/6725 */
  21 /*      ------ */
  22 /*      From: Deborah Cotton <cotton@hq.acm.org> */
  23 /*      Date: Fri, 14 Sep 2007 12:35:55 -0500 */
  24 /*      Subject: RE: Algorithm License requested */
  25 /*      To: Alan Isaac */
  26
  27 /*      Prof. Issac, */
  28
  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. */
  32
  33 /*      Regards, */
  34
  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 */
  42 /*      ------ */
  43
  44 /********************************* BLAS1 routines *************************/
  45
  46 /*     COPIES A VECTOR, X, TO A VECTOR, Y, with the given increments */
  47 static void dcopy___(int *n_, const double *dx, int incx,
  48                      double *dy, int incy)
  49 {
  50      int i, n = *n_;
  51
  52      if (n <= 0) return;
  53      if (incx == 1 && incy == 1)
  54           memcpy(dy, dx, sizeof(double) * ((unsigned) n));
  55      else if (incx == 0 && incy == 1) {
  56           double x = dx[0];
  57           for (i = 0; i < n; ++i) dy[i] = x;
  58      }
  59      else {
  60           for (i = 0; i < n; ++i) dy[i*incy] = dx[i*incx];
  61      }
  62 } /* dcopy___ */
  63
  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)
  67 {
  68      int n = *n_, i;
  69      double da = *da_;
  70
  71      if (n <= 0 || da == 0) return;
  72      for (i = 0; i < n; ++i) dy[i*incy] += da * dx[i*incx];
  73 }
  74
  75 /* dot product dx dot dy. */
  76 static double ddot_sl__(int *n_, double *dx, int incx, double *dy, int incy)
  77 {
  78      int n = *n_, i;
  79      long double sum = 0;
  80      if (n <= 0) return 0;
  81      for (i = 0; i < n; ++i) sum += dx[i*incx] * dy[i*incy];
  82      return (double) sum;
  83 }
  84
  85 /* compute the L2 norm of array DX of length N, stride INCX */
  86 static double dnrm2___(int *n_, double *dx, int incx)
  87 {
  88      int i, n = *n_;
  89      double xmax = 0, scale;
  90      long double sum = 0;
  91      for (i = 0; i < n; ++i) {
  92           double xabs = fabs(dx[incx*i]);
  93           if (xmax < xabs) xmax = xabs;
  94      }
  95      if (xmax == 0) return 0;
  96      scale = 1.0 / xmax;
  97      for (i = 0; i < n; ++i) {
  98           double xs = scale * dx[incx*i];
  99           sum += xs * xs;
 100      }
 101      return xmax * sqrt((double) sum);
 102 }
 103
 104 /* apply Givens rotation */
 105 static void dsrot_(int n, double *dx, int incx,
 106                    double *dy, int incy, double *c__, double *s_)
 107 {
 108      int i;
 109      double c = *c__, s = *s_;
 110
 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;
 115      }
 116 }
 117
 118 /* construct Givens rotation */
 119 static void dsrotg_(double *da, double *db, double *c, double *s)
 120 {
 121      double absa, absb, roe, scale;
 122
 123      absa = fabs(*da); absb = fabs(*db);
 124      if (absa > absb) {
 125           roe = *da;
 126           scale = absa;
 127      }
 128      else {
 129           roe = *db;
 130           scale = absb;
 131      }
 132
 133      if (scale != 0) {
 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;
 138           *da = r;
 139           if (*c != 0 && fabs(*c) <= *s) *db = 1 / *c;
 140           else *db = *s;
 141      }
 142      else {
 143           *c = 1;
 144           *s = *da = *db = 0;
 145      }
 146 }
 147
 148 /* scales vector X(n) by constant da */
 149 static void dscal_sl__(int *n_, const double *da, double *dx, int incx)
 150 {
 151      int i, n = *n_;
 152      double alpha = *da;
 153      for (i = 0; i < n; ++i) dx[i*incx] *= alpha;
 154 }
 155
 156 /**************************************************************************/
 157
 158 static const int c__0 = 0;
 159 static const int c__1 = 1;
 160 static const int c__2 = 2;
 161
 162 #define MIN2(a,b) ((a) <= (b) ? (a) : (b))
 163 #define MAX2(a,b) ((a) >= (b) ? (a) : (b))
 164
 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)
 168 {
 169     /* Initialized data */
 170
 171     const double one = 1.;
 172
 173     /* System generated locals */
 174     int u_dim1, u_offset, i__1, i__2;
 175     double d__1;
 176
 177     /* Local variables */
 178     double b;
 179     int i__, j, i2, i3, i4;
 180     double cl, sm;
 181     int incr;
 182     double clinv;
 183
 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. */
 193 /*     U(),IUE,UP */
 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 */
 210     u_dim1 = *iue;
 211     u_offset = 1 + u_dim1;
 212     u -= u_offset;
 213     --c__;
 214
 215     /* Function Body */
 216     if (0 >= *lpivot || *lpivot >= *l1 || *l1 > *m) {
 217         goto L80;
 218     }
 219     cl = (d__1 = u[*lpivot * u_dim1 + 1], fabs(d__1));
 220     if (*mode == 2) {
 221         goto L30;
 222     }
 223 /*     ****** CONSTRUCT THE TRANSFORMATION ****** */
 224     i__1 = *m;
 225     for (j = *l1; j <= i__1; ++j) {
 226         sm = (d__1 = u[j * u_dim1 + 1], fabs(d__1));
 227 /* L10: */
 228         cl = MAX2(sm,cl);
 229     }
 230     if (cl <= 0.0) {
 231         goto L80;
 232     }
 233     clinv = one / cl;
 234 /* Computing 2nd power */
 235     d__1 = u[*lpivot * u_dim1 + 1] * clinv;
 236     sm = d__1 * d__1;
 237     i__1 = *m;
 238     for (j = *l1; j <= i__1; ++j) {
 239 /* L20: */
 240 /* Computing 2nd power */
 241         d__1 = u[j * u_dim1 + 1] * clinv;
 242         sm += d__1 * d__1;
 243     }
 244     cl *= sqrt(sm);
 245     if (u[*lpivot * u_dim1 + 1] > 0.0) {
 246         cl = -cl;
 247     }
 248     *up = u[*lpivot * u_dim1 + 1] - cl;
 249     u[*lpivot * u_dim1 + 1] = cl;
 250     goto L40;
 251 /*     ****** APPLY THE TRANSFORMATION  I+U*(U**T)/B  TO C ****** */
 252 L30:
 253     if (cl <= 0.0) {
 254         goto L80;
 255     }
 256 L40:
 257     if (*ncv <= 0) {
 258         goto L80;
 259     }
 260     b = *up * u[*lpivot * u_dim1 + 1];
 261     if (b >= 0.0) {
 262         goto L80;
 263     }
 264     b = one / b;
 265     i2 = 1 - *icv + *ice * (*lpivot - 1);
 266     incr = *ice * (*l1 - *lpivot);
 267     i__1 = *ncv;
 268     for (j = 1; j <= i__1; ++j) {
 269         i2 += *icv;
 270         i3 = i2 + incr;
 271         i4 = i3;
 272         sm = c__[i2] * *up;
 273         i__2 = *m;
 274         for (i__ = *l1; i__ <= i__2; ++i__) {
 275             sm += c__[i3] * u[i__ * u_dim1 + 1];
 276 /* L50: */
 277             i3 += *ice;
 278         }
 279         if (sm == 0.0) {
 280             goto L70;
 281         }
 282         sm *= b;
 283         c__[i2] += sm * *up;
 284         i__2 = *m;
 285         for (i__ = *l1; i__ <= i__2; ++i__) {
 286             c__[i4] += sm * u[i__ * u_dim1 + 1];
 287 /* L60: */
 288             i4 += *ice;
 289         }
 290 L70:
 291         ;
 292     }
 293 L80:
 294     return;
 295 } /* h12_ */
 296
 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)
 300 {
 301     /* Initialized data */
 302
 303     const double one = 1.;
 304     const double factor = .01;
 305
 306     /* System generated locals */
 307     int a_dim1, a_offset, i__1, i__2;
 308     double d__1;
 309
 310     /* Local variables */
 311     double c__;
 312     int i__, j, k, l;
 313     double s, t;
 314     int ii, jj, ip, iz, jz;
 315     double up;
 316     int iz1, iz2, npp1, iter;
 317     double wmax, alpha, asave;
 318     int itmax, izmax = 0, nsetp;
 319     double unorm;
 320
 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 */
 327 /*     A(),MDA,M,N */
 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 */
 358     --z__;
 359     --b;
 360     --indx;
 361     --w;
 362     --x;
 363     a_dim1 = *mda;
 364     a_offset = 1 + a_dim1;
 365     a -= a_offset;
 366
 367     /* Function Body */
 368 /*     revised          Dieter Kraft, March 1983 */
 369     *mode = 2;
 370     if (*m <= 0 || *n <= 0) {
 371         goto L290;
 372     }
 373     *mode = 1;
 374     iter = 0;
 375     itmax = *n * 3;
 376 /* STEP ONE (INITIALIZE) */
 377     i__1 = *n;
 378     for (i__ = 1; i__ <= i__1; ++i__) {
 379 /* L100: */
 380         indx[i__] = i__;
 381     }
 382     iz1 = 1;
 383     iz2 = *n;
 384     nsetp = 0;
 385     npp1 = 1;
 386     x[1] = 0.0;
 387     dcopy___(n, &x[1], 0, &x[1], 1);
 388 /* STEP TWO (COMPUTE DUAL VARIABLES) */
 389 /* .....ENTRY LOOP A */
 390 L110:
 391     if (iz1 > iz2 || nsetp >= *m) {
 392         goto L280;
 393     }
 394     i__1 = iz2;
 395     for (iz = iz1; iz <= i__1; ++iz) {
 396         j = indx[iz];
 397 /* L120: */
 398         i__2 = *m - nsetp;
 399         w[j] = ddot_sl__(&i__2, &a[npp1 + j * a_dim1], 1, &b[npp1], 1)
 400                 ;
 401     }
 402 /* STEP THREE (TEST DUAL VARIABLES) */
 403 L130:
 404     wmax = 0.0;
 405     i__2 = iz2;
 406     for (iz = iz1; iz <= i__2; ++iz) {
 407         j = indx[iz];
 408         if (w[j] <= wmax) {
 409             goto L140;
 410         }
 411         wmax = w[j];
 412         izmax = iz;
 413 L140:
 414         ;
 415     }
 416 /* .....EXIT LOOP A */
 417     if (wmax <= 0.0) {
 418         goto L280;
 419     }
 420     iz = izmax;
 421     j = indx[iz];
 422 /* STEP FOUR (TEST INDX J FOR LINEAR DEPENDENCY) */
 423     asave = a[npp1 + j * a_dim1];
 424     i__2 = npp1 + 1;
 425     h12_(&c__1, &npp1, &i__2, m, &a[j * a_dim1 + 1], &c__1, &up, &z__[1], &
 426             c__1, &c__1, &c__0);
 427     unorm = dnrm2___(&nsetp, &a[j * a_dim1 + 1], 1);
 428     t = factor * (d__1 = a[npp1 + j * a_dim1], fabs(d__1));
 429     d__1 = unorm + t;
 430     if (d__1 - unorm <= 0.0) {
 431         goto L150;
 432     }
 433     dcopy___(m, &b[1], 1, &z__[1], 1);
 434     i__2 = npp1 + 1;
 435     h12_(&c__2, &npp1, &i__2, m, &a[j * a_dim1 + 1], &c__1, &up, &z__[1], &
 436             c__1, &c__1, &c__1);
 437     if (z__[npp1] / a[npp1 + j * a_dim1] > 0.0) {
 438         goto L160;
 439     }
 440 L150:
 441     a[npp1 + j * a_dim1] = asave;
 442     w[j] = 0.0;
 443     goto L130;
 444 /* STEP FIVE (ADD COLUMN) */
 445 L160:
 446     dcopy___(m, &z__[1], 1, &b[1], 1);
 447     indx[iz] = indx[iz1];
 448     indx[iz1] = j;
 449     ++iz1;
 450     nsetp = npp1;
 451     ++npp1;
 452     i__2 = iz2;
 453     for (jz = iz1; jz <= i__2; ++jz) {
 454         jj = indx[jz];
 455 /* L170: */
 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);
 458     }
 459     k = MIN2(npp1,*mda);
 460     w[j] = 0.0;
 461     i__2 = *m - nsetp;
 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 */
 465 L180:
 466     for (ip = nsetp; ip >= 1; --ip) {
 467         if (ip == nsetp) {
 468             goto L190;
 469         }
 470         d__1 = -z__[ip + 1];
 471         daxpy_sl__(&ip, &d__1, &a[jj * a_dim1 + 1], 1, &z__[1], 1);
 472 L190:
 473         jj = indx[ip];
 474 /* L200: */
 475         z__[ip] /= a[ip + jj * a_dim1];
 476     }
 477     ++iter;
 478     if (iter <= itmax) {
 479         goto L220;
 480     }
 481 L210:
 482     *mode = 3;
 483     goto L280;
 484 /* STEP SEVEN TO TEN (STEP LENGTH ALGORITHM) */
 485 L220:
 486     alpha = one;
 487     jj = 0;
 488     i__2 = nsetp;
 489     for (ip = 1; ip <= i__2; ++ip) {
 490         if (z__[ip] > 0.0) {
 491             goto L230;
 492         }
 493         l = indx[ip];
 494         t = -x[l] / (z__[ip] - x[l]);
 495         if (alpha < t) {
 496             goto L230;
 497         }
 498         alpha = t;
 499         jj = ip;
 500 L230:
 501         ;
 502     }
 503     i__2 = nsetp;
 504     for (ip = 1; ip <= i__2; ++ip) {
 505         l = indx[ip];
 506 /* L240: */
 507         x[l] = (one - alpha) * x[l] + alpha * z__[ip];
 508     }
 509 /* .....EXIT LOOP B */
 510     if (jj == 0) {
 511         goto L110;
 512     }
 513 /* STEP ELEVEN (DELETE COLUMN) */
 514     i__ = indx[jj];
 515 L250:
 516     x[i__] = 0.0;
 517     ++jj;
 518     i__2 = nsetp;
 519     for (j = jj; j <= i__2; ++j) {
 520         ii = indx[j];
 521         indx[j - 1] = ii;
 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;
 527 /* L260: */
 528         dsrot_(1, &b[j - 1], 1, &b[j], 1, &c__, &s);
 529     }
 530     npp1 = nsetp;
 531     --nsetp;
 532     --iz1;
 533     indx[iz1] = i__;
 534     if (nsetp <= 0) {
 535         goto L210;
 536     }
 537     i__2 = nsetp;
 538     for (jj = 1; jj <= i__2; ++jj) {
 539         i__ = indx[jj];
 540         if (x[i__] <= 0.0) {
 541             goto L250;
 542         }
 543 /* L270: */
 544     }
 545     dcopy___(m, &b[1], 1, &z__[1], 1);
 546     goto L180;
 547 /* STEP TWELVE (SOLUTION) */
 548 L280:
 549     k = MIN2(npp1,*m);
 550     i__2 = *m - nsetp;
 551     *rnorm = dnrm2___(&i__2, &b[k], 1);
 552     if (npp1 > *m) {
 553         w[1] = 0.0;
 554         dcopy___(n, &w[1], 0, &w[1], 1);
 555     }
 556 /* END OF SUBROUTINE NNLS */
 557 L290:
 558     return;
 559 } /* nnls_ */
 560
 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)
 564 {
 565     /* Initialized data */
 566
 567     const double one = 1.;
 568
 569     /* System generated locals */
 570     int g_dim1, g_offset, i__1, i__2;
 571     double d__1;
 572
 573     /* Local variables */
 574     int i__, j, n1, if__, iw, iy, iz;
 575     double fac;
 576     double rnorm;
 577     int iwdual;
 578
 579 /*                     T */
 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 */
 602 /*                  MEANINGS: */
 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 */
 608     --indx;
 609     --h__;
 610     --x;
 611     g_dim1 = *mg;
 612     g_offset = 1 + g_dim1;
 613     g -= g_offset;
 614     --w;
 615
 616     /* Function Body */
 617     *mode = 2;
 618     if (*n <= 0) {
 619         goto L50;
 620     }
 621 /*  STATE DUAL PROBLEM */
 622     *mode = 1;
 623     x[1] = 0.0;
 624     dcopy___(n, &x[1], 0, &x[1], 1);
 625     *xnorm = 0.0;
 626     if (*m == 0) {
 627         goto L50;
 628     }
 629     iw = 0;
 630     i__1 = *m;
 631     for (j = 1; j <= i__1; ++j) {
 632         i__2 = *n;
 633         for (i__ = 1; i__ <= i__2; ++i__) {
 634             ++iw;
 635 /* L10: */
 636             w[iw] = g[j + i__ * g_dim1];
 637         }
 638         ++iw;
 639 /* L20: */
 640         w[iw] = h__[j];
 641     }
 642     if__ = iw + 1;
 643     i__1 = *n;
 644     for (i__ = 1; i__ <= i__1; ++i__) {
 645         ++iw;
 646 /* L30: */
 647         w[iw] = 0.0;
 648     }
 649     w[iw + 1] = one;
 650     n1 = *n + 1;
 651     iz = iw + 2;
 652     iy = iz + n1;
 653     iwdual = iy + *m;
 654 /*  SOLVE DUAL PROBLEM */
 655     nnls_(&w[1], &n1, &n1, m, &w[if__], &w[iy], &rnorm, &w[iwdual], &w[iz], &
 656             indx[1], mode);
 657     if (*mode != 1) {
 658         goto L50;
 659     }
 660     *mode = 4;
 661     if (rnorm <= 0.0) {
 662         goto L50;
 663     }
 664 /*  COMPUTE SOLUTION OF PRIMAL PROBLEM */
 665     fac = one - ddot_sl__(m, &h__[1], 1, &w[iy], 1);
 666     d__1 = one + fac;
 667     if (d__1 - one <= 0.0) {
 668         goto L50;
 669     }
 670     *mode = 1;
 671     fac = one / fac;
 672     i__1 = *n;
 673     for (j = 1; j <= i__1; ++j) {
 674 /* L40: */
 675         x[j] = fac * ddot_sl__(m, &g[j * g_dim1 + 1], 1, &w[iy], 1);
 676     }
 677     *xnorm = dnrm2___(n, &x[1], 1);
 678 /*  COMPUTE LAGRANGE MULTIPLIERS FOR PRIMAL PROBLEM */
 679     w[1] = 0.0;
 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 */
 683 L50:
 684     return;
 685 } /* ldp_ */
 686
 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 *
 690         jw, int *mode)
 691 {
 692     /* Initialized data */
 693
 694     const double epmach = 2.22e-16;
 695     const double one = 1.;
 696
 697     /* System generated locals */
 698     int e_dim1, e_offset, g_dim1, g_offset, i__1, i__2, i__3;
 699     double d__1;
 700
 701     /* Local variables */
 702     int i__, j;
 703     double t;
 704
 705 /*     FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF */
 706 /*     INEQUALITY CONSTRAINED LINEAR LEAST SQUARES PROBLEM: */
 707 /*                    MIN ||E*X-F|| */
 708 /*                     X */
 709 /*                    S.T.  G*X >= H */
 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 */
 713 /*     ARE NECESSARY */
 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  ) */
 718 /*     DIM(X) :   N */
 719 /*     DIM(W) :   (N+1)*(MG+2) + 2*MG */
 720 /*     DIM(JW):   LG */
 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 */
 726 /*           MG ELEMENTS */
 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 */
 736     --f;
 737     --jw;
 738     --h__;
 739     --x;
 740     g_dim1 = *lg;
 741     g_offset = 1 + g_dim1;
 742     g -= g_offset;
 743     e_dim1 = *le;
 744     e_offset = 1 + e_dim1;
 745     e -= e_offset;
 746     --w;
 747
 748     /* Function Body */
 749 /*  QR-FACTORS OF E AND APPLICATION TO F */
 750     i__1 = *n;
 751     for (i__ = 1; i__ <= i__1; ++i__) {
 752 /* Computing MIN */
 753         i__2 = i__ + 1;
 754         j = MIN2(i__2,*n);
 755         i__2 = i__ + 1;
 756         i__3 = *n - 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);
 759 /* L10: */
 760         i__2 = i__ + 1;
 761         h12_(&c__2, &i__, &i__2, me, &e[i__ * e_dim1 + 1], &c__1, &t, &f[1], &
 762                 c__1, &c__1, &c__1);
 763     }
 764 /*  TRANSFORM G AND H TO GET LEAST DISTANCE PROBLEM */
 765     *mode = 5;
 766     i__2 = *mg;
 767     for (i__ = 1; i__ <= i__2; ++i__) {
 768         i__1 = *n;
 769         for (j = 1; j <= i__1; ++j) {
 770             if ((d__1 = e[j + j * e_dim1], fabs(d__1)) < epmach) {
 771                 goto L50;
 772             }
 773 /* L20: */
 774             i__3 = j - 1;
 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 *
 777                      e_dim1];
 778         }
 779 /* L30: */
 780         h__[i__] -= ddot_sl__(n, &g[i__ + g_dim1], *lg, &f[1], 1);
 781     }
 782 /*  SOLVE LEAST DISTANCE PROBLEM */
 783     ldp_(&g[g_offset], lg, mg, n, &h__[1], &x[1], xnorm, &w[1], &jw[1], mode);
 784     if (*mode != 1) {
 785         goto L50;
 786     }
 787 /*  SOLUTION OF ORIGINAL PROBLEM */
 788     daxpy_sl__(n, &one, &f[1], 1, &x[1], 1);
 789     for (i__ = *n; i__ >= 1; --i__) {
 790 /* Computing MIN */
 791         i__2 = i__ + 1;
 792         j = MIN2(i__2,*n);
 793 /* L40: */
 794         i__2 = *n - 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];
 797     }
 798 /* Computing MIN */
 799     i__2 = *n + 1;
 800     j = MIN2(i__2,*me);
 801     i__2 = *me - *n;
 802     t = dnrm2___(&i__2, &f[j], 1);
 803     *xnorm = sqrt(*xnorm * *xnorm + t * t);
 804 /*  END OF SUBROUTINE LSI */
 805 L50:
 806     return;
 807 } /* lsi_ */
 808
 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 *
 812         ip)
 813 {
 814     /* Initialized data */
 815
 816     const double factor = .001;
 817
 818     /* System generated locals */
 819     int a_dim1, a_offset, b_dim1, b_offset, i__1, i__2, i__3;
 820     double d__1;
 821
 822     /* Local variables */
 823     int i__, j, k, l;
 824     int jb, kp1;
 825     double tmp, hmax;
 826     int lmax, ldiag;
 827
 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 */
 856     --ip;
 857     --g;
 858     --h__;
 859     a_dim1 = *mda;
 860     a_offset = 1 + a_dim1;
 861     a -= a_offset;
 862     --rnorm;
 863     b_dim1 = *mdb;
 864     b_offset = 1 + b_dim1;
 865     b -= b_offset;
 866
 867     /* Function Body */
 868     k = 0;
 869     ldiag = MIN2(*m,*n);
 870     if (ldiag <= 0) {
 871         goto L270;
 872     }
 873 /*   COMPUTE LMAX */
 874     i__1 = ldiag;
 875     for (j = 1; j <= i__1; ++j) {
 876         if (j == 1) {
 877             goto L20;
 878         }
 879         lmax = j;
 880         i__2 = *n;
 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;
 885 /* L10: */
 886             if (h__[l] > h__[lmax]) {
 887                 lmax = l;
 888             }
 889         }
 890         d__1 = hmax + factor * h__[lmax];
 891         if (d__1 - hmax > 0.0) {
 892             goto L50;
 893         }
 894 L20:
 895         lmax = j;
 896         i__2 = *n;
 897         for (l = j; l <= i__2; ++l) {
 898             h__[l] = 0.0;
 899             i__3 = *m;
 900             for (i__ = j; i__ <= i__3; ++i__) {
 901 /* L30: */
 902 /* Computing 2nd power */
 903                 d__1 = a[i__ + l * a_dim1];
 904                 h__[l] += d__1 * d__1;
 905             }
 906 /* L40: */
 907             if (h__[l] > h__[lmax]) {
 908                 lmax = l;
 909             }
 910         }
 911         hmax = h__[lmax];
 912 /*   COLUMN INTERCHANGES IF NEEDED */
 913 L50:
 914         ip[j] = lmax;
 915         if (ip[j] == j) {
 916             goto L70;
 917         }
 918         i__2 = *m;
 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];
 922 /* L60: */
 923             a[i__ + lmax * a_dim1] = tmp;
 924         }
 925         h__[lmax] = h__[j];
 926 /*   J-TH TRANSFORMATION AND APPLICATION TO A AND B */
 927 L70:
 928 /* Computing MIN */
 929         i__2 = j + 1;
 930         i__ = MIN2(i__2,*n);
 931         i__2 = j + 1;
 932         i__3 = *n - j;
 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);
 935 /* L80: */
 936         i__2 = j + 1;
 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);
 939     }
 940 /*   DETERMINE PSEUDORANK */
 941     i__2 = ldiag;
 942     for (j = 1; j <= i__2; ++j) {
 943 /* L90: */
 944         if ((d__1 = a[j + j * a_dim1], fabs(d__1)) <= *tau) {
 945             goto L100;
 946         }
 947     }
 948     k = ldiag;
 949     goto L110;
 950 L100:
 951     k = j - 1;
 952 L110:
 953     kp1 = k + 1;
 954 /*   NORM OF RESIDUALS */
 955     i__2 = *nb;
 956     for (jb = 1; jb <= i__2; ++jb) {
 957 /* L130: */
 958         i__1 = *m - k;
 959         rnorm[jb] = dnrm2___(&i__1, &b[kp1 + jb * b_dim1], 1);
 960     }
 961     if (k > 0) {
 962         goto L160;
 963     }
 964     i__1 = *nb;
 965     for (jb = 1; jb <= i__1; ++jb) {
 966         i__2 = *n;
 967         for (i__ = 1; i__ <= i__2; ++i__) {
 968 /* L150: */
 969             b[i__ + jb * b_dim1] = 0.0;
 970         }
 971     }
 972     goto L270;
 973 L160:
 974     if (k == *n) {
 975         goto L180;
 976     }
 977 /*   HOUSEHOLDER DECOMPOSITION OF FIRST K ROWS */
 978     for (i__ = k; i__ >= 1; --i__) {
 979 /* L170: */
 980         i__2 = i__ - 1;
 981         h12_(&c__1, &i__, &kp1, n, &a[i__ + a_dim1], mda, &g[i__], &a[
 982                 a_offset], mda, &c__1, &i__2);
 983     }
 984 L180:
 985     i__2 = *nb;
 986     for (jb = 1; jb <= i__2; ++jb) {
 987 /*   SOLVE K*K TRIANGULAR SYSTEM */
 988         for (i__ = k; i__ >= 1; --i__) {
 989 /* Computing MIN */
 990             i__1 = i__ + 1;
 991             j = MIN2(i__1,*n);
 992 /* L210: */
 993             i__1 = k - 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];
 997         }
 998 /*   COMPLETE SOLUTION VECTOR */
 999         if (k == *n) {
1000             goto L240;
1001         }
1002         i__1 = *n;
1003         for (j = kp1; j <= i__1; ++j) {
1004 /* L220: */
1005             b[j + jb * b_dim1] = 0.0;
1006         }
1007         i__1 = k;
1008         for (i__ = 1; i__ <= i__1; ++i__) {
1009 /* L230: */
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);
1012         }
1013 /*   REORDER SOLUTION ACCORDING TO PREVIOUS COLUMN INTERCHANGES */
1014 L240:
1015         for (j = ldiag; j >= 1; --j) {
1016             if (ip[j] == j) {
1017                 goto L250;
1018             }
1019             l = ip[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;
1023 L250:
1024             ;
1025         }
1026     }
1027 L270:
1028     *krank = k;
1029 } /* hfti_ */
1030
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 *
1035         mode)
1036 {
1037     /* Initialized data */
1038
1039     const double epmach = 2.22e-16;
1040
1041     /* System generated locals */
1042     int c_dim1, c_offset, e_dim1, e_offset, g_dim1, g_offset, i__1, i__2,
1043             i__3;
1044     double d__1;
1045
1046     /* Local variables */
1047     int i__, j, k, l;
1048     double t;
1049     int ie, if__, ig, iw, mc1;
1050     int krank;
1051
1052 /*     FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF */
1053 /*     EQUALITY & INEQUALITY CONSTRAINED LEAST SQUARES PROBLEM LSEI : */
1054 /*                MIN ||E*X - F|| */
1055 /*                 X */
1056 /*                S.T.  C*X  = D, */
1057 /*                      G*X >= H. */
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 */
1061 /*     ARE NECESSARY */
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 */
1089     --d__;
1090     --f;
1091     --h__;
1092     --x;
1093     g_dim1 = *lg;
1094     g_offset = 1 + g_dim1;
1095     g -= g_offset;
1096     e_dim1 = *le;
1097     e_offset = 1 + e_dim1;
1098     e -= e_offset;
1099     c_dim1 = *lc;
1100     c_offset = 1 + c_dim1;
1101     c__ -= c_offset;
1102     --w;
1103     --jw;
1104
1105     /* Function Body */
1106     *mode = 2;
1107     if (*mc > *n) {
1108         goto L75;
1109     }
1110     l = *n - *mc;
1111     mc1 = *mc + 1;
1112     iw = (l + 1) * (*mg + 2) + (*mg << 1) + *mc;
1113     ie = iw + *mc + 1;
1114     if__ = ie + *me * l;
1115     ig = if__ + *me;
1116 /*  TRIANGULARIZE C AND APPLY FACTORS TO E AND G */
1117     i__1 = *mc;
1118     for (i__ = 1; i__ <= i__1; ++i__) {
1119 /* Computing MIN */
1120         i__2 = i__ + 1;
1121         j = MIN2(i__2,*lc);
1122         i__2 = i__ + 1;
1123         i__3 = *mc - 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);
1126         i__2 = i__ + 1;
1127         h12_(&c__2, &i__, &i__2, n, &c__[i__ + c_dim1], lc, &w[iw + i__], &e[
1128                 e_offset], le, &c__1, me);
1129 /* L10: */
1130         i__2 = i__ + 1;
1131         h12_(&c__2, &i__, &i__2, n, &c__[i__ + c_dim1], lc, &w[iw + i__], &g[
1132                 g_offset], lg, &c__1, mg);
1133     }
1134 /*  SOLVE C*X=D AND MODIFY F */
1135     *mode = 6;
1136     i__2 = *mc;
1137     for (i__ = 1; i__ <= i__2; ++i__) {
1138         if ((d__1 = c__[i__ + i__ * c_dim1], fabs(d__1)) < epmach) {
1139             goto L75;
1140         }
1141         i__1 = i__ - 1;
1142         x[i__] = (d__[i__] - ddot_sl__(&i__1, &c__[i__ + c_dim1], *lc, &x[1], 1))
1143              / c__[i__ + i__ * c_dim1];
1144 /* L15: */
1145     }
1146     *mode = 1;
1147     w[mc1] = 0.0;
1148     i__2 = *mg; /* BUGFIX for *mc == *n: changed from *mg - *mc, SGJ 2010 */
1149     dcopy___(&i__2, &w[mc1], 0, &w[mc1], 1);
1150     if (*mc == *n) {
1151         goto L50;
1152     }
1153     i__2 = *me;
1154     for (i__ = 1; i__ <= i__2; ++i__) {
1155 /* L20: */
1156         w[if__ - 1 + i__] = f[i__] - ddot_sl__(mc, &e[i__ + e_dim1], *le, &x[1], 1);
1157     }
1158 /*  STORE TRANSFORMED E & G */
1159     i__2 = *me;
1160     for (i__ = 1; i__ <= i__2; ++i__) {
1161 /* L25: */
1162         dcopy___(&l, &e[i__ + mc1 * e_dim1], *le, &w[ie - 1 + i__], *me);
1163     }
1164     i__2 = *mg;
1165     for (i__ = 1; i__ <= i__2; ++i__) {
1166 /* L30: */
1167         dcopy___(&l, &g[i__ + mc1 * g_dim1], *lg, &w[ig - 1 + i__], *mg);
1168     }
1169     if (*mg > 0) {
1170         goto L40;
1171     }
1172 /*  SOLVE LS WITHOUT INEQUALITY CONSTRAINTS */
1173     *mode = 7;
1174     k = MAX2(*le,*n);
1175     t = sqrt(epmach);
1176     hfti_(&w[ie], me, me, &l, &w[if__], &k, &c__1, &t, &krank, xnrm, &w[1], &
1177             w[l + 1], &jw[1]);
1178     dcopy___(&l, &w[if__], 1, &x[mc1], 1);
1179     if (krank != l) {
1180         goto L75;
1181     }
1182     *mode = 1;
1183     goto L50;
1184 /*  MODIFY H AND SOLVE INEQUALITY CONSTRAINED LS PROBLEM */
1185 L40:
1186     i__2 = *mg;
1187     for (i__ = 1; i__ <= i__2; ++i__) {
1188 /* L45: */
1189         h__[i__] -= ddot_sl__(mc, &g[i__ + g_dim1], *lg, &x[1], 1);
1190     }
1191     lsi_(&w[ie], &w[if__], &w[ig], &h__[1], me, me, mg, mg, &l, &x[mc1], xnrm,
1192              &w[mc1], &jw[1], mode);
1193     if (*mc == 0) {
1194         goto L75;
1195     }
1196     t = dnrm2___(mc, &x[1], 1);
1197     *xnrm = sqrt(*xnrm * *xnrm + t * t);
1198     if (*mode != 1) {
1199         goto L75;
1200     }
1201 /*  SOLUTION OF ORIGINAL PROBLEM AND LAGRANGE MULTIPLIERS */
1202 L50:
1203     i__2 = *me;
1204     for (i__ = 1; i__ <= i__2; ++i__) {
1205 /* L55: */
1206         f[i__] = ddot_sl__(n, &e[i__ + e_dim1], *le, &x[1], 1) - f[i__];
1207     }
1208     i__2 = *mc;
1209     for (i__ = 1; i__ <= i__2; ++i__) {
1210 /* L60: */
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);
1213     }
1214     for (i__ = *mc; i__ >= 1; --i__) {
1215 /* L65: */
1216         i__2 = i__ + 1;
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);
1219     }
1220     for (i__ = *mc; i__ >= 1; --i__) {
1221 /* Computing MIN */
1222         i__2 = i__ + 1;
1223         j = MIN2(i__2,*lc);
1224         i__2 = *mc - 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];
1227 /* L70: */
1228     }
1229 /*  END OF SUBROUTINE LSEI */
1230 L75:
1231     return;
1232 } /* lsei_ */
1233
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)
1238 {
1239     /* Initialized data */
1240
1241     const double one = 1.;
1242
1243     /* System generated locals */
1244     int a_dim1, a_offset, i__1, i__2;
1245     double d__1;
1246
1247     /* Local variables */
1248     int i__, i1, i2, i3, i4, m1, n1, n2, n3, ic, id, ie, if__, ig, ih, il,
1249              im, ip, iu, iw;
1250     double diag;
1251     int mineq;
1252     double xnorm;
1253
1254 /*   MINIMIZE with respect to X */
1255 /*             ||E*X - F|| */
1256 /*                                      1/2  T */
1257 /*   WITH UPPER TRIANGULAR MATRIX E = +D   *L , */
1258 /*                                      -1/2  -1 */
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 */
1263 /*   SUBJECT TO */
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 */
1289     --y;
1290     --x;
1291     --xu;
1292     --xl;
1293     --g;
1294     --l;
1295     --b;
1296     a_dim1 = *la;
1297     a_offset = 1 + a_dim1;
1298     a -= a_offset;
1299     --w;
1300     --jw;
1301
1302     /* Function Body */
1303     n1 = *n + 1;
1304     mineq = *m - *meq;
1305     m1 = mineq + *n + *n;
1306 /*  determine whether to solve problem */
1307 /*  with inconsistent linerarization (n2=1) */
1308 /*  or not (n2=0) */
1309     n2 = n1 * *n / 2 + 1;
1310     if (n2 == *nl) {
1311         n2 = 0;
1312     } else {
1313         n2 = 1;
1314     }
1315     n3 = *n - n2;
1316 /*  RECOVER MATRIX E AND VECTOR F FROM L AND G */
1317     i2 = 1;
1318     i3 = 1;
1319     i4 = 1;
1320     ie = 1;
1321     if__ = *n * *n + 1;
1322     i__1 = n3;
1323     for (i__ = 1; i__ <= i__1; ++i__) {
1324         i1 = n1 - i__;
1325         diag = sqrt(l[i2]);
1326         w[i3] = 0.0;
1327         dcopy___(&i1, &w[i3], 0, &w[i3], 1);
1328         i__2 = i1 - n2;
1329         dcopy___(&i__2, &l[i2], 1, &w[i3], *n);
1330         i__2 = i1 - n2;
1331         dscal_sl__(&i__2, &diag, &w[i3], *n);
1332         w[i3] = diag;
1333         i__2 = i__ - 1;
1334         w[if__ - 1 + i__] = (g[i__] - ddot_sl__(&i__2, &w[i4], 1, &w[if__]
1335                 , 1)) / diag;
1336         i2 = i2 + i1 - n2;
1337         i3 += n1;
1338         i4 += *n;
1339 /* L10: */
1340     }
1341     if (n2 == 1) {
1342         w[i3] = l[*nl];
1343         w[i4] = 0.0;
1344         dcopy___(&n3, &w[i4], 0, &w[i4], 1);
1345         w[if__ - 1 + *n] = 0.0;
1346     }
1347     d__1 = -one;
1348     dscal_sl__(n, &d__1, &w[if__], 1);
1349     ic = if__ + *n;
1350     id = ic + *meq * *n;
1351     if (*meq > 0) {
1352 /*  RECOVER MATRIX C FROM UPPER PART OF A */
1353         i__1 = *meq;
1354         for (i__ = 1; i__ <= i__1; ++i__) {
1355             dcopy___(n, &a[i__ + a_dim1], *la, &w[ic - 1 + i__], *meq);
1356 /* L20: */
1357         }
1358 /*  RECOVER VECTOR D FROM UPPER PART OF B */
1359         dcopy___(meq, &b[1], 1, &w[id], 1);
1360         d__1 = -one;
1361         dscal_sl__(meq, &d__1, &w[id], 1);
1362     }
1363     ig = id + *meq;
1364     if (mineq > 0) {
1365 /*  RECOVER MATRIX G FROM LOWER PART OF A */
1366         i__1 = mineq;
1367         for (i__ = 1; i__ <= i__1; ++i__) {
1368             dcopy___(n, &a[*meq + i__ + a_dim1], *la, &w[ig - 1 + i__], m1);
1369 /* L30: */
1370         }
1371     }
1372 /*  AUGMENT MATRIX G BY +I AND -I */
1373     ip = ig + mineq;
1374     i__1 = *n;
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);
1378 /* L40: */
1379     }
1380     i__1 = m1 + 1;
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); */
1385     im = ip + *n;
1386     i__1 = *n;
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);
1390 /* L50: */
1391     }
1392     i__1 = m1 + 1;
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); */
1397     ih = ig + m1 * *n;
1398     if (mineq > 0) {
1399 /*  RECOVER H FROM LOWER PART OF B */
1400         dcopy___(&mineq, &b[*meq + 1], 1, &w[ih], 1);
1401         d__1 = -one;
1402         dscal_sl__(&mineq, &d__1, &w[ih], 1);
1403     }
1404 /*  AUGMENT VECTOR H BY XL AND XU */
1405     il = ih + mineq;
1406     iu = il + *n;
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__];
1411     }
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); */
1415     iw = iu + *n;
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);
1419     if (*mode == 1) {
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);
1424
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 */
1428         i__1 = *n;
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__];
1432         }
1433     }
1434 /*   END OF SUBROUTINE LSQ */
1435 } /* lsq_ */
1436
1437 static void ldl_(int *n, double *a, double *z__,
1438         double *sigma, double *w)
1439 {
1440     /* Initialized data */
1441
1442     const double one = 1.;
1443     const double four = 4.;
1444     const double epmach = 2.22e-16;
1445
1446     /* System generated locals */
1447     int i__1, i__2;
1448
1449     /* Local variables */
1450     int i__, j;
1451     double t, u, v;
1452     int ij;
1453     double tp, beta, gamma_, alpha, delta;
1454
1455 /*   LDL     LDL' - RANK-ONE - UPDATE */
1456 /*   PURPOSE: */
1457 /*           UPDATES THE LDL' FACTORS OF MATRIX A BY RANK-ONE MATRIX */
1458 /*           SIGMA*Z*Z' */
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 */
1466 /*             MULTIPLIED */
1467 /*   OUTPUT ARGUMENTS: */
1468 /*     A     : UPDATED LDL' FACTORS */
1469 /*   WORKING ARRAY: */
1470 /*     W     : VECTOR OP DIMENSION N (USED ONLY IF SIGMA .LT. ZERO) */
1471 /*   METHOD: */
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 */
1481     --w;
1482     --z__;
1483     --a;
1484
1485     /* Function Body */
1486     if (*sigma == 0.0) {
1487         goto L280;
1488     }
1489     ij = 1;
1490     t = one / *sigma;
1491     if (*sigma > 0.0) {
1492         goto L220;
1493     }
1494 /* PREPARE NEGATIVE UPDATE */
1495     i__1 = *n;
1496     for (i__ = 1; i__ <= i__1; ++i__) {
1497 /* L150: */
1498         w[i__] = z__[i__];
1499     }
1500     i__1 = *n;
1501     for (i__ = 1; i__ <= i__1; ++i__) {
1502         v = w[i__];
1503         t += v * v / a[ij];
1504         i__2 = *n;
1505         for (j = i__ + 1; j <= i__2; ++j) {
1506             ++ij;
1507 /* L160: */
1508             w[j] -= v * a[ij];
1509         }
1510 /* L170: */
1511         ++ij;
1512     }
1513     if (t >= 0.0) {
1514         t = epmach / *sigma;
1515     }
1516     i__1 = *n;
1517     for (i__ = 1; i__ <= i__1; ++i__) {
1518         j = *n + 1 - i__;
1519         ij -= i__;
1520         u = w[j];
1521         w[j] = t;
1522 /* L210: */
1523         t -= u * u / a[ij];
1524     }
1525 L220:
1526 /* HERE UPDATING BEGINS */
1527     i__1 = *n;
1528     for (i__ = 1; i__ <= i__1; ++i__) {
1529         v = z__[i__];
1530         delta = v / a[ij];
1531         if (*sigma < 0.0) {
1532             tp = w[i__];
1533         }
1534         else /* if (*sigma > 0.0), since *sigma != 0 from above */ {
1535             tp = t + delta * v;
1536         }
1537         alpha = tp / t;
1538         a[ij] = alpha * a[ij];
1539         if (i__ == *n) {
1540             goto L280;
1541         }
1542         beta = delta / tp;
1543         if (alpha > four) {
1544             goto L240;
1545         }
1546         i__2 = *n;
1547         for (j = i__ + 1; j <= i__2; ++j) {
1548             ++ij;
1549             z__[j] -= v * a[ij];
1550 /* L230: */
1551             a[ij] += beta * z__[j];
1552         }
1553         goto L260;
1554 L240:
1555         gamma_ = t / tp;
1556         i__2 = *n;
1557         for (j = i__ + 1; j <= i__2; ++j) {
1558             ++ij;
1559             u = a[ij];
1560             a[ij] = gamma_ * u + beta * z__[j];
1561 /* L250: */
1562             z__[j] -= v * u;
1563         }
1564 L260:
1565         ++ij;
1566 /* L270: */
1567         t = tp;
1568     }
1569 L280:
1570     return;
1571 /* END OF LDL */
1572 } /* ldl_ */
1573
1574 #if 0
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 *
1586         f, double *tol)
1587 {
1588     /* Initialized data */
1589
1590     const double c__ = .381966011;
1591     const double eps = 1.5e-8;
1592
1593     /* System generated locals */
1594     double ret_val, d__1;
1595
1596     /* Local variables */
1597     double a, b, d__, e, m, p, q, r__, u, v, w, x, fu, fv, fw, fx, tol1,
1598             tol2;
1599
1600 /*   LINMIN  LINESEARCH WITHOUT DERIVATIVES */
1601 /*   PURPOSE: */
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: */
1619 /*  NONE */
1620 /*   METHOD: */
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 */
1632     switch (*mode) {
1633         case 1:  goto L10;
1634         case 2:  goto L55;
1635     }
1636 /*  INITIALIZATION */
1637     a = *ax;
1638     b = *bx;
1639     e = 0.0;
1640     v = a + c__ * (b - a);
1641     w = v;
1642     x = w;
1643     ret_val = x;
1644     *mode = 1;
1645     goto L100;
1646 /*  MAIN LOOP STARTS HERE */
1647 L10:
1648     fx = *f;
1649     fv = fx;
1650     fw = fv;
1651 L20:
1652     m = (a + b) * .5;
1653     tol1 = eps * fabs(x) + *tol;
1654     tol2 = tol1 + tol1;
1655 /*  TEST CONVERGENCE */
1656     if ((d__1 = x - m, fabs(d__1)) <= tol2 - (b - a) * .5) {
1657         goto L90;
1658     }
1659     r__ = 0.0;
1660     q = r__;
1661     p = q;
1662     if (fabs(e) <= tol1) {
1663         goto L30;
1664     }
1665 /*  FIT PARABOLA */
1666     r__ = (x - w) * (fx - fv);
1667     q = (x - v) * (fx - fw);
1668     p = (x - v) * q - (x - w) * r__;
1669     q -= r__;
1670     q += q;
1671     if (q > 0.0) {
1672         p = -p;
1673     }
1674     if (q < 0.0) {
1675         q = -q;
1676     }
1677     r__ = e;
1678     e = d__;
1679 /*  IS PARABOLA ACCEPTABLE */
1680 L30:
1681     if (fabs(p) >= (d__1 = q * r__, fabs(d__1)) * .5 || p <= q * (a - x) || p >=
1682              q * (b - x)) {
1683         goto L40;
1684     }
1685 /*  PARABOLIC INTERPOLATION STEP */
1686     d__ = p / q;
1687 /*  F MUST NOT BE EVALUATED TOO CLOSE TO A OR B */
1688     if (u - a < tol2) {
1689         d__1 = m - x;
1690         d__ = d_sign(tol1, d__1);
1691     }
1692     if (b - u < tol2) {
1693         d__1 = m - x;
1694         d__ = d_sign(tol1, d__1);
1695     }
1696     goto L50;
1697 /*  GOLDEN SECTION STEP */
1698 L40:
1699     if (x >= m) {
1700         e = a - x;
1701     }
1702     if (x < m) {
1703         e = b - x;
1704     }
1705     d__ = c__ * e;
1706 /*  F MUST NOT BE EVALUATED TOO CLOSE TO X */
1707 L50:
1708     if (fabs(d__) < tol1) {
1709         d__ = d_sign(tol1, d__);
1710     }
1711     u = x + d__;
1712     ret_val = u;
1713     *mode = 2;
1714     goto L100;
1715 L55:
1716     fu = *f;
1717 /*  UPDATE A, B, V, W, AND X */
1718     if (fu > fx) {
1719         goto L60;
1720     }
1721     if (u >= x) {
1722         a = x;
1723     }
1724     if (u < x) {
1725         b = x;
1726     }
1727     v = w;
1728     fv = fw;
1729     w = x;
1730     fw = fx;
1731     x = u;
1732     fx = fu;
1733     goto L85;
1734 L60:
1735     if (u < x) {
1736         a = u;
1737     }
1738     if (u >= x) {
1739         b = u;
1740     }
1741     if (fu <= fw || w == x) {
1742         goto L70;
1743     }
1744     if (fu <= fv || v == x || v == w) {
1745         goto L80;
1746     }
1747     goto L85;
1748 L70:
1749     v = w;
1750     fv = fw;
1751     w = u;
1752     fw = fu;
1753     goto L85;
1754 L80:
1755     v = u;
1756     fv = fu;
1757 L85:
1758     goto L20;
1759 /*  END OF MAIN LOOP */
1760 L90:
1761     ret_val = x;
1762     *mode = 3;
1763 L100:
1764     return ret_val;
1765 /*  END OF LINMIN */
1766 } /* linmin_ */
1767 #endif
1768
1769 typedef struct {
1770     double t, f0, h1, h2, h3, h4;
1771     int n1, n2, n3;
1772     double t0, gs;
1773     double tol;
1774     int line;
1775     double alpha;
1776     int iexact;
1777     int incons, ireset, itermx;
1778     double *x0;
1779 } slsqpb_state;
1780
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); \
1785      SS(t0); SS(gs); \
1786      SS(tol); \
1787      SS(line); \
1788      SS(alpha); \
1789      SS(iexact); \
1790      SS(incons); SS(ireset); SS(itermx)
1791
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); \
1796      RS(t0); RS(gs); \
1797      RS(tol); \
1798      RS(line); \
1799      RS(alpha); \
1800      RS(iexact); \
1801      RS(incons); RS(ireset); RS(itermx)
1802
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)
1810 {
1811     /* Initialized data */
1812
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.;
1818
1819     /* System generated locals */
1820     int a_dim1, a_offset, i__1, i__2;
1821     double d__1, d__2;
1822
1823     /* Local variables */
1824     int i__, j, k;
1825
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;
1829     int n1, n2, n3;
1830     double t0, gs;
1831     double tol;
1832     int line;
1833     double alpha;
1834     int iexact;
1835     int incons, ireset, itermx;
1836     RESTORE_STATE;
1837
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 */
1846     --mu;
1847     --c__;
1848     --v;
1849     --u;
1850     --s;
1851     --x0;
1852     --l;
1853     --r__;
1854     a_dim1 = *la;
1855     a_offset = 1 + a_dim1;
1856     a -= a_offset;
1857     --g;
1858     --xu;
1859     --xl;
1860     --x;
1861     --w;
1862     --iw;
1863
1864     /* Function Body */
1865     if (*mode == -1) {
1866         goto L260;
1867     } else if (*mode == 0) {
1868         goto L100;
1869     } else {
1870         goto L220;
1871     }
1872 L100:
1873     itermx = *iter;
1874     if (*acc >= 0.0) {
1875         iexact = 0;
1876     } else {
1877         iexact = 1;
1878     }
1879     *acc = fabs(*acc);
1880     tol = ten * *acc;
1881     *iter = 0;
1882     ireset = 0;
1883     n1 = *n + 1;
1884     n2 = n1 * *n / 2;
1885     n3 = n2 + 1;
1886     s[1] = 0.0;
1887     mu[1] = 0.0;
1888     dcopy___(n, &s[1], 0, &s[1], 1);
1889     dcopy___(m, &mu[1], 0, &mu[1], 1);
1890 /*   RESET BFGS MATRIX */
1891 L110:
1892     ++ireset;
1893     if (ireset > 5) {
1894         goto L255;
1895     }
1896     l[1] = 0.0;
1897     dcopy___(&n2, &l[1], 0, &l[1], 1);
1898     j = 1;
1899     i__1 = *n;
1900     for (i__ = 1; i__ <= i__1; ++i__) {
1901         l[j] = one;
1902         j = j + n1 - i__;
1903 /* L120: */
1904     }
1905 /*   MAIN ITERATION : SEARCH DIRECTION, STEPLENGTH, LDL'-UPDATE */
1906 L130:
1907     ++(*iter);
1908     *mode = 9;
1909     if (*iter > itermx && itermx > 0) { /* SGJ 2010: ignore if itermx <= 0 */
1910         goto L330;
1911     }
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);
1915     d__1 = -one;
1916     daxpy_sl__(n, &d__1, &x[1], 1, &u[1], 1);
1917     d__1 = -one;
1918     daxpy_sl__(n, &d__1, &x[1], 1, &v[1], 1);
1919     h4 = one;
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);
1922
1923 /*   AUGMENTED PROBLEM FOR INCONSISTENT LINEARIZATION */
1924     if (*mode == 6) {
1925         if (*n == *meq) {
1926             *mode = 4;
1927         }
1928     }
1929     if (*mode == 4) {
1930         i__1 = *m;
1931         for (j = 1; j <= i__1; ++j) {
1932             if (j <= *meq) {
1933                 a[j + n1 * a_dim1] = -c__[j];
1934             } else {
1935 /* Computing MAX */
1936                 d__1 = -c__[j];
1937                 a[j + n1 * a_dim1] = MAX2(d__1,0.0);
1938             }
1939 /* L140: */
1940         }
1941         s[1] = 0.0;
1942         dcopy___(n, &s[1], 0, &s[1], 1);
1943         h3 = 0.0;
1944         g[n1] = 0.0;
1945         l[n3] = hun;
1946         s[n1] = one;
1947         u[n1] = 0.0;
1948         v[n1] = one;
1949         incons = 0;
1950 L150:
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);
1953         h4 = one - s[n1];
1954         if (*mode == 4) {
1955             l[n3] = ten * l[n3];
1956             ++incons;
1957             if (incons > 5) {
1958                 goto L330;
1959             }
1960             goto L150;
1961         } else if (*mode != 1) {
1962             goto L330;
1963         }
1964     } else if (*mode != 1) {
1965         goto L330;
1966     }
1967 /*   UPDATE MULTIPLIERS FOR L1-TEST */
1968     i__1 = *n;
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);
1971 /* L160: */
1972     }
1973     f0 = *f;
1974     dcopy___(n, &x[1], 1, &x0[1], 1);
1975     gs = ddot_sl__(n, &g[1], 1, &s[1], 1);
1976     h1 = fabs(gs);
1977     h2 = 0.0;
1978     i__1 = *m;
1979     for (j = 1; j <= i__1; ++j) {
1980         if (j <= *meq) {
1981             h3 = c__[j];
1982         } else {
1983             h3 = 0.0;
1984         }
1985 /* Computing MAX */
1986         d__1 = -c__[j];
1987         h2 += MAX2(d__1,h3);
1988         h3 = (d__1 = r__[j], fabs(d__1));
1989 /* Computing MAX */
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));
1993 /* L170: */
1994     }
1995 /*   CHECK CONVERGENCE */
1996     *mode = 0;
1997     if (h1 < *acc && h2 < *acc) {
1998         goto L330;
1999     }
2000     h1 = 0.0;
2001     i__1 = *m;
2002     for (j = 1; j <= i__1; ++j) {
2003         if (j <= *meq) {
2004             h3 = c__[j];
2005         } else {
2006             h3 = 0.0;
2007         }
2008 /* Computing MAX */
2009         d__1 = -c__[j];
2010         h1 += mu[j] * MAX2(d__1,h3);
2011 /* L180: */
2012     }
2013     t0 = *f + h1;
2014     h3 = gs - h1 * h4;
2015     *mode = 8;
2016     if (h3 >= 0.0) {
2017         goto L110;
2018     }
2019 /*   LINE SEARCH WITH AN L1-TESTFUNCTION */
2020     line = 0;
2021     alpha = one;
2022     if (iexact == 1) {
2023         goto L210;
2024     }
2025 /*   INEXACT LINESEARCH */
2026 L190:
2027     ++line;
2028     h3 = alpha * h3;
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);
2032
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__];
2037     }
2038
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;
2045     goto L330;
2046 L200:
2047     if (nlopt_isfinite(h1)) {
2048             if (h1 <= h3 / ten || line > 10) {
2049                     goto L240;
2050             }
2051             /* Computing MAX */
2052             d__1 = h3 / (two * (h3 - h1));
2053             alpha = MAX2(d__1,alfmin);
2054     } else {
2055             alpha = MAX2(alpha*.5,alfmin);
2056     }
2057     goto L190;
2058 /*   EXACT LINESEARCH */
2059 L210:
2060 #if 0
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 */
2063     if (line != 3) {
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);
2067         *mode = 1;
2068         goto L330;
2069     }
2070 #else
2071     *mode = 9 /* will yield nlopt_failure */; return;
2072 #endif
2073     dscal_sl__(n, &alpha, &s[1], 1);
2074     goto L240;
2075 /*   CALL FUNCTIONS AT CURRENT X */
2076 L220:
2077     t = *f;
2078     i__1 = *m;
2079     for (j = 1; j <= i__1; ++j) {
2080         if (j <= *meq) {
2081             h1 = c__[j];
2082         } else {
2083             h1 = 0.0;
2084         }
2085 /* Computing MAX */
2086         d__1 = -c__[j];
2087         t += mu[j] * MAX2(d__1,h1);
2088 /* L230: */
2089     }
2090     h1 = t - t0;
2091     switch (iexact + 1) {
2092         case 1:  goto L200;
2093         case 2:  goto L210;
2094     }
2095 /*   CHECK CONVERGENCE */
2096 L240:
2097     h3 = 0.0;
2098     i__1 = *m;
2099     for (j = 1; j <= i__1; ++j) {
2100         if (j <= *meq) {
2101             h1 = c__[j];
2102         } else {
2103             h1 = 0.0;
2104         }
2105 /* Computing MAX */
2106         d__1 = -c__[j];
2107         h3 += MAX2(d__1,h1);
2108 /* L250: */
2109     }
2110     if (((d__1 = *f - f0, fabs(d__1)) < *acc || dnrm2___(n, &s[1], 1) < *
2111             acc) && h3 < *acc) {
2112         *mode = 0;
2113     } else {
2114         *mode = -1;
2115     }
2116     goto L330;
2117 /*   CHECK relaxed CONVERGENCE in case of positive directional derivative */
2118 L255:
2119     if (((d__1 = *f - f0, fabs(d__1)) < tol || dnrm2___(n, &s[1], 1) < tol)
2120              && h3 < tol) {
2121         *mode = 0;
2122     } else {
2123         *mode = 8;
2124     }
2125     goto L330;
2126 /*   CALL JACOBIAN AT CURRENT X */
2127 /*   UPDATE CHOLESKY-FACTORS OF HESSIAN MATRIX BY MODIFIED BFGS FORMULA */
2128 L260:
2129     i__1 = *n;
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__];
2132 /* L270: */
2133     }
2134 /*   L'*S */
2135     k = 0;
2136     i__1 = *n;
2137     for (i__ = 1; i__ <= i__1; ++i__) {
2138         h1 = 0.0;
2139         ++k;
2140         i__2 = *n;
2141         for (j = i__ + 1; j <= i__2; ++j) {
2142             ++k;
2143             h1 += l[k] * s[j];
2144 /* L280: */
2145         }
2146         v[i__] = s[i__] + h1;
2147 /* L290: */
2148     }
2149 /*   D*L'*S */
2150     k = 1;
2151     i__1 = *n;
2152     for (i__ = 1; i__ <= i__1; ++i__) {
2153         v[i__] = l[k] * v[i__];
2154         k = k + n1 - i__;
2155 /* L300: */
2156     }
2157 /*   L*D*L'*S */
2158     for (i__ = *n; i__ >= 1; --i__) {
2159         h1 = 0.0;
2160         k = i__;
2161         i__1 = i__ - 1;
2162         for (j = 1; j <= i__1; ++j) {
2163             h1 += l[k] * v[j];
2164             k = k + *n - j;
2165 /* L310: */
2166         }
2167         v[i__] += h1;
2168 /* L320: */
2169     }
2170     h1 = ddot_sl__(n, &s[1], 1, &u[1], 1);
2171     h2 = ddot_sl__(n, &s[1], 1, &v[1], 1);
2172     h3 = h2 * .2;
2173     if (h1 < h3) {
2174         h4 = (h2 - h3) / (h2 - h1);
2175         h1 = h3;
2176         dscal_sl__(n, &h4, &u[1], 1);
2177         d__1 = one - h4;
2178         daxpy_sl__(n, &d__1, &v[1], 1, &u[1], 1);
2179     }
2180     d__1 = one / h1;
2181     ldl_(n, &l[1], &u[1], &d__1, &v[1]);
2182     d__1 = -one / h2;
2183     ldl_(n, &l[1], &v[1], &d__1, &u[1]);
2184 /*   END OF MAIN ITERATION */
2185     goto L130;
2186 /*   END OF SLSQPB */
2187 L330:
2188     SAVE_STATE;
2189 } /* slsqpb_ */
2190
2191 /* *********************************************************************** */
2192 /*                              optimizer                               * */
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)
2199 {
2200     /* System generated locals */
2201     int a_dim1, a_offset, i__1, i__2;
2202
2203     /* Local variables */
2204     int n1, il, im, ir, is, iu, iv, iw, ix, mineq;
2205
2206 /*   SLSQP       S EQUENTIAL  L EAST  SQ UARES  P ROGRAMMING */
2207 /*            TO SOLVE GENERAL NONLINEAR OPTIMIZATION PROBLEMS */
2208 /* *********************************************************************** */
2209 /* *                                                                     * */
2210 /* *                                                                     * */
2211 /* *            A NONLINEAR PROGRAMMING METHOD WITH                      * */
2212 /* *            QUADRATIC  PROGRAMMING  SUBPROBLEMS                      * */
2213 /* *                                                                     * */
2214 /* *                                                                     * */
2215 /* *  THIS SUBROUTINE SOLVES THE GENERAL NONLINEAR PROGRAMMING PROBLEM   * */
2216 /* *                                                                     * */
2217 /* *            MINIMIZE    F(X)                                         * */
2218 /* *                                                                     * */
2219 /* *            SUBJECT TO  C (X) .EQ. 0  ,  J = 1,...,MEQ               * */
2220 /* *                         J                                           * */
2221 /* *                                                                     * */
2222 /* *                        C (X) .GE. 0  ,  J = MEQ+1,...,M             * */
2223 /* *                         J                                           * */
2224 /* *                                                                     * */
2225 /* *                        XL .LE. X .LE. XU , I = 1,...,N.             * */
2226 /* *                          I      I       I                           * */
2227 /* *                                                                     * */
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.                                   * */
2231 /* *                                                                     * */
2232 /* *    PARAMETER DESCRIPTION:                                           * */
2233 /* *    ( * MEANS THIS PARAMETER WILL BE CHANGED DURING CALCULATION )    * */
2234 /* *                                                                     * */
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 * */
2270 /* *                   OF SQP.                                           * */
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)                        * */
2276 /* *                                                                     * */
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       * */
2326 /* *                   MINEQ                                             * */
2327 /* *                   with MINEQ = M - MEQ + 2*N1  &  N1 = N+1          * */
2328 /* *                                                                     * */
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,....) :              * */
2333 /* *                                                                     * */
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               * */
2344 /* *                                                                     * */
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. * */
2350 /* *                                                                     * */
2351 /* *     TOGETHER WITH A COUPLE OF SUBROUTINES FROM BLAS LEVEL 1         * */
2352 /* *                                                                     * */
2353 /* *     SQP IS HEAD SUBROUTINE FOR BODY SUBROUTINE SQPBDY               * */
2354 /* *     IN WHICH THE ALGORITHM HAS BEEN IMPLEMENTED.                    * */
2355 /* *                                                                     * */
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  * */
2361 /* *                                                                     * */
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           * */
2372 /* *                                                                     * */
2373 /* *********************************************************************** */
2374 /* *                                                                     * */
2375 /* *  Copyright 1991: Dieter Kraft, FHM                                  * */
2376 /* *                                                                     * */
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 */
2385     --c__;
2386     a_dim1 = *la;
2387     a_offset = 1 + a_dim1;
2388     a -= a_offset;
2389     --g;
2390     --xu;
2391     --xl;
2392     --x;
2393     --w;
2394     --jw;
2395
2396     /* Function Body */
2397     n1 = *n + 1;
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;
2402 /* Computing MAX */
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);
2408         return;
2409     }
2410 /*   PREPARE DATA FOR CALLING SQPBDY  -  INITIAL ADDRESSES IN W */
2411     im = 1;
2412     il = im + MAX2(1,*m);
2413     il = im + *la;
2414     ix = il + n1 * *n / 2 + 1;
2415     ir = ix + *n;
2416     is = ir + *n + *n + MAX2(1,*m);
2417     is = ir + *n + *n + *la;
2418     iu = is + n1;
2419     iv = iu + n1;
2420     iw = iv + n1;
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);
2424     state->x0 = &w[ix];
2425     return;
2426 } /* slsqp_ */
2427
2428 static void length_work(int *LEN_W, int *LEN_JW, int M, int MEQ, int N)
2429 {
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;
2435      *LEN_JW = MINEQ;
2436 }
2437
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)
2444 {
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 */
2455      unsigned i, ii;
2456      nlopt_result ret = NLOPT_SUCCESS;
2457      int feasible, feasible_cur;
2458      double infeasibility = HUGE_VAL, infeasibility_cur = HUGE_VAL;
2459      unsigned max_cdim;
2460      int want_grad = 1;
2461
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);
2465
2466 #define U(n) ((unsigned) (n))
2467      work = (double *) malloc(sizeof(double) * (U(mpi1) * (n + 1)
2468                                                 + U(mpi)
2469                                                 + n+1 + n + n + max_cdim*n
2470                                                 + U(len_w))
2471                               + sizeof(int) * U(len_jw));
2472      if (!work) return NLOPT_OUT_OF_MEMORY;
2473      cgrad = work;
2474      c = cgrad + U(mpi1) * (n + 1);
2475      grad = c + mpi;
2476      xcur = grad + n+1;
2477      xprev = xcur + n;
2478      cgradtmp = xprev + n;
2479      w = cgradtmp + max_cdim*n;
2480      jw = (int *) (w + len_w);
2481
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;
2486
2487      goto eval_f_and_grad; /* eval before calling slsqp the first time */
2488
2489      do {
2490           slsqp(&mpi, &pi, &mpi1, &ni,
2491                 xcur, lb, ub, &fcur,
2492                 c, grad, cgrad,
2493                 &acc, &iter, &mode,
2494                 w, &len_w, jw, &len_jw,
2495                 &state);
2496
2497           switch (mode) {
2498           case -1:  /* objective & gradient evaluation */
2499               if (prev_mode == -2 && !want_grad) break; /* just evaluated this point */
2500           case -2:
2501               eval_f_and_grad:
2502               want_grad = 1;
2503           case 1:{ /* don't need grad unless we don't have it yet */
2504               double *newgrad = 0;
2505               double *newcgrad = 0;
2506               if (want_grad) {
2507                   newgrad = grad;
2508                   newcgrad = cgradtmp;
2509               }
2510               feasible_cur = 1; infeasibility_cur = 0;
2511               fcur = f(n, xcur, newgrad, f_data);
2512               ++ *(stop->nevals_p);
2513               if (nlopt_stop_forced(stop)) {
2514                   fcur = HUGE_VAL; ret = NLOPT_FORCED_STOP; goto done; }
2515               if (nlopt_isfinite(fcur)) {
2516                   want_grad = 0;
2517                   ii = 0;
2518                   for (i = 0; i < p; ++i) {
2519                       unsigned j, k;
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) {
2524                           infeasibility_cur =
2525                               MAX2(infeasibility_cur, fabs(c[ii]));
2526                           feasible_cur =
2527                               feasible_cur && fabs(c[ii]) <= h[i].tol[k];
2528                           if (newcgrad) {
2529                               for (j = 0; j < n; ++ j)
2530                                   cgrad[j*U(mpi1) + ii] = cgradtmp[k*n + j];
2531                           }
2532                       }
2533                   }
2534                   for (i = 0; i < m; ++i) {
2535                       unsigned j, k;
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) {
2540                           infeasibility_cur =
2541                               MAX2(infeasibility_cur, c[ii]);
2542                           feasible_cur =
2543                               feasible_cur && c[ii] <= fc[i].tol[k];
2544                           if (newcgrad) {
2545                               for (j = 0; j < n; ++ j)
2546                                   cgrad[j*U(mpi1) + ii] = -cgradtmp[k*n + j];
2547                           }
2548                           c[ii] = -c[ii]; /* slsqp sign convention */
2549                       }
2550                   }
2551               }
2552               break;}
2553               case 0: /* required accuracy for solution obtained */
2554                   goto done;
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 */
2559                   if (feasible_cur) {
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;
2573                   }
2574                   goto done;
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;
2579                   goto done;
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;
2585                   goto done;
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;
2590                   goto done;
2591           }
2592           prev_mode = mode;
2593
2594           /* update best point so far */
2595           if (nlopt_isfinite(fcur) && ((fcur < *minf && (feasible_cur || !feasible))
2596                                        || (!feasible && infeasibility_cur < infeasibility))) {
2597                *minf = fcur;
2598                feasible = feasible_cur;
2599                infeasibility = infeasibility_cur;
2600                memcpy(x, xcur, sizeof(double)*n);
2601           }
2602
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) */
2605           if (mode == -1) {
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;
2611                }
2612                fprev = fcur;
2613                memcpy(xprev, xcur, sizeof(double)*n);
2614           }
2615
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);
2621
2622 done:
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. */
2625                *minf = fprev;
2626                memcpy(x, xprev, sizeof(double)*n);
2627           }
2628           else {
2629                *minf = fcur;
2630                memcpy(x, xcur, sizeof(double)*n);
2631           }
2632      }
2633
2634      free(work);
2635      return ret;
2636 }