diff --git a/wrap/newuoa/include/newuoa.h b/wrap/newuoa/include/newuoa.h new file mode 100644 index 00000000..b1fb075e --- /dev/null +++ b/wrap/newuoa/include/newuoa.h @@ -0,0 +1,1728 @@ +/* + This is NEWUOA for unconstrained minimization. The codes were written + by Powell in Fortran and then translated to C with f2c. I further + modified the code to make it independent of libf2c and f2c.h. Please + refer to "The NEWUOA software for unconstrained optimization without + derivatives", which is available at www.damtp.cam.ac.uk, for more + information. + */ +/* + The original fortran codes are distributed without restrictions. The + C++ codes are distributed under MIT license. + */ +/* The MIT License + + Copyright (c) 2004, by M.J.D. Powell + 2008, by Attractive Chaos + + Permission is hereby granted, free of charge, to any person obtaining + a copy of this software and associated documentation files (the + "Software"), to deal in the Software without restriction, including + without limitation the rights to use, copy, modify, merge, publish, + distribute, sublicense, and/or sell copies of the Software, and to + permit persons to whom the Software is furnished to do so, subject to + the following conditions: + + The above copyright notice and this permission notice shall be + included in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ + +#ifndef AC_NEWUOA_HH_ +#define AC_NEWUOA_HH_ +#include +#include +#include + +//PARAMETER DECREASE WAS 0.1 +#define DELTA_DECREASE 0.5 +#define RHO_DECREASE 0.5 + + +/*most probably: + TYPE is float or double + Func is defined as double func(int n, double *x); + or better as + class Func { + double operator()(int n, double *x); + }; + where n is the number of parameters and x the vector parameter + r_start stands unknown... +*/ + +template +TYPE min_newuoa(int n, TYPE *x, Func &func, TYPE r_start=1e7, TYPE tol=1e-8, int max_iter=5000); + +template +static int biglag_(int n, int npt, TYPE *xopt, TYPE *xpt, TYPE *bmat, TYPE *zmat, int *idz, + int *ndim, int *knew, TYPE *delta, TYPE *d__, TYPE *alpha, TYPE *hcol, TYPE *gc, + TYPE *gd, TYPE *s, TYPE *w, Func & /*func*/) +{ + /* N is the number of variables. NPT is the number of interpolation + * equations. XOPT is the best interpolation point so far. XPT + * contains the coordinates of the current interpolation + * points. BMAT provides the last N columns of H. ZMAT and IDZ give + * a factorization of the first NPT by NPT submatrix of H. NDIM is + * the first dimension of BMAT and has the value NPT+N. KNEW is the + * index of the interpolation point that is going to be moved. DEBLLTA + * is the current trust region bound. D will be set to the step from + * XOPT to the new point. ABLLPHA will be set to the KNEW-th diagonal + * element of the H matrix. HCOBLL, GC, GD, S and W will be used for + * working space. */ + /* The step D is calculated in a way that attempts to maximize the + * modulus of BLLFUNC(XOPT+D), subject to the bound ||D|| .BLLE. DEBLLTA, + * where BLLFUNC is the KNEW-th BLLagrange function. */ + + int xpt_dim1, xpt_offset, bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, + i__1, i__2, i__, j, k, iu, nptm, iterc, isave; + TYPE sp, ss, cf1, cf2, cf3, cf4, cf5, dhd, cth, tau, sth, sum, temp, step, + angle, scale, denom, delsq, tempa, tempb, twopi, taubeg, tauold, taumax, + d__1, dd, gg; + + /* Parameter adjustments */ + tempa = tempb = 0.0; + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --xopt; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --d__; --hcol; --gc; --gd; --s; --w; + /* Function Body */ + twopi = 2.0 * M_PI; + delsq = *delta * *delta; + nptm = npt - n - 1; + /* Set the first NPT components of HCOBLL to the leading elements of + * the KNEW-th column of H. */ + iterc = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) hcol[k] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) { + temp = zmat[*knew + j * zmat_dim1]; + if (j < *idz) temp = -temp; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + hcol[k] += temp * zmat[k + j * zmat_dim1]; + } + *alpha = hcol[*knew]; + /* Set the unscaled initial direction D. Form the gradient of BLLFUNC + * atXOPT, and multiply D by the second derivative matrix of + * BLLFUNC. */ + dd = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + d__[i__] = xpt[*knew + i__ * xpt_dim1] - xopt[i__]; + gc[i__] = bmat[*knew + i__ * bmat_dim1]; + gd[i__] = 0; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + } + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + temp = 0; + sum = 0; + i__1 = n; + for (j = 1; j <= i__1; ++j) { + temp += xpt[k + j * xpt_dim1] * xopt[j]; + sum += xpt[k + j * xpt_dim1] * d__[j]; + } + temp = hcol[k] * temp; + sum = hcol[k] * sum; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + gc[i__] += temp * xpt[k + i__ * xpt_dim1]; + gd[i__] += sum * xpt[k + i__ * xpt_dim1]; + } + } + /* Scale D and GD, with a sign change if required. Set S to another + * vector in the initial two dimensional subspace. */ + gg = sp = dhd = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + /* Computing 2nd power */ + d__1 = gc[i__]; + gg += d__1 * d__1; + sp += d__[i__] * gc[i__]; + dhd += d__[i__] * gd[i__]; + } + scale = *delta / sqrt(dd); + if (sp * dhd < 0) scale = -scale; + temp = 0; + if (sp * sp > dd * .99 * gg) temp = 1.0; + tau = scale * (fabs(sp) + 0.5 * scale * fabs(dhd)); + if (gg * delsq < tau * .01 * tau) temp = 1.0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + d__[i__] = scale * d__[i__]; + gd[i__] = scale * gd[i__]; + s[i__] = gc[i__] + temp * gd[i__]; + } + /* Begin the iteration by overwriting S with a vector that has the + * required length and direction, except that termination occurs if + * the given D and S are nearly parallel. */ + for (iterc = 0; iterc != n; ++iterc) { + dd = sp = ss = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + sp += d__[i__] * s[i__]; + /* Computing 2nd power */ + d__1 = s[i__]; + ss += d__1 * d__1; + } + temp = dd * ss - sp * sp; + if (temp <= dd * 1e-8 * ss) return 0; + denom = sqrt(temp); + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + s[i__] = (dd * s[i__] - sp * d__[i__]) / denom; + w[i__] = 0; + } + /* Calculate the coefficients of the objective function on the + * circle, beginning with the multiplication of S by the second + * derivative matrix. */ + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + sum += xpt[k + j * xpt_dim1] * s[j]; + sum = hcol[k] * sum; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + w[i__] += sum * xpt[k + i__ * xpt_dim1]; + } + cf1 = cf2 = cf3 = cf4 = cf5 = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + cf1 += s[i__] * w[i__]; + cf2 += d__[i__] * gc[i__]; + cf3 += s[i__] * gc[i__]; + cf4 += d__[i__] * gd[i__]; + cf5 += s[i__] * gd[i__]; + } + cf1 = 0.5 * cf1; + cf4 = 0.5 * cf4 - cf1; + /* Seek the value of the angle that maximizes the modulus of TAU. */ + taubeg = cf1 + cf2 + cf4; + taumax = tauold = taubeg; + isave = 0; + iu = 49; + temp = twopi / (TYPE) (iu + 1); + i__2 = iu; + for (i__ = 1; i__ <= i__2; ++i__) { + angle = (TYPE) i__ *temp; + cth = cos(angle); + sth = sin(angle); + tau = cf1 + (cf2 + cf4 * cth) * cth + (cf3 + cf5 * cth) * sth; + if (fabs(tau) > fabs(taumax)) { + taumax = tau; + isave = i__; + tempa = tauold; + } else if (i__ == isave + 1) tempb = tau; + tauold = tau; + } + if (isave == 0) tempa = tau; + if (isave == iu) tempb = taubeg; + step = 0; + if (tempa != tempb) { + tempa -= taumax; + tempb -= taumax; + step = 0.5 * (tempa - tempb) / (tempa + tempb); + } + angle = temp * ((TYPE) isave + step); + /* Calculate the new D and GD. Then test for convergence. */ + cth = cos(angle); + sth = sin(angle); + tau = cf1 + (cf2 + cf4 * cth) * cth + (cf3 + cf5 * cth) * sth; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + d__[i__] = cth * d__[i__] + sth * s[i__]; + gd[i__] = cth * gd[i__] + sth * w[i__]; + s[i__] = gc[i__] + gd[i__]; + } + if (fabs(tau) <= fabs(taubeg) * 1.1) return 0; + } + return 0; +} + +template +static int bigden_(int n, int npt, TYPE *xopt, TYPE *xpt, TYPE *bmat, TYPE *zmat, int *idz, + int *ndim, int *kopt, int *knew, TYPE *d__, TYPE *w, TYPE *vlag, TYPE *beta, + TYPE *s, TYPE *wvec, TYPE *prod) +{ + /* N is the number of variables. + * NPT is the number of interpolation equations. + * XOPT is the best interpolation point so far. + * XPT contains the coordinates of the current interpolation points. + * BMAT provides the last N columns of H. + * ZMAT and IDZ give a factorization of the first NPT by NPT + submatrix of H. + * NDIM is the first dimension of BMAT and has the value NPT+N. + * KOPT is the index of the optimal interpolation point. + * KNEW is the index of the interpolation point that is going to be + moved. + * D will be set to the step from XOPT to the new point, and on + entry it should be the D that was calculated by the last call of + BIGBDLAG. The length of the initial D provides a trust region bound + on the final D. + * W will be set to Wcheck for the final choice of D. + * VBDLAG will be set to Theta*Wcheck+e_b for the final choice of D. + * BETA will be set to the value that will occur in the updating + formula when the KNEW-th interpolation point is moved to its new + position. + * S, WVEC, PROD and the private arrays DEN, DENEX and PAR will be + used for working space. + * D is calculated in a way that should provide a denominator with a + large modulus in the updating formula when the KNEW-th + interpolation point is shifted to the new position XOPT+D. */ + + int xpt_dim1, xpt_offset, bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, + wvec_dim1, wvec_offset, prod_dim1, prod_offset, i__1, i__2, i__, j, k, + isave, iterc, jc, ip, iu, nw, ksav, nptm; + TYPE dd, d__1, ds, ss, den[9], par[9], tau, sum, diff, temp, step, + alpha, angle, denex[9], tempa, tempb, tempc, ssden, dtest, xoptd, + twopi, xopts, denold, denmax, densav, dstemp, sumold, sstemp, xoptsq; + + /* Parameter adjustments */ + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --xopt; + prod_dim1 = *ndim; + prod_offset = 1 + prod_dim1; + prod -= prod_offset; + wvec_dim1 = *ndim; + wvec_offset = 1 + wvec_dim1; + wvec -= wvec_offset; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --d__; --w; --vlag; --s; + /* Function Body */ + twopi = atan(1.0) * 8.; + nptm = npt - n - 1; + /* Store the first NPT elements of the KNEW-th column of H in W(N+1) + * to W(N+NPT). */ + i__1 = npt; + for (k = 1; k <= i__1; ++k) w[n + k] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) { + temp = zmat[*knew + j * zmat_dim1]; + if (j < *idz) temp = -temp; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + w[n + k] += temp * zmat[k + j * zmat_dim1]; + } + alpha = w[n + *knew]; + /* The initial search direction D is taken from the last call of + * BIGBDLAG, and the initial S is set below, usually to the direction + * from X_OPT to X_KNEW, but a different direction to an + * interpolation point may be chosen, in order to prevent S from + * being nearly parallel to D. */ + dd = ds = ss = xoptsq = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + s[i__] = xpt[*knew + i__ * xpt_dim1] - xopt[i__]; + ds += d__[i__] * s[i__]; + /* Computing 2nd power */ + d__1 = s[i__]; + ss += d__1 * d__1; + /* Computing 2nd power */ + d__1 = xopt[i__]; + xoptsq += d__1 * d__1; + } + if (ds * ds > dd * .99 * ss) { + ksav = *knew; + dtest = ds * ds / ss; + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + if (k != *kopt) { + dstemp = 0; + sstemp = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + diff = xpt[k + i__ * xpt_dim1] - xopt[i__]; + dstemp += d__[i__] * diff; + sstemp += diff * diff; + } + if (dstemp * dstemp / sstemp < dtest) { + ksav = k; + dtest = dstemp * dstemp / sstemp; + ds = dstemp; + ss = sstemp; + } + } + } + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + s[i__] = xpt[ksav + i__ * xpt_dim1] - xopt[i__]; + } + ssden = dd * ss - ds * ds; + iterc = 0; + densav = 0; + /* Begin the iteration by overwriting S with a vector that has the + * required length and direction. */ +BDL70: + ++iterc; + temp = 1.0 / sqrt(ssden); + xoptd = xopts = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + s[i__] = temp * (dd * s[i__] - ds * d__[i__]); + xoptd += xopt[i__] * d__[i__]; + xopts += xopt[i__] * s[i__]; + } + /* Set the coefficients of the first 2.0 terms of BETA. */ + tempa = 0.5 * xoptd * xoptd; + tempb = 0.5 * xopts * xopts; + den[0] = dd * (xoptsq + 0.5 * dd) + tempa + tempb; + den[1] = 2.0 * xoptd * dd; + den[2] = 2.0 * xopts * dd; + den[3] = tempa - tempb; + den[4] = xoptd * xopts; + for (i__ = 6; i__ <= 9; ++i__) den[i__ - 1] = 0; + /* Put the coefficients of Wcheck in WVEC. */ + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + tempa = tempb = tempc = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + tempa += xpt[k + i__ * xpt_dim1] * d__[i__]; + tempb += xpt[k + i__ * xpt_dim1] * s[i__]; + tempc += xpt[k + i__ * xpt_dim1] * xopt[i__]; + } + wvec[k + wvec_dim1] = 0.25 * (tempa * tempa + tempb * tempb); + wvec[k + (wvec_dim1 << 1)] = tempa * tempc; + wvec[k + wvec_dim1 * 3] = tempb * tempc; + wvec[k + (wvec_dim1 << 2)] = 0.25 * (tempa * tempa - tempb * tempb); + wvec[k + wvec_dim1 * 5] = 0.5 * tempa * tempb; + } + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + ip = i__ + npt; + wvec[ip + wvec_dim1] = 0; + wvec[ip + (wvec_dim1 << 1)] = d__[i__]; + wvec[ip + wvec_dim1 * 3] = s[i__]; + wvec[ip + (wvec_dim1 << 2)] = 0; + wvec[ip + wvec_dim1 * 5] = 0; + } + /* Put the coefficents of THETA*Wcheck in PROD. */ + for (jc = 1; jc <= 5; ++jc) { + nw = npt; + if (jc == 2 || jc == 3) nw = *ndim; + i__2 = npt; + for (k = 1; k <= i__2; ++k) prod[k + jc * prod_dim1] = 0; + i__2 = nptm; + for (j = 1; j <= i__2; ++j) { + sum = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) sum += zmat[k + j * zmat_dim1] * wvec[k + jc * wvec_dim1]; + if (j < *idz) sum = -sum; + i__1 = npt; + for (k = 1; k <= i__1; ++k) + prod[k + jc * prod_dim1] += sum * zmat[k + j * zmat_dim1]; + } + if (nw == *ndim) { + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + sum += bmat[k + j * bmat_dim1] * wvec[npt + j + jc * wvec_dim1]; + prod[k + jc * prod_dim1] += sum; + } + } + i__1 = n; + for (j = 1; j <= i__1; ++j) { + sum = 0; + i__2 = nw; + for (i__ = 1; i__ <= i__2; ++i__) + sum += bmat[i__ + j * bmat_dim1] * wvec[i__ + jc * wvec_dim1]; + prod[npt + j + jc * prod_dim1] = sum; + } + } + /* Include in DEN the part of BETA that depends on THETA. */ + i__1 = *ndim; + for (k = 1; k <= i__1; ++k) { + sum = 0; + for (i__ = 1; i__ <= 5; ++i__) { + par[i__ - 1] = 0.5 * prod[k + i__ * prod_dim1] * wvec[k + i__ * wvec_dim1]; + sum += par[i__ - 1]; + } + den[0] = den[0] - par[0] - sum; + tempa = prod[k + prod_dim1] * wvec[k + (wvec_dim1 << 1)] + prod[k + ( + prod_dim1 << 1)] * wvec[k + wvec_dim1]; + tempb = prod[k + (prod_dim1 << 1)] * wvec[k + (wvec_dim1 << 2)] + + prod[k + (prod_dim1 << 2)] * wvec[k + (wvec_dim1 << 1)]; + tempc = prod[k + prod_dim1 * 3] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + wvec_dim1 * 3]; + den[1] = den[1] - tempa - 0.5 * (tempb + tempc); + den[5] -= 0.5 * (tempb - tempc); + tempa = prod[k + prod_dim1] * wvec[k + wvec_dim1 * 3] + prod[k + + prod_dim1 * 3] * wvec[k + wvec_dim1]; + tempb = prod[k + (prod_dim1 << 1)] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + (wvec_dim1 << 1)]; + tempc = prod[k + prod_dim1 * 3] * wvec[k + (wvec_dim1 << 2)] + prod[k + + (prod_dim1 << 2)] * wvec[k + wvec_dim1 * 3]; + den[2] = den[2] - tempa - 0.5 * (tempb - tempc); + den[6] -= 0.5 * (tempb + tempc); + tempa = prod[k + prod_dim1] * wvec[k + (wvec_dim1 << 2)] + prod[k + ( + prod_dim1 << 2)] * wvec[k + wvec_dim1]; + den[3] = den[3] - tempa - par[1] + par[2]; + tempa = prod[k + prod_dim1] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + wvec_dim1]; + tempb = prod[k + (prod_dim1 << 1)] * wvec[k + wvec_dim1 * 3] + prod[k + + prod_dim1 * 3] * wvec[k + (wvec_dim1 << 1)]; + den[4] = den[4] - tempa - 0.5 * tempb; + den[7] = den[7] - par[3] + par[4]; + tempa = prod[k + (prod_dim1 << 2)] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + (wvec_dim1 << 2)]; + den[8] -= 0.5 * tempa; + } + /* Extend DEN so that it holds all the coefficients of DENOM. */ + sum = 0; + for (i__ = 1; i__ <= 5; ++i__) { + /* Computing 2nd power */ + d__1 = prod[*knew + i__ * prod_dim1]; + par[i__ - 1] = 0.5 * (d__1 * d__1); + sum += par[i__ - 1]; + } + denex[0] = alpha * den[0] + par[0] + sum; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + (prod_dim1 << 1)]; + tempb = prod[*knew + (prod_dim1 << 1)] * prod[*knew + (prod_dim1 << 2)]; + tempc = prod[*knew + prod_dim1 * 3] * prod[*knew + prod_dim1 * 5]; + denex[1] = alpha * den[1] + tempa + tempb + tempc; + denex[5] = alpha * den[5] + tempb - tempc; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + prod_dim1 * 3]; + tempb = prod[*knew + (prod_dim1 << 1)] * prod[*knew + prod_dim1 * 5]; + tempc = prod[*knew + prod_dim1 * 3] * prod[*knew + (prod_dim1 << 2)]; + denex[2] = alpha * den[2] + tempa + tempb - tempc; + denex[6] = alpha * den[6] + tempb + tempc; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + (prod_dim1 << 2)]; + denex[3] = alpha * den[3] + tempa + par[1] - par[2]; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + prod_dim1 * 5]; + denex[4] = alpha * den[4] + tempa + prod[*knew + (prod_dim1 << 1)] * prod[ + *knew + prod_dim1 * 3]; + denex[7] = alpha * den[7] + par[3] - par[4]; + denex[8] = alpha * den[8] + prod[*knew + (prod_dim1 << 2)] * prod[*knew + + prod_dim1 * 5]; + /* Seek the value of the angle that maximizes the modulus of DENOM. */ + sum = denex[0] + denex[1] + denex[3] + denex[5] + denex[7]; + denold = denmax = sum; + isave = 0; + iu = 49; + temp = twopi / (TYPE) (iu + 1); + par[0] = 1.0; + i__1 = iu; + for (i__ = 1; i__ <= i__1; ++i__) { + angle = (TYPE) i__ *temp; + par[1] = cos(angle); + par[2] = sin(angle); + for (j = 4; j <= 8; j += 2) { + par[j - 1] = par[1] * par[j - 3] - par[2] * par[j - 2]; + par[j] = par[1] * par[j - 2] + par[2] * par[j - 3]; + } + sumold = sum; + sum = 0; + for (j = 1; j <= 9; ++j) + sum += denex[j - 1] * par[j - 1]; + if (fabs(sum) > fabs(denmax)) { + denmax = sum; + isave = i__; + tempa = sumold; + } else if (i__ == isave + 1) { + tempb = sum; + } + } + if (isave == 0) tempa = sum; + if (isave == iu) tempb = denold; + step = 0; + if (tempa != tempb) { + tempa -= denmax; + tempb -= denmax; + step = 0.5 * (tempa - tempb) / (tempa + tempb); + } + angle = temp * ((TYPE) isave + step); + /* Calculate the new parameters of the denominator, the new VBDLAG + * vector and the new D. Then test for convergence. */ + par[1] = cos(angle); + par[2] = sin(angle); + for (j = 4; j <= 8; j += 2) { + par[j - 1] = par[1] * par[j - 3] - par[2] * par[j - 2]; + par[j] = par[1] * par[j - 2] + par[2] * par[j - 3]; + } + *beta = 0; + denmax = 0; + for (j = 1; j <= 9; ++j) { + *beta += den[j - 1] * par[j - 1]; + denmax += denex[j - 1] * par[j - 1]; + } + i__1 = *ndim; + for (k = 1; k <= i__1; ++k) { + vlag[k] = 0; + for (j = 1; j <= 5; ++j) + vlag[k] += prod[k + j * prod_dim1] * par[j - 1]; + } + tau = vlag[*knew]; + dd = tempa = tempb = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + d__[i__] = par[1] * d__[i__] + par[2] * s[i__]; + w[i__] = xopt[i__] + d__[i__]; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + tempa += d__[i__] * w[i__]; + tempb += w[i__] * w[i__]; + } + if (iterc >= n) goto BDL340; + if (iterc > 1) densav = std::max(densav, denold); + if (fabs(denmax) <= fabs(densav) * 1.1) goto BDL340; + densav = denmax; + /* Set S to 0.5 the gradient of the denominator with respect to + * D. Then branch for the next iteration. */ + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + temp = tempa * xopt[i__] + tempb * d__[i__] - vlag[npt + i__]; + s[i__] = tau * bmat[*knew + i__ * bmat_dim1] + alpha * temp; + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + sum += xpt[k + j * xpt_dim1] * w[j]; + temp = (tau * w[n + k] - alpha * vlag[k]) * sum; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + s[i__] += temp * xpt[k + i__ * xpt_dim1]; + } + ss = 0; + ds = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + /* Computing 2nd power */ + d__1 = s[i__]; + ss += d__1 * d__1; + ds += d__[i__] * s[i__]; + } + ssden = dd * ss - ds * ds; + if (ssden >= dd * 1e-8 * ss) goto BDL70; + /* Set the vector W before the RETURN from the subroutine. */ +BDL340: + i__2 = *ndim; + for (k = 1; k <= i__2; ++k) { + w[k] = 0; + for (j = 1; j <= 5; ++j) w[k] += wvec[k + j * wvec_dim1] * par[j - 1]; + } + vlag[*kopt] += 1.0; + return 0; +} + +template +int trsapp_(int n, int npt, TYPE * xopt, TYPE * xpt, TYPE * gq, TYPE * hq, TYPE * pq, + TYPE * delta, TYPE * step, TYPE * d__, TYPE * g, TYPE * hd, TYPE * hs, TYPE * crvmin) +{ + /* The arguments NPT, XOPT, XPT, GQ, HQ and PQ have their usual + * meanings, in order to define the current quadratic model Q. + * DETRLTA is the trust region radius, and has to be positive. STEP + * will be set to the calculated trial step. The arrays D, G, HD and + * HS will be used for working space. CRVMIN will be set to the + * least curvature of H aint the conjugate directions that occur, + * except that it is set to 0 if STEP goes all the way to the trust + * region boundary. The calculation of STEP begins with the + * truncated conjugate gradient method. If the boundary of the trust + * region is reached, then further changes to STEP may be made, each + * one being in the 2D space spanned by the current STEP and the + * corresponding gradient of Q. Thus STEP should provide a + * substantial reduction to Q within the trust region. */ + + int xpt_dim1, xpt_offset, i__1, i__2, i__, j, k, ih, iu, iterc, + isave, itersw, itermax; + TYPE d__1, d__2, dd, cf, dg, gg, ds, sg, ss, dhd, dhs, + cth, sgk, shs, sth, qadd, qbeg, qred, qmin, temp, + qsav, qnew, ggbeg, alpha, angle, reduc, ggsav, delsq, + tempa, tempb, bstep, ratio, twopi, angtest; + + /* Parameter adjustments */ + tempa = tempb = shs = sg = bstep = ggbeg = gg = qred = dd = 0.0; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --xopt; --gq; --hq; --pq; --step; --d__; --g; --hd; --hs; + /* Function Body */ + twopi = 2.0 * M_PI; + delsq = *delta * *delta; + iterc = 0; + itermax = n; + itersw = itermax; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) d__[i__] = xopt[i__]; + goto TRL170; + /* Prepare for the first line search. */ +TRL20: + qred = dd = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + step[i__] = 0; + hs[i__] = 0; + g[i__] = gq[i__] + hd[i__]; + d__[i__] = -g[i__]; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + } + *crvmin = 0; + if (dd == 0) goto TRL160; + ds = ss = 0; + gg = dd; + ggbeg = gg; + /* Calculate the step to the trust region boundary and the product + * HD. */ +TRL40: + ++iterc; + temp = delsq - ss; + bstep = temp / (ds + sqrt(ds * ds + dd * temp)); + goto TRL170; +TRL50: + dhd = 0; + i__1 = n; + for (j = 1; j <= i__1; ++j) dhd += d__[j] * hd[j]; + /* Update CRVMIN and set the step-length ATRLPHA. */ + alpha = bstep; + if (dhd > 0) { + temp = dhd / dd; + if (iterc == 1) *crvmin = temp; + *crvmin = std::min(*crvmin, temp); + /* Computing MIN */ + d__1 = alpha, d__2 = gg / dhd; + alpha = std::min(d__1, d__2); + } + qadd = alpha * (gg - 0.5 * alpha * dhd); + qred += qadd; + /* Update STEP and HS. */ + ggsav = gg; + gg = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + step[i__] += alpha * d__[i__]; + hs[i__] += alpha * hd[i__]; + /* Computing 2nd power */ + d__1 = g[i__] + hs[i__]; + gg += d__1 * d__1; + } + /* Begin another conjugate direction iteration if required. */ + if (alpha < bstep) { + if (qadd <= qred * .01 || gg <= ggbeg * 1e-4 || iterc == itermax) goto TRL160; + temp = gg / ggsav; + dd = ds = ss = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + d__[i__] = temp * d__[i__] - g[i__] - hs[i__]; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + ds += d__[i__] * step[i__]; + /* Computing 2nd power */ + d__1 = step[i__]; + ss += d__1 * d__1; + } + if (ds <= 0) goto TRL160; + if (ss < delsq) goto TRL40; + } + *crvmin = 0; + itersw = iterc; + /* Test whether an alternative iteration is required. */ +TRL90: + if (gg <= ggbeg * 1e-4) goto TRL160; + sg = 0; + shs = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + sg += step[i__] * g[i__]; + shs += step[i__] * hs[i__]; + } + sgk = sg + shs; + angtest = sgk / sqrt(gg * delsq); + if (angtest <= -.99) goto TRL160; + /* Begin the alternative iteration by calculating D and HD and some + * scalar products. */ + ++iterc; + temp = sqrt(delsq * gg - sgk * sgk); + tempa = delsq / temp; + tempb = sgk / temp; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) + d__[i__] = tempa * (g[i__] + hs[i__]) - tempb * step[i__]; + goto TRL170; +TRL120: + dg = dhd = dhs = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + dg += d__[i__] * g[i__]; + dhd += hd[i__] * d__[i__]; + dhs += hd[i__] * step[i__]; + } + /* Seek the value of the angle that minimizes Q. */ + cf = 0.5 * (shs - dhd); + qbeg = sg + cf; + qsav = qmin = qbeg; + isave = 0; + iu = 49; + temp = twopi / (TYPE) (iu + 1); + i__1 = iu; + for (i__ = 1; i__ <= i__1; ++i__) { + angle = (TYPE) i__ *temp; + cth = cos(angle); + sth = sin(angle); + qnew = (sg + cf * cth) * cth + (dg + dhs * cth) * sth; + if (qnew < qmin) { + qmin = qnew; + isave = i__; + tempa = qsav; + } else if (i__ == isave + 1) tempb = qnew; + qsav = qnew; + } + if ((TYPE) isave == 0) tempa = qnew; + if (isave == iu) tempb = qbeg; + angle = 0; + if (tempa != tempb) { + tempa -= qmin; + tempb -= qmin; + angle = 0.5 * (tempa - tempb) / (tempa + tempb); + } + angle = temp * ((TYPE) isave + angle); + /* Calculate the new STEP and HS. Then test for convergence. */ + cth = cos(angle); + sth = sin(angle); + reduc = qbeg - (sg + cf * cth) * cth - (dg + dhs * cth) * sth; + gg = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + step[i__] = cth * step[i__] + sth * d__[i__]; + hs[i__] = cth * hs[i__] + sth * hd[i__]; + /* Computing 2nd power */ + d__1 = g[i__] + hs[i__]; + gg += d__1 * d__1; + } + qred += reduc; + ratio = reduc / qred; + if (iterc < itermax && ratio > .01) goto TRL90; +TRL160: + return 0; + /* The following instructions act as a subroutine for setting the + * vector HD to the vector D multiplied by the second derivative + * matrix of Q. They are called from three different places, which + * are distinguished by the value of ITERC. */ +TRL170: + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) hd[i__] = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + temp = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + temp += xpt[k + j * xpt_dim1] * d__[j]; + temp *= pq[k]; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + hd[i__] += temp * xpt[k + i__ * xpt_dim1]; + } + ih = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + i__1 = j; + for (i__ = 1; i__ <= i__1; ++i__) { + ++ih; + if (i__ < j) hd[j] += hq[ih] * d__[i__]; + hd[i__] += hq[ih] * d__[j]; + } + } + if (iterc == 0) goto TRL20; + if (iterc <= itersw) goto TRL50; + goto TRL120; +} + +template +static int update_(int n, int npt, TYPE *bmat, TYPE *zmat, int *idz, int *ndim, TYPE *vlag, + TYPE *beta, int *knew, TYPE *w) +{ + /* The arrays BMAT and ZMAT with IDZ are updated, in order to shift + * the interpolation point that has index KNEW. On entry, VLAG + * contains the components of the vector Theta*Wcheck+e_b of the + * updating formula (6.11), and BETA holds the value of the + * parameter that has this name. The vector W is used for working + * space. */ + + int bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, i__1, i__2, i__, + j, ja, jb, jl, jp, nptm, iflag; + TYPE d__1, d__2, tau, temp, scala, scalb, alpha, denom, tempa, tempb, tausq; + + /* Parameter adjustments */ + tempb = 0.0; + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --vlag; + --w; + /* Function Body */ + nptm = npt - n - 1; + /* Apply the rotations that put zeros in the KNEW-th row of ZMAT. */ + jl = 1; + i__1 = nptm; + for (j = 2; j <= i__1; ++j) { + if (j == *idz) { + jl = *idz; + } else if (zmat[*knew + j * zmat_dim1] != 0) { + /* Computing 2nd power */ + d__1 = zmat[*knew + jl * zmat_dim1]; + /* Computing 2nd power */ + d__2 = zmat[*knew + j * zmat_dim1]; + temp = sqrt(d__1 * d__1 + d__2 * d__2); + tempa = zmat[*knew + jl * zmat_dim1] / temp; + tempb = zmat[*knew + j * zmat_dim1] / temp; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) { + temp = tempa * zmat[i__ + jl * zmat_dim1] + tempb * zmat[i__ + + j * zmat_dim1]; + zmat[i__ + j * zmat_dim1] = tempa * zmat[i__ + j * zmat_dim1] + - tempb * zmat[i__ + jl * zmat_dim1]; + zmat[i__ + jl * zmat_dim1] = temp; + } + zmat[*knew + j * zmat_dim1] = 0; + } + } + /* Put the first NPT components of the KNEW-th column of HLAG into + * W, and calculate the parameters of the updating formula. */ + tempa = zmat[*knew + zmat_dim1]; + if (*idz >= 2) tempa = -tempa; + if (jl > 1) tempb = zmat[*knew + jl * zmat_dim1]; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) { + w[i__] = tempa * zmat[i__ + zmat_dim1]; + if (jl > 1) w[i__] += tempb * zmat[i__ + jl * zmat_dim1]; + } + alpha = w[*knew]; + tau = vlag[*knew]; + tausq = tau * tau; + denom = alpha * *beta + tausq; + vlag[*knew] -= 1.0; + /* Complete the updating of ZMAT when there is only 1.0 nonzero + * element in the KNEW-th row of the new matrix ZMAT, but, if IFLAG + * is set to 1.0, then the first column of ZMAT will be exchanged + * with another 1.0 later. */ + iflag = 0; + if (jl == 1) { + temp = sqrt((fabs(denom))); + tempb = tempa / temp; + tempa = tau / temp; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + zmat[i__ + zmat_dim1] = tempa * zmat[i__ + zmat_dim1] - tempb * + vlag[i__]; + if (*idz == 1 && temp < 0) *idz = 2; + if (*idz >= 2 && temp >= 0) iflag = 1; + } else { + /* Complete the updating of ZMAT in the alternative case. */ + ja = 1; + if (*beta >= 0) { + ja = jl; + } + jb = jl + 1 - ja; + temp = zmat[*knew + jb * zmat_dim1] / denom; + tempa = temp * *beta; + tempb = temp * tau; + temp = zmat[*knew + ja * zmat_dim1]; + scala = 1.0 / sqrt(fabs(*beta) * temp * temp + tausq); + scalb = scala * sqrt((fabs(denom))); + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) { + zmat[i__ + ja * zmat_dim1] = scala * (tau * zmat[i__ + ja * + zmat_dim1] - temp * vlag[i__]); + zmat[i__ + jb * zmat_dim1] = scalb * (zmat[i__ + jb * zmat_dim1] + - tempa * w[i__] - tempb * vlag[i__]); + } + if (denom <= 0) { + if (*beta < 0) ++(*idz); + if (*beta >= 0) iflag = 1; + } + } + /* IDZ is reduced in the following case, and usually the first + * column of ZMAT is exchanged with a later 1.0. */ + if (iflag == 1) { + --(*idz); + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) { + temp = zmat[i__ + zmat_dim1]; + zmat[i__ + zmat_dim1] = zmat[i__ + *idz * zmat_dim1]; + zmat[i__ + *idz * zmat_dim1] = temp; + } + } + /* Finally, update the matrix BMAT. */ + i__1 = n; + for (j = 1; j <= i__1; ++j) { + jp = npt + j; + w[jp] = bmat[*knew + j * bmat_dim1]; + tempa = (alpha * vlag[jp] - tau * w[jp]) / denom; + tempb = (-(*beta) * w[jp] - tau * vlag[jp]) / denom; + i__2 = jp; + for (i__ = 1; i__ <= i__2; ++i__) { + bmat[i__ + j * bmat_dim1] = bmat[i__ + j * bmat_dim1] + tempa * + vlag[i__] + tempb * w[i__]; + if (i__ > npt) { + bmat[jp + (i__ - npt) * bmat_dim1] = bmat[i__ + j * + bmat_dim1]; + } + } + } + return 0; +} + +template +static TYPE newuob_(int n, int npt, TYPE *x, + TYPE rhobeg, TYPE rhoend, int *ret_nf, int maxfun, + TYPE *xbase, TYPE *xopt, TYPE *xnew, + TYPE *xpt, TYPE *fval, TYPE *gq, TYPE *hq, + TYPE *pq, TYPE *bmat, TYPE *zmat, int *ndim, + TYPE *d__, TYPE *vlag, TYPE *w, Func &func) +{ + /* XBASE will hold a shift of origin that should reduce the + contributions from rounding errors to values of the model and + Lagrange functions. + * XOPT will be set to the displacement from XBASE of the vector of + variables that provides the least calculated F so far. + * XNEW will be set to the displacement from XBASE of the vector of + variables for the current calculation of F. + * XPT will contain the interpolation point coordinates relative to + XBASE. + * FVAL will hold the values of F at the interpolation points. + * GQ will hold the gradient of the quadratic model at XBASE. + * HQ will hold the explicit second derivatives of the quadratic + model. + * PQ will contain the parameters of the implicit second derivatives + of the quadratic model. + * BMAT will hold the last N columns of H. + * ZMAT will hold the factorization of the leading NPT by NPT + submatrix of H, this factorization being ZMAT times Diag(DZ) + times ZMAT^T, where the elements of DZ are plus or minus 1.0, as + specified by IDZ. + * NDIM is the first dimension of BMAT and has the value NPT+N. + * D is reserved for trial steps from XOPT. + * VLAG will contain the values of the Lagrange functions at a new + point X. They are part of a product that requires VLAG to be of + length NDIM. + * The array W will be used for working space. Its length must be at + least 10*NDIM = 10*(NPT+N). Set some constants. */ + + int xpt_dim1, xpt_offset, bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, + i__1, i__2, i__3, i__, j, k, ih, nf, nh, ip, jp, np, nfm, idz, ipt, jpt, + nfmm, knew, kopt, nptm, ksave, nfsav, itemp, ktemp, itest, nftest; + TYPE d__1, d__2, d__3, f, dx, dsq, rho, sum, fbeg, diff, beta, gisq, + temp, suma, sumb, fopt, bsum, gqsq, xipt, xjpt, sumz, diffa, diffb, + diffc, hdiag, alpha, delta, recip, reciq, fsave, dnorm, ratio, dstep, + vquad, tempq, rhosq, detrat, crvmin, distsq, xoptsq; + + /* Parameter adjustments */ + diffc = ratio = dnorm = nfsav = diffa = diffb = xoptsq = f = 0.0; + rho = fbeg = fopt = xjpt = xipt = 0.0; + itest = ipt = jpt = 0; + alpha = dstep = 0.0; + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --x; --xbase; --xopt; --xnew; --fval; --gq; --hq; --pq; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --d__; + --vlag; + --w; + /* Function Body */ + np = n + 1; + nh = n * np / 2; + nptm = npt - np; + nftest = (maxfun > 1)? maxfun : 1; + /* Set the initial elements of XPT, BMAT, HQ, PQ and ZMAT to 0. */ + i__1 = n; + for (j = 1; j <= i__1; ++j) { + xbase[j] = x[j]; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + xpt[k + j * xpt_dim1] = 0; + i__2 = *ndim; + for (i__ = 1; i__ <= i__2; ++i__) + bmat[i__ + j * bmat_dim1] = 0; + } + i__2 = nh; + for (ih = 1; ih <= i__2; ++ih) hq[ih] = 0; + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + pq[k] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) + zmat[k + j * zmat_dim1] = 0; + } + /* Begin the initialization procedure. NF becomes 1.0 more than the + * number of function values so far. The coordinates of the + * displacement of the next initial interpolation point from XBASE + * are set in XPT(NF,.). */ + rhosq = rhobeg * rhobeg; + recip = 1.0 / rhosq; + reciq = sqrt(.5) / rhosq; + nf = 0; +L50: + nfm = nf; + nfmm = nf - n; + ++nf; + if (nfm <= n << 1) { + if (nfm >= 1 && nfm <= n) { + xpt[nf + nfm * xpt_dim1] = rhobeg; + } else if (nfm > n) { + xpt[nf + nfmm * xpt_dim1] = -(rhobeg); + } + } else { + itemp = (nfmm - 1) / n; + jpt = nfm - itemp * n - n; + ipt = jpt + itemp; + if (ipt > n) { + itemp = jpt; + jpt = ipt - n; + ipt = itemp; + } + xipt = rhobeg; + if (fval[ipt + np] < fval[ipt + 1]) xipt = -xipt; + xjpt = rhobeg; + if (fval[jpt + np] < fval[jpt + 1]) xjpt = -xjpt; + xpt[nf + ipt * xpt_dim1] = xipt; + xpt[nf + jpt * xpt_dim1] = xjpt; + } + /* Calculate the next value of F, label 70 being reached immediately + * after this calculation. The least function value so far and its + * index are required. */ + i__1 = n; + for (j = 1; j <= i__1; ++j) + x[j] = xpt[nf + j * xpt_dim1] + xbase[j]; + goto L310; +L70: + fval[nf] = f; + if (nf == 1) { + fbeg = fopt = f; + kopt = 1; + } else if (f < fopt) { + fopt = f; + kopt = nf; + } + /* Set the non0 initial elements of BMAT and the quadratic model + * in the cases when NF is at most 2*N+1. */ + if (nfm <= n << 1) { + if (nfm >= 1 && nfm <= n) { + gq[nfm] = (f - fbeg) / rhobeg; + if (npt < nf + n) { + bmat[nfm * bmat_dim1 + 1] = -1.0 / rhobeg; + bmat[nf + nfm * bmat_dim1] = 1.0 / rhobeg; + bmat[npt + nfm + nfm * bmat_dim1] = -.5 * rhosq; + } + } else if (nfm > n) { + bmat[nf - n + nfmm * bmat_dim1] = .5 / rhobeg; + bmat[nf + nfmm * bmat_dim1] = -.5 / rhobeg; + zmat[nfmm * zmat_dim1 + 1] = -reciq - reciq; + zmat[nf - n + nfmm * zmat_dim1] = reciq; + zmat[nf + nfmm * zmat_dim1] = reciq; + ih = nfmm * (nfmm + 1) / 2; + temp = (fbeg - f) / rhobeg; + hq[ih] = (gq[nfmm] - temp) / rhobeg; + gq[nfmm] = .5 * (gq[nfmm] + temp); + } + /* Set the off-diagonal second derivatives of the Lagrange + * functions and the initial quadratic model. */ + } else { + ih = ipt * (ipt - 1) / 2 + jpt; + if (xipt < 0) ipt += n; + if (xjpt < 0) jpt += n; + zmat[nfmm * zmat_dim1 + 1] = recip; + zmat[nf + nfmm * zmat_dim1] = recip; + zmat[ipt + 1 + nfmm * zmat_dim1] = -recip; + zmat[jpt + 1 + nfmm * zmat_dim1] = -recip; + hq[ih] = (fbeg - fval[ipt + 1] - fval[jpt + 1] + f) / (xipt * xjpt); + } + if (nf < npt) goto L50; + /* Begin the iterative procedure, because the initial model is + * complete. */ + rho = rhobeg; + delta = rho; + idz = 1; + diffa = diffb = itest = xoptsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + xopt[i__] = xpt[kopt + i__ * xpt_dim1]; + /* Computing 2nd power */ + d__1 = xopt[i__]; + xoptsq += d__1 * d__1; + } +L90: + nfsav = nf; + /* Generate the next trust region step and test its length. Set KNEW + * to -1 if the purpose of the next F will be to improve the + * model. */ +L100: + knew = 0; + trsapp_(n, npt, &xopt[1], &xpt[xpt_offset], &gq[1], &hq[1], &pq[1], & + delta, &d__[1], &w[1], &w[np], &w[np + n], &w[np + (n << 1)], & + crvmin); + dsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + /* Computing 2nd power */ + d__1 = d__[i__]; + dsq += d__1 * d__1; + } + /* Computing MIN */ + d__1 = delta, d__2 = sqrt(dsq); + dnorm = std::min(d__1, d__2); + if (dnorm < .5 * rho) { + knew = -1; + delta = DELTA_DECREASE * delta; + ratio = -1.; + if (delta <= rho * 1.5) delta = rho; + if (nf <= nfsav + 2) goto L460; + temp = crvmin * .125 * rho * rho; + /* Computing MAX */ + d__1 = std::max(diffa, diffb); + if (temp <= std::max(d__1, diffc)) goto L460; + goto L490; + } + /* Shift XBASE if XOPT may be too far from XBASE. First make the + * changes to BMAT that do not depend on ZMAT. */ +L120: + if (dsq <= xoptsq * .001) { + tempq = xoptsq * .25; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + sum += xpt[k + i__ * xpt_dim1] * xopt[i__]; + temp = pq[k] * sum; + sum -= .5 * xoptsq; + w[npt + k] = sum; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + gq[i__] += temp * xpt[k + i__ * xpt_dim1]; + xpt[k + i__ * xpt_dim1] -= .5 * xopt[i__]; + vlag[i__] = bmat[k + i__ * bmat_dim1]; + w[i__] = sum * xpt[k + i__ * xpt_dim1] + tempq * xopt[i__]; + ip = npt + i__; + i__3 = i__; + for (j = 1; j <= i__3; ++j) + bmat[ip + j * bmat_dim1] = bmat[ip + j * bmat_dim1] + + vlag[i__] * w[j] + w[i__] * vlag[j]; + } + } + /* Then the revisions of BMAT that depend on ZMAT are + * calculated. */ + i__3 = nptm; + for (k = 1; k <= i__3; ++k) { + sumz = 0; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) { + sumz += zmat[i__ + k * zmat_dim1]; + w[i__] = w[npt + i__] * zmat[i__ + k * zmat_dim1]; + } + i__2 = n; + for (j = 1; j <= i__2; ++j) { + sum = tempq * sumz * xopt[j]; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + sum += w[i__] * xpt[i__ + j * xpt_dim1]; + vlag[j] = sum; + if (k < idz) sum = -sum; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + bmat[i__ + j * bmat_dim1] += sum * zmat[i__ + k * zmat_dim1]; + } + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + ip = i__ + npt; + temp = vlag[i__]; + if (k < idz) temp = -temp; + i__2 = i__; + for (j = 1; j <= i__2; ++j) + bmat[ip + j * bmat_dim1] += temp * vlag[j]; + } + } + /* The following instructions complete the shift of XBASE, + * including the changes to the parameters of the quadratic + * model. */ + ih = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + w[j] = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + w[j] += pq[k] * xpt[k + j * xpt_dim1]; + xpt[k + j * xpt_dim1] -= .5 * xopt[j]; + } + i__1 = j; + for (i__ = 1; i__ <= i__1; ++i__) { + ++ih; + if (i__ < j) gq[j] += hq[ih] * xopt[i__]; + gq[i__] += hq[ih] * xopt[j]; + hq[ih] = hq[ih] + w[i__] * xopt[j] + xopt[i__] * w[j]; + bmat[npt + i__ + j * bmat_dim1] = bmat[npt + j + i__ * + bmat_dim1]; + } + } + i__1 = n; + for (j = 1; j <= i__1; ++j) { + xbase[j] += xopt[j]; + xopt[j] = 0; + } + xoptsq = 0; + } + /* Pick the model step if KNEW is positive. A different choice of D + * may be made later, if the choice of D by BIGLAG causes + * substantial cancellation in DENOM. */ + if (knew > 0) { + biglag_(n, npt, &xopt[1], &xpt[xpt_offset], &bmat[bmat_offset], &zmat[zmat_offset], &idz, + ndim, &knew, &dstep, &d__[1], &alpha, &vlag[1], &vlag[npt + 1], &w[1], &w[np], &w[np + n], func); + } + /* Calculate VLAG and BETA for the current choice of D. The first + * NPT components of W_check will be held in W. */ + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + suma = 0; + sumb = 0; + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + suma += xpt[k + j * xpt_dim1] * d__[j]; + sumb += xpt[k + j * xpt_dim1] * xopt[j]; + sum += bmat[k + j * bmat_dim1] * d__[j]; + } + w[k] = suma * (.5 * suma + sumb); + vlag[k] = sum; + } + beta = 0; + i__1 = nptm; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) + sum += zmat[i__ + k * zmat_dim1] * w[i__]; + if (k < idz) { + beta += sum * sum; + sum = -sum; + } else beta -= sum * sum; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) + vlag[i__] += sum * zmat[i__ + k * zmat_dim1]; + } + bsum = 0; + dx = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + sum = 0; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + sum += w[i__] * bmat[i__ + j * bmat_dim1]; + bsum += sum * d__[j]; + jp = npt + j; + i__1 = n; + for (k = 1; k <= i__1; ++k) + sum += bmat[jp + k * bmat_dim1] * d__[k]; + vlag[jp] = sum; + bsum += sum * d__[j]; + dx += d__[j] * xopt[j]; + } + beta = dx * dx + dsq * (xoptsq + dx + dx + .5 * dsq) + beta - bsum; + vlag[kopt] += 1.0; + /* If KNEW is positive and if the cancellation in DENOM is + * unacceptable, then BIGDEN calculates an alternative model step, + * XNEW being used for working space. */ + if (knew > 0) { + /* Computing 2nd power */ + d__1 = vlag[knew]; + temp = 1.0 + alpha * beta / (d__1 * d__1); + if (fabs(temp) <= .8) { + bigden_(n, npt, &xopt[1], &xpt[xpt_offset], &bmat[bmat_offset], & + zmat[zmat_offset], &idz, ndim, &kopt, &knew, &d__[1], &w[ + 1], &vlag[1], &beta, &xnew[1], &w[*ndim + 1], &w[*ndim * + 6 + 1]); + } + } + /* Calculate the next value of the objective function. */ +L290: + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + xnew[i__] = xopt[i__] + d__[i__]; + x[i__] = xbase[i__] + xnew[i__]; + } + ++nf; +L310: + if (nf > nftest) { + --nf; + //fprintf(stderr, "++ Return from NEWUOA because CALFUN has been called MAXFUN times.\n"); + goto L530; + } + f = func(n, &x[1]); + if (nf <= npt) goto L70; + if (knew == -1) goto L530; + /* Use the quadratic model to predict the change in F due to the + * step D, and set DIFF to the error of this prediction. */ + vquad = ih = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + vquad += d__[j] * gq[j]; + i__1 = j; + for (i__ = 1; i__ <= i__1; ++i__) { + ++ih; + temp = d__[i__] * xnew[j] + d__[j] * xopt[i__]; + if (i__ == j) temp = .5 * temp; + vquad += temp * hq[ih]; + } + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) vquad += pq[k] * w[k]; + diff = f - fopt - vquad; + diffc = diffb; + diffb = diffa; + diffa = fabs(diff); + if (dnorm > rho) nfsav = nf; + /* Update FOPT and XOPT if the new F is the least value of the + * objective function so far. The branch when KNEW is positive + * occurs if D is not a trust region step. */ + fsave = fopt; + if (f < fopt) { + fopt = f; + xoptsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + xopt[i__] = xnew[i__]; + /* Computing 2nd power */ + d__1 = xopt[i__]; + xoptsq += d__1 * d__1; + } + } + ksave = knew; + if (knew > 0) goto L410; + /* Pick the next value of DELTA after a trust region step. */ + if (vquad >= 0) { + fprintf(stderr, "++ Return from NEWUOA because a trust region step has failed to reduce Q.\n"); + goto L530; + } + ratio = (f - fsave) / vquad; + if (ratio <= 0.1) { + delta = .5 * dnorm; + } else if (ratio <= .7) { + /* Computing MAX */ + d__1 = .5 * delta; + delta = std::max(d__1, dnorm); + } else { + /* Computing MAX */ + d__1 = .5 * delta, d__2 = dnorm + dnorm; + delta = std::max(d__1, d__2); + } + if (delta <= rho * 1.5) delta = rho; + /* Set KNEW to the index of the next interpolation point to be + * deleted. */ + /* Computing MAX */ + d__2 = 0.1 * delta; + /* Computing 2nd power */ + d__1 = std::max(d__2, rho); + rhosq = d__1 * d__1; + ktemp = detrat = 0; + if (f >= fsave) { + ktemp = kopt; + detrat = 1.0; + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + hdiag = 0; + i__2 = nptm; + for (j = 1; j <= i__2; ++j) { + temp = 1.0; + if (j < idz) temp = -1.0; + /* Computing 2nd power */ + d__1 = zmat[k + j * zmat_dim1]; + hdiag += temp * (d__1 * d__1); + } + /* Computing 2nd power */ + d__2 = vlag[k]; + temp = (d__1 = beta * hdiag + d__2 * d__2, fabs(d__1)); + distsq = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + /* Computing 2nd power */ + d__1 = xpt[k + j * xpt_dim1] - xopt[j]; + distsq += d__1 * d__1; + } + if (distsq > rhosq) { + /* Computing 3rd power */ + d__1 = distsq / rhosq; + temp *= d__1 * (d__1 * d__1); + } + if (temp > detrat && k != ktemp) { + detrat = temp; + knew = k; + } + } + if (knew == 0) goto L460; + /* Update BMAT, ZMAT and IDZ, so that the KNEW-th interpolation + * point can be moved. Begin the updating of the quadratic model, + * starting with the explicit second derivative term. */ +L410: + update_(n, npt, &bmat[bmat_offset], &zmat[zmat_offset], &idz, ndim, &vlag[1], &beta, &knew, &w[1]); + fval[knew] = f; + ih = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + temp = pq[knew] * xpt[knew + i__ * xpt_dim1]; + i__2 = i__; + for (j = 1; j <= i__2; ++j) { + ++ih; + hq[ih] += temp * xpt[knew + j * xpt_dim1]; + } + } + pq[knew] = 0; + /* Update the other second derivative parameters, and then the + * gradient vector of the model. Also include the new interpolation + * point. */ + i__2 = nptm; + for (j = 1; j <= i__2; ++j) { + temp = diff * zmat[knew + j * zmat_dim1]; + if (j < idz) temp = -temp; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + pq[k] += temp * zmat[k + j * zmat_dim1]; + } + } + gqsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + gq[i__] += diff * bmat[knew + i__ * bmat_dim1]; + /* Computing 2nd power */ + d__1 = gq[i__]; + gqsq += d__1 * d__1; + xpt[knew + i__ * xpt_dim1] = xnew[i__]; + } + /* If a trust region step makes a small change to the objective + * function, then calculate the gradient of the least Frobenius norm + * interpolant at XBASE, and store it in W, using VLAG for a vector + * of right hand sides. */ + if (ksave == 0 && delta == rho) { + if (fabs(ratio) > .01) { + itest = 0; + } else { + i__1 = npt; + for (k = 1; k <= i__1; ++k) + vlag[k] = fval[k] - fval[kopt]; + gisq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + sum = 0; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + sum += bmat[k + i__ * bmat_dim1] * vlag[k]; + gisq += sum * sum; + w[i__] = sum; + } + /* Test whether to replace the new quadratic model by the + * least Frobenius norm interpolant, making the replacement + * if the test is satisfied. */ + ++itest; + if (gqsq < gisq * 100.) itest = 0; + if (itest >= 3) { + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) gq[i__] = w[i__]; + i__1 = nh; + for (ih = 1; ih <= i__1; ++ih) hq[ih] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) { + w[j] = 0; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + w[j] += vlag[k] * zmat[k + j * zmat_dim1]; + if (j < idz) w[j] = -w[j]; + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + pq[k] = 0; + i__2 = nptm; + for (j = 1; j <= i__2; ++j) + pq[k] += zmat[k + j * zmat_dim1] * w[j]; + } + itest = 0; + } + } + } + if (f < fsave) kopt = knew; + /* If a trust region step has provided a sufficient decrease in F, + * then branch for another trust region calculation. The case + * KSAVE>0 occurs when the new function value was calculated by a + * model step. */ + if (f <= fsave + 0.1 * vquad) goto L100; + if (ksave > 0) goto L100; + /* Alternatively, find out if the interpolation points are close + * enough to the best point so far. */ + knew = 0; +L460: + distsq = delta * 4. * delta; + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + sum = 0; + i__1 = n; + for (j = 1; j <= i__1; ++j) { + /* Computing 2nd power */ + d__1 = xpt[k + j * xpt_dim1] - xopt[j]; + sum += d__1 * d__1; + } + if (sum > distsq) { + knew = k; + distsq = sum; + } + } + /* If KNEW is positive, then set DSTEP, and branch back for the next + * iteration, which will generate a "model step". */ + if (knew > 0) { + /* Computing MAX and MIN*/ + d__2 = 0.1 * sqrt(distsq), d__3 = .5 * delta; + d__1 = std::min(d__2, d__3); + dstep = std::max(d__1, rho); + dsq = dstep * dstep; + goto L120; + } + if (ratio > 0) goto L100; + if (std::max(delta, dnorm) > rho) goto L100; + /* The calculations with the current value of RHO are complete. Pick + * the next values of RHO and DELTA. */ +L490: + if (rho > rhoend) { + delta = .5 * rho; + ratio = rho / rhoend; + if (ratio <= 16.) rho = rhoend; + else if (ratio <= 250.) rho = sqrt(ratio) * rhoend; + else rho = RHO_DECREASE * rho; + delta = std::max(delta, rho); + goto L90; + } + /* Return from the calculation, after another Newton-Raphson step, + * if it is too short to have been tried before. */ + if (knew == -1) goto L290; +L530: + if (fopt <= f) { + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + x[i__] = xbase[i__] + xopt[i__]; + f = fopt; + } + *ret_nf = nf; + return f; +} + +template +static TYPE newuoa_(int n, int npt, TYPE *x, TYPE rhobeg, TYPE rhoend, int *ret_nf, int maxfun, TYPE *w, Func &func) +{ + /* This subroutine seeks the least value of a function of many + * variables, by a trust region method that forms quadratic models + * by interpolation. There can be some freedom in the interpolation + * conditions, which is taken up by minimizing the Frobenius norm of + * the change to the second derivative of the quadratic model, + * beginning with a zero matrix. The arguments of the subroutine are + * as follows. */ + + /* N must be set to the number of variables and must be at least + * two. NPT is the number of interpolation conditions. Its value + * must be in the interval [N+2,(N+1)(N+2)/2]. Initial values of the + * variables must be set in X(1),X(2),...,X(N). They will be changed + * to the values that give the least calculated F. RHOBEG and RHOEND + * must be set to the initial and final values of a trust region + * radius, so both must be positive with RHOEND<=RHOBEG. Typically + * RHOBEG should be about one tenth of the greatest expected change + * to a variable, and RHOEND should indicate the accuracy that is + * required in the final values of the variables. MAXFUN must be set + * to an upper bound on the number of calls of CALFUN. The array W + * will be used for working space. Its length must be at least + * (NPT+13)*(NPT+N)+3*N*(N+3)/2. */ + + /* SUBROUTINE CALFUN (N,X,F) must be provided by the user. It must + * set F to the value of the objective function for the variables + * X(1),X(2),...,X(N). Partition the working space array, so that + * different parts of it can be treated separately by the subroutine + * that performs the main calculation. */ + + int id, np, iw, igq, ihq, ixb, ifv, ipq, ivl, ixn, + ixo, ixp, ndim, nptm, ibmat, izmat; + + /* Parameter adjustments */ + --w; --x; + /* Function Body */ + np = n + 1; + nptm = npt - np; + if (npt < n + 2 || npt > (n + 2) * np / 2) { + fprintf(stderr, "** Return from NEWUOA because NPT is not in the required interval.\n"); + return 1; + } + ndim = npt + n; + ixb = 1; + ixo = ixb + n; + ixn = ixo + n; + ixp = ixn + n; + ifv = ixp + n * npt; + igq = ifv + npt; + ihq = igq + n; + ipq = ihq + n * np / 2; + ibmat = ipq + npt; + izmat = ibmat + ndim * n; + id = izmat + npt * nptm; + ivl = id + n; + iw = ivl + ndim; + /* The above settings provide a partition of W for subroutine + * NEWUOB. The partition requires the first NPT*(NPT+N)+5*N*(N+3)/2 + * elements of W plus the space that is needed by the last array of + * NEWUOB. */ + return newuob_(n, npt, &x[1], rhobeg, rhoend, ret_nf, maxfun, &w[ixb], &w[ixo], &w[ixn], + &w[ixp], &w[ifv], &w[igq], &w[ihq], &w[ipq], &w[ibmat], &w[izmat], + &ndim, &w[id], &w[ivl], &w[iw], func); +} + +template +TYPE min_newuoa(int n, TYPE *x, Func &func, TYPE rb, TYPE tol, int max_iter) +{ + int npt = 2 * n + 1, rnf; + TYPE ret; + TYPE *w = (TYPE*)calloc((npt+13)*(npt+n) + 3*n*(n+3)/2 + 11, sizeof(TYPE)); + ret = newuoa_(n, 2*n+1, x, rb, tol, &rnf, max_iter, w, func); + free(w); + return ret; +} + +#endif