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