13int slsqp_(
int *
m,
int *meq,
int *la,
int *
n,
double *
x,
double *xl,
double *xu,
double *f,
double *c__,
double *g,
14 double *a,
double *acc,
int *iter,
int *mode,
double *w,
const int *l_w__,
int *jw,
const int *l_jw__);
29 int la = std::max(1,
m);
32 std::vector<double>
x = x0;
33 std::vector<double> xl(
n, -1e12), xu(
n, 1e12);
37 std::vector<double> constraintValues(
m, 0.0);
40 std::vector<double> gradient(
m_nparams + 1, 0.0);
48 int mineq =
m - meq + n1 + n1;
49 int len_w = (3 * n1 +
m) * (n1 + 1) + (n1 - meq + 1) * (mineq + 2) + 2 * mineq + (n1 + mineq) * (n1 - meq) + 2 * meq +
50 n1 + (
n + 1) *
n / 2 + 2 *
m + 3 *
n + 3 * n1 + 1;
52 std::vector<double> vec_w(len_w, 0.0);
53 double *w = vec_w.data();
54 std::vector<int> vec_jw(len_jw, 0);
55 int *jw = vec_jw.data();
58 if (mode == 0 || mode == 1)
64 if (mode == 0 || mode == -1)
70 slsqp_(&
m, &meq, &la, &
n,
x.data(), xl.data(), xu.data(), &fx, constraintValues.data(), gradient.data(),
74 if (mode != 1 && mode != -1)
88 const double epsilon(1e-08);
91 std::vector<double>
tmp =
x;
94 const double curx = xi;
110 const size_t numConstrs(constrValues.size());
111 for (
size_t i = 0; i < numConstrs; ++i) {
116 constrValues[i] =
value;
131 if (totalNumConstr == 0)
135 for (
size_t i = 0; i < 2; ++i) {
143 matrix =
"inequality";
147 std::ostringstream os;
148 os <<
"SLSQPMinimizer::initializeConstraints - Invalid " << matrix
149 <<
" constraint matrix. Number of columns must match number of "
152 throw std::invalid_argument(os.str());
164 const size_t constrVecSize = totalNumConstr *
numParameters() + totalNumConstr;
167 size_t constrCounter(0);
168 while (constrCounter < totalNumConstr) {
171 constrMatrix = &equality;
173 constrMatrix = &inequality;
175 for (
size_t i = 0; i < constrMatrix->
numRows(); ++i, ++constrCounter) {
176 const auto matrixRow = (*constrMatrix)[i];
177 for (
size_t j = 0; j < constrMatrix->
numCols(); ++j) {
182 assert(totalNumConstr == constrCounter);
211double d_sign(
const double *a,
const double *b) {
213 x = (*a >= 0 ? *a : -*a);
214 return (*b >= 0 ?
x : -
x);
218int slsqpb_(
int * ,
int * ,
int * ,
int * ,
double * ,
double * ,
double * ,
219 const double * ,
double * ,
double * ,
double * ,
double * ,
int * ,
220 int * ,
double * ,
double * ,
double * ,
double * ,
double * ,
221 double * ,
double * ,
double * ,
int * );
222int dcopy___(
const int *
n,
double *dx,
const int *incx,
double *dy,
const int *incy);
223int daxpy_sl__(
const int *
n,
const double *da,
double *dx,
const int *incx,
double *dy,
const int *incy);
224int lsq_(
int *
m,
int *meq,
int *
n,
const int *nl,
int *la,
double *l,
double *g,
double *a,
double *b,
double *xl,
225 double *xu,
double *
x,
double *
y,
double *w,
int *jw,
int *mode);
226double ddot_sl__(
const int *
n,
double *dx,
const int *incx,
double *dy,
const int *incy);
227int dscal_sl__(
const int *
n,
const double *da,
double *dx,
const int *incx);
228double linmin_(
int *mode,
const double *ax,
const double *bx,
const double *f,
const double *tol);
229double dnrm2___(
const int *
n,
double *dx,
const int *incx);
230int ldl_(
const int *
n,
double *a,
double *z__,
const double *
sigma,
double *w);
231int lsei_(
double *c__,
double *d__,
double *e,
double *f,
double *g,
double *h__,
int *lc,
int *mc,
int *le,
int *me,
232 int *lg,
int *mg,
int *
n,
double *
x,
double *xnrm,
double *w,
int *jw,
int *mode);
233int h12_(
const int *mode,
const int *lpivot,
const int *l1,
const int *
m,
double *u,
const int *iue,
double *up,
234 double *c__,
const int *ice,
const int *icv,
const int *ncv);
235int hfti_(
double *a,
int *mda,
int *
m,
int *
n,
double *b,
int *mdb,
int *nb,
const double *tau,
int *krank,
236 double *rnorm,
double *h__,
double *g,
int *ip);
237int lsi_(
double *e,
double *f,
double *g,
double *h__,
int *le,
int *me,
int *lg,
int *mg,
int *
n,
double *
x,
238 double *xnorm,
double *w,
int *jw,
int *mode);
239int ldp_(
double *g,
const int *mg,
int *
m,
int *
n,
double *h__,
double *
x,
double *xnorm,
double *w,
int *
index,
241int nnls_(
double *a,
int *mda,
int *
m,
int *
n,
double *b,
double *
x,
double *rnorm,
double *w,
double *z__,
int *
index,
243int dsrotg_(
double *da,
double *db,
double *c__,
double *s);
244int dsrot_(
const int *
n,
double *dx,
const int *incx,
double *dy,
const int *incy,
const double *c__,
const double *s);
280int slsqp_(
int *
m,
int *meq,
int *la,
int *
n,
double *
x,
double *xl,
double *xu,
double *f,
double *c__,
double *g,
281 double *a,
double *acc,
int *iter,
int *mode,
double *w,
const int *l_w__,
int *jw,
const int *l_jw__) {
283 int a_dim1, a_offset, i__1, i__2;
286 static int n1, il, im, ir, is, iu, iv, iw, ix, mineq;
469 a_offset = 1 + a_dim1;
480 mineq = *
m - *meq + n1 + n1;
481 il = (n1 * 3 + *
m) * (n1 + 1) + (n1 - *meq + 1) * (mineq + 2) + (mineq << 1) + (n1 + mineq) * (n1 - *meq) +
482 (*meq << 1) + n1 * *
n / 2 + (*
m << 1) + *
n * 3 + (n1 << 2) + 1;
484 i__1 = mineq, i__2 = n1 - *meq;
485 im = std::max(i__1, i__2);
486 if (*l_w__ < il || *l_jw__ < im) {
487 *mode = std::max(10, il) * 1000;
488 *mode += std::max(10, im);
493 il = im + std::max(1, *
m);
495 ix = il + n1 * *
n / 2 + 1;
497 is = ir + *
n + *
n + std::max(1, *
m);
498 is = ir + *
n + *
n + *la;
503 slsqpb_(
m, meq, la,
n, &
x[1], &xl[1], &xu[1], f, &c__[1], &g[1], &a[a_offset], acc, iter, mode, &w[ir], &w[il],
504 &w[ix], &w[im], &w[is], &w[iu], &w[iv], &w[iw], &jw[1]);
508int slsqpb_(
int *
m,
int *meq,
int *la,
int *
n,
double *
x,
double *xl,
double *xu,
const double *f,
double *c__,
509 double *g,
double *a,
double *acc,
int *iter,
int *mode,
double *r__,
double *l,
double *x0,
double *
mu,
510 double *s,
double *u,
double *v,
double *w,
int *iw) {
513 static double zero = 0.;
514 static double one = 1.;
515 static double alfmin = .1;
516 static double hun = 100.;
517 static double ten = 10.;
518 static double two = 2.;
521 int a_dim1, a_offset, i__1, i__2;
525 static int i__, j, k;
526 static double t, f0, h1, h2, h3, h4;
527 static int n1, n2, n3;
528 static double t0, gs;
533 static int incons, ireset, itermx;
552 a_offset = 1 + a_dim1;
564 }
else if (*mode == 0) {
576 *acc = std::abs(*acc);
585 dcopy___(
n, &s[1], &c__0, &s[1], &c__1);
586 dcopy___(
m, &
mu[1], &c__0, &
mu[1], &c__1);
594 dcopy___(&n2, &l[1], &c__0, &l[1], &c__1);
597 for (i__ = 1; i__ <= i__1; ++i__) {
606 if (*iter > itermx) {
610 dcopy___(
n, &xl[1], &c__1, &u[1], &c__1);
611 dcopy___(
n, &xu[1], &c__1, &v[1], &c__1);
613 daxpy_sl__(
n, &d__1, &
x[1], &c__1, &u[1], &c__1);
615 daxpy_sl__(
n, &d__1, &
x[1], &c__1, &v[1], &c__1);
617 lsq_(
m, meq,
n, &n3, la, &l[1], &g[1], &a[a_offset], &c__[1], &u[1], &v[1], &s[1], &r__[1], &w[1], &iw[1], mode);
626 for (j = 1; j <= i__1; ++j) {
628 a[j + n1 * a_dim1] = -c__[j];
632 a[j + n1 * a_dim1] = std::max(d__1, zero);
637 dcopy___(
n, &s[1], &c__0, &s[1], &c__1);
646 lsq_(
m, meq, &n1, &n3, la, &l[1], &g[1], &a[a_offset], &c__[1], &u[1], &v[1], &s[1], &r__[1], &w[1], &iw[1], mode);
655 }
else if (*mode != 1) {
658 }
else if (*mode != 1) {
663 for (i__ = 1; i__ <= i__1; ++i__) {
664 v[i__] = g[i__] - ddot_sl__(
m, &a[i__ * a_dim1 + 1], &c__1, &r__[1], &c__1);
668 dcopy___(
n, &
x[1], &c__1, &x0[1], &c__1);
669 gs = ddot_sl__(
n, &g[1], &c__1, &s[1], &c__1);
673 for (j = 1; j <= i__1; ++j) {
681 h2 += std::max(d__1, h3);
682 h3 = (d__1 = r__[j], std::abs(d__1));
684 d__1 = h3, d__2 = (
mu[j] + h3) / two;
685 mu[j] = std::max(d__1, d__2);
686 h1 += h3 * (d__1 = c__[j], std::abs(d__1));
691 if (h1 < *acc && h2 < *acc) {
696 for (j = 1; j <= i__1; ++j) {
704 h1 +=
mu[j] * std::max(d__1, h3);
723 dscal_sl__(
n, &alpha, &s[1], &c__1);
724 dcopy___(
n, &x0[1], &c__1, &
x[1], &c__1);
725 daxpy_sl__(
n, &one, &s[1], &c__1, &
x[1], &c__1);
729 if (h1 <= h3 / ten || line > 10) {
733 d__1 = h3 / (two * (h3 - h1));
734 alpha = std::max(d__1, alfmin);
739 alpha = linmin_(&line, &alfmin, &one, &t, &tol);
740 dcopy___(
n, &x0[1], &c__1, &
x[1], &c__1);
741 daxpy_sl__(
n, &alpha, &s[1], &c__1, &
x[1], &c__1);
745 dscal_sl__(
n, &alpha, &s[1], &c__1);
751 for (j = 1; j <= i__1; ++j) {
759 t +=
mu[j] * std::max(d__1, h1);
763 switch (iexact + 1) {
773 for (j = 1; j <= i__1; ++j) {
781 h3 += std::max(d__1, h1);
784 if (((d__1 = *f - f0, std::abs(d__1)) < *acc || dnrm2___(
n, &s[1], &c__1) < *acc) && h3 < *acc) {
792 if (((d__1 = *f - f0, std::abs(d__1)) < tol || dnrm2___(
n, &s[1], &c__1) < tol) && h3 < tol) {
802 for (i__ = 1; i__ <= i__1; ++i__) {
803 u[i__] = g[i__] - ddot_sl__(
m, &a[i__ * a_dim1 + 1], &c__1, &r__[1], &c__1) - v[i__];
809 for (i__ = 1; i__ <= i__1; ++i__) {
813 for (j = i__ + 1; j <= i__2; ++j) {
818 v[i__] = s[i__] + h1;
824 for (i__ = 1; i__ <= i__1; ++i__) {
825 v[i__] = l[k] * v[i__];
830 for (i__ = *
n; i__ >= 1; --i__) {
834 for (j = 1; j <= i__1; ++j) {
842 h1 = ddot_sl__(
n, &s[1], &c__1, &u[1], &c__1);
843 h2 = ddot_sl__(
n, &s[1], &c__1, &v[1], &c__1);
846 h4 = (h2 - h3) / (h2 - h1);
848 dscal_sl__(
n, &h4, &u[1], &c__1);
850 daxpy_sl__(
n, &d__1, &v[1], &c__1, &u[1], &c__1);
853 ldl_(
n, &l[1], &u[1], &d__1, &v[1]);
855 ldl_(
n, &l[1], &v[1], &d__1, &u[1]);
863int lsq_(
int *
m,
int *meq,
int *
n,
const int *nl,
int *la,
double *l,
double *g,
double *a,
double *b,
double *xl,
864 double *xu,
double *
x,
double *
y,
double *w,
int *jw,
int *mode) {
867 static double zero = 0.;
868 static double one = 1.;
871 int a_dim1, a_offset, i__1, i__2;
875 static int i__, i1, i2, i3, i4, m1, n1, n2, n3, ic, id, ie, if__, ig, ih, il, im, ip, iu, iw;
923 a_offset = 1 + a_dim1;
931 m1 = mineq + *
n + *
n;
935 n2 = n1 * *
n / 2 + 1;
949 for (i__ = 1; i__ <= i__1; ++i__) {
953 dcopy___(&i1, &w[i3], &c__0, &w[i3], &c__1);
955 dcopy___(&i__2, &l[i2], &c__1, &w[i3],
n);
957 dscal_sl__(&i__2, &diag, &w[i3],
n);
960 w[if__ - 1 + i__] = (g[i__] - ddot_sl__(&i__2, &w[i4], &c__1, &w[if__], &c__1)) / diag;
969 dcopy___(&n3, &w[i4], &c__0, &w[i4], &c__1);
970 w[if__ - 1 + *
n] = zero;
973 dscal_sl__(
n, &d__1, &w[if__], &c__1);
979 for (i__ = 1; i__ <= i__1; ++i__) {
980 dcopy___(
n, &a[i__ + a_dim1], la, &w[ic - 1 + i__], meq);
984 dcopy___(meq, &b[1], &c__1, &w[
id], &c__1);
986 dscal_sl__(meq, &d__1, &w[
id], &c__1);
992 for (i__ = 1; i__ <= i__1; ++i__) {
993 dcopy___(
n, &a[*meq + i__ + a_dim1], la, &w[ig - 1 + i__], &m1);
1000 for (i__ = 1; i__ <= i__1; ++i__) {
1001 w[ip - 1 + i__] = zero;
1002 dcopy___(
n, &w[ip - 1 + i__], &c__0, &w[ip - 1 + i__], &m1);
1007 dcopy___(
n, &w[ip], &c__0, &w[ip], &i__1);
1010 for (i__ = 1; i__ <= i__1; ++i__) {
1011 w[im - 1 + i__] = zero;
1012 dcopy___(
n, &w[im - 1 + i__], &c__0, &w[im - 1 + i__], &m1);
1017 dcopy___(
n, &w[im], &c__0, &w[im], &i__1);
1021 dcopy___(&mineq, &b[*meq + 1], &c__1, &w[ih], &c__1);
1023 dscal_sl__(&mineq, &d__1, &w[ih], &c__1);
1027 dcopy___(
n, &xl[1], &c__1, &w[il], &c__1);
1029 dcopy___(
n, &xu[1], &c__1, &w[iu], &c__1);
1031 dscal_sl__(
n, &d__1, &w[iu], &c__1);
1033 i__1 = std::max(1, *meq);
1034 lsei_(&w[ic], &w[
id], &w[ie], &w[if__], &w[ig], &w[ih], &i__1, meq,
n,
n, &m1, &m1,
n, &
x[1], &xnorm, &w[iw], &jw[1],
1038 dcopy___(
m, &w[iw], &c__1, &
y[1], &c__1);
1039 dcopy___(&n3, &w[iw + *
m], &c__1, &
y[*
m + 1], &c__1);
1040 dcopy___(&n3, &w[iw + *
m + *
n], &c__1, &
y[*
m + n3 + 1], &c__1);
1046int lsei_(
double *c__,
double *d__,
double *e,
double *f,
double *g,
double *h__,
int *lc,
int *mc,
int *le,
int *me,
1047 int *lg,
int *mg,
int *
n,
double *
x,
double *xnrm,
double *w,
int *jw,
int *mode) {
1050 static double epmach = 2.22e-16;
1051 static double zero = 0.;
1054 int c_dim1, c_offset, e_dim1, e_offset, g_dim1, g_offset, i__1, i__2, i__3;
1058 static int i__, j, k, l;
1060 static int ie, if__, ig, iw, mc1;
1105 g_offset = 1 + g_dim1;
1108 e_offset = 1 + e_dim1;
1111 c_offset = 1 + c_dim1;
1123 iw = (l + 1) * (*mg + 2) + (*mg << 1) + *mc;
1125 if__ = ie + *me * l;
1129 for (i__ = 1; i__ <= i__1; ++i__) {
1132 j = std::min(i__2, *lc);
1135 h12_(&c__1, &i__, &i__2,
n, &c__[i__ + c_dim1], lc, &w[iw + i__], &c__[j + c_dim1], lc, &c__1, &i__3);
1137 h12_(&c__2, &i__, &i__2,
n, &c__[i__ + c_dim1], lc, &w[iw + i__], &e[e_offset], le, &c__1, me);
1140 h12_(&c__2, &i__, &i__2,
n, &c__[i__ + c_dim1], lc, &w[iw + i__], &g[g_offset], lg, &c__1, mg);
1145 for (i__ = 1; i__ <= i__2; ++i__) {
1146 if ((d__1 = c__[i__ + i__ * c_dim1], std::abs(d__1)) < epmach) {
1150 x[i__] = (d__[i__] - ddot_sl__(&i__1, &c__[i__ + c_dim1], lc, &
x[1], &c__1)) / c__[i__ + i__ * c_dim1];
1156 dcopy___(&i__2, &w[mc1], &c__0, &w[mc1], &c__1);
1161 for (i__ = 1; i__ <= i__2; ++i__) {
1163 w[if__ - 1 + i__] = f[i__] - ddot_sl__(mc, &e[i__ + e_dim1], le, &
x[1], &c__1);
1167 for (i__ = 1; i__ <= i__2; ++i__) {
1169 dcopy___(&l, &e[i__ + mc1 * e_dim1], le, &w[ie - 1 + i__], me);
1172 for (i__ = 1; i__ <= i__2; ++i__) {
1174 dcopy___(&l, &g[i__ + mc1 * g_dim1], lg, &w[ig - 1 + i__], mg);
1181 k = std::max(*le, *
n);
1183 hfti_(&w[ie], me, me, &l, &w[if__], &k, &c__1, &t, &krank, xnrm, &w[1], &w[l + 1], &jw[1]);
1184 dcopy___(&l, &w[if__], &c__1, &
x[mc1], &c__1);
1193 for (i__ = 1; i__ <= i__2; ++i__) {
1195 h__[i__] -= ddot_sl__(mc, &g[i__ + g_dim1], lg, &
x[1], &c__1);
1197 lsi_(&w[ie], &w[if__], &w[ig], &h__[1], me, me, mg, mg, &l, &
x[mc1], xnrm, &w[mc1], &jw[1], mode);
1201 t = dnrm2___(mc, &
x[1], &c__1);
1202 *xnrm = sqrt(*xnrm * *xnrm + t * t);
1209 for (i__ = 1; i__ <= i__2; ++i__) {
1211 f[i__] = ddot_sl__(
n, &e[i__ + e_dim1], le, &
x[1], &c__1) - f[i__];
1214 for (i__ = 1; i__ <= i__2; ++i__) {
1216 d__[i__] = ddot_sl__(me, &e[i__ * e_dim1 + 1], &c__1, &f[1], &c__1) -
1217 ddot_sl__(mg, &g[i__ * g_dim1 + 1], &c__1, &w[mc1], &c__1);
1219 for (i__ = *mc; i__ >= 1; --i__) {
1222 h12_(&c__2, &i__, &i__2,
n, &c__[i__ + c_dim1], lc, &w[iw + i__], &
x[1], &c__1, &c__1, &c__1);
1224 for (i__ = *mc; i__ >= 1; --i__) {
1227 j = std::min(i__2, *lc);
1229 w[i__] = (d__[i__] - ddot_sl__(&i__2, &c__[j + i__ * c_dim1], &c__1, &w[j], &c__1)) / c__[i__ + i__ * c_dim1];
1237int lsi_(
double *e,
double *f,
double *g,
double *h__,
int *le,
int *me,
int *lg,
int *mg,
int *
n,
double *
x,
1238 double *xnorm,
double *w,
int *jw,
int *mode) {
1241 static double epmach = 2.22e-16;
1242 static double one = 1.;
1245 int e_dim1, e_offset, g_dim1, g_offset, i__1, i__2, i__3;
1288 g_offset = 1 + g_dim1;
1291 e_offset = 1 + e_dim1;
1298 for (i__ = 1; i__ <= i__1; ++i__) {
1301 j = std::min(i__2, *
n);
1304 h12_(&c__1, &i__, &i__2, me, &e[i__ * e_dim1 + 1], &c__1, &t, &e[j * e_dim1 + 1], &c__1, le, &i__3);
1307 h12_(&c__2, &i__, &i__2, me, &e[i__ * e_dim1 + 1], &c__1, &t, &f[1], &c__1, &c__1, &c__1);
1312 for (i__ = 1; i__ <= i__2; ++i__) {
1314 for (j = 1; j <= i__1; ++j) {
1315 if ((d__1 = e[j + j * e_dim1], std::abs(d__1)) < epmach) {
1320 g[i__ + j * g_dim1] =
1321 (g[i__ + j * g_dim1] - ddot_sl__(&i__3, &g[i__ + g_dim1], lg, &e[j * e_dim1 + 1], &c__1)) / e[j + j * e_dim1];
1324 h__[i__] -= ddot_sl__(
n, &g[i__ + g_dim1], lg, &f[1], &c__1);
1327 ldp_(&g[g_offset], lg, mg,
n, &h__[1], &
x[1], xnorm, &w[1], &jw[1], mode);
1332 daxpy_sl__(
n, &one, &f[1], &c__1, &
x[1], &c__1);
1333 for (i__ = *
n; i__ >= 1; --i__) {
1336 j = std::min(i__2, *
n);
1339 x[i__] = (
x[i__] - ddot_sl__(&i__2, &e[i__ + j * e_dim1], le, &
x[j], &c__1)) / e[i__ + i__ * e_dim1];
1343 j = std::min(i__2, *me);
1345 t = dnrm2___(&i__2, &f[j], &c__1);
1346 *xnorm = sqrt(*xnorm * *xnorm + t * t);
1352int ldp_(
double *g,
const int *mg,
int *
m,
int *
n,
double *h__,
double *
x,
double *xnorm,
double *w,
int *
index,
1356 static double zero = 0.;
1357 static double one = 1.;
1360 int g_dim1, g_offset, i__1, i__2;
1364 static int i__, j, n1, if__, iw, iy, iz;
1366 static double rnorm;
1402 g_offset = 1 + g_dim1;
1414 dcopy___(
n, &
x[1], &c__0, &
x[1], &c__1);
1421 for (j = 1; j <= i__1; ++j) {
1423 for (i__ = 1; i__ <= i__2; ++i__) {
1426 w[iw] = g[j + i__ * g_dim1];
1434 for (i__ = 1; i__ <= i__1; ++i__) {
1445 nnls_(&w[1], &n1, &n1,
m, &w[if__], &w[iy], &rnorm, &w[iwdual], &w[iz], &
index[1], mode);
1450 if (rnorm <= zero) {
1454 fac = one - ddot_sl__(
m, &h__[1], &c__1, &w[iy], &c__1);
1456 if (d__1 - one <= zero) {
1462 for (j = 1; j <= i__1; ++j) {
1464 x[j] = fac * ddot_sl__(
m, &g[j * g_dim1 + 1], &c__1, &w[iy], &c__1);
1466 *xnorm = dnrm2___(
n, &
x[1], &c__1);
1469 dcopy___(
m, &w[1], &c__0, &w[1], &c__1);
1470 daxpy_sl__(
m, &fac, &w[iy], &c__1, &w[1], &c__1);
1476int nnls_(
double *a,
int *mda,
int *
m,
int *
n,
double *b,
double *
x,
double *rnorm,
double *w,
double *z__,
int *
index,
1480 static double zero = 0.;
1481 static double one = 1.;
1482 static double factor = .01;
1485 int a_dim1, a_offset, i__1, i__2;
1490 static int i__, j, k, l;
1492 static int ii, jj, ip, iz, jz;
1494 static int iz1, iz2, npp1, iter;
1495 static double wmax, alpha, asave;
1496 static int itmax, izmax, nsetp;
1497 static double unorm;
1542 a_offset = 1 + a_dim1;
1548 if (*
m <= 0 || *
n <= 0) {
1556 for (i__ = 1; i__ <= i__1; ++i__) {
1565 dcopy___(
n, &
x[1], &c__0, &
x[1], &c__1);
1569 if (iz1 > iz2 || nsetp >= *
m) {
1573 for (iz = iz1; iz <= i__1; ++iz) {
1577 w[j] = ddot_sl__(&i__2, &a[npp1 + j * a_dim1], &c__1, &b[npp1], &c__1);
1583 for (iz = iz1; iz <= i__2; ++iz) {
1599 asave = a[npp1 + j * a_dim1];
1601 h12_(&c__1, &npp1, &i__2,
m, &a[j * a_dim1 + 1], &c__1, &up, &z__[1], &c__1, &c__1, &c__0);
1602 unorm = dnrm2___(&nsetp, &a[j * a_dim1 + 1], &c__1);
1603 t = factor * (d__1 = a[npp1 + j * a_dim1], std::abs(d__1));
1605 if (d__1 - unorm <= zero) {
1608 dcopy___(
m, &b[1], &c__1, &z__[1], &c__1);
1610 h12_(&c__2, &npp1, &i__2,
m, &a[j * a_dim1 + 1], &c__1, &up, &z__[1], &c__1, &c__1, &c__1);
1611 if (z__[npp1] / a[npp1 + j * a_dim1] > zero) {
1615 a[npp1 + j * a_dim1] = asave;
1620 dcopy___(
m, &z__[1], &c__1, &b[1], &c__1);
1627 for (jz = iz1; jz <= i__2; ++jz) {
1630 h12_(&c__2, &nsetp, &npp1,
m, &a[j * a_dim1 + 1], &c__1, &up, &a[jj * a_dim1 + 1], &c__1, mda, &c__1);
1632 k = std::min(npp1, *mda);
1635 dcopy___(&i__2, &w[j], &c__0, &a[k + j * a_dim1], &c__1);
1639 for (ip = nsetp; ip >= 1; --ip) {
1643 d__1 = -z__[ip + 1];
1644 daxpy_sl__(&ip, &d__1, &a[jj * a_dim1 + 1], &c__1, &z__[1], &c__1);
1648 z__[ip] /= a[ip + jj * a_dim1];
1651 if (iter <= itmax) {
1662 for (ip = 1; ip <= i__2; ++ip) {
1663 if (z__[ip] > zero) {
1667 t = -
x[l] / (z__[ip] -
x[l]);
1676 for (ip = 1; ip <= i__2; ++ip) {
1679 x[l] = (one - alpha) *
x[l] + alpha * z__[ip];
1691 for (j = jj; j <= i__2; ++j) {
1694 dsrotg_(&a[j - 1 + ii * a_dim1], &a[j + ii * a_dim1], &c__, &s);
1695 t = a[j - 1 + ii * a_dim1];
1696 dsrot_(
n, &a[j - 1 + a_dim1], mda, &a[j + a_dim1], mda, &c__, &s);
1697 a[j - 1 + ii * a_dim1] = t;
1698 a[j + ii * a_dim1] = zero;
1700 dsrot_(&c__1, &b[j - 1], &c__1, &b[j], &c__1, &c__, &s);
1710 for (jj = 1; jj <= i__2; ++jj) {
1712 if (
x[i__] <= zero) {
1717 dcopy___(
m, &b[1], &c__1, &z__[1], &c__1);
1721 k = std::min(npp1, *
m);
1723 *rnorm = dnrm2___(&i__2, &b[k], &c__1);
1726 dcopy___(
n, &w[1], &c__0, &w[1], &c__1);
1733int hfti_(
double *a,
int *mda,
int *
m,
int *
n,
double *b,
int *mdb,
int *nb,
const double *tau,
int *krank,
1734 double *rnorm,
double *h__,
double *g,
int *ip) {
1737 static double zero = 0.;
1738 static double factor = .001;
1741 int a_dim1, a_offset, b_dim1, b_offset, i__1, i__2, i__3;
1745 static int i__, j, k, l;
1747 static double tmp, hmax;
1748 static int lmax, ldiag;
1782 a_offset = 1 + a_dim1;
1786 b_offset = 1 + b_dim1;
1791 ldiag = std::min(*
m, *
n);
1797 for (j = 1; j <= i__1; ++j) {
1803 for (l = j; l <= i__2; ++l) {
1805 d__1 = a[j - 1 + l * a_dim1];
1806 h__[l] -= d__1 * d__1;
1808 if (h__[l] > h__[lmax]) {
1812 d__1 = hmax + factor * h__[lmax];
1813 if (d__1 - hmax > zero) {
1819 for (l = j; l <= i__2; ++l) {
1822 for (i__ = j; i__ <= i__3; ++i__) {
1825 d__1 = a[i__ + l * a_dim1];
1826 h__[l] += d__1 * d__1;
1829 if (h__[l] > h__[lmax]) {
1841 for (i__ = 1; i__ <= i__2; ++i__) {
1842 tmp = a[i__ + j * a_dim1];
1843 a[i__ + j * a_dim1] = a[i__ + lmax * a_dim1];
1845 a[i__ + lmax * a_dim1] =
tmp;
1852 i__ = std::min(i__2, *
n);
1855 h12_(&c__1, &j, &i__2,
m, &a[j * a_dim1 + 1], &c__1, &h__[j], &a[i__ * a_dim1 + 1], &c__1, mda, &i__3);
1858 h12_(&c__2, &j, &i__2,
m, &a[j * a_dim1 + 1], &c__1, &h__[j], &b[b_offset], &c__1, mdb, nb);
1862 for (j = 1; j <= i__2; ++j) {
1864 if ((d__1 = a[j + j * a_dim1], std::abs(d__1)) <= *tau) {
1876 for (jb = 1; jb <= i__2; ++jb) {
1879 rnorm[jb] = dnrm2___(&i__1, &b[kp1 + jb * b_dim1], &c__1);
1885 for (jb = 1; jb <= i__1; ++jb) {
1887 for (i__ = 1; i__ <= i__2; ++i__) {
1889 b[i__ + jb * b_dim1] = zero;
1898 for (i__ = k; i__ >= 1; --i__) {
1901 h12_(&c__1, &i__, &kp1,
n, &a[i__ + a_dim1], mda, &g[i__], &a[a_offset], mda, &c__1, &i__2);
1905 for (jb = 1; jb <= i__2; ++jb) {
1907 for (i__ = k; i__ >= 1; --i__) {
1910 j = std::min(i__1, *
n);
1913 b[i__ + jb * b_dim1] =
1914 (b[i__ + jb * b_dim1] - ddot_sl__(&i__1, &a[i__ + j * a_dim1], mda, &b[j + jb * b_dim1], &c__1)) /
1915 a[i__ + i__ * a_dim1];
1922 for (j = kp1; j <= i__1; ++j) {
1924 b[j + jb * b_dim1] = zero;
1927 for (i__ = 1; i__ <= i__1; ++i__) {
1929 h12_(&c__2, &i__, &kp1,
n, &a[i__ + a_dim1], mda, &g[i__], &b[jb * b_dim1 + 1], &c__1, mdb, &c__1);
1933 for (j = ldiag; j >= 1; --j) {
1938 tmp = b[l + jb * b_dim1];
1939 b[l + jb * b_dim1] = b[j + jb * b_dim1];
1940 b[j + jb * b_dim1] =
tmp;
1949int h12_(
const int *mode,
const int *lpivot,
const int *l1,
const int *
m,
double *u,
const int *iue,
double *up,
1950 double *c__,
const int *ice,
const int *icv,
const int *ncv) {
1953 static double one = 1.;
1954 static double zero = 0.;
1957 int u_dim1, u_offset, i__1, i__2;
1962 static int i__, j, i2, i3, i4;
1963 static double cl, sm;
1965 static double clinv;
1994 u_offset = 1 + u_dim1;
1999 if (0 >= *lpivot || *lpivot >= *l1 || *l1 > *
m) {
2002 cl = (d__1 = u[*lpivot * u_dim1 + 1], std::abs(d__1));
2008 for (j = *l1; j <= i__1; ++j) {
2009 sm = (d__1 = u[j * u_dim1 + 1], std::abs(d__1));
2011 cl = std::max(sm, cl);
2018 d__1 = u[*lpivot * u_dim1 + 1] * clinv;
2021 for (j = *l1; j <= i__1; ++j) {
2024 d__1 = u[j * u_dim1 + 1] * clinv;
2028 if (u[*lpivot * u_dim1 + 1] > zero) {
2031 *up = u[*lpivot * u_dim1 + 1] - cl;
2032 u[*lpivot * u_dim1 + 1] = cl;
2043 b = *up * u[*lpivot * u_dim1 + 1];
2048 i2 = 1 - *icv + *ice * (*lpivot - 1);
2049 incr = *ice * (*l1 - *lpivot);
2051 for (j = 1; j <= i__1; ++j) {
2057 for (i__ = *l1; i__ <= i__2; ++i__) {
2058 sm += c__[i3] * u[i__ * u_dim1 + 1];
2066 c__[i2] += sm * *up;
2068 for (i__ = *l1; i__ <= i__2; ++i__) {
2069 c__[i4] += sm * u[i__ * u_dim1 + 1];
2079int ldl_(
const int *
n,
double *a,
double *z__,
const double *
sigma,
double *w) {
2082 static double zero = 0.;
2083 static double one = 1.;
2084 static double four = 4.;
2085 static double epmach = 2.22e-16;
2092 static double t, u, v;
2094 static double tp, beta, gamma, alpha,
delta;
2127 if (*
sigma == zero) {
2132 if (*
sigma > zero) {
2137 for (i__ = 1; i__ <= i__1; ++i__) {
2142 for (i__ = 1; i__ <= i__1; ++i__) {
2146 for (j = i__ + 1; j <= i__2; ++j) {
2155 t = epmach / *
sigma;
2158 for (i__ = 1; i__ <= i__1; ++i__) {
2169 for (i__ = 1; i__ <= i__1; ++i__) {
2172 if (*
sigma < zero) {
2175 if (*
sigma > zero) {
2179 a[ij] = alpha * a[ij];
2188 for (j = i__ + 1; j <= i__2; ++j) {
2190 z__[j] -= v * a[ij];
2192 a[ij] += beta * z__[j];
2198 for (j = i__ + 1; j <= i__2; ++j) {
2201 a[ij] = gamma * u + beta * z__[j];
2215double linmin_(
int *mode,
const double *ax,
const double *bx,
const double *f,
const double *tol) {
2218 static double c__ = .381966011;
2219 static double eps = 1.5e-8;
2220 static double zero = 0.;
2223 double ret_val, d__1;
2226 static double a, b, d__, e,
m, p, q, r__, u, v, w,
x, fu, fv, fw, fx, tol1, tol2;
2270 v = a + c__ * (b - a);
2283 tol1 = eps * std::abs(
x) + *tol;
2286 if ((d__1 =
x -
m, std::abs(d__1)) <= tol2 - (b - a) * .5) {
2292 if (std::abs(e) <= tol1) {
2296 r__ = (
x - w) * (fx - fv);
2297 q = (
x - v) * (fx - fw);
2298 p = (
x - v) * q - (
x - w) * r__;
2311 if (std::abs(p) >= (d__1 = q * r__, std::abs(d__1)) * .5 || p <= q * (a -
x) || p >= q * (b -
x)) {
2319 d__ = d_sign(&tol1, &d__1);
2323 d__ = d_sign(&tol1, &d__1);
2337 if (std::abs(d__) < tol1) {
2338 d__ = d_sign(&tol1, &d__);
2370 if (fu <= fw || w ==
x) {
2373 if (fu <= fv || v ==
x || v == w) {
2398int daxpy_sl__(
const int *
n,
const double *da,
double *dx,
const int *incx,
double *dy,
const int *incy) {
2403 static int i__,
m, ix, iy, mp1;
2419 if (*incx == 1 && *incy == 1) {
2427 ix = (-(*n) + 1) * *incx + 1;
2430 iy = (-(*n) + 1) * *incy + 1;
2433 for (i__ = 1; i__ <= i__1; ++i__) {
2434 dy[iy] += *da * dx[ix];
2448 for (i__ = 1; i__ <= i__1; ++i__) {
2449 dy[i__] += *da * dx[i__];
2458 for (i__ = mp1; i__ <= i__1; i__ += 4) {
2459 dy[i__] += *da * dx[i__];
2460 dy[i__ + 1] += *da * dx[i__ + 1];
2461 dy[i__ + 2] += *da * dx[i__ + 2];
2462 dy[i__ + 3] += *da * dx[i__ + 3];
2468int dcopy___(
const int *
n,
double *dx,
const int *incx,
double *dy,
const int *incy) {
2473 static int i__,
m, ix, iy, mp1;
2486 if (*incx == 1 && *incy == 1) {
2494 ix = (-(*n) + 1) * *incx + 1;
2497 iy = (-(*n) + 1) * *incy + 1;
2500 for (i__ = 1; i__ <= i__1; ++i__) {
2515 for (i__ = 1; i__ <= i__1; ++i__) {
2525 for (i__ = mp1; i__ <= i__1; i__ += 7) {
2527 dy[i__ + 1] = dx[i__ + 1];
2528 dy[i__ + 2] = dx[i__ + 2];
2529 dy[i__ + 3] = dx[i__ + 3];
2530 dy[i__ + 4] = dx[i__ + 4];
2531 dy[i__ + 5] = dx[i__ + 5];
2532 dy[i__ + 6] = dx[i__ + 6];
2538double ddot_sl__(
const int *
n,
double *dx,
const int *incx,
double *dy,
const int *incy) {
2544 static int i__,
m, ix, iy, mp1;
2545 static double dtemp;
2560 if (*incx == 1 && *incy == 1) {
2568 ix = (-(*n) + 1) * *incx + 1;
2571 iy = (-(*n) + 1) * *incy + 1;
2574 for (i__ = 1; i__ <= i__1; ++i__) {
2575 dtemp += dx[ix] * dy[iy];
2590 for (i__ = 1; i__ <= i__1; ++i__) {
2591 dtemp += dx[i__] * dy[i__];
2600 for (i__ = mp1; i__ <= i__1; i__ += 5) {
2601 dtemp = dtemp + dx[i__] * dy[i__] + dx[i__ + 1] * dy[i__ + 1] + dx[i__ + 2] * dy[i__ + 2] +
2602 dx[i__ + 3] * dy[i__ + 3] + dx[i__ + 4] * dy[i__ + 4];
2610double dnrm2___(
const int *
n,
double *dx,
const int *incx) {
2613 static double zero = 0.;
2614 static double one = 1.;
2615 static double cutlo = 8.232e-11;
2616 static double cuthi = 1.304e19;
2627 double ret_val, d__1;
2630 static int i__, j, nn;
2631 static double sum, xmax;
2633 static double hitest;
2698 if ((d__1 = dx[i__], std::abs(d__1)) > cutlo) {
2706 if (dx[i__] == zero) {
2709 if ((d__1 = dx[i__], std::abs(d__1)) > cutlo) {
2721 sum = sum / dx[i__] / dx[i__];
2723 xmax = (d__1 = dx[i__], std::abs(d__1));
2728 if ((d__1 = dx[i__], std::abs(d__1)) > cutlo) {
2734 if ((d__1 = dx[i__], std::abs(d__1)) <= xmax) {
2738 d__1 = xmax / dx[i__];
2739 sum = one + sum * (d__1 * d__1);
2740 xmax = (d__1 = dx[i__], std::abs(d__1));
2744 d__1 = dx[i__] / xmax;
2749 sum = sum * xmax * xmax;
2753 hitest = cuthi /
static_cast<float>(*n);
2757 for (j = i__; i__2 < 0 ? j >= i__1 : j <= i__1; j += i__2) {
2758 if ((d__1 = dx[j], std::abs(d__1)) >= hitest) {
2766 ret_val = sqrt(sum);
2775 ret_val = xmax * sqrt(sum);
2780int dsrot_(
const int *
n,
double *dx,
const int *incx,
double *dy,
const int *incy,
const double *c__,
const double *s) {
2785 static int i__, ix, iy;
2786 static double dtemp;
2798 if (*incx == 1 && *incy == 1) {
2806 ix = (-(*n) + 1) * *incx + 1;
2809 iy = (-(*n) + 1) * *incy + 1;
2812 for (i__ = 1; i__ <= i__1; ++i__) {
2813 dtemp = *c__ * dx[ix] + *s * dy[iy];
2814 dy[iy] = *c__ * dy[iy] - *s * dx[ix];
2824 for (i__ = 1; i__ <= i__1; ++i__) {
2825 dtemp = *c__ * dx[i__] + *s * dy[i__];
2826 dy[i__] = *c__ * dy[i__] - *s * dx[i__];
2833int dsrotg_(
double *da,
double *db,
double *c__,
double *s) {
2836 static double one = 1.;
2837 static double zero = 0.;
2843 static double r__, z__, roe, scale;
2849 if (std::abs(*da) > std::abs(*db)) {
2852 scale = std::abs(*da) + std::abs(*db);
2853 if (scale != zero) {
2865 r__ = scale * sqrt(d__1 * d__1 + d__2 * d__2);
2866 r__ = d_sign(&one, &roe) * r__;
2871 if (std::abs(*c__) > zero && std::abs(*c__) <= *s) {
2879int dscal_sl__(
const int *
n,
const double *da,
double *dx,
const int *incx) {
2884 static int i__,
m, mp1, nincx;
2903 for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
2904 dx[i__] = *da * dx[i__];
2916 for (i__ = 1; i__ <= i__2; ++i__) {
2917 dx[i__] = *da * dx[i__];
2926 for (i__ = mp1; i__ <= i__2; i__ += 5) {
2927 dx[i__] = *da * dx[i__];
2928 dx[i__ + 1] = *da * dx[i__ + 1];
2929 dx[i__ + 2] = *da * dx[i__ + 2];
2930 dx[i__ + 3] = *da * dx[i__ + 3];
2931 dx[i__ + 4] = *da * dx[i__ + 4];
static std::unique_ptr< QThreadPool > tp
double value
The value of the point.
std::map< DeltaEMode::Type, std::string > index
void initializeConstraints(const DblMatrix &equality, const DblMatrix &inequality)
Create constraint array.
size_t numInequalityConstraints() const
size_t numParameters() const
size_t numEqualityConstraints() const
std::vector< double > minimize(const std::vector< double > &x0) const
Perform the minimization.
const size_t m_nparams
Number of parameters under minimization.
void evaluateConstraints(std::vector< double > &constrValues, const std::vector< double > &x) const
Compute values of constraints.
void fprime(std::vector< double > &grad, const std::vector< double > &x) const
Compute derivative numerically.
double fvalue(const std::vector< double > &x) const
Compute the value of the objective function.
std::vector< double > m_constraintNorms
Holder for constraint normals.
size_t numRows() const
Return the number of rows in the matrix.
size_t numCols() const
Return the number of columns in the matrix.