2026-06-05 14:42:49 +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);
|
| | | }
|
| | | };
|
| | | //+------------------------------------------------------------------+
|
