int volt_step(double *y, double t, double dt, int neq, double *yg, double *yp,
double *yp2, double *ytemp, double *errvec, double *jac) {
int i0, iend, ishift, i, iter = 0, info, ipivot[MAXODE1], j, ind;
int n1 = NODE + 1;
double dt2 = .5 * dt, err;
double del, yold, fac, delinv;
i0 = MAX(0, CurrentPoint - MaxPoints);
iend = MIN(CurrentPoint - 1, MaxPoints - 1);
ishift = i0 % MaxPoints;
init_sums(T0, CurrentPoint, dt, i0, iend,
ishift); /* initialize all the sums */
KnFlag = 0;
for (i = 0; i < neq; i++) {
set_ivar(i + 1, y[i]);
yg[i] = y[i];
}
for (i = NODE; i < NODE + NMarkov; i++)
set_ivar(i + 1 + FIX_VAR, y[i]);
set_ivar(0, t - dt);
for (i = NODE; i < NODE + FIX_VAR; i++)
set_ivar(i + 1, evaluate(my_ode[i]));
for (i = 0; i < NODE; i++) {
if (!EqType[i])
yp2[i] = y[i] + dt2 * evaluate(my_ode[i]);
else
yp2[i] = 0.0;
}
KnFlag = 1;
while (1) {
get_kn(yg, t);
for (i = NODE; i < NODE + FIX_VAR; i++)
set_ivar(i + 1, evaluate(my_ode[i]));
for (i = 0; i < NODE; i++) {
yp[i] = evaluate(my_ode[i]);
/* plintf(" yp[%d]=%g\n",i,yp[i]); */
if (EqType[i])
errvec[i] = -yg[i] + yp[i];
else
errvec[i] = -yg[i] + dt2 * yp[i] + yp2[i];
}
/* Compute Jacobian */
for (i = 0; i < NODE; i++) {
del = NEWT_ERR * MAX(NEWT_ERR, fabs(yg[i]));
yold = yg[i];
yg[i] += del;
delinv = 1. / del;
get_kn(yg, t);
for (j = NODE; j < NODE + FIX_VAR; j++)
set_ivar(j + 1, evaluate(my_ode[j]));
for (j = 0; j < NODE; j++) {
fac = delinv;
if (!EqType[j])
fac *= dt2;
jac[j * NODE + i] = (evaluate(my_ode[j]) - yp[j]) * fac;
}
yg[i] = yold;
}
for (i = 0; i < NODE; i++)
jac[n1 * i] -= 1.0;
sgefa(jac, NODE, NODE, ipivot, &info);
if (info != -1) {
return (-1); /* Jacobian is singular */
}
err = 0.0;
sgesl(jac, NODE, NODE, ipivot, errvec, 0);
for (i = 0; i < NODE; i++) {
err = MAX(fabs(errvec[i]), err);
yg[i] -= errvec[i];
}
if (err < EulTol)
break;
iter++;
if (iter > MaxEulIter)
return (-2); /* too many iterates */
}
/* We have a good point; lets save it */
get_kn(yg, t);
/* for(i=NODE;i<NODE+FIX_VAR;i++)
set_ivar(i+1,evaluate(my_ode[i])); */
for (i = 0; i < NODE; i++)
y[i] = yg[i];
ind = CurrentPoint % MaxPoints;
for (i = 0; i < NODE + FIX_VAR + NMarkov; i++)
Memory[i][ind] = get_ivar(i + 1);
CurrentPoint++;
return (0);
}
Var* ff_boxfilter(vfuncptr func, Var* arg)
{
Var* v = NULL;
Var *rcount, *rmean, *rs, *rn, *rsigma;
Var* a;
int x = 0;
int y = 0;
int z = 0;
int size = 0;
double ignore = FLT_MIN;
int verbose = 0;
Alist alist[8];
alist[0] = make_alist("obj", ID_VAL, NULL, &v);
alist[1] = make_alist("x", DV_INT32, NULL, &x);
alist[2] = make_alist("y", DV_INT32, NULL, &y);
alist[3] = make_alist("z", DV_INT32, NULL, &z);
alist[4] = make_alist("size", DV_INT32, NULL, &size);
alist[5] = make_alist("ignore", DV_DOUBLE, NULL, &ignore);
alist[6] = make_alist("verbose", DV_INT32, NULL, &verbose);
alist[7].name = NULL;
if (parse_args(func, arg, alist) == 0) return (NULL);
if (v == NULL) {
parse_error("%s(): No object specified\n", func->name);
return (NULL);
}
if (x && y && size) {
parse_error("%s(): Specify either size or (x, y, z), not both", func->name);
return (NULL);
}
if (x == 0) x = 1;
if (y == 0) y = 1;
if (z == 0) z = 1;
if (size != 0) x = y = size;
if (x == 0 || y == 0) {
parse_error("%s(): No x or y specified", func->name);
return (NULL);
}
init_sums(v, x, y, z, &rn, &rs, &rcount, &rmean, &rsigma, ignore);
if (verbose) {
a = new_struct(0);
add_struct(a, "count", rcount);
add_struct(a, "mean", rmean);
add_struct(a, "sigma", rsigma);
add_struct(a, "n", rn);
add_struct(a, "s", rs);
return (a);
} else {
mem_claim(rcount);
free_var(rcount);
mem_claim(rn);
free_var(rn);
mem_claim(rs);
free_var(rs);
return (rmean);
}
}
