1 /* cobyla : contrained optimization by linear approximation */
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)
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:
15 * The above copyright notice and this permission notice shall be included
16 * in all copies or substantial portions of the Software.
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.
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.
31 * The original source code can be found at :
32 * http://plato.la.asu.edu/topics/problems/nlores.html
35 static char const rcsid[] =
36 "@(#) $Jeannot: cobyla.c,v 1.11 2004/04/18 09:51:36 js Exp $";
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
55 #define ENFORCE_BOUNDS 1
57 #define MIN2(a,b) ((a) <= (b) ? (a) : (b))
58 #define MAX2(a,b) ((a) >= (b) ? (a) : (b))
60 #define U(n) ((unsigned) (n))
62 /**************************************************************************/
63 /* SGJ, 2008: NLopt-style interface function: */
74 double *con_tol, *scale;
78 static int func_wrap(int ni, int mi, double *x, double *f, double *con,
83 double *xtmp = s->xtmp;
84 const double *lb = s->lb, *ub = s->ub;
86 (void) mi; /* unused */
88 /* in nlopt, we guarante that the function is never evaluated outside
89 the lb and ub bounds, so we need force this with xtmp ... note
90 that this leads to discontinuity in the first derivative, which
91 slows convergence if we don't enable the ENFORCE_BOUNDS feature
93 for (j = 0; j < n; ++j) {
94 if (x[j] < lb[j]) xtmp[j] = lb[j];
95 else if (x[j] > ub[j]) xtmp[j] = ub[j];
98 nlopt_unscale(n, s->scale, xtmp, xtmp);
100 *f = s->f(n, xtmp, NULL, s->f_data);
101 if (nlopt_stop_forced(s->stop)) return 1;
103 for (j = 0; j < s->m_orig; ++j) {
104 nlopt_eval_constraint(con + i, NULL, s->fc+j, n, xtmp);
105 if (nlopt_stop_forced(s->stop)) return 1;
106 for (k = 0; k < s->fc[j].m; ++k)
107 con[i + k] = -con[i + k];
110 for (j = 0; j < s->p; ++j) {
111 nlopt_eval_constraint(con + i, NULL, s->h+j, n, xtmp);
112 if (nlopt_stop_forced(s->stop)) return 1;
113 for (k = 0; k < s->h[j].m; ++k)
114 con[(i + s->h[j].m) + k] = -con[i + k];
117 for (j = 0; j < n; ++j) {
118 if (!nlopt_isinf(lb[j]))
119 con[i++] = x[j] - lb[j];
120 if (!nlopt_isinf(ub[j]))
121 con[i++] = ub[j] - x[j];
130 COBYLA_MSG_NONE = 0, /* No messages */
131 COBYLA_MSG_EXIT = 1, /* Exit reasons */
132 COBYLA_MSG_ITER = 2, /* Rho and Sigma changes */
133 COBYLA_MSG_INFO = 3 /* Informational messages */
137 * A function as required by cobyla
138 * state is a void pointer provided to the function at each call
140 * n : the number of variables
141 * m : the number of constraints
142 * x : on input, then vector of variables (should not be modified)
143 * f : on output, the value of the function
144 * con : on output, the value of the constraints (vector of size m)
145 * state : on input, the value of the state variable as provided to cobyla
147 * COBYLA will try to make all the values of the constraints positive.
148 * So if you want to input a constraint j such as x[i] <= MAX, set:
149 * con[j] = MAX - x[i]
150 * The function must returns 0 if no error occurs or 1 to immediately end the
154 typedef int cobyla_function(int n, int m, double *x, double *f, double *con,
155 func_wrap_state *state);
158 * cobyla : minimize a function subject to constraints
160 * n : number of variables (>=0)
161 * m : number of constraints (>=0)
162 * x : on input, initial estimate ; on output, the solution
163 * minf : on output, minimum objective function
164 * rhobeg : a reasonable initial change to the variables
165 * stop : the NLopt stopping criteria
166 * lb, ub : lower and upper bounds on x
167 * message : see the cobyla_message enum
168 * calcfc : the function to minimize (see cobyla_function)
169 * state : used by function (see cobyla_function)
171 * The cobyla function returns the usual nlopt_result codes.
174 extern nlopt_result cobyla(int n, int m, double *x, double *minf, double rhobeg, double rhoend, nlopt_stopping *stop, const double *lb, const double *ub,
175 int message, cobyla_function *calcfc, func_wrap_state *state);
177 nlopt_result cobyla_minimize(unsigned n, nlopt_func f, void *f_data,
178 unsigned m, nlopt_constraint *fc,
179 unsigned p, nlopt_constraint *h,
180 const double *lb, const double *ub, /* bounds */
181 double *x, /* in: initial guess, out: minimizer */
183 nlopt_stopping *stop,
189 double rhobeg, rhoend;
191 s.f = f; s.f_data = f_data;
197 s.lb = s.ub = s.xtmp = s.con_tol = s.scale = NULL;
199 s.scale = nlopt_compute_rescaling(n, dx);
200 if (!s.scale) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
201 for (j = 0; j < n; ++j)
202 if (s.scale[j] == 0 || !nlopt_isfinite(s.scale[j])) {
203 nlopt_stop_msg(stop, "invalid scaling %g of dimension %d: possible over/underflow?", s.scale[j], j);
204 ret = NLOPT_INVALID_ARGS; goto done;
207 s.lb = nlopt_new_rescaled(n, s.scale, lb);
208 if (!s.lb) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
209 s.ub = nlopt_new_rescaled(n, s.scale, ub);
210 if (!s.ub) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
211 nlopt_reorder_bounds(n, s.lb, s.ub);
213 s.xtmp = (double *) malloc(sizeof(double) * n);
214 if (!s.xtmp) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
216 /* SGJ, 2008: compute rhoend from NLopt stop info */
217 rhobeg = fabs(dx[0] / s.scale[0]);
218 rhoend = stop->xtol_rel * (rhobeg);
219 for (j = 0; j < n; ++j)
220 if (rhoend < stop->xtol_abs[j] / fabs(s.scale[j]))
221 rhoend = stop->xtol_abs[j] / fabs(s.scale[j]);
223 /* each equality constraint gives two inequality constraints */
224 m = nlopt_count_constraints(m, fc) + 2 * nlopt_count_constraints(p, h);
226 /* add constraints for lower/upper bounds (if any) */
227 for (j = 0; j < n; ++j) {
228 if (!nlopt_isinf(lb[j]))
230 if (!nlopt_isinf(ub[j]))
234 s.con_tol = (double *) malloc(sizeof(double) * m);
235 if (m && !s.con_tol) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
237 for (j = 0; j < m; ++j) s.con_tol[j] = 0;
238 for (j = i = 0; i < s.m_orig; ++i) {
239 unsigned ji = j, jnext = j + fc[i].m;
240 for (; j < jnext; ++j) s.con_tol[j] = fc[i].tol[j - ji];
242 for (i = 0; i < s.p; ++i) {
243 unsigned ji = j, jnext = j + h[i].m;
244 for (; j < jnext; ++j) s.con_tol[j] = h[i].tol[j - ji];
245 ji = j; jnext = j + h[i].m;
246 for (; j < jnext; ++j) s.con_tol[j] = h[i].tol[j - ji];
249 nlopt_rescale(n, s.scale, x, x);
250 ret = cobyla((int) n, (int) m, x, minf, rhobeg, rhoend,
251 stop, s.lb, s.ub, COBYLA_MSG_NONE,
253 nlopt_unscale(n, s.scale, x, x);
255 /* make sure e.g. rounding errors didn't push us slightly out of bounds */
256 for (j = 0; j < n; ++j) {
257 if (x[j] < lb[j]) x[j] = lb[j];
258 if (x[j] > ub[j]) x[j] = ub[j];
270 /**************************************************************************/
272 /* SGJ, 2010: I find that it seems to increase robustness of the algorithm
273 if, on "simplex" steps (which are intended to improve the linear
274 independence of the simplex, not improve the objective), we multiply
275 the steps by pseudorandom numbers in [0,1). Intuitively, pseudorandom
276 steps are likely to avoid any accidental dependency in the simplex
277 vertices. However, since I still want COBYLA to be a deterministic
278 algorithm, and I don't care all that much about the quality of the
279 randomness here, I implement a simple linear congruential generator
280 rather than use nlopt_urand, and set the initial seed deterministically. */
282 #if defined(HAVE_STDINT_H)
286 #ifndef HAVE_UINT32_T
287 # if SIZEOF_UNSIGNED_LONG == 4
288 typedef unsigned long uint32_t;
289 # elif SIZEOF_UNSIGNED_INT == 4
290 typedef unsigned int uint32_t;
292 # error No 32-bit unsigned integer type
296 /* a simple linear congruential generator */
298 static uint32_t lcg_rand(uint32_t *seed)
300 return (*seed = *seed * 1103515245 + 12345);
303 static double lcg_urand(uint32_t *seed, double a, double b)
305 return a + lcg_rand(seed) * (b - a) / ((uint32_t) -1);
308 /**************************************************************************/
310 static nlopt_result cobylb(int *n, int *m, int *mpp, double *x, double *minf, double *rhobeg, double rhoend,
311 nlopt_stopping *stop, const double *lb, const double *ub, int *iprint, double *con, double *sim,
312 double *simi, double *datmat, double *a, double *vsig, double *veta,
313 double *sigbar, double *dx, double *w, int *iact, cobyla_function *calcfc,
314 func_wrap_state *state);
315 static nlopt_result trstlp(int *n, int *m, double *a, double *b, double *rho,
316 double *dx, int *ifull, int *iact, double *z__, double *zdota, double *vmultc,
317 double *sdirn, double *dxnew, double *vmultd);
319 /* ------------------------------------------------------------------------ */
321 nlopt_result cobyla(int n, int m, double *x, double *minf, double rhobeg, double rhoend, nlopt_stopping *stop, const double *lb, const double *ub, int iprint,
322 cobyla_function *calcfc, func_wrap_state *state)
324 int icon, isim, isigb, idatm, iveta, isimi, ivsig, iwork, ia, idx, mpp;
330 * This subroutine minimizes an objective function F(X) subject to M
331 * inequality constraints on X, where X is a vector of variables that has
332 * N components. The algorithm employs linear approximations to the
333 * objective and constraint functions, the approximations being formed by
334 * linear interpolation at N+1 points in the space of the variables.
335 * We regard these interpolation points as vertices of a simplex. The
336 * parameter RHO controls the size of the simplex and it is reduced
337 * automatically from RHOBEG to RHOEND. For each RHO the subroutine tries
338 * to achieve a good vector of variables for the current size, and then
339 * RHO is reduced until the value RHOEND is reached. Therefore RHOBEG and
340 * RHOEND should be set to reasonable initial changes to and the required
341 * accuracy in the variables respectively, but this accuracy should be
342 * viewed as a subject for experimentation because it is not guaranteed.
343 * The subroutine has an advantage over many of its competitors, however,
344 * which is that it treats each constraint individually when calculating
345 * a change to the variables, instead of lumping the constraints together
346 * into a single penalty function. The name of the subroutine is derived
347 * from the phrase Constrained Optimization BY Linear Approximations.
349 * The user must set the values of N, M, RHOBEG and RHOEND, and must
350 * provide an initial vector of variables in X. Further, the value of
351 * IPRINT should be set to 0, 1, 2 or 3, which controls the amount of
352 * printing during the calculation. Specifically, there is no output if
353 * IPRINT=0 and there is output only at the end of the calculation if
354 * IPRINT=1. Otherwise each new value of RHO and SIGMA is printed.
355 * Further, the vector of variables and some function information are
356 * given either when RHO is reduced or when each new value of F(X) is
357 * computed in the cases IPRINT=2 or IPRINT=3 respectively. Here SIGMA
358 * is a penalty parameter, it being assumed that a change to X is an
359 * improvement if it reduces the merit function
360 * F(X)+SIGMA*MAX(0.0,-C1(X),-C2(X),...,-CM(X)),
361 * where C1,C2,...,CM denote the constraint functions that should become
362 * nonnegative eventually, at least to the precision of RHOEND. In the
363 * printed output the displayed term that is multiplied by SIGMA is
364 * called MAXCV, which stands for 'MAXimum Constraint Violation'. The
365 * argument MAXFUN is an int variable that must be set by the user to a
366 * limit on the number of calls of CALCFC, the purpose of this routine being
367 * given below. The value of MAXFUN will be altered to the number of calls
368 * of CALCFC that are made. The arguments W and IACT provide real and
369 * int arrays that are used as working space. Their lengths must be at
370 * least N*(3*N+2*M+11)+4*M+6 and M+1 respectively.
372 * In order to define the objective and constraint functions, we require
373 * a subroutine that has the name and arguments
374 * SUBROUTINE CALCFC (N,M,X,F,CON)
375 * DIMENSION X(*),CON(*) .
376 * The values of N and M are fixed and have been defined already, while
377 * X is now the current vector of variables. The subroutine should return
378 * the objective and constraint functions at X in F and CON(1),CON(2),
379 * ...,CON(M). Note that we are trying to adjust X so that F(X) is as
380 * small as possible subject to the constraint functions being nonnegative.
382 * Partition the working space array W to provide the storage that is needed
383 * for the main calculation.
390 if (iprint>=1) fprintf(stderr, "cobyla: N==0.\n");
391 return NLOPT_SUCCESS;
396 if (iprint>=1) fprintf(stderr, "cobyla: N<0 or M<0.\n");
397 return NLOPT_INVALID_ARGS;
400 /* workspace allocation */
401 w = (double*) malloc(U(n*(3*n+2*m+11)+4*m+6)*sizeof(*w));
404 if (iprint>=1) fprintf(stderr, "cobyla: memory allocation error.\n");
405 return NLOPT_OUT_OF_MEMORY;
407 iact = (int*)malloc(U(m+1)*sizeof(*iact));
410 if (iprint>=1) fprintf(stderr, "cobyla: memory allocation error.\n");
412 return NLOPT_OUT_OF_MEMORY;
415 /* Parameter adjustments */
425 isimi = isim + n * n + n;
426 idatm = isimi + n * n;
427 ia = idatm + n * mpp + mpp;
428 ivsig = ia + m * n + n;
433 rc = cobylb(&n, &m, &mpp, &x[1], minf, &rhobeg, rhoend, stop, &lb[1], &ub[1], &iprint,
434 &w[icon], &w[isim], &w[isimi], &w[idatm], &w[ia], &w[ivsig], &w[iveta],
435 &w[isigb], &w[idx], &w[iwork], &iact[1], calcfc, state);
437 /* Parameter adjustments (reverse) */
447 /* ------------------------------------------------------------------------- */
448 static nlopt_result cobylb(int *n, int *m, int *mpp,
449 double *x, double *minf, double *rhobeg, double rhoend,
450 nlopt_stopping *stop, const double *lb, const double *ub,
451 int *iprint, double *con, double *sim, double *simi,
452 double *datmat, double *a, double *vsig, double *veta,
453 double *sigbar, double *dx, double *w, int *iact, cobyla_function *calcfc,
454 func_wrap_state *state)
456 /* System generated locals */
457 int sim_dim1, sim_offset, simi_dim1, simi_offset, datmat_dim1,
458 datmat_offset, a_dim1, a_offset, i__1, i__2, i__3;
461 /* Local variables */
462 double alpha, delta, denom, tempa, barmu;
463 double beta, cmin = 0.0, cmax = 0.0;
464 double cvmaxm, dxsign, prerem = 0.0;
465 double edgmax, pareta, prerec = 0.0, phimin, parsig = 0.0;
467 double phi, rho, sum = 0.0;
468 double ratio, vmold, parmu, error, vmnew;
469 double resmax, cvmaxp;
470 double resnew, trured;
471 double temp, wsig, f;
480 int mp, np, iz, ibrnch;
481 int nbest, ifull, iptem, jdrop;
482 nlopt_result rc = NLOPT_SUCCESS;
483 uint32_t seed = (uint32_t) (*n + *m); /* arbitrary deterministic LCG seed */
486 /* SGJ, 2008: added code to keep track of minimum feasible function val */
489 /* Set the initial values of some parameters. The last column of SIM holds */
490 /* the optimal vertex of the current simplex, and the preceding N columns */
491 /* hold the displacements from the optimal vertex to the other vertices. */
492 /* Further, SIMI holds the inverse of the matrix that is contained in the */
493 /* first N columns of SIM. */
495 /* Parameter adjustments */
497 a_offset = 1 + a_dim1 * 1;
500 simi_offset = 1 + simi_dim1 * 1;
503 sim_offset = 1 + sim_dim1 * 1;
506 datmat_offset = 1 + datmat_dim1 * 1;
507 datmat -= datmat_offset;
531 "cobyla: the initial value of RHO is %12.6E and PARMU is set to zero.\n",
536 for (i__ = 1; i__ <= i__1; ++i__) {
538 sim[i__ + np * sim_dim1] = x[i__];
540 for (j = 1; j <= i__2; ++j) {
541 sim[i__ + j * sim_dim1] = 0.;
542 simi[i__ + j * simi_dim1] = 0.;
546 /* SGJ: make sure step rhocur stays inside [lb,ub] */
547 if (x[i__] + rhocur > ub[i__]) {
548 if (x[i__] - rhocur >= lb[i__])
550 else if (ub[i__] - x[i__] > x[i__] - lb[i__])
551 rhocur = 0.5 * (ub[i__] - x[i__]);
553 rhocur = 0.5 * (x[i__] - lb[i__]);
556 sim[i__ + i__ * sim_dim1] = rhocur;
557 simi[i__ + i__ * simi_dim1] = 1.0 / rhocur;
562 /* Make the next call of the user-supplied subroutine CALCFC. These */
563 /* instructions are also used for calling CALCFC during the iterations of */
566 /* SGJ comment: why the hell couldn't he just use a damn subroutine?
567 #*&!%*@ Fortran-66 spaghetti code */
570 if (nlopt_stop_forced(stop)) rc = NLOPT_FORCED_STOP;
571 else if (stop->nevals > 0) {
572 if (nlopt_stop_evals(stop)) rc = NLOPT_MAXEVAL_REACHED;
573 else if (nlopt_stop_time(stop)) rc = NLOPT_MAXTIME_REACHED;
575 if (rc != NLOPT_SUCCESS) goto L600;
578 if (calcfc(*n, *m, &x[1], &f, &con[1], state))
581 fprintf(stderr, "cobyla: user requested end of minimization.\n");
583 rc = NLOPT_FORCED_STOP;
588 feasible = 1; /* SGJ, 2010 */
591 for (k = 1; k <= i__1; ++k) {
592 d__1 = resmax, d__2 = -con[k];
593 resmax = MAX2(d__1,d__2);
594 if (d__2 > state->con_tol[k-1])
595 feasible = 0; /* SGJ, 2010 */
599 /* SGJ, 2008: check for minf_max reached by feasible point */
600 if (f < stop->minf_max && feasible) {
601 rc = NLOPT_MINF_MAX_REACHED;
602 goto L620; /* not L600 because we want to use current x, f, resmax */
605 if (stop->nevals == *iprint - 1 || *iprint == 3) {
606 fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
607 stop->nevals, f, resmax);
609 fprintf(stderr, "cobyla: X =");
610 for (i__ = 1; i__ <= i__1; ++i__) {
611 if (i__>1) fprintf(stderr, " ");
612 fprintf(stderr, "%13.6E", x[i__]);
616 for (i__ = iptemp; i__ <= i__1; ++i__) {
617 if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla: ");
618 fprintf(stderr, "%15.6E", x[i__]);
621 fprintf(stderr, "\n");
629 /* Set the recently calculated function values in a column of DATMAT. This */
630 /* array has a column for each vertex of the current simplex, the entries of */
631 /* each column being the values of the constraint functions (if any) */
632 /* followed by the objective function and the greatest constraint violation */
636 for (k = 1; k <= i__1; ++k) {
637 datmat[k + jdrop * datmat_dim1] = con[k];
639 if (stop->nevals > np) {
643 /* Exchange the new vertex of the initial simplex with the optimal vertex if */
644 /* necessary. Then, if the initial simplex is not complete, pick its next */
645 /* vertex and calculate the function values there. */
648 if (datmat[mp + np * datmat_dim1] <= f) {
649 x[jdrop] = sim[jdrop + np * sim_dim1];
650 } else { /* improvement in function val */
651 double rhocur = x[jdrop] - sim[jdrop + np * sim_dim1];
652 /* SGJ: use rhocur instead of rho. In original code, rhocur == rho
653 always, but I want to change this to ensure that simplex points
654 fall within [lb,ub]. */
655 sim[jdrop + np * sim_dim1] = x[jdrop];
657 for (k = 1; k <= i__1; ++k) {
658 datmat[k + jdrop * datmat_dim1] = datmat[k + np * datmat_dim1]
660 datmat[k + np * datmat_dim1] = con[k];
663 for (k = 1; k <= i__1; ++k) {
664 sim[jdrop + k * sim_dim1] = -rhocur;
667 for (i__ = k; i__ <= i__2; ++i__) {
668 temp -= simi[i__ + k * simi_dim1];
670 simi[jdrop + k * simi_dim1] = temp;
674 if (stop->nevals <= *n) { /* evaluating initial simplex */
675 jdrop = stop->nevals;
676 /* SGJ: was += rho, but using sim[jdrop,jdrop] enforces consistency
677 if we change the stepsize above to stay in [lb,ub]. */
678 x[jdrop] += sim[jdrop + jdrop * sim_dim1];
684 /* Identify the optimal vertex of the current simplex. */
687 phimin = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
691 for (j = 1; j <= i__1; ++j) {
692 temp = datmat[mp + j * datmat_dim1] + parmu * datmat[*mpp + j *
697 } else if (temp == phimin && parmu == 0.) {
698 if (datmat[*mpp + j * datmat_dim1] < datmat[*mpp + nbest *
705 /* Switch the best vertex into pole position if it is not there already, */
706 /* and also update SIM, SIMI and DATMAT. */
710 for (i__ = 1; i__ <= i__1; ++i__) {
711 temp = datmat[i__ + np * datmat_dim1];
712 datmat[i__ + np * datmat_dim1] = datmat[i__ + nbest * datmat_dim1]
714 datmat[i__ + nbest * datmat_dim1] = temp;
717 for (i__ = 1; i__ <= i__1; ++i__) {
718 temp = sim[i__ + nbest * sim_dim1];
719 sim[i__ + nbest * sim_dim1] = 0.;
720 sim[i__ + np * sim_dim1] += temp;
723 for (k = 1; k <= i__2; ++k) {
724 sim[i__ + k * sim_dim1] -= temp;
725 tempa -= simi[k + i__ * simi_dim1];
727 simi[nbest + i__ * simi_dim1] = tempa;
731 /* Make an error return if SIGI is a poor approximation to the inverse of */
732 /* the leading N by N submatrix of SIG. */
736 for (i__ = 1; i__ <= i__1; ++i__) {
738 for (j = 1; j <= i__2; ++j) {
744 for (k = 1; k <= i__3; ++k) if (sim[k + j * sim_dim1] != 0) {
745 temp += simi[i__ + k * simi_dim1] * sim[k + j * sim_dim1];
747 d__1 = error, d__2 = fabs(temp);
748 error = MAX2(d__1,d__2);
753 fprintf(stderr, "cobyla: rounding errors are becoming damaging.\n");
755 rc = NLOPT_ROUNDOFF_LIMITED;
759 /* Calculate the coefficients of the linear approximations to the objective */
760 /* and constraint functions, placing minus the objective function gradient */
761 /* after the constraint gradients in the array A. The vector W is used for */
765 for (k = 1; k <= i__2; ++k) {
766 con[k] = -datmat[k + np * datmat_dim1];
768 for (j = 1; j <= i__1; ++j) {
769 w[j] = datmat[k + j * datmat_dim1] + con[k];
772 for (i__ = 1; i__ <= i__1; ++i__) {
775 for (j = 1; j <= i__3; ++j) {
776 temp += w[j] * simi[j + i__ * simi_dim1];
781 a[i__ + k * a_dim1] = temp;
785 /* Calculate the values of sigma and eta, and set IFLAG=0 if the current */
786 /* simplex is not acceptable. */
789 parsig = alpha * rho;
792 for (j = 1; j <= i__1; ++j) {
796 for (i__ = 1; i__ <= i__2; ++i__) {
797 d__1 = simi[j + i__ * simi_dim1];
799 d__1 = sim[i__ + j * sim_dim1];
802 vsig[j] = 1. / sqrt(wsig);
803 veta[j] = sqrt(weta);
804 if (vsig[j] < parsig || veta[j] > pareta) {
809 /* If a new vertex is needed to improve acceptability, then decide which */
810 /* vertex to drop from the simplex. */
812 if (ibrnch == 1 || iflag == 1) {
818 for (j = 1; j <= i__1; ++j) {
819 if (veta[j] > temp) {
826 for (j = 1; j <= i__1; ++j) {
827 if (vsig[j] < temp) {
834 /* Calculate the step to the new vertex and its sign. */
836 temp = gamma_ * rho * vsig[jdrop];
838 for (i__ = 1; i__ <= i__1; ++i__) {
839 dx[i__] = temp * simi[jdrop + i__ * simi_dim1];
844 for (k = 1; k <= i__1; ++k) {
847 for (i__ = 1; i__ <= i__2; ++i__) {
848 sum += a[i__ + k * a_dim1] * dx[i__];
851 temp = datmat[k + np * datmat_dim1];
852 d__1 = cvmaxp, d__2 = -sum - temp;
853 cvmaxp = MAX2(d__1,d__2);
854 d__1 = cvmaxm, d__2 = sum - temp;
855 cvmaxm = MAX2(d__1,d__2);
859 if (parmu * (cvmaxp - cvmaxm) > sum + sum) {
863 /* Update the elements of SIM and SIMI, and set the next X. */
867 for (i__ = 1; i__ <= i__1; ++i__) {
868 /* SGJ, 2010: pseudo-randomize simplex steps (see LCG comments above) */
869 dx[i__] = dxsign * dx[i__] * lcg_urand(&seed, 0.01, 1);
870 /* SGJ: make sure dx step says in [lb,ub] */
873 double xi = sim[i__ + np * sim_dim1];
875 if (xi + dx[i__] > ub[i__])
877 if (xi + dx[i__] < lb[i__]) {
878 if (xi - dx[i__] <= ub[i__])
880 else { /* try again with halved step */
887 sim[i__ + jdrop * sim_dim1] = dx[i__];
888 temp += simi[jdrop + i__ * simi_dim1] * dx[i__];
891 for (i__ = 1; i__ <= i__1; ++i__) {
892 simi[jdrop + i__ * simi_dim1] /= temp;
895 for (j = 1; j <= i__1; ++j) {
899 for (i__ = 1; i__ <= i__2; ++i__) {
900 temp += simi[j + i__ * simi_dim1] * dx[i__];
903 for (i__ = 1; i__ <= i__2; ++i__) {
904 simi[j + i__ * simi_dim1] -= temp * simi[jdrop + i__ *
908 x[j] = sim[j + np * sim_dim1] + dx[j];
912 /* Calculate DX=x(*)-x(0). Branch if the length of DX is less than 0.5*RHO. */
916 izdota = iz + *n * *n;
919 idxnew = isdirn + *n;
921 rc = trstlp(n, m, &a[a_offset], &con[1], &rho, &dx[1], &ifull, &iact[1], &w[
922 iz], &w[izdota], &w[ivmc], &w[isdirn], &w[idxnew], &w[ivmd]);
923 if (rc != NLOPT_SUCCESS) goto L600;
925 /* SGJ: since the bound constraints are linear, we should never get
926 a dx that lies outside the [lb,ub] constraints here, but we'll
927 enforce this anyway just to be paranoid */
929 for (i__ = 1; i__ <= i__1; ++i__) {
930 double xi = sim[i__ + np * sim_dim1];
931 if (xi + dx[i__] > ub[i__]) dx[i__] = ub[i__] - xi;
932 if (xi + dx[i__] < lb[i__]) dx[i__] = xi - lb[i__];
938 for (i__ = 1; i__ <= i__1; ++i__) {
942 if (temp < rho * .25 * rho) {
948 /* Predict the change to F and the new maximum constraint violation if the */
949 /* variables are altered from x(0) to x(0)+DX. */
954 for (k = 1; k <= i__1; ++k) {
957 for (i__ = 1; i__ <= i__2; ++i__) {
958 sum -= a[i__ + k * a_dim1] * dx[i__];
961 resnew = MAX2(resnew,sum);
965 /* Increase PARMU if necessary and branch back if this change alters the */
966 /* optimal vertex. Otherwise PREREM and PREREC will be set to the predicted */
967 /* reductions in the merit function and the maximum constraint violation */
971 prerec = datmat[*mpp + np * datmat_dim1] - resnew;
973 barmu = sum / prerec;
975 if (parmu < barmu * 1.5) {
978 fprintf(stderr, "cobyla: increase in PARMU to %12.6E\n", parmu);
980 phi = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
983 for (j = 1; j <= i__1; ++j) {
984 temp = datmat[mp + j * datmat_dim1] + parmu * datmat[*mpp + j *
989 if (temp == phi && parmu == 0.f) {
990 if (datmat[*mpp + j * datmat_dim1] < datmat[*mpp + np *
997 prerem = parmu * prerec - sum;
999 /* Calculate the constraint and objective functions at x(*). Then find the */
1000 /* actual reduction in the merit function. */
1003 for (i__ = 1; i__ <= i__1; ++i__) {
1004 x[i__] = sim[i__ + np * sim_dim1] + dx[i__];
1009 vmold = datmat[mp + np * datmat_dim1] + parmu * datmat[*mpp + np *
1011 vmnew = f + parmu * resmax;
1012 trured = vmold - vmnew;
1013 if (parmu == 0. && f == datmat[mp + np * datmat_dim1]) {
1015 trured = datmat[*mpp + np * datmat_dim1] - resmax;
1018 /* Begin the operations that decide whether x(*) should replace one of the */
1019 /* vertices of the current simplex, the change being mandatory if TRURED is */
1020 /* positive. Firstly, JDROP is set to the index of the vertex that is to be */
1024 if (trured <= 0.f) {
1029 for (j = 1; j <= i__1; ++j) {
1032 for (i__ = 1; i__ <= i__2; ++i__) {
1033 temp += simi[j + i__ * simi_dim1] * dx[i__];
1040 sigbar[j] = temp * vsig[j];
1043 /* Calculate the value of ell. */
1045 edgmax = delta * rho;
1048 for (j = 1; j <= i__1; ++j) {
1049 if (sigbar[j] >= parsig || sigbar[j] >= vsig[j]) {
1054 for (i__ = 1; i__ <= i__2; ++i__) {
1055 d__1 = dx[i__] - sim[i__ + j * sim_dim1];
1056 temp += d__1 * d__1;
1060 if (temp > edgmax) {
1073 /* Revise the simplex by updating the elements of SIM, SIMI and DATMAT. */
1077 for (i__ = 1; i__ <= i__1; ++i__) {
1078 sim[i__ + jdrop * sim_dim1] = dx[i__];
1079 temp += simi[jdrop + i__ * simi_dim1] * dx[i__];
1082 for (i__ = 1; i__ <= i__1; ++i__) {
1083 simi[jdrop + i__ * simi_dim1] /= temp;
1086 for (j = 1; j <= i__1; ++j) {
1090 for (i__ = 1; i__ <= i__2; ++i__) {
1091 temp += simi[j + i__ * simi_dim1] * dx[i__];
1094 for (i__ = 1; i__ <= i__2; ++i__) {
1095 simi[j + i__ * simi_dim1] -= temp * simi[jdrop + i__ *
1101 for (k = 1; k <= i__1; ++k) {
1102 datmat[k + jdrop * datmat_dim1] = con[k];
1105 /* Branch back for further iterations with the current RHO. */
1107 if (trured > 0. && trured >= prerem * .1) {
1108 /* SGJ, 2010: following a suggestion in the SAS manual (which
1109 mentions a similar modification to COBYLA, although they didn't
1110 publish their source code), increase rho if predicted reduction
1111 is sufficiently close to the actual reduction. Otherwise,
1112 COBLYA seems to easily get stuck making very small steps.
1113 Also require iflag != 0 (i.e., acceptable simplex), again
1114 following SAS suggestion (otherwise I get convergence failure
1116 if (trured >= prerem * 0.9 && trured <= prerem * 1.1 && iflag) {
1127 /* SGJ, 2008: convergence tests for function vals; note that current
1128 best val is stored in datmat[mp + np * datmat_dim1], or in f if
1129 ifull == 1, and previous best is in *minf. This seems like a
1130 sensible place to put the convergence test, as it is the same
1131 place that Powell checks the x tolerance (rho > rhoend). */
1133 double fbest = ifull == 1 ? f : datmat[mp + np * datmat_dim1];
1134 if (fbest < *minf && nlopt_stop_ftol(stop, fbest, *minf)) {
1135 rc = NLOPT_FTOL_REACHED;
1141 /* Otherwise reduce RHO if it is not at its least value and reset PARMU. */
1145 if (rho <= rhoend * 1.5) {
1151 for (k = 1; k <= i__1; ++k) {
1152 cmin = datmat[k + np * datmat_dim1];
1155 for (i__ = 1; i__ <= i__2; ++i__) {
1156 d__1 = cmin, d__2 = datmat[k + i__ * datmat_dim1];
1157 cmin = MIN2(d__1,d__2);
1158 d__1 = cmax, d__2 = datmat[k + i__ * datmat_dim1];
1159 cmax = MAX2(d__1,d__2);
1161 if (k <= *m && cmin < cmax * .5) {
1162 temp = MAX2(cmax,0.) - cmin;
1166 denom = MIN2(denom,temp);
1172 } else if (cmax - cmin < parmu * denom) {
1173 parmu = (cmax - cmin) / denom;
1177 fprintf(stderr, "cobyla: reduction in RHO to %12.6E and PARMU =%13.6E\n",
1181 fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
1182 stop->nevals, datmat[mp + np * datmat_dim1], datmat[*mpp + np * datmat_dim1]);
1184 fprintf(stderr, "cobyla: X =");
1186 for (i__ = 1; i__ <= i__1; ++i__) {
1187 if (i__>1) fprintf(stderr, " ");
1188 fprintf(stderr, "%13.6E", sim[i__ + np * sim_dim1]);
1192 for (i__ = iptemp; i__ <= i__1; ++i__) {
1193 if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla: ");
1194 fprintf(stderr, "%15.6E", x[i__]);
1197 fprintf(stderr, "\n");
1201 else /* rho <= rhoend */
1202 rc = rhoend > 0 ? NLOPT_XTOL_REACHED : NLOPT_ROUNDOFF_LIMITED;
1204 /* Return the best calculated values of the variables. */
1207 fprintf(stderr, "cobyla: normal return.\n");
1214 for (i__ = 1; i__ <= i__1; ++i__) {
1215 x[i__] = sim[i__ + np * sim_dim1];
1217 f = datmat[mp + np * datmat_dim1];
1218 resmax = datmat[*mpp + np * datmat_dim1];
1222 fprintf(stderr, "cobyla: NFVALS = %4d, F =%13.6E, MAXCV =%13.6E\n",
1223 stop->nevals, f, resmax);
1225 fprintf(stderr, "cobyla: X =");
1226 for (i__ = 1; i__ <= i__1; ++i__) {
1227 if (i__>1) fprintf(stderr, " ");
1228 fprintf(stderr, "%13.6E", x[i__]);
1232 for (i__ = iptemp; i__ <= i__1; ++i__) {
1233 if (!((i__-1) % 4)) fprintf(stderr, "\ncobyla: ");
1234 fprintf(stderr, "%15.6E", x[i__]);
1237 fprintf(stderr, "\n");
1242 /* ------------------------------------------------------------------------- */
1243 static nlopt_result trstlp(int *n, int *m, double *a,
1244 double *b, double *rho, double *dx, int *ifull,
1245 int *iact, double *z__, double *zdota, double *vmultc,
1246 double *sdirn, double *dxnew, double *vmultd)
1248 /* System generated locals */
1249 int a_dim1, a_offset, z_dim1, z_offset, i__1, i__2;
1252 /* Local variables */
1253 double alpha, tempa;
1255 double optnew, stpful, sum, tot, acca, accb;
1256 double ratio, vsave, zdotv, zdotw, dd;
1258 double sp, ss, resold = 0.0, zdvabs, zdwabs, sumabs, resmax, optold;
1262 int iout, i__, j, k;
1266 int nact, icon = 0, mcon;
1270 /* This subroutine calculates an N-component vector DX by applying the */
1271 /* following two stages. In the first stage, DX is set to the shortest */
1272 /* vector that minimizes the greatest violation of the constraints */
1273 /* A(1,K)*DX(1)+A(2,K)*DX(2)+...+A(N,K)*DX(N) .GE. B(K), K=2,3,...,M, */
1274 /* subject to the Euclidean length of DX being at most RHO. If its length is */
1275 /* strictly less than RHO, then we use the resultant freedom in DX to */
1276 /* minimize the objective function */
1277 /* -A(1,M+1)*DX(1)-A(2,M+1)*DX(2)-...-A(N,M+1)*DX(N) */
1278 /* subject to no increase in any greatest constraint violation. This */
1279 /* notation allows the gradient of the objective function to be regarded as */
1280 /* the gradient of a constraint. Therefore the two stages are distinguished */
1281 /* by MCON .EQ. M and MCON .GT. M respectively. It is possible that a */
1282 /* degeneracy may prevent DX from attaining the target length RHO. Then the */
1283 /* value IFULL=0 would be set, but usually IFULL=1 on return. */
1285 /* In general NACT is the number of constraints in the active set and */
1286 /* IACT(1),...,IACT(NACT) are their indices, while the remainder of IACT */
1287 /* contains a permutation of the remaining constraint indices. Further, Z is */
1288 /* an orthogonal matrix whose first NACT columns can be regarded as the */
1289 /* result of Gram-Schmidt applied to the active constraint gradients. For */
1290 /* J=1,2,...,NACT, the number ZDOTA(J) is the scalar product of the J-th */
1291 /* column of Z with the gradient of the J-th active constraint. DX is the */
1292 /* current vector of variables and here the residuals of the active */
1293 /* constraints should be zero. Further, the active constraints have */
1294 /* nonnegative Lagrange multipliers that are held at the beginning of */
1295 /* VMULTC. The remainder of this vector holds the residuals of the inactive */
1296 /* constraints at DX, the ordering of the components of VMULTC being in */
1297 /* agreement with the permutation of the indices of the constraints that is */
1298 /* in IACT. All these residuals are nonnegative, which is achieved by the */
1299 /* shift RESMAX that makes the least residual zero. */
1301 /* Initialize Z and some other variables. The value of RESMAX will be */
1302 /* appropriate to DX=0, while ICON will be the index of a most violated */
1303 /* constraint if RESMAX is positive. Usually during the first stage the */
1304 /* vector SDIRN gives a search direction that reduces all the active */
1305 /* constraint violations by one simultaneously. */
1307 /* Parameter adjustments */
1309 z_offset = 1 + z_dim1 * 1;
1312 a_offset = 1 + a_dim1 * 1;
1329 for (i__ = 1; i__ <= i__1; ++i__) {
1331 for (j = 1; j <= i__2; ++j) {
1332 z__[i__ + j * z_dim1] = 0.;
1334 z__[i__ + i__ * z_dim1] = 1.;
1339 for (k = 1; k <= i__1; ++k) {
1340 if (b[k] > resmax) {
1346 for (k = 1; k <= i__1; ++k) {
1348 vmultc[k] = resmax - b[k];
1355 for (i__ = 1; i__ <= i__1; ++i__) {
1359 /* End the current stage of the calculation if 3 consecutive iterations */
1360 /* have either failed to reduce the best calculated value of the objective */
1361 /* function or to increase the number of active constraints since the best */
1362 /* value was calculated. This strategy prevents cycling, but there is a */
1363 /* remote possibility that it will cause premature termination. */
1374 for (i__ = 1; i__ <= i__1; ++i__) {
1375 optnew -= dx[i__] * a[i__ + mcon * a_dim1];
1378 if (icount == 0 || optnew < optold) {
1382 } else if (nact > nactx) {
1392 /* If ICON exceeds NACT, then we add the constraint with index IACT(ICON) to */
1393 /* the active set. Apply Givens rotations so that the last N-NACT-1 columns */
1394 /* of Z are orthogonal to the gradient of the new constraint, a scalar */
1395 /* product being set to zero if its nonzero value could be due to computer */
1396 /* rounding errors. The array DXNEW is used for working space. */
1403 for (i__ = 1; i__ <= i__1; ++i__) {
1404 dxnew[i__] = a[i__ + kk * a_dim1];
1413 for (i__ = 1; i__ <= i__1; ++i__) {
1414 temp = z__[i__ + k * z_dim1] * dxnew[i__];
1416 spabs += fabs(temp);
1418 acca = spabs + fabs(sp) * .1;
1419 accb = spabs + fabs(sp) * .2;
1420 if (spabs >= acca || acca >= accb) {
1427 temp = sqrt(sp * sp + tot * tot);
1432 for (i__ = 1; i__ <= i__1; ++i__) {
1433 temp = alpha * z__[i__ + k * z_dim1] + beta * z__[i__ + kp *
1435 z__[i__ + kp * z_dim1] = alpha * z__[i__ + kp * z_dim1] -
1436 beta * z__[i__ + k * z_dim1];
1437 z__[i__ + k * z_dim1] = temp;
1444 /* Add the new constraint if this can be done without a deletion from the */
1450 vmultc[icon] = vmultc[nact];
1455 /* The next instruction is reached if a deletion has to be made from the */
1456 /* active set in order to make room for the new active constraint, because */
1457 /* the new constraint gradient is a linear combination of the gradients of */
1458 /* the old active constraints. Set the elements of VMULTD to the multipliers */
1459 /* of the linear combination. Further, set IOUT to the index of the */
1460 /* constraint to be deleted, but branch if no suitable index can be found. */
1468 for (i__ = 1; i__ <= i__1; ++i__) {
1469 temp = z__[i__ + k * z_dim1] * dxnew[i__];
1471 zdvabs += fabs(temp);
1473 acca = zdvabs + fabs(zdotv) * .1;
1474 accb = zdvabs + fabs(zdotv) * .2;
1475 if (zdvabs < acca && acca < accb) {
1476 temp = zdotv / zdota[k];
1477 if (temp > 0. && iact[k] <= *m) {
1478 tempa = vmultc[k] / temp;
1479 if (ratio < 0. || tempa < ratio) {
1487 for (i__ = 1; i__ <= i__1; ++i__) {
1488 dxnew[i__] -= temp * a[i__ + kw * a_dim1];
1503 /* Revise the Lagrange multipliers and reorder the active constraints so */
1504 /* that the one to be replaced is at the end of the list. Also calculate the */
1505 /* new value of ZDOTA(NACT) and branch if it is not acceptable. */
1508 for (k = 1; k <= i__1; ++k) {
1509 d__1 = 0., d__2 = vmultc[k] - ratio * vmultd[k];
1510 vmultc[k] = MAX2(d__1,d__2);
1514 vsave = vmultc[icon];
1521 for (i__ = 1; i__ <= i__1; ++i__) {
1522 sp += z__[i__ + k * z_dim1] * a[i__ + kw * a_dim1];
1525 temp = sqrt(sp * sp + d__1 * d__1);
1526 alpha = zdota[kp] / temp;
1528 zdota[kp] = alpha * zdota[k];
1531 for (i__ = 1; i__ <= i__1; ++i__) {
1532 temp = alpha * z__[i__ + kp * z_dim1] + beta * z__[i__ + k *
1534 z__[i__ + kp * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1535 z__[i__ + kp * z_dim1];
1536 z__[i__ + k * z_dim1] = temp;
1539 vmultc[k] = vmultc[kp];
1549 for (i__ = 1; i__ <= i__1; ++i__) {
1550 temp += z__[i__ + nact * z_dim1] * a[i__ + kk * a_dim1];
1557 vmultc[nact] = ratio;
1559 /* Update IACT and ensure that the objective function continues to be */
1560 /* treated as the last active constraint when MCON>M. */
1563 iact[icon] = iact[nact];
1565 if (mcon > *m && kk != mcon) {
1569 for (i__ = 1; i__ <= i__1; ++i__) {
1570 sp += z__[i__ + k * z_dim1] * a[i__ + kk * a_dim1];
1573 temp = sqrt(sp * sp + d__1 * d__1);
1574 alpha = zdota[nact] / temp;
1576 zdota[nact] = alpha * zdota[k];
1579 for (i__ = 1; i__ <= i__1; ++i__) {
1580 temp = alpha * z__[i__ + nact * z_dim1] + beta * z__[i__ + k *
1582 z__[i__ + nact * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1583 z__[i__ + nact * z_dim1];
1584 z__[i__ + k * z_dim1] = temp;
1586 iact[nact] = iact[k];
1589 vmultc[k] = vmultc[nact];
1590 vmultc[nact] = temp;
1593 /* If stage one is in progress, then set SDIRN to the direction of the next */
1594 /* change to the current vector of variables. */
1602 for (i__ = 1; i__ <= i__1; ++i__) {
1603 temp += sdirn[i__] * a[i__ + kk * a_dim1];
1606 temp /= zdota[nact];
1608 for (i__ = 1; i__ <= i__1; ++i__) {
1609 sdirn[i__] -= temp * z__[i__ + nact * z_dim1];
1613 /* Delete the constraint that has the index IACT(ICON) from the active set. */
1618 vsave = vmultc[icon];
1625 for (i__ = 1; i__ <= i__1; ++i__) {
1626 sp += z__[i__ + k * z_dim1] * a[i__ + kk * a_dim1];
1629 temp = sqrt(sp * sp + d__1 * d__1);
1630 alpha = zdota[kp] / temp;
1632 zdota[kp] = alpha * zdota[k];
1635 for (i__ = 1; i__ <= i__1; ++i__) {
1636 temp = alpha * z__[i__ + kp * z_dim1] + beta * z__[i__ + k *
1638 z__[i__ + kp * z_dim1] = alpha * z__[i__ + k * z_dim1] - beta *
1639 z__[i__ + kp * z_dim1];
1640 z__[i__ + k * z_dim1] = temp;
1643 vmultc[k] = vmultc[kp];
1653 /* If stage one is in progress, then set SDIRN to the direction of the next */
1654 /* change to the current vector of variables. */
1661 for (i__ = 1; i__ <= i__1; ++i__) {
1662 temp += sdirn[i__] * z__[i__ + (nact + 1) * z_dim1];
1665 for (i__ = 1; i__ <= i__1; ++i__) {
1666 sdirn[i__] -= temp * z__[i__ + (nact + 1) * z_dim1];
1670 /* Pick the next search direction of stage two. */
1673 temp = 1. / zdota[nact];
1675 for (i__ = 1; i__ <= i__1; ++i__) {
1676 sdirn[i__] = temp * z__[i__ + nact * z_dim1];
1679 /* Calculate the step to the boundary of the trust region or take the step */
1680 /* that reduces RESMAX to zero. The two statements below that include the */
1681 /* factor 1.0E-6 prevent some harmless underflows that occurred in a test */
1682 /* calculation. Further, we skip the step if it could be zero within a */
1683 /* reasonable tolerance for computer rounding errors. */
1690 for (i__ = 1; i__ <= i__1; ++i__) {
1691 if ((d__1 = dx[i__], fabs(d__1)) >= *rho * 1e-6f) {
1695 sd += dx[i__] * sdirn[i__];
1702 temp = sqrt(ss * dd);
1703 if (fabs(sd) >= temp * 1e-6f) {
1704 temp = sqrt(ss * dd + sd * sd);
1706 stpful = dd / (temp + sd);
1709 acca = step + resmax * .1;
1710 accb = step + resmax * .2;
1711 if (step >= acca || acca >= accb) {
1714 step = MIN2(step,resmax);
1717 /* SGJ, 2010: check for error here */
1718 if (nlopt_isinf(step)) return NLOPT_ROUNDOFF_LIMITED;
1720 /* Set DXNEW to the new variables if STEP is the steplength, and reduce */
1721 /* RESMAX to the corresponding maximum residual if stage one is being done. */
1722 /* Because DXNEW will be changed during the calculation of some Lagrange */
1723 /* multipliers, it will be restored to the following value later. */
1726 for (i__ = 1; i__ <= i__1; ++i__) {
1727 dxnew[i__] = dx[i__] + step * sdirn[i__];
1733 for (k = 1; k <= i__1; ++k) {
1737 for (i__ = 1; i__ <= i__2; ++i__) {
1738 temp -= a[i__ + kk * a_dim1] * dxnew[i__];
1740 resmax = MAX2(resmax,temp);
1744 /* Set VMULTD to the VMULTC vector that would occur if DX became DXNEW. A */
1745 /* device is included to force VMULTD(K)=0.0 if deviations from this value */
1746 /* can be attributed to computer rounding errors. First calculate the new */
1747 /* Lagrange multipliers. */
1754 for (i__ = 1; i__ <= i__1; ++i__) {
1755 temp = z__[i__ + k * z_dim1] * dxnew[i__];
1757 zdwabs += fabs(temp);
1759 acca = zdwabs + fabs(zdotw) * .1;
1760 accb = zdwabs + fabs(zdotw) * .2;
1761 if (zdwabs >= acca || acca >= accb) {
1764 vmultd[k] = zdotw / zdota[k];
1768 for (i__ = 1; i__ <= i__1; ++i__) {
1769 dxnew[i__] -= vmultd[k] * a[i__ + kk * a_dim1];
1775 d__1 = 0., d__2 = vmultd[nact];
1776 vmultd[nact] = MAX2(d__1,d__2);
1779 /* Complete VMULTC by finding the new constraint residuals. */
1782 for (i__ = 1; i__ <= i__1; ++i__) {
1783 dxnew[i__] = dx[i__] + step * sdirn[i__];
1788 for (k = kl; k <= i__1; ++k) {
1790 sum = resmax - b[kk];
1791 sumabs = resmax + (d__1 = b[kk], fabs(d__1));
1793 for (i__ = 1; i__ <= i__2; ++i__) {
1794 temp = a[i__ + kk * a_dim1] * dxnew[i__];
1796 sumabs += fabs(temp);
1798 acca = sumabs + fabs(sum) * .1f;
1799 accb = sumabs + fabs(sum) * .2f;
1800 if (sumabs >= acca || acca >= accb) {
1807 /* Calculate the fraction of the step from DX to DXNEW that will be taken. */
1812 for (k = 1; k <= i__1; ++k) {
1813 if (vmultd[k] < 0.) {
1814 temp = vmultc[k] / (vmultc[k] - vmultd[k]);
1822 /* Update DX, VMULTC and RESMAX. */
1826 for (i__ = 1; i__ <= i__1; ++i__) {
1827 dx[i__] = temp * dx[i__] + ratio * dxnew[i__];
1830 for (k = 1; k <= i__1; ++k) {
1831 d__1 = 0., d__2 = temp * vmultc[k] + ratio * vmultd[k];
1832 vmultc[k] = MAX2(d__1,d__2);
1835 resmax = resold + ratio * (resmax - resold);
1838 /* If the full step is not acceptable then begin another iteration. */
1839 /* Otherwise switch to stage two or end the calculation. */
1844 if (step == stpful) {
1854 /* We employ any freedom that may be available to reduce the objective */
1855 /* function before returning a DX whose length is less than RHO. */
1863 return NLOPT_SUCCESS;