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