/**
* @brief A wrapper around private functions, which downdate Cholesky factor and
* resolve the system.
*
* @param[in] ppar parameters.
* @param[in] active_set a vector of active constraints.
* @param[in] ind_exclude index of excluded constraint.
* @param[in] x initial guess.
* @param[out] dx feasible descent direction, must be allocated.
*
* @note Downdate of vector @ref pz 'z' is described on the page '@ref pRemoveICz'.
*/
void chol_solve::down_resolve(
const AS::problem_parameters& ppar,
const vector <AS::constraint>& active_set,
const int ind_exclude,
const double *x,
double *dx)
{
const int nW = active_set.size();
const int last_el_ind = ppar.N*SMPC_NUM_STATE_VAR + ind_exclude;
// for each element of z affected by removed constraint
// find a base that stays the same
double z_tmp = 0;
for (int i = nW; i > ind_exclude; --i)
{
const int zind = ppar.N*SMPC_NUM_STATE_VAR + i;
double zn = z[zind] * icL[i][zind];
z[zind] = z_tmp;
for (int j = last_el_ind; j < zind; ++j)
{
zn += z[j] * icL[i][j];
}
z_tmp = zn;
}
z[last_el_ind] = z_tmp;
// downdate L
downdate (ppar, nW, ind_exclude, x);
// recompute elements of z
for (int i = ind_exclude; i < nW; i++)
{
int zind = ppar.N*SMPC_NUM_STATE_VAR + i;
double zn = z[zind];
// zn
// start from the first !=0 element
for (int j = last_el_ind; j < zind; j++)
{
zn -= z[j] * icL[i][j];
}
z[zind] = zn/icL[i][zind];
}
// copy z to nu
memmove(nu, z, sizeof(double) * (ppar.N*SMPC_NUM_STATE_VAR + nW));
resolve (ppar, active_set, x, dx);
}
/* Return true iff the search is positive, ie if downdate was
* needed and performed. */
bool HCOD::
searchAndDowndate( const Index & stageRef )
{
assert( stageRef<=stages.size() );
double lambdamax = Stage::EPSILON; Index row;
const stage_iter_t refend = stages.begin()+ std::min((Index)(stages.size()),stageRef+1);
stage_iter_t stageDown = refend;
for( stage_iter_t iter = stages.begin(); iter!=refend; ++iter )
{
if( (*iter)->maxLambda( solution,lambdamax,row ) )
stageDown=iter;
}
if( refend!=stageDown )
{
downdate(stageDown,row);
return true;
}
return false;
}
int
gsl_integration_cquad (const gsl_function * f, double a, double b,
double epsabs, double epsrel,
gsl_integration_cquad_workspace * ws,
double *result, double *abserr, size_t * nevals)
{
/* Some constants that we will need. */
static const int n[4] = { 4, 8, 16, 32 };
static const int skip[4] = { 8, 4, 2, 1 };
static const int idx[4] = { 0, 5, 14, 31 };
static const double w = M_SQRT2 / 2;
static const int ndiv_max = 20;
/* Actual variables (as opposed to constants above). */
double m, h, temp;
double igral, err, igral_final, err_final, err_excess;
int nivals, neval = 0;
int i, j, d, split, t;
int nnans, nans[32];
gsl_integration_cquad_ival *iv, *ivl, *ivr;
double nc, ncdiff;
/* Check the input arguments. */
if (f == NULL)
GSL_ERROR ("function pointer shouldn't be NULL", GSL_EINVAL);
if (result == NULL)
GSL_ERROR ("result pointer shouldn't be NULL", GSL_EINVAL);
if (ws == NULL)
GSL_ERROR ("workspace pointer shouldn't be NULL", GSL_EINVAL);
/* Check for unreasonable accuracy demands */
if (epsabs < 0.0 || epsrel < 0.0)
GSL_ERROR ("tolerances may not be negative", GSL_EBADTOL);
if (epsabs <= 0 && epsrel < GSL_DBL_EPSILON)
GSL_ERROR ("unreasonable accuracy requirement", GSL_EBADTOL);
/* Create the first interval. */
iv = &(ws->ivals[0]);
m = (a + b) / 2;
h = (b - a) / 2;
nnans = 0;
for (i = 0; i <= n[3]; i++)
{
iv->fx[i] = GSL_FN_EVAL (f, m + xi[i] * h);
neval++;
if (!finite (iv->fx[i]))
{
nans[nnans++] = i;
iv->fx[i] = 0.0;
}
}
Vinvfx (iv->fx, &(iv->c[idx[0]]), 0);
Vinvfx (iv->fx, &(iv->c[idx[3]]), 3);
Vinvfx (iv->fx, &(iv->c[idx[2]]), 2);
for (i = 0; i < nnans; i++)
iv->fx[nans[i]] = GSL_NAN;
iv->a = a;
iv->b = b;
iv->depth = 3;
iv->rdepth = 1;
iv->ndiv = 0;
iv->igral = 2 * h * iv->c[idx[3]] * w;
nc = 0.0;
for (i = n[2] + 1; i <= n[3]; i++)
{
temp = iv->c[idx[3] + i];
nc += temp * temp;
}
ncdiff = nc;
for (i = 0; i <= n[2]; i++)
{
temp = iv->c[idx[2] + i] - iv->c[idx[3] + i];
ncdiff += temp * temp;
nc += iv->c[idx[3] + i] * iv->c[idx[3] + i];
}
ncdiff = sqrt (ncdiff);
nc = sqrt (nc);
iv->err = ncdiff * 2 * h;
if (ncdiff / nc > 0.1 && iv->err < 2 * h * nc)
iv->err = 2 * h * nc;
/* Initialize the heaps. */
for (i = 0; i < ws->size; i++)
ws->heap[i] = i;
/* Initialize some global values. */
igral = iv->igral;
err = iv->err;
nivals = 1;
igral_final = 0.0;
err_final = 0.0;
err_excess = 0.0;
/* Main loop. */
while (nivals > 0 && err > 0.0 &&
!(err <= fabs (igral) * epsrel || err <= epsabs)
&& !(err_final > fabs (igral) * epsrel
&& err - err_final < fabs (igral) * epsrel)
&& !(err_final > epsabs && err - err_final < epsabs))
{
/* Put our finger on the interval with the largest error. */
iv = &(ws->ivals[ws->heap[0]]);
m = (iv->a + iv->b) / 2;
h = (iv->b - iv->a) / 2;
/* printf
("cquad: processing ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err, iv->depth);
*/
/* Should we try to increase the degree? */
if (iv->depth < 3)
{
/* Keep tabs on some variables. */
d = ++iv->depth;
/* Get the new (missing) function values */
for (i = skip[d]; i <= 32; i += 2 * skip[d])
{
iv->fx[i] = GSL_FN_EVAL (f, m + xi[i] * h);
neval++;
}
nnans = 0;
for (i = 0; i <= 32; i += skip[d])
{
if (!finite (iv->fx[i]))
{
nans[nnans++] = i;
iv->fx[i] = 0.0;
}
}
/* Compute the new coefficients. */
Vinvfx (iv->fx, &(iv->c[idx[d]]), d);
/* Downdate any NaNs. */
if (nnans > 0)
{
downdate (&(iv->c[idx[d]]), n[d], d, nans, nnans);
for (i = 0; i < nnans; i++)
iv->fx[nans[i]] = GSL_NAN;
}
/* Compute the error estimate. */
nc = 0.0;
for (i = n[d - 1] + 1; i <= n[d]; i++)
{
temp = iv->c[idx[d] + i];
nc += temp * temp;
}
ncdiff = nc;
for (i = 0; i <= n[d - 1]; i++)
{
temp = iv->c[idx[d - 1] + i] - iv->c[idx[d] + i];
ncdiff += temp * temp;
nc += iv->c[idx[d] + i] * iv->c[idx[d] + i];
}
ncdiff = sqrt (ncdiff);
nc = sqrt (nc);
iv->err = ncdiff * 2 * h;
/* Compute the local integral. */
iv->igral = 2 * h * w * iv->c[idx[d]];
/* Split the interval prematurely? */
split = (nc > 0 && ncdiff / nc > 0.1);
}
/* Maximum degree reached, just split. */
else
{
split = 1;
}
/* Should we drop this interval? */
if ((m + h * xi[0]) >= (m + h * xi[1])
|| (m + h * xi[31]) >= (m + h * xi[32])
|| iv->err < fabs (iv->igral) * GSL_DBL_EPSILON * 10)
{
/* printf
("cquad: dumping ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err,
iv->depth);
*/
/* Keep this interval's contribution */
err_final += iv->err;
igral_final += iv->igral;
/* Swap with the last element on the heap */
t = ws->heap[nivals - 1];
ws->heap[nivals - 1] = ws->heap[0];
ws->heap[0] = t;
nivals--;
/* Fix up the heap */
i = 0;
while (2 * i + 1 < nivals)
{
/* Get the kids */
j = 2 * i + 1;
/* If the j+1st entry exists and is larger than the jth,
use it instead. */
if (j + 1 < nivals
&& ws->ivals[ws->heap[j + 1]].err >=
ws->ivals[ws->heap[j]].err)
j++;
/* Do we need to move the ith entry up? */
if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
break;
else
{
t = ws->heap[j];
ws->heap[j] = ws->heap[i];
ws->heap[i] = t;
i = j;
}
}
}
/* Do we need to split this interval? */
else if (split)
{
/* Some values we will need often... */
d = iv->depth;
/* Generate the interval on the left */
ivl = &(ws->ivals[ws->heap[nivals++]]);
ivl->a = iv->a;
ivl->b = m;
ivl->depth = 0;
ivl->rdepth = iv->rdepth + 1;
ivl->fx[0] = iv->fx[0];
ivl->fx[32] = iv->fx[16];
for (i = skip[0]; i < 32; i += skip[0])
{
ivl->fx[i] =
GSL_FN_EVAL (f, (ivl->a + ivl->b) / 2 + xi[i] * h / 2);
neval++;
}
nnans = 0;
for (i = 0; i <= 32; i += skip[0])
{
if (!finite (ivl->fx[i]))
{
nans[nnans++] = i;
ivl->fx[i] = 0.0;
}
}
Vinvfx (ivl->fx, ivl->c, 0);
if (nnans > 0)
{
downdate (ivl->c, n[0], 0, nans, nnans);
for (i = 0; i < nnans; i++)
ivl->fx[nans[i]] = GSL_NAN;
}
for (i = 0; i <= n[d]; i++)
{
ivl->c[idx[d] + i] = 0.0;
for (j = i; j <= n[d]; j++)
ivl->c[idx[d] + i] += Tleft[i * 33 + j] * iv->c[idx[d] + j];
}
ncdiff = 0.0;
for (i = 0; i <= n[0]; i++)
{
temp = ivl->c[i] - ivl->c[idx[d] + i];
ncdiff += temp * temp;
}
for (i = n[0] + 1; i <= n[d]; i++)
{
temp = ivl->c[idx[d] + i];
ncdiff += temp * temp;
}
ncdiff = sqrt (ncdiff);
ivl->err = ncdiff * h;
/* Check for divergence. */
ivl->ndiv = iv->ndiv + (fabs (iv->c[0]) > 0
&& ivl->c[0] / iv->c[0] > 2);
if (ivl->ndiv > ndiv_max && 2 * ivl->ndiv > ivl->rdepth)
{
/* need copysign(INFINITY, igral) */
*result = (igral >= 0) ? GSL_POSINF : GSL_NEGINF;
if (nevals != NULL)
*nevals = neval;
return GSL_EDIVERGE;
}
/* Compute the local integral. */
ivl->igral = h * w * ivl->c[0];
/* Generate the interval on the right */
ivr = &(ws->ivals[ws->heap[nivals++]]);
ivr->a = m;
ivr->b = iv->b;
ivr->depth = 0;
ivr->rdepth = iv->rdepth + 1;
ivr->fx[0] = iv->fx[16];
ivr->fx[32] = iv->fx[32];
for (i = skip[0]; i < 32; i += skip[0])
{
ivr->fx[i] =
GSL_FN_EVAL (f, (ivr->a + ivr->b) / 2 + xi[i] * h / 2);
neval++;
}
nnans = 0;
for (i = 0; i <= 32; i += skip[0])
{
if (!finite (ivr->fx[i]))
{
nans[nnans++] = i;
ivr->fx[i] = 0.0;
}
}
Vinvfx (ivr->fx, ivr->c, 0);
if (nnans > 0)
{
downdate (ivr->c, n[0], 0, nans, nnans);
for (i = 0; i < nnans; i++)
ivr->fx[nans[i]] = GSL_NAN;
}
for (i = 0; i <= n[d]; i++)
{
ivr->c[idx[d] + i] = 0.0;
for (j = i; j <= n[d]; j++)
ivr->c[idx[d] + i] += Tright[i * 33 + j] * iv->c[idx[d] + j];
}
ncdiff = 0.0;
for (i = 0; i <= n[0]; i++)
{
temp = ivr->c[i] - ivr->c[idx[d] + i];
ncdiff += temp * temp;
}
for (i = n[0] + 1; i <= n[d]; i++)
{
temp = ivr->c[idx[d] + i];
ncdiff += temp * temp;
}
ncdiff = sqrt (ncdiff);
ivr->err = ncdiff * h;
/* Check for divergence. */
ivr->ndiv = iv->ndiv + (fabs (iv->c[0]) > 0
&& ivr->c[0] / iv->c[0] > 2);
if (ivr->ndiv > ndiv_max && 2 * ivr->ndiv > ivr->rdepth)
{
/* need copysign(INFINITY, igral) */
*result = (igral >= 0) ? GSL_POSINF : GSL_NEGINF;
if (nevals != NULL)
*nevals = neval;
return GSL_EDIVERGE;
}
/* Compute the local integral. */
ivr->igral = h * w * ivr->c[0];
/* Fix-up the heap: we now have one interval on top
that we don't need any more and two new, unsorted
ones at the bottom. */
/* Flip the last interval to the top of the heap and
sift down. */
t = ws->heap[nivals - 1];
ws->heap[nivals - 1] = ws->heap[0];
ws->heap[0] = t;
nivals--;
/* Sift this interval back down the heap. */
i = 0;
while (2 * i + 1 < nivals - 1)
{
j = 2 * i + 1;
if (j + 1 < nivals - 1
&& ws->ivals[ws->heap[j + 1]].err >=
ws->ivals[ws->heap[j]].err)
j++;
if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
break;
else
{
t = ws->heap[j];
ws->heap[j] = ws->heap[i];
ws->heap[i] = t;
i = j;
}
}
/* Now grab the last interval and sift it up the heap. */
i = nivals - 1;
while (i > 0)
{
j = (i - 1) / 2;
if (ws->ivals[ws->heap[j]].err < ws->ivals[ws->heap[i]].err)
{
t = ws->heap[j];
ws->heap[j] = ws->heap[i];
ws->heap[i] = t;
i = j;
}
else
break;
}
}
/* Otherwise, just fix-up the heap. */
else
{
i = 0;
while (2 * i + 1 < nivals)
{
j = 2 * i + 1;
if (j + 1 < nivals
&& ws->ivals[ws->heap[j + 1]].err >=
ws->ivals[ws->heap[j]].err)
j++;
if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
break;
else
{
t = ws->heap[j];
ws->heap[j] = ws->heap[i];
ws->heap[i] = t;
i = j;
}
}
}
/* If the heap is about to overflow, remove the last two
intervals. */
while (nivals > ws->size - 2)
{
iv = &(ws->ivals[ws->heap[nivals - 1]]);
/* printf
("cquad: dumping ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err,
iv->depth);
*/
err_final += iv->err;
igral_final += iv->igral;
nivals--;
}
/* Collect the value of the integral and error. */
igral = igral_final;
err = err_final;
for (i = 0; i < nivals; i++)
{
igral += ws->ivals[ws->heap[i]].igral;
err += ws->ivals[ws->heap[i]].err;
}
}
/* Dump the contents of the heap. */
/* for (i = 0; i < nivals; i++)
{
iv = &(ws->ivals[ws->heap[i]]);
printf
("cquad: ival %i (%i) with [%e,%e], int=%e, err=%e, depth=%i, rdepth=%i\n",
i, ws->heap[i], iv->a, iv->b, iv->igral, iv->err, iv->depth,
iv->rdepth);
}
*/
/* Clean up and present the results. */
*result = igral;
if (abserr != NULL)
*abserr = err;
if (nevals != NULL)
*nevals = neval;
/* All is well that ends well. */
return GSL_SUCCESS;
}