ufranco/Article-22714-Volatility-Modeling-Part-3
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
Article-22714-Volatility-Mo.../Include/slsqp_article/slsqp.mqh

3888 lines
137 KiB
MQL5
Raw Permalink Normal View History

first commit
2026-06-03 20:14:05 +02:00
//+------------------------------------------------------------------+
//| slspq.mqh |
//| Copyright 2025, MetaQuotes Ltd. |
//| https://www.mql5.com |
//+------------------------------------------------------------------+
#property copyright "Copyright 2025, MetaQuotes Ltd."
#property link "https://www.mql5.com"
#include "num_diff.mqh"
#include<Object.mqh>
#define MIN2(a,b) ((a) <= (b) ? (a) : (b))
#define MAX2(a,b) ((a) >= (b) ? (a) : (b))
#define HUGE_VAL DBL_MAX
//+------------------------------------------------------------------+
//|copy from vector to pointer |
//+------------------------------------------------------------------+
template<typename T>
bool Copy(T& dest_array[],vector<T>& src_vec,int count,int dest_start = 0, int src_start = 0)
{
if(int(dest_array.Size())<fabs(count+dest_start) || int(src_vec.Size())<fabs(count+src_start))
{
Print(__FUNCTION__, " invalid inputs ");
return false;
}
//---
for(int i = 0; i<(count); ++i)
dest_array[dest_start+i] = src_vec[src_start+i];
//---
return true;
}
//+------------------------------------------------------------------+
//| copy from pointer to vector |
//+------------------------------------------------------------------+
template<typename T>
bool Copy(vector<T>& dest_vec, T& src_array[],int count,int dest_start = 0, int src_start = 0)
{
if(dest_vec.Size()<ulong(count+dest_start) || int(src_array.Size())<fabs(count+src_start))
{
Print(__FUNCTION__, " invalid inputs ");
return false;
}
//---
for(int i = 0; i<(count); ++i)
dest_vec[dest_start+i] = src_array[src_start+i];
//---
return true;
}
//+------------------------------------------------------------------+
//| constants |
//+------------------------------------------------------------------+
int c__0 = 0;
int c__1 = 1;
int c__2 = 2;
//+------------------------------------------------------------------+
//| function pointer |
//+------------------------------------------------------------------+
typedef double (*slsqp_func)(int n, double& x[],double& gradient[],IObjective* func_data);
//+------------------------------------------------------------------+
//| result |
//+------------------------------------------------------------------+
enum slsqp_result
{
SLSQP_FAILURE = -1,
SLSQP_INVALID_ARGS = -2,
SLSQP_OUT_OF_MEMORY = -3,
SLSQP_ROUNDOFF_LIMITED = -4,
SLSQP_FORCED_STOP = -5,
SLSQP_NUM_FAILURES = -6,
SLSQP_SUCCESS = 1, /* generic success code */
SLSQP_STOPVAL_REACHED = 2,
SLSQP_FTOL_REACHED = 3,
SLSQP_XTOL_REACHED = 4,
SLSQP_MAXEVAL_REACHED = 5,
SLSQP_MAXTIME_REACHED = 6,
SLSQP_NUM_RESULTS
};
//+------------------------------------------------------------------+
//| struct for state management |
//+------------------------------------------------------------------+
struct slsqpb_state
{
double t, f0, h1, h2, h3, h4;
int n, n1, n2, n3;
double t0, gs;
double tol;
int line;
double alpha;
int iexact;
int incons, ireset, itermx;
double x0[];
slsqpb_state(void)
{
t = f0 = h1 = h2 = h3 = h4 = 0;
n = n1 = n2 = n3 = 0;
t0 = gs = 0;
tol = 0;
line = 0;
alpha = 0;
iexact = 0;
incons = ireset = itermx = 0;
}
slsqpb_state(slsqpb_state& other)
{
n = other.n;
t = other.t;
f0 = other.f0;
h1 = other.h1;
h2 = other.h2;
h3 = other.h3;
h4 = other.h4;
n1 = other.n1;
n2 = other.n2;
n3 = other.n3;
t0 = other.t0;
gs = other.gs;
tol = other.tol;
line = other.line;
alpha = other.alpha;
iexact = other.iexact;
incons = other.incons;
ireset = other.ireset;
itermx = other.itermx;
ArrayCopy(x0,other.x0);
}
~slsqpb_state(void)
{
}
void operator=(slsqpb_state& other)
{
n = other.n;
t = other.t;
f0 = other.f0;
h1 = other.h1;
h2 = other.h2;
h3 = other.h3;
h4 = other.h4;
n1 = other.n1;
n2 = other.n2;
n3 = other.n3;
t0 = other.t0;
gs = other.gs;
tol = other.tol;
line = other.line;
alpha = other.alpha;
iexact = other.iexact;
incons = other.incons;
ireset = other.ireset;
itermx = other.itermx;
ArrayCopy(x0,other.x0);
}
};
//+------------------------------------------------------------------+
//| stopping criteria |
//+------------------------------------------------------------------+
struct slsqp_stopping
{
int n;
int nevals_p;
int maxeval;
double minf_max;
double ftol_rel;
double ftol_abs;
double xtol_rel;
double maxtime;
double start;
string stop_msg;
int force_stop[1];
double xtol_abs[];
double x_weights[];
vector gradient;
slsqpb_state slsqp_state;
slsqp_stopping(void)
{
start = n = 0;
maxeval = 100;
nevals_p = 0;
minf_max = -DBL_MAX;
ftol_abs = 0;
ftol_rel = 0;
xtol_rel = 1.0e-4;
maxtime = 0;
stop_msg = NULL;
force_stop[0] = 0.0;
gradient = vector::Zeros(0);
}
slsqp_stopping(slsqp_stopping& other)
{
n = other.n;
nevals_p = other.nevals_p;
maxeval = other.maxeval;
minf_max = other.minf_max;
ftol_abs = other.ftol_abs;
ftol_rel = other.ftol_rel;
maxtime = other.maxtime;
start = other.start;
stop_msg = other.stop_msg;
force_stop[0] = other.force_stop[0];
gradient = other.gradient;
ArrayCopy(xtol_abs,other.xtol_abs);
ArrayCopy(x_weights,other.x_weights);
slsqp_state = other.slsqp_state;
}
void operator=(slsqp_stopping& other)
{
n = other.n;
nevals_p = other.nevals_p;
maxeval = other.maxeval;
minf_max = other.minf_max;
ftol_abs = other.ftol_abs;
ftol_rel = other.ftol_rel;
maxtime = other.maxtime;
start = other.start;
stop_msg = other.stop_msg;
force_stop[0] = other.force_stop[0];
gradient = other.gradient;
ArrayCopy(xtol_abs,other.xtol_abs);
ArrayCopy(x_weights,other.x_weights);
slsqp_state = other.slsqp_state;
}
};
//+------------------------------------------------------------------+
//| constraint |
//+------------------------------------------------------------------+
struct slsqp_constraint
{
int m;
IObjective* f_data;
double tol[];
slsqp_constraint(void)
{
m=0;
}
slsqp_constraint(slsqp_constraint& other)
{
m = other.m;
f_data = other.f_data;
ArrayCopy(tol,other.tol,0,0,m);
}
~slsqp_constraint(void)
{
}
void operator=(slsqp_constraint& other)
{
m = other.m;
f_data = other.f_data;
ArrayCopy(tol,other.tol,0,0,m);
}
double f(int n, double& x[],double& gradient[],IObjective* func_data)
{
vector vx = vector::Zeros(n);
vector out;
vx.Assign(x);
if(gradient.Size())
{
ObjReturn or = func_data.fun_and_grad(vx);
for(int i = 0; i<1; ++i)
for(int j = 0; j<n; ++j)
gradient[i*n+j] = or.mg[i,j];
out = or.mf;
}
else
out = func_data.objective_function(vx);
return out[0];
}
void mf(int mm, double& result[], int n, double& x[],double& gradient[],IObjective* func_data, int offset)
{
vector vx = vector::Zeros(n);
Copy(vx,x,n);
if(gradient.Size())
{
ObjReturn or = func_data.fun_and_grad(vx);
for(int j = 0; j<n; ++j)
{
result[offset+j] = or.mf[j];
for(int i = 0; i<mm; ++i)
gradient[(i*n+j)] = or.mg[i,j];
}
}
else
{
vector res = func_data.objective_function(vx);
Copy(result,res,n,offset);
}
}
};
//+------------------------------------------------------------------+
//| linear interpolation utility |
//+------------------------------------------------------------------+
double sc(double x, double scale_min_start, double scale_max_start)
{
return scale_min_start + x * (scale_max_start - scale_min_start);
}
//+------------------------------------------------------------------+
//| calculates a variation of the L1 Norm (sum of absolute values) |
//+------------------------------------------------------------------+
double vector_norm(int n, double& vec[], double& w[],double& scale_min[],double& scale_max[],int vec_start = 0, int w_start = 0, int scale_min_start = 0, int scale_max_start = 0)
{
int i;
double ret = 0;
if(scale_min.Size() && scale_max.Size())
{
if(w.Size())
for(i = 0; i < n; i++)
ret += w[w_start+i] * fabs(sc(vec[vec_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i]));
else
for(i = 0; i < n; i++)
ret += fabs(sc(vec[vec_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i]));
}
else
{
if(w.Size())
for(i = 0; i < n; i++)
ret += w[w_start+i] * fabs(vec[vec_start+i]);
else
for(i = 0; i < n; i++)
ret += fabs(vec[vec_start+i]);
}
return ret;
}
//+------------------------------------------------------------------------------------+
//|calculates the distance (Manhattan distance/L1 norm) between two vectors, x and oldx|
//+------------------------------------------------------------------------------------+
double diff_norm(int n, double& x[], double& oldx[], double& w[], double& scale_min[], double& scale_max[], int x_start = 0, int oldx_start = 0, int w_start = 0, int scale_min_start = 0, int scale_max_start = 0)
{
int i;
double ret = 0;
if(scale_min.Size() && scale_max.Size())
{
if(w.Size())
for(i = 0; i < n; i++)
ret += w[w_start+i] * fabs(sc(x[x_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i]) - sc(oldx[oldx_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i]));
else
for(i = 0; i < n; i++)
ret += fabs(sc(x[x_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i]) - sc(oldx[oldx_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i]));
}
else
{
if(w.Size())
for(i = 0; i < n; i++)
ret += w[w_start+i] * fabs(x[x_start+i] - oldx[oldx_start+i]);
else
for(i = 0; i < n; i++)
ret += fabs(x[x_start+i] - oldx[oldx_start+i]);
}
return ret;
}
//+------------------------------------------------------------------+
//| termination criterion utility |
//+------------------------------------------------------------------+
int relstop(double vold, double vnew, double reltol, double abstol)
{
if(slsqp_isinf(vold))
return 0;
return (fabs(vnew - vold) < abstol || fabs(vnew - vold) < reltol * (fabs(vnew) + fabs(vold)) * 0.5 || (reltol > 0 && vnew == vold)); /* catch vnew == vold == 0 */
}
//+------------------------------------------------------------------+
//| termination criterion utility |
//+------------------------------------------------------------------+
int slsqp_stop_ftol(slsqp_stopping& s, double f, double oldf)
{
return (relstop(oldf, f, s.ftol_rel, s.ftol_abs));
}
//+------------------------------------------------------------------+
//| termination criterion utility |
//+------------------------------------------------------------------+
int slsqp_stop_f(slsqp_stopping& s, double f, double oldf)
{
return (f <= s.minf_max || slsqp_stop_ftol(s, f, oldf));
}
//+------------------------------------------------------------------+
//| termination criterion utility |
//+------------------------------------------------------------------+
int slsqp_stop_x(slsqp_stopping& s, double& x[], double& oldx[], int x_start = 0, int oldx_start=0)
{
int i;
double empty[];
if(diff_norm(s.n, x, oldx, s.x_weights, empty, empty,x_start,oldx_start) < s.xtol_rel * vector_norm(s.n, x, s.x_weights, empty, empty,x_start))
return 1;
if(!s.xtol_abs.Size())
return 0;
for(i = 0; i < s.n; ++i)
if(fabs(x[x_start+i] - oldx[oldx_start+i]) >= s.xtol_abs[i])
return 0;
return 1;
}
//+------------------------------------------------------------------+
//| termination check based on the change in the decision variables |
//+------------------------------------------------------------------+
int slsqp_stop_dx(slsqp_stopping& s, double& x[], double& dx[],int x_start=0, int dx_start=0)
{
int i;
double empty[];
if(vector_norm(s.n, dx, s.x_weights, empty, empty,dx_start) < s.xtol_rel * vector_norm(s.n, x, s.x_weights, empty, empty,x_start))
return 1;
if(!s.xtol_abs.Size())
return 0;
for(i = 0; i < s.n; ++i)
if(fabs(dx[dx_start+i]) >= s.xtol_abs[i])
return 0;
return 1;
}
//+------------------------------------------------------------------+
//|termination check based on scaled values |
//+------------------------------------------------------------------+
int slsqp_stop_xs(slsqp_stopping& s, double& xs[], double& oldxs[], double& scale_min[], double& scale_max[], int xs_start = 0, int oldxs_start= 0, int scale_min_start = 0, int scale_max_start = 0)
{
int i;
if(diff_norm(s.n, xs, oldxs, s.x_weights, scale_min, scale_max,xs_start,oldxs_start,0,scale_min_start,scale_max_start) < s.xtol_rel * vector_norm(s.n, xs, s.x_weights, scale_min, scale_max,xs_start,0,scale_min_start,scale_max_start))
return 1;
if(!s.xtol_abs.Size())
return 0;
for(i = 0; i < s.n; ++i)
if(fabs(sc(xs[xs_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i]) - sc(oldxs[oldxs_start+i], scale_min[scale_min_start+i], scale_max[scale_max_start+i])) >= s.xtol_abs[i])
return 0;
return 1;
}
//+------------------------------------------------------------------+
//| termination check based on number of function evals |
//+------------------------------------------------------------------+
int slsqp_stop_evals(slsqp_stopping& s)
{
return (s.maxeval > 0 && (s.nevals_p) >= s.maxeval);
}
//+------------------------------------------------------------------+
//| termination check based on runtime in seconds |
//+------------------------------------------------------------------+
int slsqp_stop_time_(double start, double maxtime)
{
return (maxtime > 0 && double(TimeLocal()) - start >= maxtime);
}
//+------------------------------------------------------------------+
//| termination check utility |
//+------------------------------------------------------------------+
int slsqp_stop_time(slsqp_stopping& s)
{
return slsqp_stop_time_(s.start, s.maxtime);
}
//+------------------------------------------------------------------+
//| termination check utility |
//+------------------------------------------------------------------+
int slsqp_stop_evalstime(slsqp_stopping& stop)
{
return slsqp_stop_evals(stop) || slsqp_stop_time(stop);
}
//+------------------------------------------------------------------+
//| termination check based on user requested forced stop |
//+------------------------------------------------------------------+
int slsqp_stop_forced(slsqp_stopping& stop)
{
return int(bool(stop.force_stop[0]) || IsStopped());
}
//+------------------------------------------------------------------+
//| count the number of constraints |
//+------------------------------------------------------------------+
int slsqp_count_constraints(int p, slsqp_constraint& c[])
{
int i, count = 0;
for(i = 0; i < p; ++i)
count += c[i].m;
return count;
}
//+------------------------------------------------------------------+
//| constraints parsing utility |
//+------------------------------------------------------------------+
int slsqp_max_constraint_dim(int p, slsqp_constraint& c[])
{
int i, max_dim = 0;
for(i = 0; i < p; ++i)
if(c[i].m > max_dim)
max_dim = c[i].m;
return max_dim;
}
//+------------------------------------------------------------------+
//| evaluates a or all constraints |
//+------------------------------------------------------------------+
void slsqp_eval_constraint(double& result[], double& grad[], slsqp_constraint& c, int n, double& x[], int offset)
{
if(c.m == 1)
result[offset] = c.f(n, x, grad, c.f_data);
else
c.mf(c.m, result, n, x, grad, c.f_data,offset);
}
//+------------------------------------------------------------------+
//| prints stop message |
//+------------------------------------------------------------------+
void slsqp_stop_msg(slsqp_stopping& s, string msg)
{
printf(msg," ",s.stop_msg);
}
//+------------------------------------------------------------------+
//| checks for infinite number |
//+------------------------------------------------------------------+
int slsqp_isinf(double x)
{
return (MathClassify(fabs(x)) == FP_INFINITE);
}
//+------------------------------------------------------------------+
//| checks if number is normal |
//+------------------------------------------------------------------+
int slsqp_isfinite(double x)
{
return (MathClassify(fabs(x)) == FP_NORMAL);
}
//+------------------------------------------------------------------+
//| checks if number is not too tiny |
//+------------------------------------------------------------------+
int slsqp_istiny(double x)
{
if(x == 0.0)
return 1;
else
{
return fpclassify(x) == FP_SUBNORMAL;
}
}
//+------------------------------------------------------------------+
//| checks for invalid number |
//+------------------------------------------------------------------+
int slsqp_isnan(double x)
{
vector x_(1);
x_[0] = x;
return (int)x_.HasNan();
}
//+------------------------------------------------------------------+
//|COPIES A VECTOR, X, TO A VECTOR, Y, with the given increments |
//+------------------------------------------------------------------+
void dcopy___(int &n_, double& dx[], int incx,double& dy[], int incy, int dx_start = 0, int dy_start = 0)
{
int i, n =n_;
if(n <= 0)
return;
if(incx == 1 && incy == 1)
ArrayCopy(dy,dx,dy_start,dx_start,n);// sizeof(double) * ((unsigned) n));
else
if(incx == 0 && incy == 1)
{
double x = dx[dx_start+0];
for(i = 0; i < n; ++i)
dy[dy_start+i] = x;
}
else
{
for(i = 0; i < n; ++i)
dy[dy_start+(i*incy)] = dx[dx_start+(i*incx)];
}
} /* dcopy___ */
//+------------------------------------------------------------------+
//|CONSTANT TIMES A VECTOR PLUS A VECTOR |
//+------------------------------------------------------------------+
void daxpy_sl__(int &n_, double& da_, double& dx[],
int incx, double& dy[], int incy, int dx_start = 0, int dy_start = 0)
{
int n = n_, i;
double da = da_;
if(n <= 0 || da == 0)
return;
for(i = 0; i < n; ++i)
dy[dy_start+(i*incy)] = dy[dy_start+(i*incy)] + da * dx[dx_start+(i*incx)];
}
//+------------------------------------------------------------------+
//|dot product dx dot dy |
//+------------------------------------------------------------------+
double ddot_sl__(int &n_, double& dx[], int incx, double& dy[], int incy, int dx_start = 0, int dy_start = 0)
{
int n = n_, i;
double sum = 0;
if(n <= 0)
return 0;
for(i = 0; i < n; ++i)
sum += dx[dx_start+(i*incx)] * dy[dy_start+(i*incy)];
return (double) sum;
}
//+------------------------------------------------------------------+
//|compute the L2 norm of array DX of length N, stride INCX |
//+------------------------------------------------------------------+
double dnrm2___(int &n_, double& dx[], int incx, int dx_start = 0)
{
int i, n =n_;
double xmax = 0, scale;
double sum = 0;
for(i = 0; i < n; ++i)
{
double xabs = fabs(dx[dx_start+(incx*i)]);
if(xmax < xabs)
xmax = xabs;
}
if(xmax == 0)
return 0;
scale = 1.0 / xmax;
for(i = 0; i < n; ++i)
{
double xs = scale * dx[dx_start+(incx*i)];
sum += xs * xs;
}
return xmax * sqrt((double) sum);
}
//+------------------------------------------------------------------+
//|apply Givens rotation |
//+------------------------------------------------------------------+
void dsrot_(int n, double& dx[], int incx,
double& dy[], int incy, double &c__, double &s_, int dx_start = 0, int dy_start = 0)
{
int i;
double c =c__, s =s_;
for(i = 0; i < n; ++i)
{
double x = dx[dx_start+(incx*i)], y = dy[dy_start+(incy*i)];
dx[dx_start+(incx*i)]= c * x + s * y;
dy[dy_start+(incy*i)]= c * y - s * x;
}
}
//+------------------------------------------------------------------+
//|construct Givens rotation |
//+------------------------------------------------------------------+
//+------------------------------------------------------------------+
//| |
//+------------------------------------------------------------------+
void dsrotg_(double &da, double &db, double &c, double &s)
{
double absa, absb, roe, scale;
absa = fabs(da);
absb = fabs(db);
if(absa > absb)
{
roe =da;
scale = absa;
}
else
{
roe =db;
scale = absb;
}
if(scale != 0)
{
double r, iscale = 1 / scale;
double tmpa = (da) * iscale, tmpb = (db) * iscale;
r = (roe < 0 ? -scale : scale) * sqrt((tmpa * tmpa) + (tmpb * tmpb));
c =da / r;
s =db / r;
da = r;
if(c != 0 && fabs(c) <=s)
db = 1 / c;
else
db =s;
}
else
{
c = 1;
s = da = db = 0;
}
}
//+------------------------------------------------------------------+
//|scales vector X(n) by constant da |
//+------------------------------------------------------------------+
//+------------------------------------------------------------------+
//| |
//+------------------------------------------------------------------+
void dscal_sl__(int &n_, double &da, double& dx[], int incx, int dx_start = 0)
{
int i, n = n_;
double alpha = da;
for(i = 0; i < n; ++i)
dx[dx_start+(i*incx)] = dx[dx_start+(i*incx)] * alpha;
}
//+------------------------------------------------------------------+
//|implements a Householder transformation |
//+------------------------------------------------------------------+
void h12_(int &mode, int &lpivot, int &l1,int &m, double&u[], int &iue, double &up,double&c__[], int &ice, int &icv, int &ncv, int u_start = 0, int c___start = 0)
{
/* Initialized data */
double one = 1.;
/* System generated locals */
int u_dim1, u_offset, i__1, i__2;
double d__1;
/* Local variables */
double b;
int i__, j, i2, i3, i4;
double cl, sm;
int incr;
double clinv;
/* C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12 */
/* TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974 */
/* CONSTRUCTION AND/OR APPLICATION OF A SINGLE */
/* HOUSEHOLDER TRANSFORMATION Q = I + U*(U**T)/B */
/* MODE = 1 OR 2 TO SELECT ALGORITHM H1 OR H2 . */
/* LPIVOT IS THE INDEX OF THE PIVOT ELEMENT. */
/* L1,M IF L1 <= M THE TRANSFORMATION WILL BE CONSTRUCTED TO */
/* ZERO ELEMENTS INDEXED FROM L1 THROUGH M. */
/* IF L1 > M THE SUBROUTINE DOES AN IDENTITY TRANSFORMATION. */
/* U(),IUE,UP */
/* ON ENTRY TO H1 U() STORES THE PIVOT VECTOR. */
/* IUE IS THE STORAGE INCREMENT BETWEEN ELEMENTS. */
/* ON EXIT FROM H1 U() AND UP STORE QUANTITIES DEFINING */
/* THE VECTOR U OF THE HOUSEHOLDER TRANSFORMATION. */
/* ON ENTRY TO H2 U() AND UP */
/* SHOULD STORE QUANTITIES PREVIOUSLY COMPUTED BY H1. */
/* THESE WILL NOT BE MODIFIED BY H2. */
/* C() ON ENTRY TO H1 OR H2 C() STORES A MATRIX WHICH WILL BE */
/* REGARDED AS A SET OF VECTORS TO WHICH THE HOUSEHOLDER */
/* TRANSFORMATION IS TO BE APPLIED. */
/* ON EXIT C() STORES THE SET OF TRANSFORMED VECTORS. */
/* ICE STORAGE INCREMENT BETWEEN ELEMENTS OF VECTORS IN C(). */
/* ICV STORAGE INCREMENT BETWEEN VECTORS IN C(). */
/* NCV NUMBER OF VECTORS IN C() TO BE TRANSFORMED. */
/* IF NCV <= 0 NO OPERATIONS WILL BE DONE ON C(). */
/* Parameter adjustments */
u_dim1 = iue;
u_offset = 1 + u_dim1;
u_start -= u_offset;
--c___start;
bool processing = true;
int goto = 0;
/* Function Body */
if(0 >= lpivot || lpivot >= l1 || l1 > m)
{
return; //goto = 80;
}
d__1 = u[u_start+(lpivot * u_dim1 + 1)];
cl = (fabs(d__1));
if(mode == 2)
{
goto = 30;
}
else
{
/* ****** CONSTRUCT THE TRANSFORMATION ****** */
i__1 = m;
for(j = l1; j <=i__1; ++j)
{
d__1 = u[u_start+(j * u_dim1 + 1)];
sm = (fabs(d__1));
/* L10: */
cl = MAX2(sm,cl);
}
if(cl <= 0.0)
{
return; //goto L80;
}
clinv = one / cl;
/* Computing 2nd power */
d__1 = u[u_start+(lpivot * u_dim1 + 1)] * clinv;
sm = d__1 * d__1;
i__1 = m;
for(j = l1; j <= i__1; ++j)
{
/* L20: */
/* Computing 2nd power */
d__1 = u[u_start+(j * u_dim1 + 1)] * clinv;
sm += d__1 * d__1;
}
cl *= sqrt(sm);
if(u[u_start+(lpivot * u_dim1 + 1)] > 0.0)
{
cl = -cl;
}
up = u[u_start+(lpivot * u_dim1 + 1)] - cl;
u[u_start+(lpivot * u_dim1 + 1)] = cl;
goto = 40;
}
/* ****** APPLY THE TRANSFORMATION I+U*(U**T)/B TO C ****** */
while(processing)
{
switch(goto)
{
case 30:
if(cl <= 0.0)
{
goto = 80;
break;
}
case 40:
if(ncv <= 0)
{
goto = 80;
break;
}
b = up * u[u_start+(lpivot * u_dim1 + 1)];
if(b >= 0.0)
{
goto = 80;
break;
}
b = one / b;
i2 = 1 - icv + ice * (lpivot - 1);
incr = ice * (l1 - lpivot);
i__1 = ncv;
for(j = 1; j <= i__1; ++j)
{
i2 += icv;
i3 = i2 + incr;
i4 = i3;
sm = c__[c___start+i2] * up;
i__2 = m;
for(i__ = l1; i__ <= i__2; ++i__)
{
sm += c__[c___start+i3] * u[u_start+(i__ * u_dim1 + 1)];
/* L50: */
i3 += ice;
}
if(sm == 0.0)
{
continue; //goto = 70;
}
sm *= b;
c__[c___start+i2] = c__[c___start+i2] + sm * up;
i__2 = m;
for(i__ = l1; i__ <= i__2; ++i__)
{
c__[c___start+i4] = c__[c___start+i4] + sm * u[u_start+(i__ * u_dim1 + 1)];
/* L60: */
i4 += ice;
}
}
case 80:
processing = false;
break;
}
}
} /* h12_ */
//+--------------------------------------------------------------------------------------+
//|implements the classic Non-Negative Least Squares (NNLS) algorithm of Lawson & Hanson.|
//+--------------------------------------------------------------------------------------+
void nnls_(double&a[], int &mda, int &m, int &n, double&b[], double&x[], double&rnorm, double&w[],double&z__[], double&indx[], int &mode, int a_start = 0, int b_start = 0,int x_start = 0, int w_start = 0, int z___start = 0, int indx_start = 0)
{
/* Initialized data */
double one = 1.;
double factor = .01;
/* System generated locals */
int a_dim1, a_offset, i__1, i__2;
double d__1;
/* Local variables */
double c__;
int i__ = 0, j = 0, k = 0, l = 0;
double s = 0, t = 0;
int ii = 0, jj = 0, ip = 0, iz = 0, jz = 0;
double up = 0;
int iz1, iz2, npp1, iter;
iz1 = iz2 = npp1 = iter = 0;
double wmax = 0.0, alpha = 0.0, asave = 0.0;
int itmax, izmax = 0, nsetp;
itmax = nsetp = 0;
double unorm;
/* C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY: */
/* 'SOLVING LEAST SQUARES PROBLEMS'. PRENTICE-HALL.1974 */
/* ********** NONNEGATIVE LEAST SQUARES ********** */
/* GIVEN AN M BY N MATRIX, A, AND AN M-VECTOR, B, COMPUTE AN */
/* N-VECTOR, X, WHICH SOLVES THE LEAST SQUARES PROBLEM */
/* A*X = B SUBJECT TO X >= 0 */
/* A(),MDA,M,N */
/* MDA IS THE FIRST DIMENSIONING PARAMETER FOR THE ARRAY,A(). */
/* ON ENTRY A() CONTAINS THE M BY N MATRIX,A. */
/* ON EXIT A() CONTAINS THE PRODUCT Q*A, */
/* WHERE Q IS AN M BY M ORTHOGONAL MATRIX GENERATED */
/* IMPLICITLY BY THIS SUBROUTINE. */
/* EITHER M>=N OR M<N IS PERMISSIBLE. */
/* THERE IS NO RESTRICTION ON THE RANK OF A. */
/* B() ON ENTRY B() CONTAINS THE M-VECTOR, B. */
/* ON EXIT B() CONTAINS Q*B. */
/* X() ON ENTRY X() NEED NOT BE INITIALIZED. */
/* ON EXIT X() WILL CONTAIN THE SOLUTION VECTOR. */
/* RNORM ON EXIT RNORM CONTAINS THE EUCLIDEAN NORM OF THE */
/* RESIDUAL VECTOR. */
/* W() AN N-ARRAY OF WORKING SPACE. */
/* ON EXIT W() WILL CONTAIN THE DUAL SOLUTION VECTOR. */
/* W WILL SATISFY W(I)=0 FOR ALL I IN SET P */
/* AND W(I)<=0 FOR ALL I IN SET Z */
/* Z() AN M-ARRAY OF WORKING SPACE. */
/* INDX()AN INT WORKING ARRAY OF LENGTH AT LEAST N. */
/* ON EXIT THE CONTENTS OF THIS ARRAY DEFINE THE SETS */
/* P AND Z AS FOLLOWS: */
/* INDX(1) THRU INDX(NSETP) = SET P. */
/* INDX(IZ1) THRU INDX (IZ2) = SET Z. */
/* IZ1=NSETP + 1 = NPP1, IZ2=N. */
/* MODE THIS IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANING: */
/* 1 THE SOLUTION HAS BEEN COMPUTED SUCCESSFULLY. */
/* 2 THE DIMENSIONS OF THE PROBLEM ARE WRONG, */
/* EITHER M <= 0 OR N <= 0. */
/* 3 ITERATION COUNT EXCEEDED, MORE THAN 3*N ITERATIONS. */
/* Parameter adjustments */
--z___start;
--b_start;
--indx_start;
--w_start;
--x_start;
a_dim1 = mda;
a_offset = 1 + a_dim1;
a_start -= a_offset;
bool processing = true;
int goto = 0;
//double temp1,temp2,temp3;
/* Function Body */
/* revised Dieter Kraft, March 1983 */
mode = 2;
if(m <= 0 || n <= 0)
{
goto = 290;
}
else
{
mode = 1;
iter = 0;
itmax = n * 3;
/* STEP ONE (INITIALIZE) */
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
/* L100: */
indx[indx_start+i__] = double(i__);
}
iz1 = 1;
iz2 = n;
nsetp = 0;
npp1 = 1;
x[x_start+1] = 0.0;
//temp1.Set(x,1);
dcopy___(n, x, 0, x, 1,x_start+1,x_start+1);
goto = 110;
}
/* STEP TWO (COMPUTE DUAL VARIABLES) */
/* .....ENTRY LOOP A */
while(processing)
{
switch(goto)
{
case 110:
if(iz1 > iz2 || nsetp >= m)
{
goto = 280;
break;
}
i__1 = iz2;
for(iz = iz1; iz <= i__1; ++iz)
{
j = (int)indx[indx_start+iz];
/* L120: */
i__2 = m - nsetp;
w[w_start+j] = ddot_sl__(i__2, a, 1, b, 1, a_start+(npp1+j*a_dim1),b_start+npp1);
}
/* STEP THREE (TEST DUAL VARIABLES) */
case 130:
wmax = 0.0;
i__2 = iz2;
for(iz = iz1; iz <= i__2; ++iz)
{
j = (int)indx[indx_start+iz];
if(w[w_start+j] <= wmax)
{
continue; //goto L140;
}
wmax = w[w_start+j];
izmax = iz;
//L140:
}
/* .....EXIT LOOP A */
if(wmax <= 0.0)
{
goto = 280;
break;
}
iz = izmax;
j = (int)indx[indx_start+iz];
/* STEP FOUR (TEST INDX J FOR LINEAR DEPENDENCY) */
asave = a[a_start+(npp1 + j * a_dim1)];
i__2 = npp1 + 1;
h12_(c__1, npp1, i__2, m, a, c__1, up, z__,
c__1, c__1, c__0,a_start+(j * a_dim1 + 1),z___start+1);
unorm = dnrm2___(nsetp, a, 1, a_start+(j * a_dim1 + 1));
d__1 = a[a_start+(npp1 + j * a_dim1)];
t = factor * (fabs(d__1));
d__1 = unorm + t;
if(d__1 - unorm <= 0.0)
{
goto = 150;
break;
}
dcopy___(m, b, 1, z__, 1,b_start+1,z___start+1);
i__2 = npp1 + 1;
h12_(c__2, npp1, i__2, m, a, c__1, up,z__, c__1, c__1, c__1,a_start+(j * a_dim1 + 1),z___start+1);
if(z__[z___start+npp1] / a[a_start+(npp1 + j * a_dim1)] > 0.0)
{
goto = 160;
break;
}
case 150:
a[a_start+(npp1 + j * a_dim1)] = asave;
w[w_start+j] = 0.0;
goto = 130;
break;
/* STEP FIVE (ADD COLUMN) */
case 160:
dcopy___(m, z__, 1, b, 1,z___start+1,b_start+1);
indx[indx_start+iz] = indx[indx_start+iz1];
indx[indx_start+iz1] = double(j);
++iz1;
nsetp = npp1;
++npp1;
i__2 = iz2;
for(jz = iz1; jz <= i__2; ++jz)
{
jj = (int)indx[indx_start+jz];
/* L170: */
h12_(c__2, nsetp, npp1, m, a, c__1, up, a, c__1, mda, c__1,a_start+(j * a_dim1 + 1),a_start+(jj *a_dim1 + 1));
}
k = MIN2(npp1,mda);
w[w_start+j]= 0.0;
i__2 = m - nsetp;
dcopy___(i__2,w, 0, a, 1,w_start+j,a_start+(k + j * a_dim1));
/* STEP SIX (SOLVE LEAST SQUARES SUB-PROBLEM) */
/* .....ENTRY LOOP B */
case 180:
for(ip = nsetp; ip >= 1; --ip)
{
if(ip != nsetp)
{
d__1 = -z__[z___start+ip + 1];
daxpy_sl__(ip, d__1, a, 1, z__, 1, a_start+(jj * a_dim1 + 1),z___start+1);
}
jj = (int)indx[indx_start+ip];
/* L200: */
z__[z___start+ip] /= (a[a_start+(ip + jj * a_dim1)]);
}
++iter;
if(iter <= itmax)
{
goto = 220;
break;
}
case 210:
mode = 3;
goto = 280;
break;
/* STEP SEVEN TO TEN (STEP LENGTH ALGORITHM) */
case 220:
alpha = one;
jj = 0;
i__2 = nsetp;
for(ip = 1; ip <= i__2; ++ip)
{
if(z__[z___start+ip] > 0.0)
{
continue; //goto = 230;
}
l = (int)indx[indx_start+ip];
t = -x[x_start+l] / (z__[z___start+ip] - x[x_start+l]);
if(alpha < t)
{
continue; //goto L230;
}
alpha = t;
jj = ip;
}
i__2 = nsetp;
for(ip = 1; ip <= i__2; ++ip)
{
l = (int)indx[indx_start+ip];
/* L240: */
x[x_start+l]=(one - alpha) * x[x_start+l] + alpha * z__[z___start+ip];
}
/* .....EXIT LOOP B */
if(jj == 0)
{
goto = 110;
break;
}
/* STEP ELEVEN (DELETE COLUMN) */
i__ = (int)indx[indx_start+jj];
case 250:
x[x_start+i__]=(0.0);
++jj;
i__2 = nsetp;
for(j = jj; j <= i__2; ++j)
{
ii = (int)indx[indx_start+j];
indx[indx_start+(j - 1)] = (double(ii));
dsrotg_(a[a_start+(j - 1 + ii * a_dim1)],a[a_start+(j + ii * a_dim1)], c__,s);
t = a[a_start+(j - 1 + ii * a_dim1)];
dsrot_(n, a, mda, a, mda, c__,s,a_start+(j - 1 + a_dim1),a_start+(j + a_dim1));
a[a_start+(j - 1 + ii * a_dim1)] = (t);
a[a_start+(j + ii * a_dim1)]=(0.0);
/* L260: */
dsrot_(1, b, 1, b, 1, c__, s,b_start+(j-1),b_start+j);
}
npp1 = nsetp;
--nsetp;
--iz1;
indx[indx_start+iz1]=(double)i__;
if(nsetp <= 0)
{
goto = 210;
break;
}
i__2 = nsetp;
for(jj = 1; jj <= i__2; ++jj)
{
i__ = (int)indx[indx_start+jj];
if(x[x_start+i__] <= 0.0)
{
goto = 250;
break;
}
/* L270: */
}
if(goto == 250)
break;
dcopy___(m, b, 1, z__, 1,b_start+1,z___start+1);
goto = 180;
break;
/* STEP TWELVE (SOLUTION) */
case 280:
k = MIN2(npp1,m);
i__2 = m - nsetp;
rnorm = dnrm2___(i__2, b, 1,b_start+1);
if(npp1 > m)
{
w[w_start+1]=(0.0);
dcopy___(n, w, 0, w, 1,w_start+1,w_start+1);
}
/* END OF SUBROUTINE NNLS */
case 290:
processing = false;
break;
}
}
} /* nnls_ */
//+------------------------------------------------------------------+
//|solves Least Distance Problem |
//+------------------------------------------------------------------+
//+------------------------------------------------------------------+
//| |
//+------------------------------------------------------------------+
void ldp_(double &g[], int &mg, int &m, int &n,
double &h__[], double &x[], double &xnorm[], double &w[],
double &indx[], int &mode, int g_start = 0, int h___start=0, int x_start = 0,int xnorm_start=0,int w_start = 0, int indx_start = 0)
{
/* Initialized data */
double one = 1.;
/* System generated locals */
int g_dim1, g_offset, i__1, i__2;
double d__1;
/* Local variables */
int i__, j, n1, if__, iw, iy, iz;
double fac;
double rnorm;
rnorm = 0;
int iwdual;
/* T */
/* MINIMIZE 1/2 X X SUBJECT TO G * X >= H. */
/* C.L. LAWSON, R.J. HANSON: 'SOLVING LEAST SQUARES PROBLEMS' */
/* PRENTICE HALL, ENGLEWOOD CLIFFS, NEW JERSEY, 1974. */
/* PARAMETER DESCRIPTION: */
/* G(),MG,M,N ON ENTRY G() STORES THE M BY N MATRIX OF */
/* LINEAR INEQUALITY CONSTRAINTS. G() HAS FIRST */
/* DIMENSIONING PARAMETER MG */
/* H() ON ENTRY H() STORES THE M VECTOR H REPRESENTING */
/* THE RIGHT SIDE OF THE INEQUALITY SYSTEM */
/* REMARK: G(),H() WILL NOT BE CHANGED DURING CALCULATIONS BY LDP */
/* X() ON ENTRY X() NEED NOT BE INITIALIZED. */
/* ON EXIT X() STORES THE SOLUTION VECTOR X IF MODE=1. */
/* XNORM ON EXIT XNORM STORES THE EUCLIDIAN NORM OF THE */
/* SOLUTION VECTOR IF COMPUTATION IS SUCCESSFUL */
/* W() W IS A ONE DIMENSIONAL WORKING SPACE, THE LENGTH */
/* OF WHICH SHOULD BE AT LEAST (M+2)*(N+1) + 2*M */
/* ON EXIT W() STORES THE LAGRANGE MULTIPLIERS */
/* ASSOCIATED WITH THE CONSTRAINTS */
/* AT THE SOLUTION OF PROBLEM LDP */
/* INDX() INDX() IS A ONE DIMENSIONAL INT WORKING SPACE */
/* OF LENGTH AT LEAST M */
/* MODE MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING */
/* MEANINGS: */
/* MODE=1: SUCCESSFUL COMPUTATION */
/* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N.LE.0) */
/* 3: ITERATION COUNT EXCEEDED BY NNLS */
/* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
/* Parameter adjustments */
--indx_start;
--h___start;
--x_start;
g_dim1 = mg;
g_offset = 1 + g_dim1;
g_start -= g_offset;
--w_start;
/* Function Body */
mode = 2;
if(n <= 0)
{
return;
}
/* STATE DUAL PROBLEM */
mode = 1;
x[x_start+1] = (0.0);
dcopy___(n, x, 0, x, 1, x_start+1,x_start+1);
xnorm[xnorm_start+0] = 0.0;
if(m == 0)
{
return;
}
iw = 0;
i__1 = m;
for(j = 1; j <= i__1; ++j)
{
i__2 = n;
for(i__ = 1; i__ <= i__2; ++i__)
{
++iw;
/* L10: */
w[w_start+iw] = g[g_start+(j + i__ * g_dim1)];
}
++iw;
/* L20: */
w[w_start+iw] = h__[h___start+j];
}
if__ = iw + 1;
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
++iw;
/* L30: */
w[w_start+iw] = (0.0);
}
w[w_start+iw + 1]=(one);
n1 = n + 1;
iz = iw + 2;
iy = iz + n1;
iwdual = iy + m;
/* SOLVE DUAL PROBLEM */
nnls_(w, n1, n1, m, w, w, rnorm, w, w, indx, mode,w_start+1,w_start+if__,w_start+iy,w_start+iwdual,w_start+iz,indx_start+1);
if(mode != 1)
{
return;
}
mode = 4;
if(rnorm <= 0.0)
{
return;
}
/* COMPUTE SOLUTION OF PRIMAL PROBLEM */
fac = one - ddot_sl__(m, w, 1, h__, 1,w_start+iy,h___start+1);
d__1 = one + fac;
if(d__1 - one <= 0.0)
{
return;
}
mode = 1;
fac = one / fac;
i__1 = n;
for(j = 1; j <= i__1; ++j)
{
/* L40: */
x[x_start+j] = fac * ddot_sl__(m, g, 1, w, 1,g_start+(j*g_dim1+1),w_start+iy);
}
xnorm[xnorm_start+0] = dnrm2___(n, x, 1,x_start+1);
/* COMPUTE LAGRANGE MULTIPLIERS FOR PRIMAL PROBLEM */
w[w_start+1] = (0.0);
dcopy___(m, w, 0, w, 1,w_start+1,w_start+1);
daxpy_sl__(m, fac, w, 1, w, 1,w_start+iy,w_start+1);
/* END OF SUBROUTINE LDP */
return;
} /* ldp_ */
//+------------------------------------------------------------------+
//|solves an inequality-constrained linear least squares problem |
//+------------------------------------------------------------------+
void lsi_(double &e[], double &f[], double &g[],
double &h__[], int &le, int &me, int &lg, int &mg,
int &n, double &x[], double &xnorm[], double &w[], double &
jw[], int &mode, int e_start = 0, int f_start=0, int g_start = 0, int h___start = 0, int x_start = 0, int xnorm_start = 0, int w_start = 0, int jw_start = 0)
{
/* Initialized data */
double epmach = 2.22e-16;
double one = 1.;
/* System generated locals */
int e_dim1, e_offset, g_dim1, g_offset, i__1, i__2, i__3;
double d__1;
/* Local variables */
int i__, j;
double t;
/* FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF */
/* INEQUALITY CONSTRAINED LINEAR LEAST SQUARES PROBLEM: */
/* MIN ||E*X-F|| */
/* X */
/* S.T. G*X >= H */
/* THE ALGORITHM IS BASED ON QR DECOMPOSITION AS DESCRIBED IN */
/* CHAPTER 23.5 OF LAWSON & HANSON: SOLVING LEAST SQUARES PROBLEMS */
/* THE FOLLOWING DIMENSIONS OF THE ARRAYS DEFINING THE PROBLEM */
/* ARE NECESSARY */
/* DIM(E) : FORMAL (LE,N), ACTUAL (ME,N) */
/* DIM(F) : FORMAL (LE ), ACTUAL (ME ) */
/* DIM(G) : FORMAL (LG,N), ACTUAL (MG,N) */
/* DIM(H) : FORMAL (LG ), ACTUAL (MG ) */
/* DIM(X) : N */
/* DIM(W) : (N+1)*(MG+2) + 2*MG */
/* DIM(JW): LG */
/* ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS E, F, G, AND H. */
/* ON RETURN, ALL ARRAYS WILL BE CHANGED BY THE SUBROUTINE. */
/* X STORES THE SOLUTION VECTOR */
/* XNORM STORES THE RESIDUUM OF THE SOLUTION IN EUCLIDIAN NORM */
/* W STORES THE VECTOR OF LAGRANGE MULTIPLIERS IN ITS FIRST */
/* MG ELEMENTS */
/* MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: */
/* MODE=1: SUCCESSFUL COMPUTATION */
/* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) */
/* 3: ITERATION COUNT EXCEEDED BY NNLS */
/* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
/* 5: MATRIX E IS NOT OF FULL RANK */
/* 03.01.1980, DIETER KRAFT: CODED */
/* 20.03.1987, DIETER KRAFT: REVISED TO FORTRAN 77 */
/* Parameter adjustments */
--f_start;
--jw_start;
--h___start;
--x_start;
g_dim1 = lg;
g_offset = 1 + g_dim1;
g_start -= g_offset;
e_dim1 = le;
e_offset = 1 + e_dim1;
e_start -= e_offset;
--w_start;
//double temp1,temp2,temp3,temp4,temp5,temp6,temp7,temp8,temp9;
/* Function Body */
/* QR-FACTORS OF E AND APPLICATION TO F */
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
/* Computing MIN */
i__2 = i__ + 1;
j = MIN2(i__2,n);
i__2 = i__ + 1;
i__3 =n - i__;
h12_(c__1, i__,i__2, me,e,c__1,t,e,c__1, le,i__3,e_start+(i__ * e_dim1 + 1),e_start+(j * e_dim1 + 1));
/* L10: */
i__2 = i__ + 1;
h12_(c__2,i__,i__2, me,e,c__1,t,f,c__1,c__1,c__1, e_start+(i__ * e_dim1 + 1),f_start+1);
}
/* TRANSFORM G AND H TO GET LEAST DISTANCE PROBLEM */
mode = 5;
i__2 =mg;
for(i__ = 1; i__ <= i__2; ++i__)
{
i__1 = n;
for(j = 1; j <= i__1; ++j)
{
d__1 = e[e_start+(j + j * e_dim1)];
if((fabs(d__1)) < epmach)
{
return;
}
/* L20: */
i__3 = j - 1;
g[g_start+i__ + j * g_dim1] = (g[g_start+i__ + j * g_dim1] - ddot_sl__(i__3,g,lg,e, 1,g_start+(i__ + g_dim1),e_start+(j * e_dim1 + 1))) / e[e_start+j + j * e_dim1];
}
/* L30: */
h__[h___start+i__] = h__[h___start+i__] - ddot_sl__(n,g,lg,f, 1,g_start+(i__ + g_dim1),f_start+1);
}
/* SOLVE LEAST DISTANCE PROBLEM */
ldp_(g, lg, mg, n,h__,x, xnorm,w,jw, mode,g_start+g_offset,h___start+1,x_start+1,xnorm_start+0,w_start+1,jw_start+1);
if(mode != 1)
{
return;
}
/* SOLUTION OF ORIGINAL PROBLEM */
daxpy_sl__(n,one,f, 1,x, 1,f_start+1,x_start+1);
for(i__ =n; i__ >= 1; --i__)
{
/* Computing MIN */
i__2 = i__ + 1;
j = MIN2(i__2,n);
/* L40: */
i__2 =n - i__;
x[x_start+i__] = (x[x_start+i__] - ddot_sl__(i__2,e,le,x, 1,e_start+(i__ + j * e_dim1),x_start+j))/ e[e_start+i__ + i__ * e_dim1];
}
/* Computing MIN */
i__2 =n + 1;
j = MIN2(i__2,me);
i__2 =me -n;
t = dnrm2___(i__2,f, 1,f_start+j);
xnorm[xnorm_start+0] = sqrt(xnorm[xnorm_start+0] * xnorm[xnorm_start+0] + t * t);
/* END OF SUBROUTINE LSI */
return;
} /* lsi_ */
//+------------------------------------------------------------------+
//|rank-deficient least-squares solver based on QR factorization |
//+------------------------------------------------------------------+
void hfti_(double &a[], int &mda, int &m, int &
n, double &b[], int &mdb, int &nb, double &tau, int
&krank, double &rnorm[], double &h__[], double &g[], double &
ip[],int a_start = 0, int b_start = 0, int rnorm_start = 0, int h___start = 0, int g_start = 0, int ip_start = 0)
{
/* Initialized data */
double factor = .001;
/* System generated locals */
int a_dim1, a_offset, b_dim1, b_offset, i__1, i__2, i__3;
double d__1;
/* Local variables */
int i__, j = 0, k, l;
int jb = 0, kp1 = 0;
double tmp, hmax = 0;
int lmax = 0, ldiag;
/* RANK-DEFICIENT LEAST SQUARES ALGORITHM AS DESCRIBED IN: */
/* C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12 */
/* TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974 */
/* A(*,*),MDA,M,N THE ARRAY A INITIALLY CONTAINS THE M x N MATRIX A */
/* OF THE LEAST SQUARES PROBLEM AX = B. */
/* THE FIRST DIMENSIONING PARAMETER MDA MUST SATISFY */
/* MDA >= M. EITHER M >= N OR M < N IS PERMITTED. */
/* THERE IS NO RESTRICTION ON THE RANK OF A. */
/* THE MATRIX A WILL BE MODIFIED BY THE SUBROUTINE. */
/* B(*,*),MDB,NB IF NB = 0 THE SUBROUTINE WILL MAKE NO REFERENCE */
/* TO THE ARRAY B. IF NB > 0 THE ARRAY B() MUST */
/* INITIALLY CONTAIN THE M x NB MATRIX B OF THE */
/* THE LEAST SQUARES PROBLEM AX = B AND ON RETURN */
/* THE ARRAY B() WILL CONTAIN THE N x NB SOLUTION X. */
/* IF NB>1 THE ARRAY B() MUST BE DOUBLE SUBSCRIPTED */
/* WITH FIRST DIMENSIONING PARAMETER MDB>=MAX(M,N), */
/* IF NB=1 THE ARRAY B() MAY BE EITHER SINGLE OR */
/* DOUBLE SUBSCRIPTED. */
/* TAU ABSOLUTE TOLERANCE PARAMETER FOR PSEUDORANK */
/* DETERMINATION, PROVIDED BY THE USER. */
/* KRANK PSEUDORANK OF A, SET BY THE SUBROUTINE. */
/* RNORM ON EXIT, RNORM(J) WILL CONTAIN THE EUCLIDIAN */
/* NORM OF THE RESIDUAL VECTOR FOR THE PROBLEM */
/* DEFINED BY THE J-TH COLUMN VECTOR OF THE ARRAY B. */
/* H(), G() ARRAYS OF WORKING SPACE OF LENGTH >= N. */
/* IP() INT ARRAY OF WORKING SPACE OF LENGTH >= N */
/* RECORDING PERMUTATION INDICES OF COLUMN VECTORS */
/* Parameter adjustments */
--ip_start;
--g_start;
--h___start;
a_dim1 = mda;
a_offset = 1 + a_dim1;
a_start -= a_offset;
--rnorm_start;
b_dim1 = mdb;
b_offset = 1 + b_dim1;
b_start -= b_offset;
bool processing = true;
int goto = 0;
/* Function Body */
k = 0;
ldiag = MIN2(m,n);
if(ldiag <= 0)
{
goto = 270;
}
else
{
/* COMPUTE LMAX */
i__1 = ldiag;
bool again = false;
for(j = 1; j <= i__1; ++j)
{
if(j == 1)
goto = 20;
else
{
lmax = j;
i__2 = n;
for(l = j; l <= i__2; ++l)
{
/* Computing 2nd power */
d__1 = a[a_start+j - 1 + l * a_dim1];
h__[h___start+l] = (h__[h___start+l] - d__1 * d__1);
/* L10: */
if(h__[h___start+l] > h__[h___start+lmax])
{
lmax = l;
}
}
d__1 = hmax + factor * h__[h___start+lmax];
if(d__1 - hmax > 0.0)
goto = 50;
else
goto = 20;
}
do
{
again = false;
switch(goto)
{
case 20:
lmax = j;
i__2 = n;
for(l = j; l <= i__2; ++l)
{
h__[h___start+l] = (0.0);
i__3 = m;
for(i__ = j; i__ <= i__3; ++i__)
{
/* L30: */
/* Computing 2nd power */
d__1 = a[a_start+i__ + l * a_dim1];
h__[h___start+l] = (h__[h___start+l] + d__1 * d__1);
}
/* L40: */
if(h__[h___start+l] > h__[h___start+lmax])
{
lmax = l;
}
}
hmax = h__[h___start+lmax];
/* COLUMN INTERCHANGES IF NEEDED */
case 50:
ip[ip_start+j] = double(lmax);
if(ip[ip_start+j] == j)
{
goto = 70;
again = true;
break;
}
i__2 = m;
for(i__ = 1; i__ <= i__2; ++i__)
{
tmp = a[a_start+i__ + j * a_dim1];
a[a_start+i__ + j * a_dim1] = (a[a_start+i__ + lmax * a_dim1]);
/* L60: */
a[a_start+i__ + lmax * a_dim1] = (tmp);
}
h__[h___start+lmax] = h__[h___start+j];
/* J-TH TRANSFORMATION AND APPLICATION TO A AND B */
case 70:
/* Computing MIN */
i__2 = j + 1;
i__ = MIN2(i__2,n);
i__2 = j + 1;
i__3 = n - j;
h12_(c__1, j, i__2, m, a, c__1,h__[h___start+j], a, c__1, mda, i__3,a_start+(j * a_dim1 + 1),a_start+(i__ * a_dim1 + 1));
/* L80: */
i__2 = j + 1;
h12_(c__2, j, i__2, m, a, c__1, h__[h___start+j], b, c__1, mdb, nb,a_start+(j * a_dim1 + 1),b_start+b_offset);
}
}
while(again);
}
/* DETERMINE PSEUDORANK */
i__2 = ldiag;
for(j = 1; j <= i__2; ++j)
{
/* L90: */
d__1 = a[a_start+j + j * a_dim1];
if(fabs(d__1) <= tau)
{
goto = 100;
break;
}
}
k = ldiag;
goto = 110;
}
while(processing)
{
switch(goto)
{
case 100:
k = j - 1;
case 110:
kp1 = k + 1;
/* NORM OF RESIDUALS */
i__2 = nb;
for(jb = 1; jb <= i__2; ++jb)
{
/* L130: */
i__1 = m - k;
rnorm[rnorm_start+jb] = dnrm2___(i__1, b, 1,b_start+(kp1 + jb * b_dim1));
}
if(k > 0)
{
goto = 160;
break;
}
i__1 = nb;
for(jb = 1; jb <= i__1; ++jb)
{
i__2 = n;
for(i__ = 1; i__ <= i__2; ++i__)
{
/* L150: */
b[b_start+i__ + jb * b_dim1] = (0.0);
}
}
goto = 270;
break;
case 160:
if(k == n)
{
goto = 180;
break;
}
/* HOUSEHOLDER DECOMPOSITION OF FIRST K ROWS */
for(i__ = k; i__ >= 1; --i__)
{
/* L170: */
i__2 = i__ - 1;
h12_(c__1, i__, kp1, n, a, mda, g[g_start+i__], a, mda, c__1, i__2,a_start+(i__ + a_dim1),a_start+a_offset);
}
case 180:
i__2 = nb;
for(jb = 1; jb <= i__2; ++jb)
{
/* SOLVE K*K TRIANGULAR SYSTEM */
for(i__ = k; i__ >= 1; --i__)
{
/* Computing MIN */
i__1 = i__ + 1;
j = MIN2(i__1,n);
/* L210: */
i__1 = k - i__;
b[b_start+i__ + jb * b_dim1] = (b[b_start+i__ + jb * b_dim1] - ddot_sl__(i__1, a, mda, b, 1,a_start+(i__ + j * a_dim1),b_start+(j + jb * b_dim1))) /a[a_start+i__ + i__ * a_dim1];
}
/* COMPLETE SOLUTION VECTOR */
if(k != n)
{
i__1 = n;
for(j = kp1; j <= i__1; ++j)
{
/* L220: */
b[b_start+j + jb * b_dim1] = (0.0);
}
i__1 = k;
for(i__ = 1; i__ <= i__1; ++i__)
{
/* L230: */
h12_(c__2, i__, kp1, n, a, mda, g[g_start+i__], b, c__1, mdb, c__1,a_start+(i__ + a_dim1),b_start+(jb * b_dim1 + 1));
}
}
/* REORDER SOLUTION ACCORDING TO PREVIOUS COLUMN INTERCHANGES */
//L240:
for(j = ldiag; j >= 1; --j)
{
if(ip[ip_start+j] == j)
{
continue;
}
l = (int)ip[ip_start+j];
tmp = b[b_start+l + jb * b_dim1];
b[b_start+l + jb * b_dim1] = b[b_start+j + jb * b_dim1];
b[b_start+j + jb * b_dim1] = (tmp);
}
}
case 270:
krank = k;
processing = false;
break;
}
}
} /* hfti_ */
//+------------------------------------------------------------------+
//|general constrained least-squares solver |
//+------------------------------------------------------------------+
void lsei_(double& c__[], double& d__[], double& e[],
double& f[], double& g[], double& h__[], int &lc, int &
mc, int &le, int &me, int &lg, int &mg, int &n,
double& x[], double& xnrm[], double& w[], double& jw[], int &
mode, int c___start = 0, int d_start = 0, int e_start = 0, int f_start= 0,
int g_start = 0, int h___start = 0, int x_start = 0, int xnrm_start = 0, int w_start=0,
int jw_start = 0)
{
/* Initialized data */
double epmach = 2.22e-16;
/* System generated locals */
int c_dim1, c_offset, e_dim1, e_offset, g_dim1, g_offset, i__1, i__2,
i__3;
double d__1;
/* Local variables */
int i__, j, k, l;
double t;
int ie, if__, ig, iw, mc1;
int krank;
/* FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF */
/* EQUALITY & INEQUALITY CONSTRAINED LEAST SQUARES PROBLEM LSEI : */
/* MIN ||E*X - F|| */
/* X */
/* S.T. C*X = D, */
/* G*X >= H. */
/* USING QR DECOMPOSITION & ORTHOGONAL BASIS OF NULLSPACE OF C */
/* CHAPTER 23.6 OF LAWSON & HANSON: SOLVING LEAST SQUARES PROBLEMS. */
/* THE FOLLOWING DIMENSIONS OF THE ARRAYS DEFINING THE PROBLEM */
/* ARE NECESSARY */
/* DIM(E) : FORMAL (LE,N), ACTUAL (ME,N) */
/* DIM(F) : FORMAL (LE ), ACTUAL (ME ) */
/* DIM(C) : FORMAL (LC,N), ACTUAL (MC,N) */
/* DIM(D) : FORMAL (LC ), ACTUAL (MC ) */
/* DIM(G) : FORMAL (LG,N), ACTUAL (MG,N) */
/* DIM(H) : FORMAL (LG ), ACTUAL (MG ) */
/* DIM(X) : FORMAL (N ), ACTUAL (N ) */
/* DIM(W) : 2*MC+ME+(ME+MG)*(N-MC) for LSEI */
/* +(N-MC+1)*(MG+2)+2*MG for LSI */
/* DIM(JW): MAX(MG,L) */
/* ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS C, D, E, F, G, AND H. */
/* ON RETURN, ALL ARRAYS WILL BE CHANGED BY THE SUBROUTINE. */
/* X STORES THE SOLUTION VECTOR */
/* XNORM STORES THE RESIDUUM OF THE SOLUTION IN EUCLIDIAN NORM */
/* W STORES THE VECTOR OF LAGRANGE MULTIPLIERS IN ITS FIRST */
/* MC+MG ELEMENTS */
/* MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: */
/* MODE=1: SUCCESSFUL COMPUTATION */
/* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) */
/* 3: ITERATION COUNT EXCEEDED BY NNLS */
/* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
/* 5: MATRIX E IS NOT OF FULL RANK */
/* 6: MATRIX C IS NOT OF FULL RANK */
/* 7: RANK DEFECT IN HFTI */
/* 18.5.1981, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN */
/* 20.3.1987, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN */
/* Parameter adjustments */
--d_start;
--f_start;
--h___start;
--x_start;
g_dim1 = lg;
g_offset = 1 + g_dim1;
g_start -= g_offset;
e_dim1 = le;
e_offset = 1 + e_dim1;
e_start -= e_offset;
c_dim1 = lc;
c_offset = 1 + c_dim1;
c___start -= c_offset;
--w_start;
--jw_start;
//double temp1,temp2,temp3,temp4,temp5,temp6,temp7,temp8, temp9;
bool processing = true;
int goto = 0;
/* Function Body */
mode = 2;
if(mc > n)
{
return;//goto = 75;
}
l = n - mc;
mc1 = mc + 1;
iw = (l + 1) * (mg + 2) + (mg << 1) + mc;
ie = iw + mc + 1;
if__ = ie + me * l;
ig = if__ + me;
/* TRIANGULARIZE C AND APPLY FACTORS TO E AND G */
i__1 = mc;
for(i__ = 1; i__ <= i__1; ++i__)
{
/* Computing MIN */
i__2 = i__ + 1;
j = MIN2(i__2,lc);
i__2 = i__ + 1;
i__3 = mc - i__;
h12_(c__1, i__, i__2, n, c__, lc, w[w_start+iw+i__], c__, lc, c__1, i__3,c___start+(i__ + c_dim1),c___start+(j + c_dim1));
i__2 = i__ + 1;
h12_(c__2, i__, i__2, n, c__, lc, w[w_start+iw+i__], e, le, c__1, me,c___start+(i__ + c_dim1),e_start+e_offset);
/* L10: */
i__2 = i__ + 1;
h12_(c__2, i__, i__2, n, c__, lc, w[w_start+iw+i__], g, lg, c__1, mg,c___start+(i__ + c_dim1),g_start+g_offset);
}
/* SOLVE C*X=D AND MODIFY F */
mode = 6;
i__2 = mc;
for(i__ = 1; i__ <= i__2; ++i__)
{
d__1 = c__[c___start+i__ + i__ * c_dim1];
if(fabs(d__1) < epmach)
{
return; //goto = 75;
}
i__1 = i__ - 1;
x[x_start+i__] = (d__[d_start+i__] - ddot_sl__(i__1, c__, lc,x, 1,c___start+(i__ + c_dim1),x_start+1))/ c__[c___start+(i__ + i__ * c_dim1)];
/* L15: */
}
mode = 1;
w[w_start+mc1] = (0.0);
i__2 = mg; /* BUGFIX for *mc == *n: changed from *mg - *mc, SGJ 2010 */
dcopy___(i__2, w, 0, w, 1,w_start+mc1,w_start+mc1);
if(mc == n)
{
goto = 50;
}
else
{
i__2 = me;
for(i__ = 1; i__ <= i__2; ++i__)
{
/* L20: */
w[w_start+if__ - 1 + i__] = (f[f_start+i__] - ddot_sl__(mc, e, le, x, 1,e_start+(i__ + e_dim1),x_start+1));
}
/* STORE TRANSFORMED E & G */
i__2 = me;
for(i__ = 1; i__ <= i__2; ++i__)
{
/* L25: */
dcopy___(l, e, le, w, me,e_start+(i__ + mc1 * e_dim1),w_start+(ie - 1 + i__));
}
i__2 = mg;
for(i__ = 1; i__ <= i__2; ++i__)
{
/* L30: */
dcopy___(l, g, lg, w, mg,g_start+(i__ + mc1 * g_dim1),w_start+(ig - 1 + i__));
}
if(mg > 0)
{
goto = 40;
}
/* SOLVE LS WITHOUT INEQUALITY CONSTRAINTS */
else
{
mode = 7;
k = MAX2(le,n);
t = sqrt(epmach);
hfti_(w, me, me, l, w, k, c__1, t, krank, xnrm, w, w, jw,w_start+ie,w_start+if__,xnrm_start+0,w_start+1,w_start+l+1,jw_start+1);
dcopy___(l, w, 1, x, 1,w_start+if__,x_start+mc1);
if(krank != l)
{
return; //goto L75;
}
mode = 1;
goto = 50;
}
}
while(processing)
{
switch(goto)
{
/* MODIFY H AND SOLVE INEQUALITY CONSTRAINED LS PROBLEM */
case 40:
i__2 = mg;
for(i__ = 1; i__ <= i__2; ++i__)
{
/* L45: */
h__[h___start+i__] = h__[h___start+i__] - ddot_sl__(mc, g, lg, x, 1,g_start+(i__ + g_dim1),x_start+1);
}
lsi_(w, w, w, h__, me, me, mg, mg, l, x, xnrm,w, jw, mode,w_start+ie,w_start+if__,w_start+ig,h___start+1,x_start+mc1,0,w_start+mc1,jw_start+1);
if(mc == 0)
{
return; //goto L75;
}
t = dnrm2___(mc, x, 1,x_start+1);
xnrm[xnrm_start+0] = sqrt(xnrm[xnrm_start+0] * xnrm[xnrm_start+0] + t * t);
if(mode != 1)
{
return;
}
/* SOLUTION OF ORIGINAL PROBLEM AND LAGRANGE MULTIPLIERS */
case 50:
i__2 = me;
for(i__ = 1; i__ <= i__2; ++i__)
{
/* L55: */
f[f_start+i__] = ddot_sl__(n, e, le, x, 1,e_start+(i__ + e_dim1),x_start+1) - f[f_start+i__];
}
i__2 = mc;
for(i__ = 1; i__ <= i__2; ++i__)
{
/* L60: */
d__[d_start+i__] = (ddot_sl__(me, e, 1, f, 1,e_start+(i__ * e_dim1 + 1),f_start+1) - ddot_sl__(mg, g, 1, w, 1,g_start+(i__ * g_dim1 + 1),w_start+mc1));
}
for(i__ = mc; i__ >= 1; --i__)
{
/* L65: */
i__2 = i__ + 1;
h12_(c__2, i__, i__2, n, c__, lc,w[w_start+iw+i__], x, c__1, c__1, c__1,c___start+(i__ + c_dim1),x_start+1);
}
for(i__ = mc; i__ >= 1; --i__)
{
/* Computing MIN */
i__2 = i__ + 1;
j = MIN2(i__2,lc);
i__2 = mc - i__;
w[w_start+i__] = (d__[d_start+i__] - ddot_sl__(i__2, c__, 1, w, 1,c___start+(j + i__ * c_dim1),w_start+j)) / c__[c___start+i__ + i__ * c_dim1];
/* L70: */
}
/* END OF SUBROUTINE LSEI */
case 75:
//processing = false;
return;
}
}
} /* lsei_ */
//+----------------------------------------------------------------------------+
//|the central constrained least-squares solver used inside the SLSQP algorithm|
//+----------------------------------------------------------------------------+
void lsq_(int &m, int &meq, int &n, int &nl,
int &la, double &l[], double &g[], double &a[], double &
b[], double &xl[], double &xu[], double &x[], double &y[],
double &w[], double &jw[], int &mode,int l_start = 0, int g_start = 0,
int a_start = 0, int b_start = 0, int xl_start = 0, int xu_start = 0, int x_start = 0,
int y_start = 0, int w_start = 0, int jw_start = 0)
{
/* Initialized data */
double one = 1.;
/* System generated locals */
int a_dim1, a_offset, i__1, i__2;
double d__1;
/* Local variables */
int i__, i1, i2, i3, i4, m1, n1, n2, n3, ic, id, ie, if__, ig, ih, il,
im, ip, iu, iw;
double diag;
int mineq;
double xnorm[1];
xnorm[0] = 0.;
/* MINIMIZE with respect to X */
/* ||E*X - F|| */
/* 1/2 T */
/* WITH UPPER TRIANGULAR MATRIX E = +D *L , */
/* -1/2 -1 */
/* AND VECTOR F = -D *L *G, */
/* WHERE THE UNIT LOWER TRIDIANGULAR MATRIX L IS STORED COLUMNWISE */
/* DENSE IN THE N*(N+1)/2 ARRAY L WITH VECTOR D STORED IN ITS */
/* 'DIAGONAL' THUS SUBSTITUTING THE ONE-ELEMENTS OF L */
/* SUBJECT TO */
/* A(J)*X - B(J) = 0 , J=1,...,MEQ, */
/* A(J)*X - B(J) >=0, J=MEQ+1,...,M, */
/* XL(I) <= X(I) <= XU(I), I=1,...,N, */
/* ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS L, G, A, B, XL, XU. */
/* WITH DIMENSIONS: L(N*(N+1)/2), G(N), A(LA,N), B(M), XL(N), XU(N) */
/* THE WORKING ARRAY W MUST HAVE AT LEAST THE FOLLOWING DIMENSION: */
/* DIM(W) = (3*N+M)*(N+1) for LSQ */
/* +(N-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI */
/* +(N+MINEQ)*(N-MEQ) + 2*MEQ + N for LSEI */
/* with MINEQ = M - MEQ + 2*N */
/* ON RETURN, NO ARRAY WILL BE CHANGED BY THE SUBROUTINE. */
/* X STORES THE N-DIMENSIONAL SOLUTION VECTOR */
/* Y STORES THE VECTOR OF LAGRANGE MULTIPLIERS OF DIMENSION */
/* M+N+N (CONSTRAINTS+LOWER+UPPER BOUNDS) */
/* MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: */
/* MODE=1: SUCCESSFUL COMPUTATION */
/* 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) */
/* 3: ITERATION COUNT EXCEEDED BY NNLS */
/* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE */
/* 5: MATRIX E IS NOT OF FULL RANK */
/* 6: MATRIX C IS NOT OF FULL RANK */
/* 7: RANK DEFECT IN HFTI */
/* coded Dieter Kraft, april 1987 */
/* revised march 1989 */
/* Parameter adjustments */
--y_start;
--x_start;
--xu_start;
--xl_start;
--g_start;
--l_start;
--b_start;
a_dim1 = la;
a_offset = 1 + a_dim1;
a_start -= a_offset;
--w_start;
--jw_start;
/* Function Body */
n1 = n + 1;
mineq = m - meq;
m1 = mineq + n + n;
/* determine whether to solve problem */
/* with inconsistent linerarization (n2=1) */
/* or not (n2=0) */
n2 = n1 * n / 2 + 1;
if(n2 == nl)
{
n2 = 0;
}
else
{
n2 = 1;
}
n3 = n - n2;
/* RECOVER MATRIX E AND VECTOR F FROM L AND G */
i2 = 1;
i3 = 1;
i4 = 1;
ie = 1;
if__ = n * n + 1;
i__1 = n3;
for(i__ = 1; i__ <= i__1; ++i__)
{
i1 = n1 - i__;
diag = sqrt(l[l_start+i2]);
w[w_start+i3] = (0.0);
dcopy___(i1, w, 0, w, 1,w_start+i3,w_start+i3);
i__2 = i1 - n2;
dcopy___(i__2, l, 1, w, n,l_start+i2,w_start+i3);
i__2 = i1 - n2;
dscal_sl__(i__2, diag, w, n,w_start+i3);
w[w_start+i3] = diag;
i__2 = i__ - 1;
w[w_start+if__ - 1 + i__] = ((g[g_start+i__] - ddot_sl__(i__2, w, 1, w, 1,w_start+i4,w_start+if__)) / diag);
i2 = i2 + i1 - n2;
i3 += n1;
i4 += n;
/* L10: */
}
if(n2 == 1)
{
w[w_start+i3] = (l[l_start+nl]);
w[w_start+i4] = (0.0);
dcopy___(n3, w, 0, w, 1,w_start+i4,w_start+i4);
w[w_start+if__ - 1 + n] = (0.0);
}
d__1 = -one;
dscal_sl__(n, d__1, w, 1,w_start+if__);
ic = if__ + n;
id = ic + meq * n;
if(meq > 0)
{
/* RECOVER MATRIX C FROM UPPER PART OF A */
i__1 = meq;
for(i__ = 1; i__ <= i__1; ++i__)
{
dcopy___(n, a, la, w, meq,a_start+(i__ + a_dim1),w_start+(ic - 1 + i__));
/* L20: */
}
/* RECOVER VECTOR D FROM UPPER PART OF B */
dcopy___(meq, b, 1, w, 1,b_start+1,w_start+id);
d__1 = -one;
dscal_sl__(meq, d__1, w, 1,w_start+id);
}
ig = id + meq;
if(mineq > 0)
{
/* RECOVER MATRIX G FROM LOWER PART OF A */
i__1 = mineq;
for(i__ = 1; i__ <= i__1; ++i__)
{
dcopy___(n, a, la, w, m1,a_start+(meq + i__ + a_dim1),w_start+(ig - 1 + i__));
/* L30: */
}
}
/* AUGMENT MATRIX G BY +I AND -I */
ip = ig + mineq;
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
w[w_start+(ip - 1 + i__)] = (0.0);
dcopy___(n, w, 0, w, m1,w_start+(ip - 1 + i__),w_start+(ip - 1 + i__));
/* L40: */
}
i__1 = m1 + 1;
/* SGJ, 2010: skip constraints for infinite bounds */
for(i__ = 1; i__ <= n; ++i__)
if(!slsqp_isinf(xl[xl_start+i__]))
w[w_start+(ip - i__1) + i__ * i__1] = (1.0);
/* Old code: w[w_start+ip] = one; dcopy___(n, &w[w_start+ip], 0, &w[w_start+ip], i__1); */
im = ip + n;
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
w[w_start+im - 1 + i__] = (0.0);
dcopy___(n, w, 0, w, m1,w_start+(im-1+i__),w_start+(im - 1 + i__));
/* L50: */
}
i__1 = m1 + 1;
/* SGJ, 2010: skip constraints for infinite bounds */
for(i__ = 1; i__ <= n; ++i__)
if(!slsqp_isinf(xu[xu_start+i__]))
w[w_start+((im - i__1) + i__ * i__1)] = (-1.0);
/* Old code: w[w_start+im] = -one; dcopy___(n, &w[w_start+im], 0, &w[w_start+im], i__1); */
ih = ig + m1 * n;
if(mineq > 0)
{
/* RECOVER H FROM LOWER PART OF B */
dcopy___(mineq, b, 1, w, 1,b_start+meq+1,w_start+ih);
d__1 = -one;
dscal_sl__(mineq, d__1, w, 1,w_start+ih);
}
/* AUGMENT VECTOR H BY XL AND XU */
il = ih + mineq;
iu = il + n;
/* SGJ, 2010: skip constraints for infinite bounds */
for(i__ = 1; i__ <= n; ++i__)
{
w[w_start+((il-1) + i__)] = slsqp_isinf(xl[xl_start+i__]) ? 0 : xl[xl_start+i__];
w[w_start+((iu-1) + i__)] = slsqp_isinf(xu[xu_start+i__]) ? 0 : -xu[xu_start+i__];
}
/* Old code: dcopy___(n, &xl[xl_start[l_start+1], 1, &w[w_start+il], 1);
dcopy___(n, &xu[xu_start+1], 1, &w[w_start+iu], 1);
d__1 = -one; dscal_sl__(n, &d__1, &w[w_start+iu], 1); */
iw = iu + n;
i__1 = MAX2(1,meq);
lsei_(w, w, w, w, w, w, i__1, meq, n, n,m1, m1, n, x, xnorm, w, jw, mode,w_start+ic,w_start+id,w_start+ie,w_start+if__,w_start+ig,w_start+ih,x_start+1,0,w_start+iw,jw_start+1);
if(mode == 1)
{
/* restore Lagrange multipliers */
dcopy___(m, w, 1, y, 1,w_start+iw,y_start+1);
dcopy___(n3, w, 1, y, 1,w_start+iw+m,y_start+m+1);
dcopy___(n3, w, 1, y, 1,w_start+iw+m+n,y_start+n3+1);
/* SGJ, 2010: make sure bound constraints are satisfied, since
roundoff error sometimes causes slight violations and
NLopt guarantees that bounds are strictly obeyed */
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
if(x[x_start+i__] < xl[xl_start+i__])
x[x_start+i__] = (xl[xl_start+i__]);
else
if(x[x_start+i__] > xu[xu_start+i__])
x[x_start+i__] = (xu[xu_start+i__]);
}
}
/* END OF SUBROUTINE LSQ */
} /* lsq_ */
//+------------------------------------------------------------------------------------------------+
//|performs a rank-one update (or downdate) of an existing LDLᵀ factorization of a symmetric matrix|
//+------------------------------------------------------------------------------------------------+
void ldl_(int &n, double &a[], double &z__[],double &sigma, double &w[], int a_start = 0, int z___start = 0, int w_start = 0)
{
/* Initialized data */
double one = 1.;
double four = 4.;
double epmach = 2.22e-16;
/* System generated locals */
int i__1, i__2;
/* Local variables */
int i__, j;
double t, u, v = 0;
int ij;
double tp = 0, beta = 0, gamma_ = 0, alpha = 0, delta = 0;
/* LDL LDL' - RANK-ONE - UPDATE */
/* PURPOSE: */
/* UPDATES THE LDL' FACTORS OF MATRIX A BY RANK-ONE MATRIX */
/* SIGMA*Z*Z' */
/* INPUT ARGUMENTS: (* MEANS PARAMETERS ARE CHANGED DURING EXECUTION) */
/* N : ORDER OF THE COEFFICIENT MATRIX A */
/* * A : POSITIVE DEFINITE MATRIX OF DIMENSION N; */
/* ONLY THE LOWER TRIANGLE IS USED AND IS STORED COLUMN BY */
/* COLUMN AS ONE DIMENSIONAL ARRAY OF DIMENSION N*(N+1)/2. */
/* * Z : VECTOR OF DIMENSION N OF UPDATING ELEMENTS */
/* SIGMA : SCALAR FACTOR BY WHICH THE MODIFYING DYADE Z*Z' IS */
/* MULTIPLIED */
/* OUTPUT ARGUMENTS: */
/* A : UPDATED LDL' FACTORS */
/* WORKING ARRAY: */
/* W : VECTOR OP DIMENSION N (USED ONLY IF SIGMA .LT. ZERO) */
/* METHOD: */
/* THAT OF FLETCHER AND POWELL AS DESCRIBED IN : */
/* FLETCHER,R.,(1974) ON THE MODIFICATION OF LDL' FACTORIZATION. */
/* POWELL,M.J.D. MATH.COMPUTATION 28, 1067-1078. */
/* IMPLEMENTED BY: */
/* KRAFT,D., DFVLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME */
/* D-8031 OBERPFAFFENHOFEN */
/* STATUS: 15. JANUARY 1980 */
/* SUBROUTINES REQUIRED: NONE */
/* Parameter adjustments */
--w_start;
--z___start;
--a_start;
bool processing = true;
int goto = 0;
/* Function Body */
if(sigma == 0.0)
{
return; //goto = 280;
}
ij = 1;
t = one / sigma;
goto = 220;
if(sigma <= 0.0)
{
/* PREPARE NEGATIVE UPDATE */
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
/* L150: */
w[w_start+i__] = (z__[z___start+i__]);
}
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
v = w[w_start+i__];
t += v * v / a[a_start+ij];
i__2 = n;
for(j = i__ + 1; j <= i__2; ++j)
{
++ij;
/* L160: */
w[w_start+j] = (w[w_start+j] - v * a[a_start+ij]);
}
/* L170: */
++ij;
}
if(t >= 0.0)
{
t = epmach / sigma;
}
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
j = n + 1 - i__;
ij -= i__;
u = w[w_start+j];
w[w_start+j] =(t);
/* L210: */
t -= u * u / a[a_start+ij];
}
}
switch(goto)
{
case 220:
/* HERE UPDATING BEGINS */
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
goto = 220;
v = z__[z___start+i__];
delta = v / a[a_start+ij];
if(sigma < 0.0)
{
tp = w[w_start+i__];
}
else /* if (*sigma > 0.0), since *sigma != 0 from above */
{
tp = t + delta * v;
}
alpha = tp / t;
a[a_start+ij] = (alpha * a[a_start+ij]);
if(i__ == n)
{
return; //goto 280;
}
beta = delta / tp;
if(alpha > four)
{
goto = 240;
//break;
}
if(goto != 240)
{
i__2 = n;
for(j = i__ + 1; j <= i__2; ++j)
{
++ij;
z__[z___start+j] = (z__[z___start+j] - v * a[a_start+ij]);
/* L230: */
a[a_start+ij] = (a[a_start+ij] + beta * z__[z___start+j]);
}
goto = 260;
//break;
}
//case 240:
if(goto != 260)
{
gamma_ = t / tp;
i__2 = n;
for(j = i__ + 1; j <= i__2; ++j)
{
++ij;
u = a[a_start+ij];
a[a_start+ij] = (gamma_ * u + beta * z__[z___start+j]);
/* L250: */
z__[z___start+j] = (z__[z___start+j] - v * u);
}
}
//case 260:
++ij;
/* L270: */
t = tp;
}
case 280:
return;
}
/* END OF LDL */
} /* ldl_ */
//+------------------------------------------------------------------+
//| sign check |
//+------------------------------------------------------------------+
double d_sign(double a, double s) { return s < 0 ? -a : a; }
//+------------------------------------------------------------------------------+
//|one-dimensional derivative-free line search algorithm based on Brent’s method.|
//+------------------------------------------------------------------------------+
double linmin_(int &mode, double &ax, double &bx, double &f, double &tol)
{
/* Initialized data */
double c__ = .381966011;
double eps = 1.5e-8;
/* System generated locals */
double ret_val = 0.0, d__1 = 0.0;
/* Local variables */
double a, b, d__, e, m, p, q, r__, u, v, w, x, fu, fv, fw, fx, tol1,
tol2;
a= b= d__= e= m= p= q= r__= u= v= w= x= fu= fv= fw= fx= tol1=tol2 = 0.0;
/* LINMIN LINESEARCH WITHOUT DERIVATIVES */
/* PURPOSE: */
/* TO FIND THE ARGUMENT LINMIN WHERE THE FUNCTION F TAKES ITS MINIMUM */
/* ON THE INTERVAL AX, BX. */
/* COMBINATION OF GOLDEN SECTION AND SUCCESSIVE QUADRATIC INTERPOLATION. */
/* INPUT ARGUMENTS: (* MEANS PARAMETERS ARE CHANGED DURING EXECUTION) */
/* *MODE SEE OUTPUT ARGUMENTS */
/* AX LEFT ENDPOINT OF INITIAL INTERVAL */
/* BX RIGHT ENDPOINT OF INITIAL INTERVAL */
/* F FUNCTION VALUE AT LINMIN WHICH IS TO BE BROUGHT IN BY */
/* REVERSE COMMUNICATION CONTROLLED BY MODE */
/* TOL DESIRED LENGTH OF INTERVAL OF UNCERTAINTY OF FINAL RESULT */
/* OUTPUT ARGUMENTS: */
/* LINMIN ABSCISSA APPROXIMATING THE POINT WHERE F ATTAINS A MINIMUM */
/* MODE CONTROLS REVERSE COMMUNICATION */
/* MUST BE SET TO 0 INITIALLY, RETURNS WITH INTERMEDIATE */
/* VALUES 1 AND 2 WHICH MUST NOT BE CHANGED BY THE USER, */
/* ENDS WITH CONVERGENCE WITH VALUE 3. */
/* WORKING ARRAY: */
/* NONE */
/* METHOD: */
/* THIS FUNCTION SUBPROGRAM IS A SLIGHTLY MODIFIED VERSION OF THE */
/* ALGOL 60 PROCEDURE LOCALMIN GIVEN IN */
/* R.P. BRENT: ALGORITHMS FOR MINIMIZATION WITHOUT DERIVATIVES, */
/* PRENTICE-HALL (1973). */
/* IMPLEMENTED BY: */
/* KRAFT, D., DFVLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME */
/* D-8031 OBERPFAFFENHOFEN */
/* STATUS: 31. AUGUST 1984 */
/* SUBROUTINES REQUIRED: NONE */
/* EPS = SQUARE - ROOT OF MACHINE PRECISION */
/* C = GOLDEN SECTION RATIO = (3-SQRT(5))/2 */
bool processing = true;
int goto = 0;
switch(mode)
{
case 1:
goto = 10;
break;
case 2:
goto = 55;
break;
default:
break;
}
/* INITIALIZATION */
if(!goto)
{
a = ax;
b = bx;
e = 0.0;
v = a + c__ * (b - a);
w = v;
x = w;
ret_val = x;
mode = 1;
goto = 100;
}
/* MAIN LOOP STARTS HERE */
while(processing)
{
switch(goto)
{
case 10:
fx = f;
fv = fx;
fw = fv;
case 20:
m = (a + b) * .5;
tol1 = eps * fabs(x) + tol;
tol2 = tol1 + tol1;
/* TEST CONVERGENCE */
d__1 = x - m;
if((fabs(d__1)) <= tol2 - (b - a) * .5)
{
goto = 90;
break;
}
r__ = 0.0;
q = r__;
p = q;
if(fabs(e) <= tol1)
{
goto = 30;
break;
}
/* FIT PARABOLA */
r__ = (x - w) * (fx - fv);
q = (x - v) * (fx - fw);
p = (x - v) * q - (x - w) * r__;
q -= r__;
q += q;
if(q > 0.0)
{
p = -p;
}
if(q < 0.0)
{
q = -q;
}
r__ = e;
e = d__;
/* IS PARABOLA ACCEPTABLE */
case 30:
d__1 = q * r__;
if(fabs(p) >= (fabs(d__1)) * .5 || p <= q * (a - x) || p >= q * (b - x))
{
goto = 40;
break;
}
/* PARABOLIC INTERPOLATION STEP */
d__ = p / q;
/* F MUST NOT BE EVALUATED TOO CLOSE TO A OR B */
if(u - a < tol2)
{
d__1 = m - x;
d__ = d_sign(tol1, d__1);
}
if(b - u < tol2)
{
d__1 = m - x;
d__ = d_sign(tol1, d__1);
}
goto = 50;
break;
/* GOLDEN SECTION STEP */
case 40:
if(x >= m)
{
e = a - x;
}
if(x < m)
{
e = b - x;
}
d__ = c__ * e;
/* F MUST NOT BE EVALUATED TOO CLOSE TO X */
case 50:
if(fabs(d__) < tol1)
{
d__ = d_sign(tol1, d__);
}
u = x + d__;
ret_val = u;
mode = 2;
goto = 100;
break;
case 55:
fu = f;
/* UPDATE A, B, V, W, AND X */
if(fu > fx)
{
goto = 60;
break;
}
if(u >= x)
{
a = x;
}
if(u < x)
{
b = x;
}
v = w;
fv = fw;
w = x;
fw = fx;
x = u;
fx = fu;
goto = 85;
break;
case 60:
if(u < x)
{
a = u;
}
if(u >= x)
{
b = u;
}
if(fu <= fw || w == x)
{
goto = 70;
break;
}
if(fu <= fv || v == x || v == w)
{
goto = 80;
break;
}
goto = 85;
break;
case 70:
v = w;
fv = fw;
w = u;
fw = fu;
goto = 85;
break;
case 80:
v = u;
fv = fu;
case 85:
goto = 20;
break;
/* END OF MAIN LOOP */
case 90:
ret_val = x;
mode = 3;
case 100:
processing = false;
break;
}
}
return ret_val;
/* END OF LINMIN */
} /* linmin_ */
//+------------------------------------------------------------------+
//|core optimization routine |
//+------------------------------------------------------------------+
void slsqpb_(int &m, int &meq, int &la, int &
n, double &x[], double &xl[], double &xu[], double &f,
double &c__[], double &g[], double &a[], double &acc,
int &iter, int &mode, double &r__[], double &l[],
double &x0[], double &mu[], double &s[], double &u[],
double &v[], double &w[], double &iw[],
slsqpb_state &state,int x_start = 0, int xl_start = 0, int xu_start = 0,
int c___start = 0, int g_start = 0, int a_start = 0, int r___start = 0, int l_start = 0,
int x0_start = 0, int mu_start = 0, int s_start = 0, int u_start = 0, int v_start = 0,
int w_start = 0, int iw_start = 0)
{
/* Initialized data */
double one = 1.;
double alfmin = .1;
double hun = 100.;
double ten = 10.;
double two = 2.;
slsqpb_state state_copy = state;
/* System generated locals */
int a_dim1, a_offset, i__1, i__2;
double d__1, d__2;
/* Local variables */
int i__, j, k;
/* saved state from one call to the next;
SGJ 2010: save/restore via state parameter, to make re-entrant.
double t, f0, h1, h2, h3, h4;
int n1, n2, n3;
double t0, gs;
double tol;
int line;
double alpha;
int iexact;
int incons, ireset, itermx;
/* NONLINEAR PROGRAMMING BY SOLVING SEQUENTIALLY QUADRATIC PROGRAMS */
/* - L1 - LINE SEARCH, POSITIVE DEFINITE BFGS UPDATE - */
/* BODY SUBROUTINE FOR SLSQP */
/* dim(W) = N1*(N1+1) + MEQ*(N1+1) + MINEQ*(N1+1) for LSQ */
/* +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ */
/* +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI */
/* with MINEQ = M - MEQ + 2*N1 & N1 = N+1 */
/* Parameter adjustments */
--mu_start;
--c___start;
--v_start;
--u_start;
--s_start;
--x0_start;
--l_start;
--r___start;
a_dim1 = la;
a_offset = 1 + a_dim1;
a_start -= a_offset;
--g_start;
--xu_start;
--xl_start;
--x_start;
--w_start;
--iw_start;
double aa = 0.0;
bool processing = true;
int goto = 0;
double ref = 0;
/* Function Body */
if(mode == -1)
{
goto = 260;
}
else
if(mode == 0)
{
goto = 100;
}
else
{
goto = 220;
}
while(processing)
{
switch(goto)
{
case 100:
state_copy.itermx = iter;
if(acc >= 0.0)
{
state_copy.iexact = 0;
}
else
{
state_copy.iexact = 1;
}
acc = fabs(acc);
state_copy.tol = ten * acc;
iter = 0;
state_copy.ireset = 0;
state_copy.n1 = n + 1;
state_copy.n2 = state_copy.n1 * n / 2;
state_copy.n3 = state_copy.n2 + 1;
s[s_start+1] = (0.0);
mu[mu_start+1] = (0.0);
dcopy___(n, s, 0, s, 1,s_start+1,s_start+1);
dcopy___(m, mu, 0, mu, 1,mu_start+1,mu_start+1);
/* RESET BFGS MATRIX */
case 110:
++state_copy.ireset;
if(state_copy.ireset > 5)
{
goto = 255;
break;
}
l[l_start+1] = (0.0);
dcopy___(state_copy.n2,l, 0, l, 1,l_start+1,l_start+1);
j = 1;
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
l[l_start+j] = (one);
j = j + state_copy.n1 - i__;
/* L120: */
}
/* MAIN ITERATION : SEARCH DIRECTION, STEPLENGTH, LDL'-UPDATE */
case 130:
++(iter);
mode = 9;
if(iter > state_copy.itermx && state_copy.itermx > 0) /* SGJ 2010: ignore if state_copy.itermx <= 0 */
{
goto = 330;
break;
}
/* SEARCH DIRECTION AS SOLUTION OF QP - SUBPROBLEM */
dcopy___(n, xl, 1, u, 1,xl_start+1,u_start+1);
dcopy___(n, xu, 1, v, 1,xu_start+1,v_start+1);
d__1 = -one;
daxpy_sl__(n, d__1, x, 1, u, 1,x_start+1,u_start+1);
d__1 = -one;
daxpy_sl__(n, d__1, x, 1, v, 1,x_start+1,v_start+1);
state_copy.h4 = one;
lsq_(m, meq, n, state_copy.n3, la, l, g, a, c__, u, v,s, r__, w, iw, mode,l_start+1,g_start+1,a_start+a_offset,c___start+1,u_start+1,v_start+1,s_start+1,r___start+1,w_start+1,iw_start+1);
//a[a_start+a_offset] = aa;
/* AUGMENTED PROBLEM FOR INCONSISTENT LINEARIZATION */
if(mode == 6)
{
if(n == meq)
{
mode = 4;
}
}
if(mode == 4)
{
i__1 = m;
for(j = 1; j <= i__1; ++j)
{
if(j <= meq)
{
a[a_start+j + state_copy.n1 * a_dim1] = (-c__[c___start+j]);
}
else
{
/* Computing MAX */
d__1 = -c__[c___start+j];
a[a_start+j + state_copy.n1 * a_dim1] = (MAX2(d__1,0.0));
}
/* L140: */
}
s[s_start+1] = (0.0);
dcopy___(n, s, 0, s, 1,s_start+1,s_start+1);
state_copy.h3 = 0.0;
g[g_start+state_copy.n1] = (0.0);
l[l_start+state_copy.n3] = (hun);
s[s_start+state_copy.n1] = (one);
u[u_start+state_copy.n1] = (0.0);
v[v_start+state_copy.n1] = (one);
state_copy.incons = 0;
}
case 150:
if(mode == 4)
{
//double aa = a[a_start+a_offset];
lsq_(m, meq, state_copy.n1, state_copy.n3, la, l, g, a, c__, u, v,s, r__, w, iw, mode,l_start+1,g_start+1,a_start+a_offset,c___start+1,u_start+1,v_start+1,s_start+1,r___start+1,w_start+1,iw_start+1);
//a[a_start+a_offset] = aa;
state_copy.h4 = one - s[s_start+state_copy.n1];
if(mode == 4)
{
l[l_start+state_copy.n3] = ten * l[l_start+state_copy.n3];
++state_copy.incons;
if(state_copy.incons > 5)
{
goto = 330;
break;
}
goto = 150;
break;
}
else
if(mode != 1)
{
goto = 330;
break;
}
}
else
if(mode != 1)
{
goto = 330;
break;
}
/* UPDATE MULTIPLIERS FOR L1-TEST */
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
v[v_start+i__] = (g[g_start+i__] - ddot_sl__(m, a, 1, r__, 1,a_start+(i__*a_dim1 + 1),r___start+1));
/* L160: */
}
state_copy.f0 = f;
dcopy___(n, x, 1, x0, 1,x_start+1,x0_start+1);
state_copy.gs = ddot_sl__(n, g, 1, s, 1,g_start+1,s_start+1);
state_copy.h1 = fabs(state_copy.gs);
state_copy.h2 = 0.0;
i__1 = m;
for(j = 1; j <= i__1; ++j)
{
if(j <= meq)
{
state_copy.h3 = c__[c___start+j];
}
else
{
state_copy.h3 = 0.0;
}
/* Computing MAX */
d__1 = -c__[c___start+j];
state_copy.h2 += MAX2(d__1,state_copy.h3);
d__1 = r__[r___start+j];
state_copy.h3 = (fabs(d__1));
/* Computing MAX */
d__1 = state_copy.h3;
d__2 = (mu[mu_start+j] + state_copy.h3) / two;
mu[mu_start+j] = MAX2(d__1,d__2);
d__1 = c__[c___start+j];
state_copy.h1 += state_copy.h3 * (fabs(d__1));
/* L170: */
}
/* CHECK CONVERGENCE */
mode = 0;
if(state_copy.h1 < acc && state_copy.h2 < acc)
{
goto = 330;
break;
}
state_copy.h1 = 0.0;
i__1 = m;
for(j = 1; j <= i__1; ++j)
{
if(j <= meq)
{
state_copy.h3 = c__[c___start+j];
}
else
{
state_copy.h3 = 0.0;
}
/* Computing MAX */
d__1 = -c__[c___start+j];
state_copy.h1 += mu[mu_start+j] * MAX2(d__1,state_copy.h3);
/* L180: */
}
state_copy.t0 = f + state_copy.h1;
state_copy.h3 = state_copy.gs - state_copy.h1 * state_copy.h4;
mode = 8;
if(state_copy.h3 >= 0.0)
{
goto = 110;
break;
}
/* LINE SEARCH WITH AN L1-TESTFUNCTION */
state_copy.line = 0;
state_copy.alpha = one;
if(state_copy.iexact == 1)
{
goto = 210;
break;
}
/* INEXACT LINESEARCH */
case 190:
++state_copy.line;
state_copy.h3 = state_copy.alpha * state_copy.h3;
dscal_sl__(n, state_copy.alpha, s, 1,s_start+1);
dcopy___(n, x0, 1, x, 1,x0_start+1,x_start+1);
daxpy_sl__(n, one, s, 1, x, 1,s_start+1,x_start+1);
/* SGJ 2010: ensure roundoff doesn'state_copy.t push us past bound constraints */
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
if(x[x_start+i__] < xl[xl_start+i__])
x[x_start+i__] = (xl[xl_start+i__]);
else
if(x[x_start+i__] > xu[xu_start+i__])
x[x_start+i__] = (xu[xu_start+i__]);
}
/* SGJ 2010: optimizing for the common case where the inexact state_copy.line
search succeeds in one step, use special mode = -2 here to
eliminate a a subsequent unnecessary mode = -1 call, at the
expense of extra gradient evaluations when more than one inexact
state_copy.line-search step is required */
mode = state_copy.line == 1 ? -2 : 1;
goto = 330;
break;
case 200:
if(slsqp_isfinite(state_copy.h1))
{
if(state_copy.h1 <= state_copy.h3 / ten || state_copy.line > 10)
{
goto = 240;
break;
}
/* Computing MAX */
d__1 = state_copy.h3 / (two * (state_copy.h3 - state_copy.h1));
state_copy.alpha = MAX2(d__1,alfmin);
}
else
{
state_copy.alpha = MAX2(state_copy.alpha*.5,alfmin);
}
goto = 190;
break;
/* EXACT LINESEARCH */
case 210:
/*if 0
/* SGJ: see comments by linmin_ above: if we want to do an exact linesearch
(which usually we probably don'state_copy.t), we should call NLopt recursively
if(state_copy.line != 3)
{
state_copy.alpha = linmin_(&state_copy.line, &alfmin, &one, &state_copy.t, &state_copy.tol);
dcopy___(n, &x0[x0_start+1], 1, &x[x_start+1], 1);
daxpy_sl__(n, &state_copy.alpha, &s[s_start+1], 1, &x[x_start+1], 1);
*mode = 1;
goto L330;
}
else*/
Print(__FUNCTION__, " ", __LINE__);
mode = 9 /* will yield slsqp_failure */;
return;
//#endif
dscal_sl__(n, state_copy.alpha, s, 1,s_start+1);
goto = 240;
break;
/* CALL FUNCTIONS AT CURRENT X */
case 220:
state_copy.t = f;
i__1 = m;
for(j = 1; j <= i__1; ++j)
{
if(j <= meq)
{
state_copy.h1 = c__[c___start+j];
}
else
{
state_copy.h1 = 0.0;
}
/* Computing MAX */
d__1 = -c__[c___start+j];
state_copy.t += mu[mu_start+j] * MAX2(d__1,state_copy.h1);
/* L230: */
}
state_copy.h1 = state_copy.t - state_copy.t0;
if((state_copy.iexact + 1) == 1)
{
goto = 200;
break;
}
else
if((state_copy.iexact + 1) == 2)
{
goto = 210;
break;
}
/* CHECK CONVERGENCE */
case 240:
state_copy.h3 = 0.0;
i__1 = m;
for(j = 1; j <= i__1; ++j)
{
if(j <= meq)
{
state_copy.h1 = c__[c___start+j];
}
else
{
state_copy.h1 = 0.0;
}
/* Computing MAX */
d__1 = -c__[c___start+j];
state_copy.h3 += MAX2(d__1,state_copy.h1);
/* L250: */
}
d__1 = f - state_copy.f0;
//temps[s_start+0].Set(s,1);
if((fabs(d__1) < acc || dnrm2___(n, s, 1,s_start+1) < acc) && state_copy.h3 < acc)
{
mode = 0;
}
else
{
mode = -1;
}
goto = 330;
break;
/* CHECK relaxed CONVERGENCE in case of positive directional derivative */
case 255:
d__1 = f - state_copy.f0;
//temps[s_start+0].Set(s,1);
if((fabs(d__1) < state_copy.tol || dnrm2___(n, s, 1,s_start+1) < state_copy.tol) && state_copy.h3 < state_copy.tol)
{
mode = 0;
}
else
{
mode = 8;
}
goto = 330;
break;
/* CALL JACOBIAN AT CURRENT X */
/* UPDATE CHOLESKY-FACTORS OF HESSIAN MATRIX BY MODIFIED BFGS FORMULA */
case 260:
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
u[u_start+i__] = (g[g_start+i__] - ddot_sl__(m, a, 1, r__, 1,a_start+(i__*a_dim1+1),r___start+1) - v[v_start+i__]);
/* L270: */
}
/* L'*S */
k = 0;
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
state_copy.h1 = 0.0;
++k;
i__2 = n;
for(j = i__ + 1; j <= i__2; ++j)
{
++k;
state_copy.h1 += l[l_start+k] * s[s_start+j];
/* L280: */
}
v[v_start+i__] = (s[s_start+i__] + state_copy.h1);
/* L290: */
}
/* D*L'*S */
k = 1;
i__1 = n;
for(i__ = 1; i__ <= i__1; ++i__)
{
v[v_start+i__] = (l[l_start+k] * v[v_start+i__]);
k = k + state_copy.n1 - i__;
/* L300: */
}
/* L*D*L'*S */
for(i__ = n; i__ >= 1; --i__)
{
state_copy.h1 = 0.0;
k = i__;
i__1 = i__ - 1;
for(j = 1; j <= i__1; ++j)
{
state_copy.h1 += l[l_start+k] * v[v_start+j];
k = k + n - j;
/* L310: */
}
v[v_start+i__] = (v[v_start+i__] + state_copy.h1);
/* L320: */
}
state_copy.h1 = ddot_sl__(n, s, 1, u, 1,s_start+1,u_start+1);
state_copy.h2 = ddot_sl__(n, s, 1, v, 1,s_start+1,v_start+1);
state_copy.h3 = state_copy.h2 * .2;
if(state_copy.h1 < state_copy.h3)
{
state_copy.h4 = (state_copy.h2 - state_copy.h3) / (state_copy.h2 - state_copy.h1);
state_copy.h1 = state_copy.h3;
dscal_sl__(n, state_copy.h4, u, 1,u_start+1);
d__1 = one - state_copy.h4;
daxpy_sl__(n, d__1, v, 1, u, 1,v_start+1,u_start+1);
}
d__1 = one / state_copy.h1;
ldl_(n, l, u, d__1, v,l_start+1,u_start+1,v_start+1);
d__1 = -one / state_copy.h2;
ldl_(n, l, v, d__1, u,l_start+1,v_start+1,u_start+1);
/* END OF MAIN ITERATION */
goto = 130;
break;
/* END OF SLSQPB */
case 330:
processing = false;
state = state_copy;
break;
}
}
} /* slsqpb_ */
//+------------------------------------------------------------------+
//| slsq optimizer |
//+------------------------------------------------------------------+
void slsqp(int &m, int &meq, int &la, int &n,
double &x[], double &xl[], double &xu[], double &f,
double &c__[], double &g[],double &a[], double &acc,
int &iter, int &mode, double &w[], int &l_w__, double &
jw[], int &l_jw__, slsqpb_state &state, int x_start = 0,
int xl_start = 0, int xu_start = 0, int c___start = 0, int g_start = 0,
int a_start = 0, int w_start = 0, int jw_start = 0)
{
/* System generated locals */
int a_dim1, a_offset, i__1, i__2;
/* Local variables */
int n1, il, im, ir, is, iu, iv, iw, ix, mineq;
/* SLSQP S EQUENTIAL L EAST SQ UARES P ROGRAMMING */
/* TO SOLVE GENERAL NONLINEAR OPTIMIZATION PROBLEMS */
/* *********************************************************************** */
/* * * */
/* * * */
/* * A NONLINEAR PROGRAMMING METHOD WITH * */
/* * QUADRATIC PROGRAMMING SUBPROBLEMS * */
/* * * */
/* * * */
/* * THIS SUBROUTINE SOLVES THE GENERAL NONLINEAR PROGRAMMING PROBLEM * */
/* * * */
/* * MINIMIZE F(X) * */
/* * * */
/* * SUBJECT TO C (X) .EQ. 0 , J = 1,...,MEQ * */
/* * J * */
/* * * */
/* * C (X) .GE. 0 , J = MEQ+1,...,M * */
/* * J * */
/* * * */
/* * XL .LE. X .LE. XU , I = 1,...,N. * */
/* * I I I * */
/* * * */
/* * THE ALGORITHM IMPLEMENTS THE METHOD OF HAN AND POWELL * */
/* * WITH BFGS-UPDATE OF THE B-MATRIX AND L1-TEST FUNCTION * */
/* * WITHIN THE STEPLENGTH ALGORITHM. * */
/* * * */
/* * PARAMETER DESCRIPTION: * */
/* * ( * MEANS THIS PARAMETER WILL BE CHANGED DURING CALCULATION ) * */
/* * * */
/* * M IS THE TOTAL NUMBER OF CONSTRAINTS, M .GE. 0 * */
/* * MEQ IS THE NUMBER OF EQUALITY CONSTRAINTS, MEQ .GE. 0 * */
/* * LA SEE A, LA .GE. MAX(M,1) * */
/* * N IS THE NUMBER OF VARIABLES, N .GE. 1 * */
/* * * X() X() STORES THE CURRENT ITERATE OF THE N VECTOR X * */
/* * ON ENTRY X() MUST BE INITIALIZED. ON EXIT X() * */
/* * STORES THE SOLUTION VECTOR X IF MODE = 0. * */
/* * XL() XL() STORES AN N VECTOR OF LOWER BOUNDS XL TO X. * */
/* * XU() XU() STORES AN N VECTOR OF UPPER BOUNDS XU TO X. * */
/* * F IS THE VALUE OF THE OBJECTIVE FUNCTION. * */
/* * C() C() STORES THE M VECTOR C OF CONSTRAINTS, * */
/* * EQUALITY CONSTRAINTS (IF ANY) FIRST. * */
/* * DIMENSION OF C MUST BE GREATER OR EQUAL LA, * */
/* * which must be GREATER OR EQUAL MAX(1,M). * */
/* * G() G() STORES THE N VECTOR G OF PARTIALS OF THE * */
/* * OBJECTIVE FUNCTION; DIMENSION OF G MUST BE * */
/* * GREATER OR EQUAL N+1. * */
/* * A(),LA,M,N THE LA BY N + 1 ARRAY A() STORES * */
/* * THE M BY N MATRIX A OF CONSTRAINT NORMALS. * */
/* * A() HAS FIRST DIMENSIONING PARAMETER LA, * */
/* * WHICH MUST BE GREATER OR EQUAL MAX(1,M). * */
/* * F,C,G,A MUST ALL BE SET BY THE USER BEFORE EACH CALL. * */
/* * * ACC ABS(ACC) CONTROLS THE FINAL ACCURACY. * */
/* * IF ACC .LT. ZERO AN EXACT LINESEARCH IS PERFORMED,* */
/* * OTHERWISE AN ARMIJO-TYPE LINESEARCH IS USED. * */
/* * * ITER PRESCRIBES THE MAXIMUM NUMBER OF ITERATIONS. * */
/* * ON EXIT ITER INDICATES THE NUMBER OF ITERATIONS. * */
/* * * MODE MODE CONTROLS CALCULATION: * */
/* * REVERSE COMMUNICATION IS USED IN THE SENSE THAT * */
/* * THE PROGRAM IS INITIALIZED BY MODE = 0; THEN IT IS* */
/* * TO BE CALLED REPEATEDLY BY THE USER UNTIL A RETURN* */
/* * WITH MODE .NE. IABS(1) TAKES PLACE. * */
/* * IF MODE = -1 GRADIENTS HAVE TO BE CALCULATED, * */
/* * WHILE WITH MODE = 1 FUNCTIONS HAVE TO BE CALCULATED */
/* * MODE MUST NOT BE CHANGED BETWEEN SUBSEQUENT CALLS * */
/* * OF SQP. * */
/* * EVALUATION MODES: * */
/* * MODE = -2,-1: GRADIENT EVALUATION, (G&A) * */
/* * 0: ON ENTRY: INITIALIZATION, (F,G,C&A) * */
/* * ON EXIT : REQUIRED ACCURACY FOR SOLUTION OBTAINED * */
/* * 1: FUNCTION EVALUATION, (F&C) * */
/* * * */
/* * FAILURE MODES: * */
/* * 2: NUMBER OF EQUALITY CONSTRAINTS LARGER THAN N * */
/* * 3: MORE THAN 3*N ITERATIONS IN LSQ SUBPROBLEM * */
/* * 4: INEQUALITY CONSTRAINTS INCOMPATIBLE * */
/* * 5: SINGULAR MATRIX E IN LSQ SUBPROBLEM * */
/* * 6: SINGULAR MATRIX C IN LSQ SUBPROBLEM * */
/* * 7: RANK-DEFICIENT EQUALITY CONSTRAINT SUBPROBLEM HFTI* */
/* * 8: POSITIVE DIRECTIONAL DERIVATIVE FOR LINESEARCH * */
/* * 9: MORE THAN ITER ITERATIONS IN SQP * */
/* * >=10: WORKING SPACE W OR JW TOO SMALL, * */
/* * W SHOULD BE ENLARGED TO L_W=MODE/1000 * */
/* * JW SHOULD BE ENLARGED TO L_JW=MODE-1000*L_W * */
/* * * W(), L_W W() IS A ONE DIMENSIONAL WORKING SPACE, * */
/* * THE LENGTH L_W OF WHICH SHOULD BE AT LEAST * */
/* * (3*N1+M)*(N1+1) for LSQ * */
/* * +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI * */
/* * +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI * */
/* * + N1*N/2 + 2*M + 3*N + 3*N1 + 1 for SLSQPB * */
/* * with MINEQ = M - MEQ + 2*N1 & N1 = N+1 * */
/* * NOTICE: FOR PROPER DIMENSIONING OF W IT IS RECOMMENDED TO * */
/* * COPY THE FOLLOWING STATEMENTS INTO THE HEAD OF * */
/* * THE CALLING PROGRAM (AND REMOVE THE COMMENT C) * */
/* ####################################################################### */
/* INT LEN_W, LEN_JW, M, N, N1, MEQ, MINEQ */
/* PARAMETER (M=... , MEQ=... , N=... ) */
/* PARAMETER (N1= N+1, MINEQ= M-MEQ+N1+N1) */
/* PARAMETER (LEN_W= */
/* $ (3*N1+M)*(N1+1) */
/* $ +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ */
/* $ +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 */
/* $ +(N+1)*N/2 + 2*M + 3*N + 3*N1 + 1, */
/* $ LEN_JW=MINEQ) */
/* DOUBLE PRECISION W(LEN_W) */
/* INT JW(LEN_JW) */
/* ####################################################################### */
/* * THE FIRST M+N+N*N1/2 ELEMENTS OF W MUST NOT BE * */
/* * CHANGED BETWEEN SUBSEQUENT CALLS OF SLSQP. * */
/* * ON RETURN W(1) ... W(M) CONTAIN THE MULTIPLIERS * */
/* * ASSOCIATED WITH THE GENERAL CONSTRAINTS, WHILE * */
/* * W(M+1) ... W(M+N(N+1)/2) STORE THE CHOLESKY FACTOR* */
/* * L*D*L(T) OF THE APPROXIMATE HESSIAN OF THE * */
/* * LAGRANGIAN COLUMNWISE DENSE AS LOWER TRIANGULAR * */
/* * UNIT MATRIX L WITH D IN ITS 'DIAGONAL' and * */
/* * W(M+N(N+1)/2+N+2 ... W(M+N(N+1)/2+N+2+M+2N) * */
/* * CONTAIN THE MULTIPLIERS ASSOCIATED WITH ALL * */
/* * ALL CONSTRAINTS OF THE QUADRATIC PROGRAM FINDING * */
/* * THE SEARCH DIRECTION TO THE SOLUTION X* * */
/* * * JW(), L_JW JW() IS A ONE DIMENSIONAL INT WORKING SPACE * */
/* * THE LENGTH L_JW OF WHICH SHOULD BE AT LEAST * */
/* * MINEQ * */
/* * with MINEQ = M - MEQ + 2*N1 & N1 = N+1 * */
/* * * */
/* * THE USER HAS TO PROVIDE THE FOLLOWING SUBROUTINES: * */
/* * LDL(N,A,Z,SIG,W) : UPDATE OF THE LDL'-FACTORIZATION. * */
/* * LINMIN(A,B,F,TOL) : LINESEARCH ALGORITHM IF EXACT = 1 * */
/* * LSQ(M,MEQ,LA,N,NC,C,D,A,B,XL,XU,X,LAMBDA,W,....) : * */
/* * * */
/* * SOLUTION OF THE QUADRATIC PROGRAM * */
/* * QPSOL IS RECOMMENDED: * */
/* * PE GILL, W MURRAY, MA SAUNDERS, MH WRIGHT: * */
/* * USER'S GUIDE FOR SOL/QPSOL: * */
/* * A FORTRAN PACKAGE FOR QUADRATIC PROGRAMMING, * */
/* * TECHNICAL REPORT SOL 83-7, JULY 1983 * */
/* * DEPARTMENT OF OPERATIONS RESEARCH, STANFORD UNIVERSITY * */
/* * STANFORD, CA 94305 * */
/* * QPSOL IS THE MOST ROBUST AND EFFICIENT QP-SOLVER * */
/* * AS IT ALLOWS WARM STARTS WITH PROPER WORKING SETS * */
/* * * */
/* * IF IT IS NOT AVAILABLE USE LSEI, A CONSTRAINT LINEAR LEAST * */
/* * SQUARES SOLVER IMPLEMENTED USING THE SOFTWARE HFTI, LDP, NNLS * */
/* * FROM C.L. LAWSON, R.J.HANSON: SOLVING LEAST SQUARES PROBLEMS, * */
/* * PRENTICE HALL, ENGLEWOOD CLIFFS, 1974. * */
/* * LSEI COMES WITH THIS PACKAGE, together with all necessary SR's. * */
/* * * */
/* * TOGETHER WITH A COUPLE OF SUBROUTINES FROM BLAS LEVEL 1 * */
/* * * */
/* * SQP IS HEAD SUBROUTINE FOR BODY SUBROUTINE SQPBDY * */
/* * IN WHICH THE ALGORITHM HAS BEEN IMPLEMENTED. * */
/* * * */
/* * IMPLEMENTED BY: DIETER KRAFT, DFVLR OBERPFAFFENHOFEN * */
/* * as described in Dieter Kraft: A Software Package for * */
/* * Sequential Quadratic Programming * */
/* * DFVLR-FB 88-28, 1988 * */
/* * which should be referenced if the user publishes results of SLSQP * */
/* * * */
/* * DATE: APRIL - OCTOBER, 1981. * */
/* * STATUS: DECEMBER, 31-ST, 1984. * */
/* * STATUS: MARCH , 21-ST, 1987, REVISED TO FORTRAN 77 * */
/* * STATUS: MARCH , 20-th, 1989, REVISED TO MS-FORTRAN * */
/* * STATUS: APRIL , 14-th, 1989, HESSE in-line coded * */
/* * STATUS: FEBRUARY, 28-th, 1991, FORTRAN/2 Version 1.04 * */
/* * accepts Statement Functions * */
/* * STATUS: MARCH , 1-st, 1991, tested with SALFORD * */
/* * FTN77/386 COMPILER VERS 2.40* */
/* * in protected mode * */
/* * * */
/* *********************************************************************** */
/* * * */
/* * Copyright 1991: Dieter Kraft, FHM * */
/* * * */
/* *********************************************************************** */
/* dim(W) = N1*(N1+1) + MEQ*(N1+1) + MINEQ*(N1+1) for LSQ */
/* +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI */
/* +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI */
/* + N1*N/2 + 2*M + 3*N +3*N1 + 1 for SLSQPB */
/* with MINEQ = M - MEQ + 2*N1 & N1 = N+1 */
/* CHECK LENGTH OF WORKING ARRAYS */
/* Parameter adjustments */
--c___start;
a_dim1 = la;
a_offset = 1 + a_dim1;
a_start -= a_offset;
--g_start;
--xu_start;
--xl_start;
--x_start;
--w_start;
--jw_start;
/* Function Body */
n1 = n + 1;
mineq = m - meq + n1 + n1;
il = (n1 * 3 + m) * (n1 + 1) + (n1 - meq + 1) * (mineq + 2) + (mineq <<
1) + (n1 + mineq) * (n1 - meq) + (meq << 1) + n1 * n / 2 + (m
<< 1) + n * 3 + (n1 << 2) + 1;
/* Computing MAX */
i__1 = mineq;
i__2 = n1 - meq;
im = MAX2(i__1,i__2);
if(l_w__ < il || l_jw__ < im)
{
mode = MAX2(10,il) * 1000;
mode += MAX2(10,im);
return;
}
/* PREPARE DATA FOR CALLING SQPBDY - INITIAL ADDRESSES IN W */
im = 1;
il = im + MAX2(1,m);
il = im + la;
ix = il + n1 * n / 2 + 1;
ir = ix + n;
is = ir + n + n + MAX2(1,m);
is = ir + n + n + la;
iu = is + n1;
iv = iu + n1;
iw = iv + n1;
slsqpb_(m, meq, la, n, x, xl, xu, f, c__, g, a, acc, iter, mode,
w, w, w, w, w, w, w, w, jw,state,x_start+1,xl_start+1,xu_start+1,c___start+1,g_start+1,a_start+a_offset,
w_start+ir,w_start+il,w_start+ix,w_start+im,w_start+is,w_start+iu,w_start+iv,w_start+iw,jw_start+1);
ArrayCopy(state.x0,w,0,w_start+ix,n);
return;
} /* slsqp_ */
//+------------------------------------------------------------------+
//|calculates work buffers |
//+------------------------------------------------------------------+
void length_work(int &LEN_W, int &LEN_JW, int M, int MEQ, int N)
{
int N1 = N+1, MINEQ = M-MEQ+N1+N1;
LEN_W = (3*N1+M)*(N1+1)
+(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ
+(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1
+(N+1)*N/2 + 2*M + 3*N + 3*N1 + 1;
LEN_JW = MINEQ;
}
//+------------------------------------------------------------------+
//|function interface for slsq minimizer |
//+------------------------------------------------------------------+
slsqp_result slsqp_minimizer(int n, slsqp_func f, IObjective* f_data,
int m, slsqp_constraint &fc[],
int p, slsqp_constraint &h[],
vector &lb, vector &ub,
vector &x, double &minf,
double acc,
slsqp_stopping &stop)
{
int mtot = slsqp_count_constraints(m, fc);
int ptot = slsqp_count_constraints(p, h);
double fprev,fcur;
double cgrad[], c[], grad[], w[], xcur[], xprev[], cgradtmp[],jw[],lb_[],ub_[];
int mpi = (int)(mtot + ptot), pi = (int) ptot, ni = (int) n, mpi1 = mpi > 0 ? mpi : 1;
int len_w, len_jw;
int mode = 0, prev_mode = 0;
int iter = 0; /* tell sqsqp to ignore this check, since we check evaluation counts ourselves */
int i, ii;
slsqp_result ret = SLSQP_SUCCESS;
int feasible, feasible_cur;
double infeasibility = HUGE_VAL, infeasibility_cur = HUGE_VAL;
int max_cdim;
int want_grad = 1;
if(ptot > n)
{
slsqp_stop_msg(stop, "slsqp: more equality constraints than variables");
return SLSQP_INVALID_ARGS;
}
max_cdim = MAX2(slsqp_max_constraint_dim(m, fc),slsqp_max_constraint_dim(p, h));
length_work(len_w, len_jw, mpi, pi, ni);
if(ArrayResize(cgrad,fabs(mpi1) * (n + 1))< fabs(mpi1) * (n + 1)||
ArrayResize(c,fabs(mpi))< fabs(mpi)||
ArrayResize(grad,fabs(n+1))< fabs(n+1)||
ArrayResize(xcur,fabs(n))< fabs(n)||
ArrayResize(xprev,fabs(n))< fabs(n)||
ArrayResize(cgradtmp,fabs(max_cdim*n))< fabs(max_cdim*n)||
ArrayResize(w,fabs(len_w))< fabs(len_w)||
ArrayResize(jw,fabs(len_jw))< fabs(len_jw) ||
ArrayResize(lb_,int(lb.Size()))< 0 || !Copy(lb_,lb,ArraySize(lb_)) ||
ArrayResize(ub_,int(ub.Size()))< 0 || !Copy(ub_,ub,ArraySize(ub_)))
return SLSQP_OUT_OF_MEMORY;
Copy(xcur, x, n);
Copy(xprev, x, n);
fprev = fcur = minf = HUGE_VAL;
feasible = feasible_cur = 0;
int goto = 0;
bool eval_f_and_grad = true; /* eval before calling slsqp the first time */
do
{
if(!eval_f_and_grad && goto<1)
{
slsqp(mpi, pi, mpi1, ni,
xcur, lb_, ub_,fcur,
c, grad, cgrad,
acc, iter, mode,
w, len_w, jw, len_jw,
stop.slsqp_state);
}
else
{
if(eval_f_and_grad)
mode = -2;
if(goto)
{
goto = mode;
mode = 0;
}
}
switch(mode)
{
case -1: /* objective & gradient evaluation */
if(prev_mode == -2 && !want_grad)
break; /* just evaluated this point */
/* fall through */
case -2:
if(eval_f_and_grad)
{
mode = 0;
eval_f_and_grad = false;
}
want_grad = 1;
/* fall through */
case 1: /* don't need grad unless we don't have it yet */
{
double newgrad = 0;
double newcgrad = 0;
double null_array[];
if(want_grad)
{
newgrad = ArraySize(grad);
newcgrad = ArraySize(cgradtmp);
}
feasible_cur = 1;
infeasibility_cur = 0;
fcur = newgrad?f(n, xcur, grad, f_data):f(n, xcur, null_array, f_data);
++(stop.nevals_p);
if(slsqp_stop_forced(stop))
{
fcur = HUGE_VAL;
ret = SLSQP_FORCED_STOP;
goto = 1;
break;
}
if(slsqp_isfinite(fcur))
{
want_grad = 0;
ii = 0;
for(i = 0; i < p; ++i)
{
int j, k;
if(newcgrad)
slsqp_eval_constraint(c, cgradtmp, h[i], n, xcur,ii);
else
slsqp_eval_constraint(c, null_array, h[i], n, xcur,ii);
if(slsqp_stop_forced(stop))
{
ret = SLSQP_FORCED_STOP;
goto = 1;
break;
}
for(k = 0; k < h[i].m; ++k, ++ii)
{
infeasibility_cur =
MAX2(infeasibility_cur, fabs(c[ii]));
feasible_cur =
feasible_cur && fabs(c[ii]) <= h[i].tol[k];
if(newcgrad)
{
for(j = 0; j < n; ++ j)
cgrad[j*fabs(mpi1) + ii] = (cgradtmp[k*n + j]);
}
}
}
if(ret == SLSQP_FORCED_STOP)
break;
for(i = 0; i < m; ++i)
{
int j, k;
if(newcgrad)
slsqp_eval_constraint(c,cgradtmp, fc[i], n, xcur,ii);
else
slsqp_eval_constraint(c,null_array, fc[i], n, xcur,ii);
if(slsqp_stop_forced(stop))
{
ret = SLSQP_FORCED_STOP;
goto = 1;
break;
}
for(k = 0; k < fc[i].m; ++k, ++ii)
{
infeasibility_cur =
MAX2(infeasibility_cur, c[ii]);
feasible_cur =
feasible_cur && c[ii] <= fc[i].tol[k];
if(newcgrad)
{
for(j = 0; j < n; ++ j)
cgrad[j*fabs(mpi1) + ii] = (-cgradtmp[k*n + j]);
}
c[ii] = (-c[ii]); /* slsqp sign convention */
}
}
if(ret == SLSQP_FORCED_STOP)
break;
}
break;
}
case 0: /* required accuracy for solution obtained */
//goto done;
mode = goto;
if(slsqp_isinf(minf)) /* didn't find any feasible points, just return last point evaluated */
{
if(slsqp_isinf(fcur)) /* invalid cur. point, use previous pt. */
{
minf = fprev;
Copy(x, xprev, n);
}
else
{
minf = fcur;
Copy(x, xcur, n);
}
}
stop.gradient = vector::Zeros(n);
Copy(stop.gradient,grad,n);
stop.slsqp_state.itermx = iter;
return ret;
case 8: /* positive directional derivative for linesearch */
/* relaxed convergence check for a feasible_cur point,
as in the SLSQP code (except xtol as well as ftol) */
ret = SLSQP_ROUNDOFF_LIMITED; /* usually why deriv>0 */
if(feasible_cur)
{
double save_ftol_rel = stop.ftol_rel;
double save_xtol_rel = stop.xtol_rel;
double save_ftol_abs = stop.ftol_abs;
stop.ftol_rel *= 10;
stop.ftol_abs *= 10;
stop.xtol_rel *= 10;
if(slsqp_stop_ftol(stop, fcur, stop.slsqp_state.f0))
ret = SLSQP_FTOL_REACHED;
else
if(slsqp_stop_x(stop, xcur, stop.slsqp_state.x0))
ret = SLSQP_XTOL_REACHED;
stop.ftol_rel = save_ftol_rel;
stop.ftol_abs = save_ftol_abs;
stop.xtol_rel = save_xtol_rel;
}
goto = 1;
break;
case 5: /* singular matrix E in LSQ subproblem */
case 6: /* singular matrix C in LSQ subproblem */
case 7: /* rank-deficient equality constraint subproblem HFTI */
ret = SLSQP_ROUNDOFF_LIMITED;
goto = 1;
break;
case 4: /* inequality constraints incompatible */
case 3: /* more than 3*n iterations in LSQ subproblem */
case 9: /* more than iter iterations in SQP */
slsqp_stop_msg(stop, "bug: more than "+string(iter)+" SQP iterations");
ret = SLSQP_FAILURE;
goto = 1;
break;
case 2: /* number of equality constraints > n */
default: /* >= 10: working space w or jw too small */
slsqp_stop_msg(stop, "bug: workspace is too small");
ret = SLSQP_INVALID_ARGS;
goto = 1;
break;
}
if(!goto)
{
prev_mode = mode;
/* update best point so far */
if(slsqp_isfinite(fcur) && ((fcur < minf && (feasible_cur || !feasible))
|| (!feasible && infeasibility_cur < infeasibility)))
{
minf = fcur;
feasible = feasible_cur;
infeasibility = infeasibility_cur;
Copy(x, xcur, n);
}
/* note: mode == -1 corresponds to the completion of a line search,
and is the only time we should check convergence (as in original slsqp code) */
if(mode == -1)
{
if(!slsqp_isinf(fprev) && feasible_cur)
{
if(slsqp_stop_ftol(stop, fcur, fprev))
ret = SLSQP_FTOL_REACHED;
else
if(slsqp_stop_x(stop, xcur, xprev))
ret = SLSQP_XTOL_REACHED;
}
fprev = fcur;
ArrayCopy(xprev, xcur,0,0,n);
}
/* do some additional termination tests */
if(slsqp_stop_evals(stop))
ret = SLSQP_MAXEVAL_REACHED;
else
if(slsqp_stop_time(stop))
ret = SLSQP_MAXTIME_REACHED;
else
if(feasible && minf < stop.minf_max)
ret = SLSQP_STOPVAL_REACHED;
}
}
while(ret == SLSQP_SUCCESS);
//done:
if(slsqp_isinf(minf)) // didn't find any feasible points, just return last point evaluated
{
if(slsqp_isinf(fcur)) // invalid cur. point, use previous pt.
{
minf = fprev;
Copy(x, xprev, n);
}
else
{
minf = fcur;
Copy(x, xcur, n);
}
}
stop.gradient = vector::Zeros(n);
Copy(stop.gradient,grad,n);
stop.slsqp_state.itermx = iter;
return ret;
}
//+------------------------------------------------------------------+
//|Optimization results |
//+------------------------------------------------------------------+
struct OptimizeResult
{
int return_code;
int nfeval;
int niter;
vector solution;
vector objective_result;
vector objective_gradient;
OptimizeResult(void)
{
return_code = WRONG_VALUE;
nfeval = niter = 0;
solution = objective_result = objective_gradient = vector::Zeros(0);
}
OptimizeResult(int rc,int feval,int iter,vector &x, vector& f, vector& g)
{
return_code = rc;
nfeval = feval;
niter = iter;
solution = x;
objective_result = f;
objective_gradient = g;
}
OptimizeResult(OptimizeResult& other)
{
return_code = other.return_code;
nfeval = other.nfeval;
niter = other.niter;
solution = other.solution;
objective_result = other.objective_result;
objective_gradient = other.objective_gradient;
}
void operator=(OptimizeResult& other)
{
return_code = other.return_code;
nfeval = other.nfeval;
niter = other.niter;
solution = other.solution;
objective_result = other.objective_result;
objective_gradient = other.objective_gradient;
}
};
//+------------------------------------------------------------------+
//| function pointer |
//+------------------------------------------------------------------+
double func(int n, double& x[],double& gradient[],IObjective* func_data)
{
vector xv(n);
Copy(xv,x,n);
ObjReturn or = func_data.fun_and_grad(xv);
if(gradient.Size())
Copy(gradient,or.g,n);
return or.f;
}
//+------------------------------------------------------------------+
//| OOP interface for SLSQP minimizer |
//+------------------------------------------------------------------+
class CSlsqp : public CObject
{
protected:
double m_acc;
slsqp_constraint m_eq_constraints[];
slsqp_constraint m_ineq_constraints[];
slsqp_stopping m_stop;
slsqp_result m_slsqp_result;
bool check_constraints(void);
public:
CSlsqp(void) : m_acc(0.0) {}
~CSlsqp(void)
{
}
void SetAcc(double acc_)
{
m_acc = acc_;
}
void SetStopVal(double stopval)
{
m_stop.minf_max = stopval;
}
void SetFtolRel(double ftolrel)
{
if(ftolrel>0.0)
m_stop.ftol_rel = ftolrel;
}
void SetXtolRel(double xtolrel)
{
if(xtolrel>0.0)
m_stop.xtol_rel = xtolrel;
}
void SetFtolAbs(double ftolabs)
{
if(ftolabs>0.0)
m_stop.ftol_abs = ftolabs;
}
void SetXtolAbs(vector& xtolabs)
{
ArrayResize(m_stop.xtol_abs,(int)xtolabs.Size());
for(ulong i = 0; i<xtolabs.Size(); ++i)
m_stop.xtol_abs[i] = xtolabs[i];
}
void SetMaxEval(int maxeval)
{
m_stop.maxeval = fabs(maxeval);
}
bool SetEqualityConstraints(CConstraints &constraints, vector &tolerances)
{
if(!m_eq_constraints.Size())
if(ArrayResize(m_eq_constraints,1)!=1)
{
Print(__FUNCTION__,": Error ", GetLastError());
return false;
}
m_eq_constraints[0].f_data = GetPointer(constraints);
m_eq_constraints[0].m = constraints.numConstraints();
ArrayResize(m_eq_constraints[0].tol,m_eq_constraints[0].m);
return Copy(m_eq_constraints[0].tol,tolerances,m_eq_constraints[0].m);
}
bool SetInequalityConstraints(CConstraints &constraints, vector &tolerances)
{
if(!m_ineq_constraints.Size())
if(ArrayResize(m_ineq_constraints,1)!=1)
{
Print(__FUNCTION__, ": Error ", GetLastError());
return false;
}
m_ineq_constraints[0].f_data = GetPointer(constraints);
m_ineq_constraints[0].m = constraints.numConstraints();
ArrayResize(m_ineq_constraints[0].tol,m_ineq_constraints[0].m);
return Copy(m_ineq_constraints[0].tol,tolerances,m_ineq_constraints[0].m);
}
bool SetEqualityConstraints(CConstraints &constraints)
{
vector tolerances(constraints.numConstraints());
tolerances.Fill(1.e-8);
return SetEqualityConstraints(constraints,tolerances);
}
bool SetInequalityConstraints(CConstraints &constraints)
{
vector tolerances(constraints.numConstraints());
tolerances.Fill(1.e-8);
return SetInequalityConstraints(constraints,tolerances);
}
OptimizeResult Minimize(CFunctor &fungrad)
{
vector x0 = fungrad.initial_params();
int n = (int)x0.Size();
vector x_data = x0;
vector low = fungrad.lower_bounds();
vector up = fungrad.upper_bounds();
if((low.Size()&&low.Size()!=ulong(n)) || (up.Size()&&up.Size()!=ulong(n)))
{
Print(__FUNCTION__,": All vector inputs should be the same size if not empty");
return OptimizeResult();
}
for(int i = 0; i<n; ++i)
if(low[i]>up[i])
{
Print(__FUNCTION__ ": Invalid boundary constraints");
return OptimizeResult();
}
slsqp_func ofunc = func;
IObjective *obj = (IObjective*)GetPointer(fungrad);
double minimum = 0.0;
m_stop.n = n;
m_stop.nevals_p = 0;
m_stop.start = (double)TimeLocal();
m_slsqp_result = slsqp_minimizer(n,ofunc,obj,ArraySize(m_ineq_constraints),m_ineq_constraints,ArraySize(m_eq_constraints),m_eq_constraints,low,up,x0,minimum,m_acc,m_stop);
if(m_slsqp_result<0)
{
Print(__FUNCTION__, ": Operation failed ", EnumToString((slsqp_result)m_slsqp_result));
return OptimizeResult();
}
vector out(1);
out[0] = minimum;
vector gg;
return OptimizeResult(int(m_slsqp_result),m_stop.nevals_p,m_stop.slsqp_state.itermx,x0,out,m_stop.gradient);
}
};
//+------------------------------------------------------------------+
Reference in a new issue Copy permalink