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 void 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_FORCE_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   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 ENFORCE_BOUNDS
 854   /* SGJ: since the bound constraints are linear, we should never get
 855      a dx that lies outside the [lb,ub] constraints here, but we'll
 856      enforce this anyway just to be paranoid */
 857   i__1 = *n;
 858   for (i__ = 1; i__ <= i__1; ++i__) {
 859        double xi = sim[i__ + np * sim_dim1];
 860        if (xi + dx[i__] > ub[i__]) dx[i__] = ub[i__] - xi;
 861        if (xi + dx[i__] < lb[i__]) dx[i__] = xi - lb[i__];
 862   }
 863 #endif
 864   if (ifull == 0) {
 865     temp = 0.;
 866     i__1 = *n;
 867     for (i__ = 1; i__ <= i__1; ++i__) {
 868       d__1 = dx[i__];
 869       temp += d__1 * d__1;
 870     }
 871     if (temp < rho * .25 * rho) {
 872       ibrnch = 1;
 873       goto L550;
 874     }
 875   }
 876
 877 /* Predict the change to F and the new maximum constraint violation if the */
 878 /* variables are altered from x(0) to x(0)+DX. */
 879
 880   resnew = 0.;
 881   con[mp] = 0.;
 882   i__1 = mp;
 883   for (k = 1; k <= i__1; ++k) {
 884     sum = con[k];
 885     i__2 = *n;
 886     for (i__ = 1; i__ <= i__2; ++i__) {
 887       sum -= a[i__ + k * a_dim1] * dx[i__];
 888     }
 889     if (k < mp) {
 890       resnew = max(resnew,sum);
 891     }
 892   }
 893
 894 /* Increase PARMU if necessary and branch back if this change alters the */
 895 /* optimal vertex. Otherwise PREREM and PREREC will be set to the predicted */
 896 /* reductions in the merit function and the maximum constraint violation */
 897 /* respectively. */
 898
 899   barmu = 0.;
 900   prerec = datmat[*mpp + np * datmat_dim1] - resnew;
 901   if (prerec > 0.) {
 902     barmu = sum / prerec;
 903   }
 904   if (parmu < barmu * 1.5) {
 905     parmu = barmu * 2.;
 906     if (*iprint >= 2) {
 907       fprintf(stderr, "cobyla: increase in PARMU to %12.6E\n", parmu);
 908     }
 909     phi = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
 910         datmat_dim1];
 911     i__1 = *n;
 912     for (j = 1; j <= i__1; ++j) {
 913       temp = datmat[mp + j * datmat_dim1] + parmu * datmat[*mpp + j *
 914           datmat_dim1];
 915       if (temp < phi) {
 916         goto L140;
 917       }
 918       if (temp == phi && parmu == 0.f) {
 919         if (datmat[*mpp + j * datmat_dim1] < datmat[*mpp + np *
 920             datmat_dim1]) {
 921           goto L140;
 922         }
 923       }
 924     }
 925   }
 926   prerem = parmu * prerec - sum;
 927
 928 /* Calculate the constraint and objective functions at x(*). Then find the */
 929 /* actual reduction in the merit function. */
 930
 931   i__1 = *n;
 932   for (i__ = 1; i__ <= i__1; ++i__) {
 933     x[i__] = sim[i__ + np * sim_dim1] + dx[i__];
 934   }
 935   ibrnch = 1;
 936   goto L40;
 937 L440:
 938   vmold = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
 939       datmat_dim1];
 940   vmnew = f + parmu * resmax;
 941   trured = vmold - vmnew;
 942   if (parmu == 0. && f == datmat[mp + np * datmat_dim1]) {
 943     prerem = prerec;
 944     trured = datmat[*mpp + np * datmat_dim1] - resmax;
 945   }
 946
 947 /* Begin the operations that decide whether x(*) should replace one of the */
 948 /* vertices of the current simplex, the change being mandatory if TRURED is */
 949 /* positive. Firstly, JDROP is set to the index of the vertex that is to be */
 950 /* replaced. */
 951
 952   ratio = 0.;
 953   if (trured <= 0.f) {
 954     ratio = 1.f;
 955   }
 956   jdrop = 0;
 957   i__1 = *n;
 958   for (j = 1; j <= i__1; ++j) {
 959     temp = 0.;
 960     i__2 = *n;
 961     for (i__ = 1; i__ <= i__2; ++i__) {
 962       temp += simi[j + i__ * simi_dim1] * dx[i__];
 963     }
 964     temp = abs(temp);
 965     if (temp > ratio) {
 966       jdrop = j;
 967       ratio = temp;
 968     }
 969     sigbar[j] = temp * vsig[j];
 970   }
 971
 972 /* Calculate the value of ell. */
 973
 974   edgmax = delta * rho;
 975   l = 0;
 976   i__1 = *n;
 977   for (j = 1; j <= i__1; ++j) {
 978     if (sigbar[j] >= parsig || sigbar[j] >= vsig[j]) {
 979       temp = veta[j];
 980       if (trured > 0.) {
 981         temp = 0.;
 982         i__2 = *n;
 983         for (i__ = 1; i__ <= i__2; ++i__) {
 984           d__1 = dx[i__] - sim[i__ + j * sim_dim1];
 985           temp += d__1 * d__1;
 986         }
 987         temp = sqrt(temp);
 988       }
 989       if (temp > edgmax) {
 990         l = j;
 991         edgmax = temp;
 992       }
 993     }
 994   }
 995   if (l > 0) {
 996     jdrop = l;
 997   }
 998   if (jdrop == 0) {
 999     goto L550;
1000   }
1001
1002 /* Revise the simplex by updating the elements of SIM, SIMI and DATMAT. */
1003
1004   temp = 0.;
1005   i__1 = *n;
1006   for (i__ = 1; i__ <= i__1; ++i__) {
1007     sim[i__ + jdrop * sim_dim1] = dx[i__];
1008     temp += simi[jdrop + i__ * simi_dim1] * dx[i__];
1009   }
1010   i__1 = *n;
1011   for (i__ = 1; i__ <= i__1; ++i__) {
1012     simi[jdrop + i__ * simi_dim1] /= temp;
1013   }
1014   i__1 = *n;
1015   for (j = 1; j <= i__1; ++j) {
1016     if (j != jdrop) {
1017       temp = 0.;
1018       i__2 = *n;
1019       for (i__ = 1; i__ <= i__2; ++i__) {
1020         temp += simi[j + i__ * simi_dim1] * dx[i__];
1021       }
1022       i__2 = *n;
1023       for (i__ = 1; i__ <= i__2; ++i__) {
1024         simi[j + i__ * simi_dim1] -= temp * simi[jdrop + i__ *
1025             simi_dim1];
1026       }
1027     }
1028   }
1029   i__1 = *mpp;
1030   for (k = 1; k <= i__1; ++k) {
1031     datmat[k + jdrop * datmat_dim1] = con[k];
1032   }
1033
1034 /* Branch back for further iterations with the current RHO. */
1035
1036   if (trured > 0. && trured >= prerem * .1) {
1037     /* SGJ, 2010: following a suggestion in the SAS manual (which
1038        mentions a similar modification to COBYLA, although they didn't
1039        publish their source code), increase rho if predicted reduction
1040        is sufficiently close to the actual reduction.  Otherwise,
1041        COBLYA seems to easily get stuck making very small steps.
1042        Also require iflag != 0 (i.e., acceptable simplex), again
1043        following SAS suggestion (otherwise I get convergence failure
1044        in some cases.) */
1045     if (trured >= prerem * 0.9 && trured <= prerem * 1.1 && iflag) {
1046          rho *= 2.0;
1047     }
1048     goto L140;
1049   }
1050 L550:
1051   if (iflag == 0) {
1052     ibrnch = 0;
1053     goto L140;
1054   }
1055
1056   /* SGJ, 2008: convergence tests for function vals; note that current
1057      best val is stored in datmat[mp + np * datmat_dim1], or in f if
1058      ifull == 1, and previous best is in *minf.  This seems like a
1059      sensible place to put the convergence test, as it is the same
1060      place that Powell checks the x tolerance (rho > rhoend). */
1061   {
1062        double fbest = ifull == 1 ? f : datmat[mp + np * datmat_dim1];
1063        if (fbest < *minf && nlopt_stop_ftol(stop, fbest, *minf)) {
1064             rc = NLOPT_FTOL_REACHED;
1065             goto L600;
1066        }
1067        *minf = fbest;
1068   }
1069
1070 /* Otherwise reduce RHO if it is not at its least value and reset PARMU. */
1071
1072   if (rho > rhoend) {
1073     rho *= .5;
1074     if (rho <= rhoend * 1.5) {
1075       rho = rhoend;
1076     }
1077     if (parmu > 0.) {
1078       denom = 0.;
1079       i__1 = mp;
1080       for (k = 1; k <= i__1; ++k) {
1081         cmin = datmat[k + np * datmat_dim1];
1082         cmax = cmin;
1083         i__2 = *n;
1084         for (i__ = 1; i__ <= i__2; ++i__) {
1085           d__1 = cmin, d__2 = datmat[k + i__ * datmat_dim1];
1086           cmin = min(d__1,d__2);
1087           d__1 = cmax, d__2 = datmat[k + i__ * datmat_dim1];
1088           cmax = max(d__1,d__2);
1089         }
1090         if (k <= *m && cmin < cmax * .5) {
1091           temp = max(cmax,0.) - cmin;
1092           if (denom <= 0.) {
1093             denom = temp;
1094           } else {
1095             denom = min(denom,temp);
1096           }
1097         }
1098       }
1099       if (denom == 0.) {
1100         parmu = 0.;
1101       } else if (cmax - cmin < parmu * denom) {
1102         parmu = (cmax - cmin) / denom;
1103       }
1104     }
1105     if (*iprint >= 2) {
1106       fprintf(stderr, "cobyla: reduction in RHO to %12.6E and PARMU =%13.6E\n",
1107         rho, parmu);
1108     }
1109     if (*iprint == 2) {
1110       fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
1111         stop->nevals, datmat[mp + np * datmat_dim1], datmat[*mpp + np * datmat_dim1]);
1112
1113       fprintf(stderr, "cobyla: X =");
1114       i__1 = iptem;
1115       for (i__ = 1; i__ <= i__1; ++i__) {
1116         if (i__>1) fprintf(stderr, "  ");
1117         fprintf(stderr, "%13.6E", sim[i__ + np * sim_dim1]);
1118       }
1119       if (iptem < *n) {
1120         i__1 = *n;
1121         for (i__ = iptemp; i__ <= i__1; ++i__) {
1122           if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla:  ");
1123           fprintf(stderr, "%15.6E", x[i__]);
1124         }
1125       }
1126       fprintf(stderr, "\n");
1127     }
1128     goto L140;
1129   }
1130   else
1131        rc = NLOPT_XTOL_REACHED;
1132
1133 /* Return the best calculated values of the variables. */
1134
1135   if (*iprint >= 1) {
1136     fprintf(stderr, "cobyla: normal return.\n");
1137   }
1138   if (ifull == 1) {
1139     goto L620;
1140   }
1141 L600:
1142   i__1 = *n;
1143   for (i__ = 1; i__ <= i__1; ++i__) {
1144     x[i__] = sim[i__ + np * sim_dim1];
1145   }
1146   f = datmat[mp + np * datmat_dim1];
1147   resmax = datmat[*mpp + np * datmat_dim1];
1148 L620:
1149   *minf = f;
1150   if (*iprint >= 1) {
1151     fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
1152             stop->nevals, f, resmax);
1153     i__1 = iptem;
1154     fprintf(stderr, "cobyla: X =");
1155     for (i__ = 1; i__ <= i__1; ++i__) {
1156       if (i__>1) fprintf(stderr, "  ");
1157       fprintf(stderr, "%13.6E", x[i__]);
1158     }
1159     if (iptem < *n) {
1160       i__1 = *n;
1161       for (i__ = iptemp; i__ <= i__1; ++i__) {
1162         if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla:  ");
1163         fprintf(stderr, "%15.6E", x[i__]);
1164       }
1165     }
1166     fprintf(stderr, "\n");
1167   }
1168   return rc;
1169 } /* cobylb */
1170
1171 /* ------------------------------------------------------------------------- */
1172 static void trstlp(int *n, int *m, double *a,
1173     double *b, double *rho, double *dx, int *ifull,
1174     int *iact, double *z__, double *zdota, double *vmultc,
1175      double *sdirn, double *dxnew, double *vmultd)
1176 {
1177   /* System generated locals */
1178   int a_dim1, a_offset, z_dim1, z_offset, i__1, i__2;
1179   double d__1, d__2;
1180
1181   /* Local variables */
1182   double alpha, tempa;
1183   double beta;
1184   double optnew, stpful, sum, tot, acca, accb;
1185   double ratio, vsave, zdotv, zdotw, dd;
1186   double sd;
1187   double sp, ss, resold = 0.0, zdvabs, zdwabs, sumabs, resmax, optold;
1188   double spabs;
1189   double temp, step;
1190   int icount;
1191   int iout, i__, j, k;
1192   int isave;
1193   int kk;
1194   int kl, kp, kw;
1195   int nact, icon = 0, mcon;
1196   int nactx = 0;
1197
1198
1199 /* This subroutine calculates an N-component vector DX by applying the */
1200 /* following two stages. In the first stage, DX is set to the shortest */
1201 /* vector that minimizes the greatest violation of the constraints */
1202 /*   A(1,K)*DX(1)+A(2,K)*DX(2)+...+A(N,K)*DX(N) .GE. B(K), K=2,3,...,M, */
1203 /* subject to the Euclidean length of DX being at most RHO. If its length is */
1204 /* strictly less than RHO, then we use the resultant freedom in DX to */
1205 /* minimize the objective function */
1206 /*      -A(1,M+1)*DX(1)-A(2,M+1)*DX(2)-...-A(N,M+1)*DX(N) */
1207 /* subject to no increase in any greatest constraint violation. This */
1208 /* notation allows the gradient of the objective function to be regarded as */
1209 /* the gradient of a constraint. Therefore the two stages are distinguished */
1210 /* by MCON .EQ. M and MCON .GT. M respectively. It is possible that a */
1211 /* degeneracy may prevent DX from attaining the target length RHO. Then the */
1212 /* value IFULL=0 would be set, but usually IFULL=1 on return. */
1213
1214 /* In general NACT is the number of constraints in the active set and */
1215 /* IACT(1),...,IACT(NACT) are their indices, while the remainder of IACT */
1216 /* contains a permutation of the remaining constraint indices. Further, Z is */
1217 /* an orthogonal matrix whose first NACT columns can be regarded as the */
1218 /* result of Gram-Schmidt applied to the active constraint gradients. For */
1219 /* J=1,2,...,NACT, the number ZDOTA(J) is the scalar product of the J-th */
1220 /* column of Z with the gradient of the J-th active constraint. DX is the */
1221 /* current vector of variables and here the residuals of the active */
1222 /* constraints should be zero. Further, the active constraints have */
1223 /* nonnegative Lagrange multipliers that are held at the beginning of */
1224 /* VMULTC. The remainder of this vector holds the residuals of the inactive */
1225 /* constraints at DX, the ordering of the components of VMULTC being in */
1226 /* agreement with the permutation of the indices of the constraints that is */
1227 /* in IACT. All these residuals are nonnegative, which is achieved by the */
1228 /* shift RESMAX that makes the least residual zero. */
1229
1230 /* Initialize Z and some other variables. The value of RESMAX will be */
1231 /* appropriate to DX=0, while ICON will be the index of a most violated */
1232 /* constraint if RESMAX is positive. Usually during the first stage the */
1233 /* vector SDIRN gives a search direction that reduces all the active */
1234 /* constraint violations by one simultaneously. */
1235
1236   /* Parameter adjustments */
1237   z_dim1 = *n;
1238   z_offset = 1 + z_dim1 * 1;
1239   z__ -= z_offset;
1240   a_dim1 = *n;
1241   a_offset = 1 + a_dim1 * 1;
1242   a -= a_offset;
1243   --b;
1244   --dx;
1245   --iact;
1246   --zdota;
1247   --vmultc;
1248   --sdirn;
1249   --dxnew;
1250   --vmultd;
1251
1252   /* Function Body */
1253   *ifull = 1;
1254   mcon = *m;
1255   nact = 0;
1256   resmax = 0.;
1257   i__1 = *n;
1258   for (i__ = 1; i__ <= i__1; ++i__) {
1259     i__2 = *n;
1260     for (j = 1; j <= i__2; ++j) {
1261       z__[i__ + j * z_dim1] = 0.;
1262     }
1263     z__[i__ + i__ * z_dim1] = 1.;
1264     dx[i__] = 0.;
1265   }
1266   if (*m >= 1) {
1267     i__1 = *m;
1268     for (k = 1; k <= i__1; ++k) {
1269       if (b[k] > resmax) {
1270         resmax = b[k];
1271         icon = k;
1272       }
1273     }
1274     i__1 = *m;
1275     for (k = 1; k <= i__1; ++k) {
1276       iact[k] = k;
1277       vmultc[k] = resmax - b[k];
1278     }
1279   }
1280   if (resmax == 0.) {
1281     goto L480;
1282   }
1283   i__1 = *n;
1284   for (i__ = 1; i__ <= i__1; ++i__) {
1285     sdirn[i__] = 0.;
1286   }
1287
1288 /* End the current stage of the calculation if 3 consecutive iterations */
1289 /* have either failed to reduce the best calculated value of the objective */
1290 /* function or to increase the number of active constraints since the best */
1291 /* value was calculated. This strategy prevents cycling, but there is a */
1292 /* remote possibility that it will cause premature termination. */
1293
1294 L60:
1295   optold = 0.;
1296   icount = 0;
1297 L70:
1298   if (mcon == *m) {
1299     optnew = resmax;
1300   } else {
1301     optnew = 0.;
1302     i__1 = *n;
1303     for (i__ = 1; i__ <= i__1; ++i__) {
1304       optnew -= dx[i__] * a[i__ + mcon * a_dim1];
1305     }
1306   }
1307   if (icount == 0 || optnew < optold) {
1308     optold = optnew;
1309     nactx = nact;
1310     icount = 3;
1311   } else if (nact > nactx) {
1312     nactx = nact;
1313     icount = 3;
1314   } else {
1315     --icount;
1316     if (icount == 0) {
1317       goto L490;
1318     }
1319   }
1320
1321 /* If ICON exceeds NACT, then we add the constraint with index IACT(ICON) to */
1322 /* the active set. Apply Givens rotations so that the last N-NACT-1 columns */
1323 /* of Z are orthogonal to the gradient of the new constraint, a scalar */
1324 /* product being set to zero if its nonzero value could be due to computer */
1325 /* rounding errors. The array DXNEW is used for working space. */
1326
1327   if (icon <= nact) {
1328     goto L260;
1329   }
1330   kk = iact[icon];
1331   i__1 = *n;
1332   for (i__ = 1; i__ <= i__1; ++i__) {
1333     dxnew[i__] = a[i__ + kk * a_dim1];
1334   }
1335   tot = 0.;
1336   k = *n;
1337 L100:
1338   if (k > nact) {
1339     sp = 0.;
1340     spabs = 0.;
1341     i__1 = *n;
1342     for (i__ = 1; i__ <= i__1; ++i__) {
1343       temp = z__[i__ + k * z_dim1] * dxnew[i__];
1344       sp += temp;
1345       spabs += abs(temp);
1346     }
1347     acca = spabs + abs(sp) * .1;
1348     accb = spabs + abs(sp) * .2;
1349     if (spabs >= acca || acca >= accb) {
1350       sp = 0.;
1351     }
1352     if (tot == 0.) {
1353       tot = sp;
1354     } else {
1355       kp = k + 1;
1356       temp = sqrt(sp * sp + tot * tot);
1357       alpha = sp / temp;
1358       beta = tot / temp;
1359       tot = temp;
1360       i__1 = *n;
1361       for (i__ = 1; i__ <= i__1; ++i__) {
1362         temp = alpha * z__[i__ + k * z_dim1] + beta * z__[i__ + kp *
1363             z_dim1];
1364         z__[i__ + kp * z_dim1] = alpha * z__[i__ + kp * z_dim1] -
1365             beta * z__[i__ + k * z_dim1];
1366         z__[i__ + k * z_dim1] = temp;
1367       }
1368     }
1369     --k;
1370     goto L100;
1371   }
1372
1373 /* Add the new constraint if this can be done without a deletion from the */
1374 /* active set. */
1375
1376   if (tot != 0.) {
1377     ++nact;
1378     zdota[nact] = tot;
1379     vmultc[icon] = vmultc[nact];
1380     vmultc[nact] = 0.;
1381     goto L210;
1382   }
1383
1384 /* The next instruction is reached if a deletion has to be made from the */
1385 /* active set in order to make room for the new active constraint, because */
1386 /* the new constraint gradient is a linear combination of the gradients of */
1387 /* the old active constraints. Set the elements of VMULTD to the multipliers */
1388 /* of the linear combination. Further, set IOUT to the index of the */
1389 /* constraint to be deleted, but branch if no suitable index can be found. */
1390
1391   ratio = -1.;
1392   k = nact;
1393 L130:
1394   zdotv = 0.;
1395   zdvabs = 0.;
1396   i__1 = *n;
1397   for (i__ = 1; i__ <= i__1; ++i__) {
1398     temp = z__[i__ + k * z_dim1] * dxnew[i__];
1399     zdotv += temp;
1400     zdvabs += abs(temp);
1401   }
1402   acca = zdvabs + abs(zdotv) * .1;
1403   accb = zdvabs + abs(zdotv) * .2;
1404   if (zdvabs < acca && acca < accb) {
1405     temp = zdotv / zdota[k];
1406     if (temp > 0. && iact[k] <= *m) {
1407       tempa = vmultc[k] / temp;
1408       if (ratio < 0. || tempa < ratio) {
1409         ratio = tempa;
1410         iout = k;
1411       }
1412     }
1413     if (k >= 2) {
1414       kw = iact[k];
1415       i__1 = *n;
1416       for (i__ = 1; i__ <= i__1; ++i__) {
1417         dxnew[i__] -= temp * a[i__ + kw * a_dim1];
1418       }
1419     }
1420     vmultd[k] = temp;
1421   } else {
1422     vmultd[k] = 0.;
1423   }
1424   --k;
1425   if (k > 0) {
1426     goto L130;
1427   }
1428   if (ratio < 0.) {
1429     goto L490;
1430   }
1431
1432 /* Revise the Lagrange multipliers and reorder the active constraints so */
1433 /* that the one to be replaced is at the end of the list. Also calculate the */
1434 /* new value of ZDOTA(NACT) and branch if it is not acceptable. */
1435
1436   i__1 = nact;
1437   for (k = 1; k <= i__1; ++k) {
1438     d__1 = 0., d__2 = vmultc[k] - ratio * vmultd[k];
1439     vmultc[k] = max(d__1,d__2);
1440   }
1441   if (icon < nact) {
1442     isave = iact[icon];
1443     vsave = vmultc[icon];
1444     k = icon;
1445 L170:
1446     kp = k + 1;
1447     kw = iact[kp];
1448     sp = 0.;
1449     i__1 = *n;
1450     for (i__ = 1; i__ <= i__1; ++i__) {
1451       sp += z__[i__ + k * z_dim1] * a[i__ + kw * a_dim1];
1452     }
1453     d__1 = zdota[kp];
1454     temp = sqrt(sp * sp + d__1 * d__1);
1455     alpha = zdota[kp] / temp;
1456     beta = sp / temp;
1457     zdota[kp] = alpha * zdota[k];
1458     zdota[k] = temp;
1459     i__1 = *n;
1460     for (i__ = 1; i__ <= i__1; ++i__) {
1461       temp = alpha * z__[i__ + kp * z_dim1] + beta * z__[i__ + k *
1462           z_dim1];
1463       z__[i__ + kp * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1464           z__[i__ + kp * z_dim1];
1465       z__[i__ + k * z_dim1] = temp;
1466     }
1467     iact[k] = kw;
1468     vmultc[k] = vmultc[kp];
1469     k = kp;
1470     if (k < nact) {
1471       goto L170;
1472     }
1473     iact[k] = isave;
1474     vmultc[k] = vsave;
1475   }
1476   temp = 0.;
1477   i__1 = *n;
1478   for (i__ = 1; i__ <= i__1; ++i__) {
1479     temp += z__[i__ + nact * z_dim1] * a[i__ + kk * a_dim1];
1480   }
1481   if (temp == 0.) {
1482     goto L490;
1483   }
1484   zdota[nact] = temp;
1485   vmultc[icon] = 0.;
1486   vmultc[nact] = ratio;
1487
1488 /* Update IACT and ensure that the objective function continues to be */
1489 /* treated as the last active constraint when MCON>M. */
1490
1491 L210:
1492   iact[icon] = iact[nact];
1493   iact[nact] = kk;
1494   if (mcon > *m && kk != mcon) {
1495     k = nact - 1;
1496     sp = 0.;
1497     i__1 = *n;
1498     for (i__ = 1; i__ <= i__1; ++i__) {
1499       sp += z__[i__ + k * z_dim1] * a[i__ + kk * a_dim1];
1500     }
1501     d__1 = zdota[nact];
1502     temp = sqrt(sp * sp + d__1 * d__1);
1503     alpha = zdota[nact] / temp;
1504     beta = sp / temp;
1505     zdota[nact] = alpha * zdota[k];
1506     zdota[k] = temp;
1507     i__1 = *n;
1508     for (i__ = 1; i__ <= i__1; ++i__) {
1509       temp = alpha * z__[i__ + nact * z_dim1] + beta * z__[i__ + k *
1510           z_dim1];
1511       z__[i__ + nact * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1512           z__[i__ + nact * z_dim1];
1513       z__[i__ + k * z_dim1] = temp;
1514     }
1515     iact[nact] = iact[k];
1516     iact[k] = kk;
1517     temp = vmultc[k];
1518     vmultc[k] = vmultc[nact];
1519     vmultc[nact] = temp;
1520   }
1521
1522 /* If stage one is in progress, then set SDIRN to the direction of the next */
1523 /* change to the current vector of variables. */
1524
1525   if (mcon > *m) {
1526     goto L320;
1527   }
1528   kk = iact[nact];
1529   temp = 0.;
1530   i__1 = *n;
1531   for (i__ = 1; i__ <= i__1; ++i__) {
1532     temp += sdirn[i__] * a[i__ + kk * a_dim1];
1533   }
1534   temp += -1.;
1535   temp /= zdota[nact];
1536   i__1 = *n;
1537   for (i__ = 1; i__ <= i__1; ++i__) {
1538     sdirn[i__] -= temp * z__[i__ + nact * z_dim1];
1539   }
1540   goto L340;
1541
1542 /* Delete the constraint that has the index IACT(ICON) from the active set. */
1543
1544 L260:
1545   if (icon < nact) {
1546     isave = iact[icon];
1547     vsave = vmultc[icon];
1548     k = icon;
1549 L270:
1550     kp = k + 1;
1551     kk = iact[kp];
1552     sp = 0.;
1553     i__1 = *n;
1554     for (i__ = 1; i__ <= i__1; ++i__) {
1555       sp += z__[i__ + k * z_dim1] * a[i__ + kk * a_dim1];
1556     }
1557     d__1 = zdota[kp];
1558     temp = sqrt(sp * sp + d__1 * d__1);
1559     alpha = zdota[kp] / temp;
1560     beta = sp / temp;
1561     zdota[kp] = alpha * zdota[k];
1562     zdota[k] = temp;
1563     i__1 = *n;
1564     for (i__ = 1; i__ <= i__1; ++i__) {
1565       temp = alpha * z__[i__ + kp * z_dim1] + beta * z__[i__ + k *
1566           z_dim1];
1567       z__[i__ + kp * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1568           z__[i__ + kp * z_dim1];
1569       z__[i__ + k * z_dim1] = temp;
1570     }
1571     iact[k] = kk;
1572     vmultc[k] = vmultc[kp];
1573     k = kp;
1574     if (k < nact) {
1575       goto L270;
1576     }
1577     iact[k] = isave;
1578     vmultc[k] = vsave;
1579   }
1580   --nact;
1581
1582 /* If stage one is in progress, then set SDIRN to the direction of the next */
1583 /* change to the current vector of variables. */
1584
1585   if (mcon > *m) {
1586     goto L320;
1587   }
1588   temp = 0.;
1589   i__1 = *n;
1590   for (i__ = 1; i__ <= i__1; ++i__) {
1591     temp += sdirn[i__] * z__[i__ + (nact + 1) * z_dim1];
1592   }
1593   i__1 = *n;
1594   for (i__ = 1; i__ <= i__1; ++i__) {
1595     sdirn[i__] -= temp * z__[i__ + (nact + 1) * z_dim1];
1596   }
1597   goto L340;
1598
1599 /* Pick the next search direction of stage two. */
1600
1601 L320:
1602   temp = 1. / zdota[nact];
1603   i__1 = *n;
1604   for (i__ = 1; i__ <= i__1; ++i__) {
1605     sdirn[i__] = temp * z__[i__ + nact * z_dim1];
1606   }
1607
1608 /* Calculate the step to the boundary of the trust region or take the step */
1609 /* that reduces RESMAX to zero. The two statements below that include the */
1610 /* factor 1.0E-6 prevent some harmless underflows that occurred in a test */
1611 /* calculation. Further, we skip the step if it could be zero within a */
1612 /* reasonable tolerance for computer rounding errors. */
1613
1614 L340:
1615   dd = *rho * *rho;
1616   sd = 0.;
1617   ss = 0.;
1618   i__1 = *n;
1619   for (i__ = 1; i__ <= i__1; ++i__) {
1620     if ((d__1 = dx[i__], abs(d__1)) >= *rho * 1e-6f) {
1621       d__2 = dx[i__];
1622       dd -= d__2 * d__2;
1623     }
1624     sd += dx[i__] * sdirn[i__];
1625     d__1 = sdirn[i__];
1626     ss += d__1 * d__1;
1627   }
1628   if (dd <= 0.) {
1629     goto L490;
1630   }
1631   temp = sqrt(ss * dd);
1632   if (abs(sd) >= temp * 1e-6f) {
1633     temp = sqrt(ss * dd + sd * sd);
1634   }
1635   stpful = dd / (temp + sd);
1636   step = stpful;
1637   if (mcon == *m) {
1638     acca = step + resmax * .1;
1639     accb = step + resmax * .2;
1640     if (step >= acca || acca >= accb) {
1641       goto L480;
1642     }
1643     step = min(step,resmax);
1644   }
1645
1646 /* Set DXNEW to the new variables if STEP is the steplength, and reduce */
1647 /* RESMAX to the corresponding maximum residual if stage one is being done. */
1648 /* Because DXNEW will be changed during the calculation of some Lagrange */
1649 /* multipliers, it will be restored to the following value later. */
1650
1651   i__1 = *n;
1652   for (i__ = 1; i__ <= i__1; ++i__) {
1653     dxnew[i__] = dx[i__] + step * sdirn[i__];
1654   }
1655   if (mcon == *m) {
1656     resold = resmax;
1657     resmax = 0.;
1658     i__1 = nact;
1659     for (k = 1; k <= i__1; ++k) {
1660       kk = iact[k];
1661       temp = b[kk];
1662       i__2 = *n;
1663       for (i__ = 1; i__ <= i__2; ++i__) {
1664         temp -= a[i__ + kk * a_dim1] * dxnew[i__];
1665       }
1666       resmax = max(resmax,temp);
1667     }
1668   }
1669
1670 /* Set VMULTD to the VMULTC vector that would occur if DX became DXNEW. A */
1671 /* device is included to force VMULTD(K)=0.0 if deviations from this value */
1672 /* can be attributed to computer rounding errors. First calculate the new */
1673 /* Lagrange multipliers. */
1674
1675   k = nact;
1676 L390:
1677   zdotw = 0.;
1678   zdwabs = 0.;
1679   i__1 = *n;
1680   for (i__ = 1; i__ <= i__1; ++i__) {
1681     temp = z__[i__ + k * z_dim1] * dxnew[i__];
1682     zdotw += temp;
1683     zdwabs += abs(temp);
1684   }
1685   acca = zdwabs + abs(zdotw) * .1;
1686   accb = zdwabs + abs(zdotw) * .2;
1687   if (zdwabs >= acca || acca >= accb) {
1688     zdotw = 0.;
1689   }
1690   vmultd[k] = zdotw / zdota[k];
1691   if (k >= 2) {
1692     kk = iact[k];
1693     i__1 = *n;
1694     for (i__ = 1; i__ <= i__1; ++i__) {
1695       dxnew[i__] -= vmultd[k] * a[i__ + kk * a_dim1];
1696     }
1697     --k;
1698     goto L390;
1699   }
1700   if (mcon > *m) {
1701     d__1 = 0., d__2 = vmultd[nact];
1702     vmultd[nact] = max(d__1,d__2);
1703   }
1704
1705 /* Complete VMULTC by finding the new constraint residuals. */
1706
1707   i__1 = *n;
1708   for (i__ = 1; i__ <= i__1; ++i__) {
1709     dxnew[i__] = dx[i__] + step * sdirn[i__];
1710   }
1711   if (mcon > nact) {
1712     kl = nact + 1;
1713     i__1 = mcon;
1714     for (k = kl; k <= i__1; ++k) {
1715       kk = iact[k];
1716       sum = resmax - b[kk];
1717       sumabs = resmax + (d__1 = b[kk], abs(d__1));
1718       i__2 = *n;
1719       for (i__ = 1; i__ <= i__2; ++i__) {
1720         temp = a[i__ + kk * a_dim1] * dxnew[i__];
1721         sum += temp;
1722         sumabs += abs(temp);
1723       }
1724       acca = sumabs + abs(sum) * .1f;
1725       accb = sumabs + abs(sum) * .2f;
1726       if (sumabs >= acca || acca >= accb) {
1727         sum = 0.f;
1728       }
1729       vmultd[k] = sum;
1730     }
1731   }
1732
1733 /* Calculate the fraction of the step from DX to DXNEW that will be taken. */
1734
1735   ratio = 1.;
1736   icon = 0;
1737   i__1 = mcon;
1738   for (k = 1; k <= i__1; ++k) {
1739     if (vmultd[k] < 0.) {
1740       temp = vmultc[k] / (vmultc[k] - vmultd[k]);
1741       if (temp < ratio) {
1742         ratio = temp;
1743         icon = k;
1744       }
1745     }
1746   }
1747
1748 /* Update DX, VMULTC and RESMAX. */
1749
1750   temp = 1. - ratio;
1751   i__1 = *n;
1752   for (i__ = 1; i__ <= i__1; ++i__) {
1753     dx[i__] = temp * dx[i__] + ratio * dxnew[i__];
1754   }
1755   i__1 = mcon;
1756   for (k = 1; k <= i__1; ++k) {
1757     d__1 = 0., d__2 = temp * vmultc[k] + ratio * vmultd[k];
1758     vmultc[k] = max(d__1,d__2);
1759   }
1760   if (mcon == *m) {
1761     resmax = resold + ratio * (resmax - resold);
1762   }
1763
1764 /* If the full step is not acceptable then begin another iteration. */
1765 /* Otherwise switch to stage two or end the calculation. */
1766
1767   if (icon > 0) {
1768     goto L70;
1769   }
1770   if (step == stpful) {
1771     goto L500;
1772   }
1773 L480:
1774   mcon = *m + 1;
1775   icon = mcon;
1776   iact[mcon] = mcon;
1777   vmultc[mcon] = 0.;
1778   goto L60;
1779
1780 /* We employ any freedom that may be available to reduce the objective */
1781 /* function before returning a DX whose length is less than RHO. */
1782
1783 L490:
1784   if (mcon == *m) {
1785     goto L480;
1786   }
1787   *ifull = 0;
1788 L500:
1789   return;
1790 } /* trstlp */