cobyla/cobyla.c

   1 /* cobyla : contrained optimization by linear approximation */
   2
   3 /*
   4  * Copyright (c) 1992, Michael J. D. Powell (M.J.D.Powell@damtp.cam.ac.uk)
   5  * Copyright (c) 2004, Jean-Sebastien Roy (js@jeannot.org)
   6  *
   7  * Permission is hereby granted, free of charge, to any person obtaining a
   8  * copy of this software and associated documentation files (the
   9  * "Software"), to deal in the Software without restriction, including
  10  * without limitation the rights to use, copy, modify, merge, publish,
  11  * distribute, sublicense, and/or sell copies of the Software, and to
  12  * permit persons to whom the Software is furnished to do so, subject to
  13  * the following conditions:
  14  *
  15  * The above copyright notice and this permission notice shall be included
  16  * in all copies or substantial portions of the Software.
  17  *
  18  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  19  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  20  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
  21  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
  22  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
  23  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
  24  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  25  */
  26
  27 /*
  28  * This software is a C version of COBYLA2, a contrained optimization by linear
  29  * approximation package developed by Michael J. D. Powell in Fortran.
  30  *
  31  * The original source code can be found at :
  32  * http://plato.la.asu.edu/topics/problems/nlores.html
  33  */
  34
  35 static char const rcsid[] =
  36   "@(#) $Jeannot: cobyla.c,v 1.11 2004/04/18 09:51:36 js Exp $";
  37
  38 #include <stdlib.h>
  39 #include <stdio.h>
  40 #include <math.h>
  41
  42 #include "cobyla.h"
  43
  44 /* SGJ, 2008: modified COBYLA code to take explicit account of bound
  45    constraints.  Since bound constraints are linear, these should
  46    already be handled exactly when COBYLA optimizes it linear model.
  47    However, they were not handled properly when COBYLA created its
  48    initial simplex, or when COBYLA updated unacceptable simplices.
  49    Since NLopt guarantees that the objective will not be evaluated
  50    outside the bound constraints, this required us to handle such
  51    points by putting a slope discontinuity into the objective &
  52    constraints (below), which slows convergence considerably for
  53    smooth functions.  Instead, handling them explicitly prevents this
  54    problem */
  55 #define ENFORCE_BOUNDS 1
  56
  57 #define min(a,b) ((a) <= (b) ? (a) : (b))
  58 #define max(a,b) ((a) >= (b) ? (a) : (b))
  59 #define abs(x) ((x) >= 0 ? (x) : -(x))
  60
  61 /**************************************************************************/
  62 /* SGJ, 2008: NLopt-style interface function: */
  63
  64 typedef struct {
  65      nlopt_func f;
  66      void *f_data;
  67      int m_orig;
  68      nlopt_func fc;
  69      char *fc_data;
  70      ptrdiff_t fc_datum_size;
  71      double *xtmp;
  72      const double *lb, *ub;
  73 } func_wrap_state;
  74
  75 static int func_wrap(int n, int m, double *x, double *f, double *con,
  76                      void *state)
  77 {
  78      int i, j;
  79      func_wrap_state *s = (func_wrap_state *) state;
  80      double *xtmp = s->xtmp;
  81      const double *lb = s->lb, *ub = s->ub;
  82
  83      /* in nlopt, we guarante that the function is never evaluated outside
  84         the lb and ub bounds, so we need force this with xtmp ... note
  85         that this leads to discontinuity in the first derivative, which
  86         slows convergence if we don't enable the ENFORCE_BOUNDS feature
  87         above. */
  88      for (j = 0; j < n; ++j) {
  89           if (x[j] < lb[j]) xtmp[j] = lb[j];
  90           else if (x[j] > ub[j]) xtmp[j] = ub[j];
  91           else xtmp[j] = x[j];
  92      }
  93
  94      *f = s->f(n, xtmp, NULL, s->f_data);
  95      for (i = 0; i < s->m_orig; ++i)
  96           con[i] = -s->fc(n, xtmp, NULL, s->fc_data + i * s->fc_datum_size);
  97      for (j = 0; j < n; ++j) {
  98           if (!nlopt_isinf(lb[j]))
  99                con[i++] = x[j] - lb[j];
 100           if (!nlopt_isinf(ub[j]))
 101                con[i++] = ub[j] - x[j];
 102      }
 103      if (m != i) return 1; /* ... bug?? */
 104      return 0;
 105 }
 106
 107 nlopt_result cobyla_minimize(int n, nlopt_func f, void *f_data,
 108                              int m, nlopt_func fc,
 109                              void *fc_data_, ptrdiff_t fc_datum_size,
 110                              const double *lb, const double *ub, /* bounds */
 111                              double *x, /* in: initial guess, out: minimizer */
 112                              double *minf,
 113                              nlopt_stopping *stop,
 114                              double rhobegin)
 115 {
 116      int j;
 117      func_wrap_state s;
 118      nlopt_result ret;
 119
 120      s.f = f; s.f_data = f_data;
 121      s.m_orig = m;
 122      s.fc = fc; s.fc_data = (char*) fc_data_; s.fc_datum_size = fc_datum_size;
 123      s.lb = lb; s.ub = ub;
 124      s.xtmp = (double *) malloc(sizeof(double) * n);
 125      if (!s.xtmp) return NLOPT_OUT_OF_MEMORY;
 126
 127      /* add constraints for lower/upper bounds (if any) */
 128      for (j = 0; j < n; ++j) {
 129           if (!nlopt_isinf(lb[j]))
 130                ++m;
 131           if (!nlopt_isinf(ub[j]))
 132                ++m;
 133      }
 134
 135      ret = cobyla(n, m, x, minf, rhobegin, stop, lb, ub, COBYLA_MSG_NONE,
 136                   func_wrap, &s);
 137
 138      /* make sure e.g. rounding errors didn't push us slightly out of bounds */
 139      for (j = 0; j < n; ++j) {
 140           if (x[j] < lb[j]) x[j] = lb[j];
 141           if (x[j] > ub[j]) x[j] = ub[j];
 142      }
 143
 144      free(s.xtmp);
 145      return ret;
 146 }
 147
 148 /**************************************************************************/
 149
 150 static nlopt_result cobylb(int *n, int *m, int *mpp, double *x, double *minf, double *rhobeg,
 151   nlopt_stopping *stop, const double *lb, const double *ub, int *iprint, double *con, double *sim,
 152   double *simi, double *datmat, double *a, double *vsig, double *veta,
 153   double *sigbar, double *dx, double *w, int *iact, cobyla_function *calcfc,
 154   void *state);
 155 static void trstlp(int *n, int *m, double *a, double *b, double *rho,
 156   double *dx, int *ifull, int *iact, double *z__, double *zdota, double *vmultc,
 157   double *sdirn, double *dxnew, double *vmultd);
 158
 159 /* ------------------------------------------------------------------------ */
 160
 161 nlopt_result cobyla(int n, int m, double *x, double *minf, double rhobeg, nlopt_stopping *stop, const double *lb, const double *ub, int iprint,
 162   cobyla_function *calcfc, void *state)
 163 {
 164   int icon, isim, isigb, idatm, iveta, isimi, ivsig, iwork, ia, idx, mpp;
 165   int *iact;
 166   double *w;
 167   nlopt_result rc;
 168
 169 /*
 170  * This subroutine minimizes an objective function F(X) subject to M
 171  * inequality constraints on X, where X is a vector of variables that has
 172  * N components. The algorithm employs linear approximations to the
 173  * objective and constraint functions, the approximations being formed by
 174  * linear interpolation at N+1 points in the space of the variables.
 175  * We regard these interpolation points as vertices of a simplex. The
 176  * parameter RHO controls the size of the simplex and it is reduced
 177  * automatically from RHOBEG to RHOEND. For each RHO the subroutine tries
 178  * to achieve a good vector of variables for the current size, and then
 179  * RHO is reduced until the value RHOEND is reached. Therefore RHOBEG and
 180  * RHOEND should be set to reasonable initial changes to and the required
 181  * accuracy in the variables respectively, but this accuracy should be
 182  * viewed as a subject for experimentation because it is not guaranteed.
 183  * The subroutine has an advantage over many of its competitors, however,
 184  * which is that it treats each constraint individually when calculating
 185  * a change to the variables, instead of lumping the constraints together
 186  * into a single penalty function. The name of the subroutine is derived
 187  * from the phrase Constrained Optimization BY Linear Approximations.
 188  *
 189  * The user must set the values of N, M, RHOBEG and RHOEND, and must
 190  * provide an initial vector of variables in X. Further, the value of
 191  * IPRINT should be set to 0, 1, 2 or 3, which controls the amount of
 192  * printing during the calculation. Specifically, there is no output if
 193  * IPRINT=0 and there is output only at the end of the calculation if
 194  * IPRINT=1. Otherwise each new value of RHO and SIGMA is printed.
 195  * Further, the vector of variables and some function information are
 196  * given either when RHO is reduced or when each new value of F(X) is
 197  * computed in the cases IPRINT=2 or IPRINT=3 respectively. Here SIGMA
 198  * is a penalty parameter, it being assumed that a change to X is an
 199  * improvement if it reduces the merit function
 200  *      F(X)+SIGMA*MAX(0.0,-C1(X),-C2(X),...,-CM(X)),
 201  * where C1,C2,...,CM denote the constraint functions that should become
 202  * nonnegative eventually, at least to the precision of RHOEND. In the
 203  * printed output the displayed term that is multiplied by SIGMA is
 204  * called MAXCV, which stands for 'MAXimum Constraint Violation'. The
 205  * argument MAXFUN is an int variable that must be set by the user to a
 206  * limit on the number of calls of CALCFC, the purpose of this routine being
 207  * given below. The value of MAXFUN will be altered to the number of calls
 208  * of CALCFC that are made. The arguments W and IACT provide real and
 209  * int arrays that are used as working space. Their lengths must be at
 210  * least N*(3*N+2*M+11)+4*M+6 and M+1 respectively.
 211  *
 212  * In order to define the objective and constraint functions, we require
 213  * a subroutine that has the name and arguments
 214  *      SUBROUTINE CALCFC (N,M,X,F,CON)
 215  *      DIMENSION X(*),CON(*)  .
 216  * The values of N and M are fixed and have been defined already, while
 217  * X is now the current vector of variables. The subroutine should return
 218  * the objective and constraint functions at X in F and CON(1),CON(2),
 219  * ...,CON(M). Note that we are trying to adjust X so that F(X) is as
 220  * small as possible subject to the constraint functions being nonnegative.
 221  *
 222  * Partition the working space array W to provide the storage that is needed
 223  * for the main calculation.
 224  */
 225
 226   stop->nevals = 0;
 227
 228   if (n == 0)
 229   {
 230     if (iprint>=1) fprintf(stderr, "cobyla: N==0.\n");
 231     return NLOPT_SUCCESS;
 232   }
 233
 234   if (n < 0 || m < 0)
 235   {
 236     if (iprint>=1) fprintf(stderr, "cobyla: N<0 or M<0.\n");
 237     return NLOPT_INVALID_ARGS;
 238   }
 239
 240   /* workspace allocation */
 241   w = malloc((n*(3*n+2*m+11)+4*m+6)*sizeof(*w));
 242   if (w == NULL)
 243   {
 244     if (iprint>=1) fprintf(stderr, "cobyla: memory allocation error.\n");
 245     return NLOPT_OUT_OF_MEMORY;
 246   }
 247   iact = malloc((m+1)*sizeof(*iact));
 248   if (iact == NULL)
 249   {
 250     if (iprint>=1) fprintf(stderr, "cobyla: memory allocation error.\n");
 251     free(w);
 252     return NLOPT_OUT_OF_MEMORY;
 253   }
 254
 255   /* Parameter adjustments */
 256   --iact;
 257   --w;
 258   --x;
 259   --lb; --ub;
 260
 261   /* Function Body */
 262   mpp = m + 2;
 263   icon = 1;
 264   isim = icon + mpp;
 265   isimi = isim + n * n + n;
 266   idatm = isimi + n * n;
 267   ia = idatm + n * mpp + mpp;
 268   ivsig = ia + m * n + n;
 269   iveta = ivsig + n;
 270   isigb = iveta + n;
 271   idx = isigb + n;
 272   iwork = idx + n;
 273   rc = cobylb(&n, &m, &mpp, &x[1], minf, &rhobeg, stop, &lb[1], &ub[1], &iprint,
 274       &w[icon], &w[isim], &w[isimi], &w[idatm], &w[ia], &w[ivsig], &w[iveta],
 275       &w[isigb], &w[idx], &w[iwork], &iact[1], calcfc, state);
 276
 277   /* Parameter adjustments (reverse) */
 278   ++iact;
 279   ++w;
 280
 281   free(w);
 282   free(iact);
 283
 284   return rc;
 285 } /* cobyla */
 286
 287 /* ------------------------------------------------------------------------- */
 288 static nlopt_result cobylb(int *n, int *m, int *mpp,
 289     double *x, double *minf, double *rhobeg,
 290     nlopt_stopping *stop, const double *lb, const double *ub,
 291     int *iprint, double *con, double *sim, double *simi,
 292     double *datmat, double *a, double *vsig, double *veta,
 293      double *sigbar, double *dx, double *w, int *iact, cobyla_function *calcfc,
 294      void *state)
 295 {
 296   /* System generated locals */
 297   int sim_dim1, sim_offset, simi_dim1, simi_offset, datmat_dim1,
 298       datmat_offset, a_dim1, a_offset, i__1, i__2, i__3;
 299   double d__1, d__2;
 300
 301   /* Local variables */
 302   double alpha, delta, denom, tempa, barmu;
 303   double beta, cmin = 0.0, cmax = 0.0;
 304   double cvmaxm, dxsign, prerem = 0.0;
 305   double edgmax, pareta, prerec = 0.0, phimin, parsig = 0.0;
 306   double gamma_;
 307   double phi, rho, sum = 0.0;
 308   double ratio, vmold, parmu, error, vmnew;
 309   double resmax, cvmaxp;
 310   double resnew, trured;
 311   double temp, wsig, f;
 312   double weta;
 313   int i__, j, k, l;
 314   int idxnew;
 315   int iflag = 0;
 316   int iptemp;
 317   int isdirn, izdota;
 318   int ivmc;
 319   int ivmd;
 320   int mp, np, iz, ibrnch;
 321   int nbest, ifull, iptem, jdrop;
 322   nlopt_result rc = NLOPT_SUCCESS;
 323   double rhoend;
 324
 325   /* SGJ, 2008: compute rhoend from NLopt stop info */
 326   rhoend = stop->xtol_rel * (*rhobeg);
 327   for (j = 0; j < *n; ++j)
 328        if (rhoend < stop->xtol_abs[j])
 329             rhoend = stop->xtol_abs[j];
 330
 331   /* SGJ, 2008: added code to keep track of minimum feasible function val */
 332   *minf = HUGE_VAL;
 333
 334 /* Set the initial values of some parameters. The last column of SIM holds */
 335 /* the optimal vertex of the current simplex, and the preceding N columns */
 336 /* hold the displacements from the optimal vertex to the other vertices. */
 337 /* Further, SIMI holds the inverse of the matrix that is contained in the */
 338 /* first N columns of SIM. */
 339
 340   /* Parameter adjustments */
 341   a_dim1 = *n;
 342   a_offset = 1 + a_dim1 * 1;
 343   a -= a_offset;
 344   simi_dim1 = *n;
 345   simi_offset = 1 + simi_dim1 * 1;
 346   simi -= simi_offset;
 347   sim_dim1 = *n;
 348   sim_offset = 1 + sim_dim1 * 1;
 349   sim -= sim_offset;
 350   datmat_dim1 = *mpp;
 351   datmat_offset = 1 + datmat_dim1 * 1;
 352   datmat -= datmat_offset;
 353   --x;
 354   --con;
 355   --vsig;
 356   --veta;
 357   --sigbar;
 358   --dx;
 359   --w;
 360   --iact;
 361   --lb; --ub;
 362
 363   /* Function Body */
 364   iptem = min(*n,4);
 365   iptemp = iptem + 1;
 366   np = *n + 1;
 367   mp = *m + 1;
 368   alpha = .25;
 369   beta = 2.1;
 370   gamma_ = .5;
 371   delta = 1.1;
 372   rho = *rhobeg;
 373   parmu = 0.;
 374   if (*iprint >= 2) {
 375     fprintf(stderr,
 376       "cobyla: the initial value of RHO is %12.6E and PARMU is set to zero.\n",
 377       rho);
 378   }
 379   temp = 1. / rho;
 380   i__1 = *n;
 381   for (i__ = 1; i__ <= i__1; ++i__) {
 382     double rhocur;
 383     sim[i__ + np * sim_dim1] = x[i__];
 384     i__2 = *n;
 385     for (j = 1; j <= i__2; ++j) {
 386       sim[i__ + j * sim_dim1] = 0.;
 387       simi[i__ + j * simi_dim1] = 0.;
 388     }
 389     rhocur = rho;
 390 #if ENFORCE_BOUNDS
 391     /* SGJ: make sure step rhocur stays inside [lb,ub] */
 392     if (x[i__] + rhocur > ub[i__]) {
 393          if (x[i__] - rhocur >= lb[i__])
 394               rhocur = -rhocur;
 395          else if (ub[i__] - x[i__] > x[i__] - lb[i__])
 396               rhocur = 0.5 * (ub[i__] - x[i__]);
 397          else
 398               rhocur = 0.5 * (x[i__] - lb[i__]);
 399     }
 400 #endif
 401     sim[i__ + i__ * sim_dim1] = rhocur;
 402     simi[i__ + i__ * simi_dim1] = 1.0 / rhocur;
 403   }
 404   jdrop = np;
 405   ibrnch = 0;
 406
 407 /* Make the next call of the user-supplied subroutine CALCFC. These */
 408 /* instructions are also used for calling CALCFC during the iterations of */
 409 /* the algorithm. */
 410
 411   /* SGJ comment: why the hell couldn't he just use a damn subroutine?
 412      #*&!%*@ Fortran-66 spaghetti code */
 413
 414 L40:
 415   if (nlopt_stop_evals(stop)) rc = NLOPT_MAXEVAL_REACHED;
 416   else if (nlopt_stop_time(stop)) rc = NLOPT_MAXTIME_REACHED;
 417   if (rc != NLOPT_SUCCESS) goto L600;
 418
 419   stop->nevals++;
 420   if (calcfc(*n, *m, &x[1], &f, &con[1], state))
 421   {
 422     if (*iprint >= 1) {
 423       fprintf(stderr, "cobyla: user requested end of minimization.\n");
 424     }
 425     rc = NLOPT_FAILURE;
 426     goto L600;
 427   }
 428
 429   resmax = 0.;
 430   if (*m > 0) {
 431     i__1 = *m;
 432     for (k = 1; k <= i__1; ++k) {
 433       d__1 = resmax, d__2 = -con[k];
 434       resmax = max(d__1,d__2);
 435     }
 436   }
 437
 438   /* SGJ, 2008: check for minf_max reached by feasible point */
 439   if (f < stop->minf_max && resmax <= 0) {
 440        rc = NLOPT_MINF_MAX_REACHED;
 441        goto L620; /* not L600 because we want to use current x, f, resmax */
 442   }
 443
 444   if (stop->nevals == *iprint - 1 || *iprint == 3) {
 445     fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
 446             stop->nevals, f, resmax);
 447     i__1 = iptem;
 448     fprintf(stderr, "cobyla: X =");
 449     for (i__ = 1; i__ <= i__1; ++i__) {
 450       if (i__>1) fprintf(stderr, "  ");
 451       fprintf(stderr, "%13.6E", x[i__]);
 452     }
 453     if (iptem < *n) {
 454       i__1 = *n;
 455       for (i__ = iptemp; i__ <= i__1; ++i__) {
 456         if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla:  ");
 457         fprintf(stderr, "%15.6E", x[i__]);
 458       }
 459     }
 460     fprintf(stderr, "\n");
 461   }
 462   con[mp] = f;
 463   con[*mpp] = resmax;
 464   if (ibrnch == 1) {
 465     goto L440;
 466   }
 467
 468 /* Set the recently calculated function values in a column of DATMAT. This */
 469 /* array has a column for each vertex of the current simplex, the entries of */
 470 /* each column being the values of the constraint functions (if any) */
 471 /* followed by the objective function and the greatest constraint violation */
 472 /* at the vertex. */
 473
 474   i__1 = *mpp;
 475   for (k = 1; k <= i__1; ++k) {
 476     datmat[k + jdrop * datmat_dim1] = con[k];
 477   }
 478   if (stop->nevals > np) {
 479     goto L130;
 480   }
 481
 482 /* Exchange the new vertex of the initial simplex with the optimal vertex if */
 483 /* necessary. Then, if the initial simplex is not complete, pick its next */
 484 /* vertex and calculate the function values there. */
 485
 486   if (jdrop <= *n) {
 487     if (datmat[mp + np * datmat_dim1] <= f) {
 488       x[jdrop] = sim[jdrop + np * sim_dim1];
 489     } else { /* improvement in function val */
 490       double rhocur = x[jdrop] - sim[jdrop + np * sim_dim1];
 491       /* SGJ: use rhocur instead of rho.  In original code, rhocur == rho
 492               always, but I want to change this to ensure that simplex points
 493               fall within [lb,ub]. */
 494       sim[jdrop + np * sim_dim1] = x[jdrop];
 495       i__1 = *mpp;
 496       for (k = 1; k <= i__1; ++k) {
 497         datmat[k + jdrop * datmat_dim1] = datmat[k + np * datmat_dim1]
 498             ;
 499         datmat[k + np * datmat_dim1] = con[k];
 500       }
 501       i__1 = jdrop;
 502       for (k = 1; k <= i__1; ++k) {
 503         sim[jdrop + k * sim_dim1] = -rhocur;
 504         temp = 0.f;
 505         i__2 = jdrop;
 506         for (i__ = k; i__ <= i__2; ++i__) {
 507           temp -= simi[i__ + k * simi_dim1];
 508         }
 509         simi[jdrop + k * simi_dim1] = temp;
 510       }
 511     }
 512   }
 513   if (stop->nevals <= *n) { /* evaluating initial simplex */
 514     jdrop = stop->nevals;
 515     /* SGJ: was += rho, but using sim[jdrop,jdrop] enforces consistency
 516             if we change the stepsize above to stay in [lb,ub]. */
 517     x[jdrop] += sim[jdrop + jdrop * sim_dim1];
 518     goto L40;
 519   }
 520 L130:
 521   ibrnch = 1;
 522
 523 /* Identify the optimal vertex of the current simplex. */
 524
 525 L140:
 526   phimin = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
 527       datmat_dim1];
 528   nbest = np;
 529   i__1 = *n;
 530   for (j = 1; j <= i__1; ++j) {
 531     temp = datmat[mp + j * datmat_dim1] + parmu * datmat[*mpp + j *
 532         datmat_dim1];
 533     if (temp < phimin) {
 534       nbest = j;
 535       phimin = temp;
 536     } else if (temp == phimin && parmu == 0.) {
 537       if (datmat[*mpp + j * datmat_dim1] < datmat[*mpp + nbest *
 538           datmat_dim1]) {
 539         nbest = j;
 540       }
 541     }
 542   }
 543
 544 /* Switch the best vertex into pole position if it is not there already, */
 545 /* and also update SIM, SIMI and DATMAT. */
 546
 547   if (nbest <= *n) {
 548     i__1 = *mpp;
 549     for (i__ = 1; i__ <= i__1; ++i__) {
 550       temp = datmat[i__ + np * datmat_dim1];
 551       datmat[i__ + np * datmat_dim1] = datmat[i__ + nbest * datmat_dim1]
 552           ;
 553       datmat[i__ + nbest * datmat_dim1] = temp;
 554     }
 555     i__1 = *n;
 556     for (i__ = 1; i__ <= i__1; ++i__) {
 557       temp = sim[i__ + nbest * sim_dim1];
 558       sim[i__ + nbest * sim_dim1] = 0.;
 559       sim[i__ + np * sim_dim1] += temp;
 560       tempa = 0.;
 561       i__2 = *n;
 562       for (k = 1; k <= i__2; ++k) {
 563         sim[i__ + k * sim_dim1] -= temp;
 564         tempa -= simi[k + i__ * simi_dim1];
 565       }
 566       simi[nbest + i__ * simi_dim1] = tempa;
 567     }
 568   }
 569
 570 /* Make an error return if SIGI is a poor approximation to the inverse of */
 571 /* the leading N by N submatrix of SIG. */
 572
 573   error = 0.;
 574   i__1 = *n;
 575   for (i__ = 1; i__ <= i__1; ++i__) {
 576     i__2 = *n;
 577     for (j = 1; j <= i__2; ++j) {
 578       temp = 0.;
 579       if (i__ == j) {
 580         temp += -1.;
 581       }
 582       i__3 = *n;
 583       for (k = 1; k <= i__3; ++k) if (sim[k + j * sim_dim1] != 0) {
 584         temp += simi[i__ + k * simi_dim1] * sim[k + j * sim_dim1];
 585       }
 586       d__1 = error, d__2 = abs(temp);
 587       error = max(d__1,d__2);
 588     }
 589   }
 590   if (error > .1) {
 591     if (*iprint >= 1) {
 592       fprintf(stderr, "cobyla: rounding errors are becoming damaging.\n");
 593     }
 594     rc = NLOPT_FAILURE;
 595     goto L600;
 596   }
 597
 598 /* Calculate the coefficients of the linear approximations to the objective */
 599 /* and constraint functions, placing minus the objective function gradient */
 600 /* after the constraint gradients in the array A. The vector W is used for */
 601 /* working space. */
 602
 603   i__2 = mp;
 604   for (k = 1; k <= i__2; ++k) {
 605     con[k] = -datmat[k + np * datmat_dim1];
 606     i__1 = *n;
 607     for (j = 1; j <= i__1; ++j) {
 608       w[j] = datmat[k + j * datmat_dim1] + con[k];
 609     }
 610     i__1 = *n;
 611     for (i__ = 1; i__ <= i__1; ++i__) {
 612       temp = 0.;
 613       i__3 = *n;
 614       for (j = 1; j <= i__3; ++j) {
 615         temp += w[j] * simi[j + i__ * simi_dim1];
 616       }
 617       if (k == mp) {
 618         temp = -temp;
 619       }
 620       a[i__ + k * a_dim1] = temp;
 621     }
 622   }
 623
 624 /* Calculate the values of sigma and eta, and set IFLAG=0 if the current */
 625 /* simplex is not acceptable. */
 626
 627   iflag = 1;
 628   parsig = alpha * rho;
 629   pareta = beta * rho;
 630   i__1 = *n;
 631   for (j = 1; j <= i__1; ++j) {
 632     wsig = 0.;
 633     weta = 0.;
 634     i__2 = *n;
 635     for (i__ = 1; i__ <= i__2; ++i__) {
 636       d__1 = simi[j + i__ * simi_dim1];
 637       wsig += d__1 * d__1;
 638       d__1 = sim[i__ + j * sim_dim1];
 639       weta += d__1 * d__1;
 640     }
 641     vsig[j] = 1. / sqrt(wsig);
 642     veta[j] = sqrt(weta);
 643     if (vsig[j] < parsig || veta[j] > pareta) {
 644       iflag = 0;
 645     }
 646   }
 647
 648 /* If a new vertex is needed to improve acceptability, then decide which */
 649 /* vertex to drop from the simplex. */
 650
 651   if (ibrnch == 1 || iflag == 1) {
 652     goto L370;
 653   }
 654   jdrop = 0;
 655   temp = pareta;
 656   i__1 = *n;
 657   for (j = 1; j <= i__1; ++j) {
 658     if (veta[j] > temp) {
 659       jdrop = j;
 660       temp = veta[j];
 661     }
 662   }
 663   if (jdrop == 0) {
 664     i__1 = *n;
 665     for (j = 1; j <= i__1; ++j) {
 666       if (vsig[j] < temp) {
 667         jdrop = j;
 668         temp = vsig[j];
 669       }
 670     }
 671   }
 672
 673 /* Calculate the step to the new vertex and its sign. */
 674
 675   temp = gamma_ * rho * vsig[jdrop];
 676   i__1 = *n;
 677   for (i__ = 1; i__ <= i__1; ++i__) {
 678     dx[i__] = temp * simi[jdrop + i__ * simi_dim1];
 679   }
 680   cvmaxp = 0.;
 681   cvmaxm = 0.;
 682   i__1 = mp;
 683   for (k = 1; k <= i__1; ++k) {
 684     sum = 0.;
 685     i__2 = *n;
 686     for (i__ = 1; i__ <= i__2; ++i__) {
 687       sum += a[i__ + k * a_dim1] * dx[i__];
 688     }
 689     if (k < mp) {
 690       temp = datmat[k + np * datmat_dim1];
 691       d__1 = cvmaxp, d__2 = -sum - temp;
 692       cvmaxp = max(d__1,d__2);
 693       d__1 = cvmaxm, d__2 = sum - temp;
 694       cvmaxm = max(d__1,d__2);
 695     }
 696   }
 697   dxsign = 1.;
 698   if (parmu * (cvmaxp - cvmaxm) > sum + sum) {
 699     dxsign = -1.;
 700   }
 701
 702 /* Update the elements of SIM and SIMI, and set the next X. */
 703
 704   temp = 0.;
 705   i__1 = *n;
 706   for (i__ = 1; i__ <= i__1; ++i__) {
 707     dx[i__] = dxsign * dx[i__];
 708     /* SGJ: make sure dx step says in [lb,ub] */
 709 #if ENFORCE_BOUNDS
 710     {
 711          double xi = sim[i__ + np * sim_dim1];
 712     fixdx:
 713          if (xi + dx[i__] > ub[i__])
 714               dx[i__] = -dx[i__];
 715          if (xi + dx[i__] < lb[i__]) {
 716               if (xi - dx[i__] <= ub[i__])
 717                    dx[i__] = -dx[i__];
 718               else { /* try again with halved step */
 719                    dx[i__] *= 0.5;
 720                    goto fixdx;
 721               }
 722          }
 723     }
 724 #endif
 725     sim[i__ + jdrop * sim_dim1] = dx[i__];
 726     temp += simi[jdrop + i__ * simi_dim1] * dx[i__];
 727   }
 728   i__1 = *n;
 729   for (i__ = 1; i__ <= i__1; ++i__) {
 730     simi[jdrop + i__ * simi_dim1] /= temp;
 731   }
 732   i__1 = *n;
 733   for (j = 1; j <= i__1; ++j) {
 734     if (j != jdrop) {
 735       temp = 0.;
 736       i__2 = *n;
 737       for (i__ = 1; i__ <= i__2; ++i__) {
 738         temp += simi[j + i__ * simi_dim1] * dx[i__];
 739       }
 740       i__2 = *n;
 741       for (i__ = 1; i__ <= i__2; ++i__) {
 742         simi[j + i__ * simi_dim1] -= temp * simi[jdrop + i__ *
 743             simi_dim1];
 744       }
 745     }
 746     x[j] = sim[j + np * sim_dim1] + dx[j];
 747   }
 748   goto L40;
 749
 750 /* Calculate DX=x(*)-x(0). Branch if the length of DX is less than 0.5*RHO. */
 751
 752 L370:
 753   iz = 1;
 754   izdota = iz + *n * *n;
 755   ivmc = izdota + *n;
 756   isdirn = ivmc + mp;
 757   idxnew = isdirn + *n;
 758   ivmd = idxnew + *n;
 759   trstlp(n, m, &a[a_offset], &con[1], &rho, &dx[1], &ifull, &iact[1], &w[
 760       iz], &w[izdota], &w[ivmc], &w[isdirn], &w[idxnew], &w[ivmd]);
 761 #if ENFORCE_BOUNDS
 762   /* SGJ: since the bound constraints are linear, we should never get
 763      a dx that lies outside the [lb,ub] constraints here, but we'll
 764      enforce this anyway just to be paranoid */
 765   i__1 = *n;
 766   for (i__ = 1; i__ <= i__1; ++i__) {
 767        double xi = sim[i__ + np * sim_dim1];
 768        if (xi + dx[i__] > ub[i__]) dx[i__] = ub[i__] - xi;
 769        if (xi + dx[i__] < lb[i__]) dx[i__] = xi - lb[i__];
 770   }
 771 #endif
 772   if (ifull == 0) {
 773     temp = 0.;
 774     i__1 = *n;
 775     for (i__ = 1; i__ <= i__1; ++i__) {
 776       d__1 = dx[i__];
 777       temp += d__1 * d__1;
 778     }
 779     if (temp < rho * .25 * rho) {
 780       ibrnch = 1;
 781       goto L550;
 782     }
 783   }
 784
 785 /* Predict the change to F and the new maximum constraint violation if the */
 786 /* variables are altered from x(0) to x(0)+DX. */
 787
 788   resnew = 0.;
 789   con[mp] = 0.;
 790   i__1 = mp;
 791   for (k = 1; k <= i__1; ++k) {
 792     sum = con[k];
 793     i__2 = *n;
 794     for (i__ = 1; i__ <= i__2; ++i__) {
 795       sum -= a[i__ + k * a_dim1] * dx[i__];
 796     }
 797     if (k < mp) {
 798       resnew = max(resnew,sum);
 799     }
 800   }
 801
 802 /* Increase PARMU if necessary and branch back if this change alters the */
 803 /* optimal vertex. Otherwise PREREM and PREREC will be set to the predicted */
 804 /* reductions in the merit function and the maximum constraint violation */
 805 /* respectively. */
 806
 807   barmu = 0.;
 808   prerec = datmat[*mpp + np * datmat_dim1] - resnew;
 809   if (prerec > 0.) {
 810     barmu = sum / prerec;
 811   }
 812   if (parmu < barmu * 1.5) {
 813     parmu = barmu * 2.;
 814     if (*iprint >= 2) {
 815       fprintf(stderr, "cobyla: increase in PARMU to %12.6E\n", parmu);
 816     }
 817     phi = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
 818         datmat_dim1];
 819     i__1 = *n;
 820     for (j = 1; j <= i__1; ++j) {
 821       temp = datmat[mp + j * datmat_dim1] + parmu * datmat[*mpp + j *
 822           datmat_dim1];
 823       if (temp < phi) {
 824         goto L140;
 825       }
 826       if (temp == phi && parmu == 0.f) {
 827         if (datmat[*mpp + j * datmat_dim1] < datmat[*mpp + np *
 828             datmat_dim1]) {
 829           goto L140;
 830         }
 831       }
 832     }
 833   }
 834   prerem = parmu * prerec - sum;
 835
 836 /* Calculate the constraint and objective functions at x(*). Then find the */
 837 /* actual reduction in the merit function. */
 838
 839   i__1 = *n;
 840   for (i__ = 1; i__ <= i__1; ++i__) {
 841     x[i__] = sim[i__ + np * sim_dim1] + dx[i__];
 842   }
 843   ibrnch = 1;
 844   goto L40;
 845 L440:
 846   vmold = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
 847       datmat_dim1];
 848   vmnew = f + parmu * resmax;
 849   trured = vmold - vmnew;
 850   if (parmu == 0. && f == datmat[mp + np * datmat_dim1]) {
 851     prerem = prerec;
 852     trured = datmat[*mpp + np * datmat_dim1] - resmax;
 853   }
 854
 855 /* Begin the operations that decide whether x(*) should replace one of the */
 856 /* vertices of the current simplex, the change being mandatory if TRURED is */
 857 /* positive. Firstly, JDROP is set to the index of the vertex that is to be */
 858 /* replaced. */
 859
 860   ratio = 0.;
 861   if (trured <= 0.f) {
 862     ratio = 1.f;
 863   }
 864   jdrop = 0;
 865   i__1 = *n;
 866   for (j = 1; j <= i__1; ++j) {
 867     temp = 0.;
 868     i__2 = *n;
 869     for (i__ = 1; i__ <= i__2; ++i__) {
 870       temp += simi[j + i__ * simi_dim1] * dx[i__];
 871     }
 872     temp = abs(temp);
 873     if (temp > ratio) {
 874       jdrop = j;
 875       ratio = temp;
 876     }
 877     sigbar[j] = temp * vsig[j];
 878   }
 879
 880 /* Calculate the value of ell. */
 881
 882   edgmax = delta * rho;
 883   l = 0;
 884   i__1 = *n;
 885   for (j = 1; j <= i__1; ++j) {
 886     if (sigbar[j] >= parsig || sigbar[j] >= vsig[j]) {
 887       temp = veta[j];
 888       if (trured > 0.) {
 889         temp = 0.;
 890         i__2 = *n;
 891         for (i__ = 1; i__ <= i__2; ++i__) {
 892           d__1 = dx[i__] - sim[i__ + j * sim_dim1];
 893           temp += d__1 * d__1;
 894         }
 895         temp = sqrt(temp);
 896       }
 897       if (temp > edgmax) {
 898         l = j;
 899         edgmax = temp;
 900       }
 901     }
 902   }
 903   if (l > 0) {
 904     jdrop = l;
 905   }
 906   if (jdrop == 0) {
 907     goto L550;
 908   }
 909
 910 /* Revise the simplex by updating the elements of SIM, SIMI and DATMAT. */
 911
 912   temp = 0.;
 913   i__1 = *n;
 914   for (i__ = 1; i__ <= i__1; ++i__) {
 915     sim[i__ + jdrop * sim_dim1] = dx[i__];
 916     temp += simi[jdrop + i__ * simi_dim1] * dx[i__];
 917   }
 918   i__1 = *n;
 919   for (i__ = 1; i__ <= i__1; ++i__) {
 920     simi[jdrop + i__ * simi_dim1] /= temp;
 921   }
 922   i__1 = *n;
 923   for (j = 1; j <= i__1; ++j) {
 924     if (j != jdrop) {
 925       temp = 0.;
 926       i__2 = *n;
 927       for (i__ = 1; i__ <= i__2; ++i__) {
 928         temp += simi[j + i__ * simi_dim1] * dx[i__];
 929       }
 930       i__2 = *n;
 931       for (i__ = 1; i__ <= i__2; ++i__) {
 932         simi[j + i__ * simi_dim1] -= temp * simi[jdrop + i__ *
 933             simi_dim1];
 934       }
 935     }
 936   }
 937   i__1 = *mpp;
 938   for (k = 1; k <= i__1; ++k) {
 939     datmat[k + jdrop * datmat_dim1] = con[k];
 940   }
 941
 942 /* Branch back for further iterations with the current RHO. */
 943
 944   if (trured > 0. && trured >= prerem * .1) {
 945     goto L140;
 946   }
 947 L550:
 948   if (iflag == 0) {
 949     ibrnch = 0;
 950     goto L140;
 951   }
 952
 953   /* SGJ, 2008: convergence tests for function vals; note that current
 954      best val is stored in datmat[mp + np * datmat_dim1], or in f if
 955      ifull == 1, and previous best is in *minf.  This seems like a
 956      sensible place to put the convergence test, as it is the same
 957      place that Powell checks the x tolerance (rho > rhoend). */
 958   {
 959        double fbest = ifull == 1 ? f : datmat[mp + np * datmat_dim1];
 960        if (fbest < *minf && nlopt_stop_ftol(stop, fbest, *minf)) {
 961             rc = NLOPT_FTOL_REACHED;
 962             goto L600;
 963        }
 964        *minf = fbest;
 965   }
 966
 967 /* Otherwise reduce RHO if it is not at its least value and reset PARMU. */
 968
 969   if (rho > rhoend) {
 970     rho *= .5;
 971     if (rho <= rhoend * 1.5) {
 972       rho = rhoend;
 973     }
 974     if (parmu > 0.) {
 975       denom = 0.;
 976       i__1 = mp;
 977       for (k = 1; k <= i__1; ++k) {
 978         cmin = datmat[k + np * datmat_dim1];
 979         cmax = cmin;
 980         i__2 = *n;
 981         for (i__ = 1; i__ <= i__2; ++i__) {
 982           d__1 = cmin, d__2 = datmat[k + i__ * datmat_dim1];
 983           cmin = min(d__1,d__2);
 984           d__1 = cmax, d__2 = datmat[k + i__ * datmat_dim1];
 985           cmax = max(d__1,d__2);
 986         }
 987         if (k <= *m && cmin < cmax * .5) {
 988           temp = max(cmax,0.) - cmin;
 989           if (denom <= 0.) {
 990             denom = temp;
 991           } else {
 992             denom = min(denom,temp);
 993           }
 994         }
 995       }
 996       if (denom == 0.) {
 997         parmu = 0.;
 998       } else if (cmax - cmin < parmu * denom) {
 999         parmu = (cmax - cmin) / denom;
1000       }
1001     }
1002     if (*iprint >= 2) {
1003       fprintf(stderr, "cobyla: reduction in RHO to %12.6E and PARMU =%13.6E\n",
1004         rho, parmu);
1005     }
1006     if (*iprint == 2) {
1007       fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
1008         stop->nevals, datmat[mp + np * datmat_dim1], datmat[*mpp + np * datmat_dim1]);
1009
1010       fprintf(stderr, "cobyla: X =");
1011       i__1 = iptem;
1012       for (i__ = 1; i__ <= i__1; ++i__) {
1013         if (i__>1) fprintf(stderr, "  ");
1014         fprintf(stderr, "%13.6E", sim[i__ + np * sim_dim1]);
1015       }
1016       if (iptem < *n) {
1017         i__1 = *n;
1018         for (i__ = iptemp; i__ <= i__1; ++i__) {
1019           if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla:  ");
1020           fprintf(stderr, "%15.6E", x[i__]);
1021         }
1022       }
1023       fprintf(stderr, "\n");
1024     }
1025     goto L140;
1026   }
1027   else
1028        rc = NLOPT_XTOL_REACHED;
1029
1030 /* Return the best calculated values of the variables. */
1031
1032   if (*iprint >= 1) {
1033     fprintf(stderr, "cobyla: normal return.\n");
1034   }
1035   if (ifull == 1) {
1036     goto L620;
1037   }
1038 L600:
1039   i__1 = *n;
1040   for (i__ = 1; i__ <= i__1; ++i__) {
1041     x[i__] = sim[i__ + np * sim_dim1];
1042   }
1043   f = datmat[mp + np * datmat_dim1];
1044   resmax = datmat[*mpp + np * datmat_dim1];
1045 L620:
1046   *minf = f;
1047   if (*iprint >= 1) {
1048     fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
1049             stop->nevals, f, resmax);
1050     i__1 = iptem;
1051     fprintf(stderr, "cobyla: X =");
1052     for (i__ = 1; i__ <= i__1; ++i__) {
1053       if (i__>1) fprintf(stderr, "  ");
1054       fprintf(stderr, "%13.6E", x[i__]);
1055     }
1056     if (iptem < *n) {
1057       i__1 = *n;
1058       for (i__ = iptemp; i__ <= i__1; ++i__) {
1059         if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla:  ");
1060         fprintf(stderr, "%15.6E", x[i__]);
1061       }
1062     }
1063     fprintf(stderr, "\n");
1064   }
1065   return rc;
1066 } /* cobylb */
1067
1068 /* ------------------------------------------------------------------------- */
1069 static void trstlp(int *n, int *m, double *a,
1070     double *b, double *rho, double *dx, int *ifull,
1071     int *iact, double *z__, double *zdota, double *vmultc,
1072      double *sdirn, double *dxnew, double *vmultd)
1073 {
1074   /* System generated locals */
1075   int a_dim1, a_offset, z_dim1, z_offset, i__1, i__2;
1076   double d__1, d__2;
1077
1078   /* Local variables */
1079   double alpha, tempa;
1080   double beta;
1081   double optnew, stpful, sum, tot, acca, accb;
1082   double ratio, vsave, zdotv, zdotw, dd;
1083   double sd;
1084   double sp, ss, resold = 0.0, zdvabs, zdwabs, sumabs, resmax, optold;
1085   double spabs;
1086   double temp, step;
1087   int icount;
1088   int iout, i__, j, k;
1089   int isave;
1090   int kk;
1091   int kl, kp, kw;
1092   int nact, icon = 0, mcon;
1093   int nactx = 0;
1094
1095
1096 /* This subroutine calculates an N-component vector DX by applying the */
1097 /* following two stages. In the first stage, DX is set to the shortest */
1098 /* vector that minimizes the greatest violation of the constraints */
1099 /*   A(1,K)*DX(1)+A(2,K)*DX(2)+...+A(N,K)*DX(N) .GE. B(K), K=2,3,...,M, */
1100 /* subject to the Euclidean length of DX being at most RHO. If its length is */
1101 /* strictly less than RHO, then we use the resultant freedom in DX to */
1102 /* minimize the objective function */
1103 /*      -A(1,M+1)*DX(1)-A(2,M+1)*DX(2)-...-A(N,M+1)*DX(N) */
1104 /* subject to no increase in any greatest constraint violation. This */
1105 /* notation allows the gradient of the objective function to be regarded as */
1106 /* the gradient of a constraint. Therefore the two stages are distinguished */
1107 /* by MCON .EQ. M and MCON .GT. M respectively. It is possible that a */
1108 /* degeneracy may prevent DX from attaining the target length RHO. Then the */
1109 /* value IFULL=0 would be set, but usually IFULL=1 on return. */
1110
1111 /* In general NACT is the number of constraints in the active set and */
1112 /* IACT(1),...,IACT(NACT) are their indices, while the remainder of IACT */
1113 /* contains a permutation of the remaining constraint indices. Further, Z is */
1114 /* an orthogonal matrix whose first NACT columns can be regarded as the */
1115 /* result of Gram-Schmidt applied to the active constraint gradients. For */
1116 /* J=1,2,...,NACT, the number ZDOTA(J) is the scalar product of the J-th */
1117 /* column of Z with the gradient of the J-th active constraint. DX is the */
1118 /* current vector of variables and here the residuals of the active */
1119 /* constraints should be zero. Further, the active constraints have */
1120 /* nonnegative Lagrange multipliers that are held at the beginning of */
1121 /* VMULTC. The remainder of this vector holds the residuals of the inactive */
1122 /* constraints at DX, the ordering of the components of VMULTC being in */
1123 /* agreement with the permutation of the indices of the constraints that is */
1124 /* in IACT. All these residuals are nonnegative, which is achieved by the */
1125 /* shift RESMAX that makes the least residual zero. */
1126
1127 /* Initialize Z and some other variables. The value of RESMAX will be */
1128 /* appropriate to DX=0, while ICON will be the index of a most violated */
1129 /* constraint if RESMAX is positive. Usually during the first stage the */
1130 /* vector SDIRN gives a search direction that reduces all the active */
1131 /* constraint violations by one simultaneously. */
1132
1133   /* Parameter adjustments */
1134   z_dim1 = *n;
1135   z_offset = 1 + z_dim1 * 1;
1136   z__ -= z_offset;
1137   a_dim1 = *n;
1138   a_offset = 1 + a_dim1 * 1;
1139   a -= a_offset;
1140   --b;
1141   --dx;
1142   --iact;
1143   --zdota;
1144   --vmultc;
1145   --sdirn;
1146   --dxnew;
1147   --vmultd;
1148
1149   /* Function Body */
1150   *ifull = 1;
1151   mcon = *m;
1152   nact = 0;
1153   resmax = 0.;
1154   i__1 = *n;
1155   for (i__ = 1; i__ <= i__1; ++i__) {
1156     i__2 = *n;
1157     for (j = 1; j <= i__2; ++j) {
1158       z__[i__ + j * z_dim1] = 0.;
1159     }
1160     z__[i__ + i__ * z_dim1] = 1.;
1161     dx[i__] = 0.;
1162   }
1163   if (*m >= 1) {
1164     i__1 = *m;
1165     for (k = 1; k <= i__1; ++k) {
1166       if (b[k] > resmax) {
1167         resmax = b[k];
1168         icon = k;
1169       }
1170     }
1171     i__1 = *m;
1172     for (k = 1; k <= i__1; ++k) {
1173       iact[k] = k;
1174       vmultc[k] = resmax - b[k];
1175     }
1176   }
1177   if (resmax == 0.) {
1178     goto L480;
1179   }
1180   i__1 = *n;
1181   for (i__ = 1; i__ <= i__1; ++i__) {
1182     sdirn[i__] = 0.;
1183   }
1184
1185 /* End the current stage of the calculation if 3 consecutive iterations */
1186 /* have either failed to reduce the best calculated value of the objective */
1187 /* function or to increase the number of active constraints since the best */
1188 /* value was calculated. This strategy prevents cycling, but there is a */
1189 /* remote possibility that it will cause premature termination. */
1190
1191 L60:
1192   optold = 0.;
1193   icount = 0;
1194 L70:
1195   if (mcon == *m) {
1196     optnew = resmax;
1197   } else {
1198     optnew = 0.;
1199     i__1 = *n;
1200     for (i__ = 1; i__ <= i__1; ++i__) {
1201       optnew -= dx[i__] * a[i__ + mcon * a_dim1];
1202     }
1203   }
1204   if (icount == 0 || optnew < optold) {
1205     optold = optnew;
1206     nactx = nact;
1207     icount = 3;
1208   } else if (nact > nactx) {
1209     nactx = nact;
1210     icount = 3;
1211   } else {
1212     --icount;
1213     if (icount == 0) {
1214       goto L490;
1215     }
1216   }
1217
1218 /* If ICON exceeds NACT, then we add the constraint with index IACT(ICON) to */
1219 /* the active set. Apply Givens rotations so that the last N-NACT-1 columns */
1220 /* of Z are orthogonal to the gradient of the new constraint, a scalar */
1221 /* product being set to zero if its nonzero value could be due to computer */
1222 /* rounding errors. The array DXNEW is used for working space. */
1223
1224   if (icon <= nact) {
1225     goto L260;
1226   }
1227   kk = iact[icon];
1228   i__1 = *n;
1229   for (i__ = 1; i__ <= i__1; ++i__) {
1230     dxnew[i__] = a[i__ + kk * a_dim1];
1231   }
1232   tot = 0.;
1233   k = *n;
1234 L100:
1235   if (k > nact) {
1236     sp = 0.;
1237     spabs = 0.;
1238     i__1 = *n;
1239     for (i__ = 1; i__ <= i__1; ++i__) {
1240       temp = z__[i__ + k * z_dim1] * dxnew[i__];
1241       sp += temp;
1242       spabs += abs(temp);
1243     }
1244     acca = spabs + abs(sp) * .1;
1245     accb = spabs + abs(sp) * .2;
1246     if (spabs >= acca || acca >= accb) {
1247       sp = 0.;
1248     }
1249     if (tot == 0.) {
1250       tot = sp;
1251     } else {
1252       kp = k + 1;
1253       temp = sqrt(sp * sp + tot * tot);
1254       alpha = sp / temp;
1255       beta = tot / temp;
1256       tot = temp;
1257       i__1 = *n;
1258       for (i__ = 1; i__ <= i__1; ++i__) {
1259         temp = alpha * z__[i__ + k * z_dim1] + beta * z__[i__ + kp *
1260             z_dim1];
1261         z__[i__ + kp * z_dim1] = alpha * z__[i__ + kp * z_dim1] -
1262             beta * z__[i__ + k * z_dim1];
1263         z__[i__ + k * z_dim1] = temp;
1264       }
1265     }
1266     --k;
1267     goto L100;
1268   }
1269
1270 /* Add the new constraint if this can be done without a deletion from the */
1271 /* active set. */
1272
1273   if (tot != 0.) {
1274     ++nact;
1275     zdota[nact] = tot;
1276     vmultc[icon] = vmultc[nact];
1277     vmultc[nact] = 0.;
1278     goto L210;
1279   }
1280
1281 /* The next instruction is reached if a deletion has to be made from the */
1282 /* active set in order to make room for the new active constraint, because */
1283 /* the new constraint gradient is a linear combination of the gradients of */
1284 /* the old active constraints. Set the elements of VMULTD to the multipliers */
1285 /* of the linear combination. Further, set IOUT to the index of the */
1286 /* constraint to be deleted, but branch if no suitable index can be found. */
1287
1288   ratio = -1.;
1289   k = nact;
1290 L130:
1291   zdotv = 0.;
1292   zdvabs = 0.;
1293   i__1 = *n;
1294   for (i__ = 1; i__ <= i__1; ++i__) {
1295     temp = z__[i__ + k * z_dim1] * dxnew[i__];
1296     zdotv += temp;
1297     zdvabs += abs(temp);
1298   }
1299   acca = zdvabs + abs(zdotv) * .1;
1300   accb = zdvabs + abs(zdotv) * .2;
1301   if (zdvabs < acca && acca < accb) {
1302     temp = zdotv / zdota[k];
1303     if (temp > 0. && iact[k] <= *m) {
1304       tempa = vmultc[k] / temp;
1305       if (ratio < 0. || tempa < ratio) {
1306         ratio = tempa;
1307         iout = k;
1308       }
1309     }
1310     if (k >= 2) {
1311       kw = iact[k];
1312       i__1 = *n;
1313       for (i__ = 1; i__ <= i__1; ++i__) {
1314         dxnew[i__] -= temp * a[i__ + kw * a_dim1];
1315       }
1316     }
1317     vmultd[k] = temp;
1318   } else {
1319     vmultd[k] = 0.;
1320   }
1321   --k;
1322   if (k > 0) {
1323     goto L130;
1324   }
1325   if (ratio < 0.) {
1326     goto L490;
1327   }
1328
1329 /* Revise the Lagrange multipliers and reorder the active constraints so */
1330 /* that the one to be replaced is at the end of the list. Also calculate the */
1331 /* new value of ZDOTA(NACT) and branch if it is not acceptable. */
1332
1333   i__1 = nact;
1334   for (k = 1; k <= i__1; ++k) {
1335     d__1 = 0., d__2 = vmultc[k] - ratio * vmultd[k];
1336     vmultc[k] = max(d__1,d__2);
1337   }
1338   if (icon < nact) {
1339     isave = iact[icon];
1340     vsave = vmultc[icon];
1341     k = icon;
1342 L170:
1343     kp = k + 1;
1344     kw = iact[kp];
1345     sp = 0.;
1346     i__1 = *n;
1347     for (i__ = 1; i__ <= i__1; ++i__) {
1348       sp += z__[i__ + k * z_dim1] * a[i__ + kw * a_dim1];
1349     }
1350     d__1 = zdota[kp];
1351     temp = sqrt(sp * sp + d__1 * d__1);
1352     alpha = zdota[kp] / temp;
1353     beta = sp / temp;
1354     zdota[kp] = alpha * zdota[k];
1355     zdota[k] = temp;
1356     i__1 = *n;
1357     for (i__ = 1; i__ <= i__1; ++i__) {
1358       temp = alpha * z__[i__ + kp * z_dim1] + beta * z__[i__ + k *
1359           z_dim1];
1360       z__[i__ + kp * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1361           z__[i__ + kp * z_dim1];
1362       z__[i__ + k * z_dim1] = temp;
1363     }
1364     iact[k] = kw;
1365     vmultc[k] = vmultc[kp];
1366     k = kp;
1367     if (k < nact) {
1368       goto L170;
1369     }
1370     iact[k] = isave;
1371     vmultc[k] = vsave;
1372   }
1373   temp = 0.;
1374   i__1 = *n;
1375   for (i__ = 1; i__ <= i__1; ++i__) {
1376     temp += z__[i__ + nact * z_dim1] * a[i__ + kk * a_dim1];
1377   }
1378   if (temp == 0.) {
1379     goto L490;
1380   }
1381   zdota[nact] = temp;
1382   vmultc[icon] = 0.;
1383   vmultc[nact] = ratio;
1384
1385 /* Update IACT and ensure that the objective function continues to be */
1386 /* treated as the last active constraint when MCON>M. */
1387
1388 L210:
1389   iact[icon] = iact[nact];
1390   iact[nact] = kk;
1391   if (mcon > *m && kk != mcon) {
1392     k = nact - 1;
1393     sp = 0.;
1394     i__1 = *n;
1395     for (i__ = 1; i__ <= i__1; ++i__) {
1396       sp += z__[i__ + k * z_dim1] * a[i__ + kk * a_dim1];
1397     }
1398     d__1 = zdota[nact];
1399     temp = sqrt(sp * sp + d__1 * d__1);
1400     alpha = zdota[nact] / temp;
1401     beta = sp / temp;
1402     zdota[nact] = alpha * zdota[k];
1403     zdota[k] = temp;
1404     i__1 = *n;
1405     for (i__ = 1; i__ <= i__1; ++i__) {
1406       temp = alpha * z__[i__ + nact * z_dim1] + beta * z__[i__ + k *
1407           z_dim1];
1408       z__[i__ + nact * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1409           z__[i__ + nact * z_dim1];
1410       z__[i__ + k * z_dim1] = temp;
1411     }
1412     iact[nact] = iact[k];
1413     iact[k] = kk;
1414     temp = vmultc[k];
1415     vmultc[k] = vmultc[nact];
1416     vmultc[nact] = temp;
1417   }
1418
1419 /* If stage one is in progress, then set SDIRN to the direction of the next */
1420 /* change to the current vector of variables. */
1421
1422   if (mcon > *m) {
1423     goto L320;
1424   }
1425   kk = iact[nact];
1426   temp = 0.;
1427   i__1 = *n;
1428   for (i__ = 1; i__ <= i__1; ++i__) {
1429     temp += sdirn[i__] * a[i__ + kk * a_dim1];
1430   }
1431   temp += -1.;
1432   temp /= zdota[nact];
1433   i__1 = *n;
1434   for (i__ = 1; i__ <= i__1; ++i__) {
1435     sdirn[i__] -= temp * z__[i__ + nact * z_dim1];
1436   }
1437   goto L340;
1438
1439 /* Delete the constraint that has the index IACT(ICON) from the active set. */
1440
1441 L260:
1442   if (icon < nact) {
1443     isave = iact[icon];
1444     vsave = vmultc[icon];
1445     k = icon;
1446 L270:
1447     kp = k + 1;
1448     kk = iact[kp];
1449     sp = 0.;
1450     i__1 = *n;
1451     for (i__ = 1; i__ <= i__1; ++i__) {
1452       sp += z__[i__ + k * z_dim1] * a[i__ + kk * a_dim1];
1453     }
1454     d__1 = zdota[kp];
1455     temp = sqrt(sp * sp + d__1 * d__1);
1456     alpha = zdota[kp] / temp;
1457     beta = sp / temp;
1458     zdota[kp] = alpha * zdota[k];
1459     zdota[k] = temp;
1460     i__1 = *n;
1461     for (i__ = 1; i__ <= i__1; ++i__) {
1462       temp = alpha * z__[i__ + kp * z_dim1] + beta * z__[i__ + k *
1463           z_dim1];
1464       z__[i__ + kp * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1465           z__[i__ + kp * z_dim1];
1466       z__[i__ + k * z_dim1] = temp;
1467     }
1468     iact[k] = kk;
1469     vmultc[k] = vmultc[kp];
1470     k = kp;
1471     if (k < nact) {
1472       goto L270;
1473     }
1474     iact[k] = isave;
1475     vmultc[k] = vsave;
1476   }
1477   --nact;
1478
1479 /* If stage one is in progress, then set SDIRN to the direction of the next */
1480 /* change to the current vector of variables. */
1481
1482   if (mcon > *m) {
1483     goto L320;
1484   }
1485   temp = 0.;
1486   i__1 = *n;
1487   for (i__ = 1; i__ <= i__1; ++i__) {
1488     temp += sdirn[i__] * z__[i__ + (nact + 1) * z_dim1];
1489   }
1490   i__1 = *n;
1491   for (i__ = 1; i__ <= i__1; ++i__) {
1492     sdirn[i__] -= temp * z__[i__ + (nact + 1) * z_dim1];
1493   }
1494   goto L340;
1495
1496 /* Pick the next search direction of stage two. */
1497
1498 L320:
1499   temp = 1. / zdota[nact];
1500   i__1 = *n;
1501   for (i__ = 1; i__ <= i__1; ++i__) {
1502     sdirn[i__] = temp * z__[i__ + nact * z_dim1];
1503   }
1504
1505 /* Calculate the step to the boundary of the trust region or take the step */
1506 /* that reduces RESMAX to zero. The two statements below that include the */
1507 /* factor 1.0E-6 prevent some harmless underflows that occurred in a test */
1508 /* calculation. Further, we skip the step if it could be zero within a */
1509 /* reasonable tolerance for computer rounding errors. */
1510
1511 L340:
1512   dd = *rho * *rho;
1513   sd = 0.;
1514   ss = 0.;
1515   i__1 = *n;
1516   for (i__ = 1; i__ <= i__1; ++i__) {
1517     if ((d__1 = dx[i__], abs(d__1)) >= *rho * 1e-6f) {
1518       d__2 = dx[i__];
1519       dd -= d__2 * d__2;
1520     }
1521     sd += dx[i__] * sdirn[i__];
1522     d__1 = sdirn[i__];
1523     ss += d__1 * d__1;
1524   }
1525   if (dd <= 0.) {
1526     goto L490;
1527   }
1528   temp = sqrt(ss * dd);
1529   if (abs(sd) >= temp * 1e-6f) {
1530     temp = sqrt(ss * dd + sd * sd);
1531   }
1532   stpful = dd / (temp + sd);
1533   step = stpful;
1534   if (mcon == *m) {
1535     acca = step + resmax * .1;
1536     accb = step + resmax * .2;
1537     if (step >= acca || acca >= accb) {
1538       goto L480;
1539     }
1540     step = min(step,resmax);
1541   }
1542
1543 /* Set DXNEW to the new variables if STEP is the steplength, and reduce */
1544 /* RESMAX to the corresponding maximum residual if stage one is being done. */
1545 /* Because DXNEW will be changed during the calculation of some Lagrange */
1546 /* multipliers, it will be restored to the following value later. */
1547
1548   i__1 = *n;
1549   for (i__ = 1; i__ <= i__1; ++i__) {
1550     dxnew[i__] = dx[i__] + step * sdirn[i__];
1551   }
1552   if (mcon == *m) {
1553     resold = resmax;
1554     resmax = 0.;
1555     i__1 = nact;
1556     for (k = 1; k <= i__1; ++k) {
1557       kk = iact[k];
1558       temp = b[kk];
1559       i__2 = *n;
1560       for (i__ = 1; i__ <= i__2; ++i__) {
1561         temp -= a[i__ + kk * a_dim1] * dxnew[i__];
1562       }
1563       resmax = max(resmax,temp);
1564     }
1565   }
1566
1567 /* Set VMULTD to the VMULTC vector that would occur if DX became DXNEW. A */
1568 /* device is included to force VMULTD(K)=0.0 if deviations from this value */
1569 /* can be attributed to computer rounding errors. First calculate the new */
1570 /* Lagrange multipliers. */
1571
1572   k = nact;
1573 L390:
1574   zdotw = 0.;
1575   zdwabs = 0.;
1576   i__1 = *n;
1577   for (i__ = 1; i__ <= i__1; ++i__) {
1578     temp = z__[i__ + k * z_dim1] * dxnew[i__];
1579     zdotw += temp;
1580     zdwabs += abs(temp);
1581   }
1582   acca = zdwabs + abs(zdotw) * .1;
1583   accb = zdwabs + abs(zdotw) * .2;
1584   if (zdwabs >= acca || acca >= accb) {
1585     zdotw = 0.;
1586   }
1587   vmultd[k] = zdotw / zdota[k];
1588   if (k >= 2) {
1589     kk = iact[k];
1590     i__1 = *n;
1591     for (i__ = 1; i__ <= i__1; ++i__) {
1592       dxnew[i__] -= vmultd[k] * a[i__ + kk * a_dim1];
1593     }
1594     --k;
1595     goto L390;
1596   }
1597   if (mcon > *m) {
1598     d__1 = 0., d__2 = vmultd[nact];
1599     vmultd[nact] = max(d__1,d__2);
1600   }
1601
1602 /* Complete VMULTC by finding the new constraint residuals. */
1603
1604   i__1 = *n;
1605   for (i__ = 1; i__ <= i__1; ++i__) {
1606     dxnew[i__] = dx[i__] + step * sdirn[i__];
1607   }
1608   if (mcon > nact) {
1609     kl = nact + 1;
1610     i__1 = mcon;
1611     for (k = kl; k <= i__1; ++k) {
1612       kk = iact[k];
1613       sum = resmax - b[kk];
1614       sumabs = resmax + (d__1 = b[kk], abs(d__1));
1615       i__2 = *n;
1616       for (i__ = 1; i__ <= i__2; ++i__) {
1617         temp = a[i__ + kk * a_dim1] * dxnew[i__];
1618         sum += temp;
1619         sumabs += abs(temp);
1620       }
1621       acca = sumabs + abs(sum) * .1f;
1622       accb = sumabs + abs(sum) * .2f;
1623       if (sumabs >= acca || acca >= accb) {
1624         sum = 0.f;
1625       }
1626       vmultd[k] = sum;
1627     }
1628   }
1629
1630 /* Calculate the fraction of the step from DX to DXNEW that will be taken. */
1631
1632   ratio = 1.;
1633   icon = 0;
1634   i__1 = mcon;
1635   for (k = 1; k <= i__1; ++k) {
1636     if (vmultd[k] < 0.) {
1637       temp = vmultc[k] / (vmultc[k] - vmultd[k]);
1638       if (temp < ratio) {
1639         ratio = temp;
1640         icon = k;
1641       }
1642     }
1643   }
1644
1645 /* Update DX, VMULTC and RESMAX. */
1646
1647   temp = 1. - ratio;
1648   i__1 = *n;
1649   for (i__ = 1; i__ <= i__1; ++i__) {
1650     dx[i__] = temp * dx[i__] + ratio * dxnew[i__];
1651   }
1652   i__1 = mcon;
1653   for (k = 1; k <= i__1; ++k) {
1654     d__1 = 0., d__2 = temp * vmultc[k] + ratio * vmultd[k];
1655     vmultc[k] = max(d__1,d__2);
1656   }
1657   if (mcon == *m) {
1658     resmax = resold + ratio * (resmax - resold);
1659   }
1660
1661 /* If the full step is not acceptable then begin another iteration. */
1662 /* Otherwise switch to stage two or end the calculation. */
1663
1664   if (icon > 0) {
1665     goto L70;
1666   }
1667   if (step == stpful) {
1668     goto L500;
1669   }
1670 L480:
1671   mcon = *m + 1;
1672   icon = mcon;
1673   iact[mcon] = mcon;
1674   vmultc[mcon] = 0.;
1675   goto L60;
1676
1677 /* We employ any freedom that may be available to reduce the objective */
1678 /* function before returning a DX whose length is less than RHO. */
1679
1680 L490:
1681   if (mcon == *m) {
1682     goto L480;
1683   }
1684   *ifull = 0;
1685 L500:
1686   return;
1687 } /* trstlp */