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