///<summary>Integrates a system of ODEs over a specified time interval.</summary>
public int ros2_integrate(
double[] y,
int n,
double t,
double tnext,
double[] htry,
double[] atol,
double[] rtol,
JacobianInterface jInt,
Operation op)
{
double UROUND = 2.3e-16;
double g, ghinv, ghinv1, dghinv, ytol;
double h, hold, hmin, hmax, tplus;
double ej, err, factor, facmax;
int nfcn, njac, naccept, nreject;
int isReject;
int adjust = this.Adjust;
// Initialize counters, etc.
g = 1.0 + 1.0 / Math.Sqrt(2.0);
ghinv1 = 0.0;
tplus = t;
isReject = 0;
naccept = 0;
nreject = 0;
nfcn = 0;
njac = 0;
// Initial step size
hmax = tnext - t;
hmin = 1e-8;
h = htry[0];
if (h == 0.0)
{
jInt.solve(t, y, n, this.K1, 0, op);
nfcn += 1;
adjust = 1;
h = tnext - t;
for (int i = 1; i <= n; i++)
{
ytol = atol[i] + rtol[i] * Math.Abs(y[i]);
if (this.K1[i] != 0.0)
{
h = Math.Min(h, (ytol / Math.Abs(this.K1[i])));
}
}
}
h = Math.Max(hmin, h);
h = Math.Min(hmax, h);
// Start the time loop
while (t < tnext)
{
// Check for zero step size
if (0.10 * Math.Abs(h) <= Math.Abs(t) * UROUND)
{
return(-2);
}
// Adjust step size if interval exceeded
tplus = t + h;
if (tplus > tnext)
{
h = tnext - t;
tplus = tnext;
}
// Re-compute the Jacobian if step size accepted
if (isReject == 0)
{
Utilities.Jacobian(y, n, this.K1, this.K2, this.A, jInt, op);
njac++;
nfcn += 2 * n;
ghinv1 = 0.0;
}
// Update the Jacobian to reflect new step size
ghinv = -1.0 / (g * h);
dghinv = ghinv - ghinv1;
for (int i = 1; i <= n; i++)
{
this.A[i, i] += dghinv;
}
ghinv1 = ghinv;
if (Utilities.Factorize(this.A, n, this.K1, this.Jindx) == 0)
{
return(-1);
}
// Stage 1 solution
jInt.solve(t, y, n, this.K1, 0, op);
nfcn += 1;
for (int j = 1; j <= n; j++)
{
this.K1[j] *= ghinv;
}
Utilities.Solve(this.A, n, this.Jindx, this.K1);
// Stage 2 solution
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i] + h * this.K1[i];
}
jInt.solve(t, this.Ynew, n, this.K2, 0, op);
nfcn += 1;
for (int i = 1; i <= n; i++)
{
this.K2[i] = (this.K2[i] - 2.0 * this.K1[i]) * ghinv;
}
Utilities.Solve(this.A, n, this.Jindx, this.K2);
// Overall solution
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i] + 1.5 * h * this.K1[i] + 0.5 * h * this.K2[i];
}
// Error estimation
hold = h;
err = 0.0;
if (adjust != 0)
{
for (int i = 1; i <= n; i++)
{
ytol = atol[i] + rtol[i] * Math.Abs(this.Ynew[i]);
ej = Math.Abs(this.Ynew[i] - y[i] - h * this.K1[i]) / ytol;
err = err + ej * ej;
}
err = Math.Sqrt(err / n);
err = Math.Max(UROUND, err);
// Choose the step size
factor = 0.9 / Math.Sqrt(err);
if (isReject != 0)
{
facmax = 1.0;
}
else
{
facmax = 10.0;
}
factor = Math.Min(factor, facmax);
factor = Math.Max(factor, 1.0e-1);
h = factor * h;
h = Math.Min(hmax, h);
}
// Reject/accept the step
if (err > 1.0)
{
isReject = 1;
nreject++;
h = 0.5 * h;
}
else
{
isReject = 0;
for (int i = 1; i <= n; i++)
{
y[i] = this.Ynew[i];
if (y[i] <= UROUND)
{
y[i] = 0.0;
}
}
if (adjust != 0)
{
htry[0] = h;
}
t = tplus;
naccept++;
}
// End of the time loop
}
return(nfcn);
}
///<summary>Integrates system of equations dY/dt = F(t,Y) over a given interval.</summary>
public int rk5_integrate(
double[] y,
int n,
double t,
double tnext,
double[] htry,
double[] atol,
double[] rtol,
JacobianInterface jInt,
Operation op)
{
double c2 = 0.20, c3 = 0.30, c4 = 0.80, c5 = 8.0 / 9.0;
double a21 = 0.20,
a31 = 3.0 / 40.0,
a32 = 9.0 / 40.0,
a41 = 44.0 / 45.0,
a42 = -56.0 / 15.0,
a43 = 32.0 / 9.0,
a51 = 19372.0 / 6561.0,
a52 = -25360.0 / 2187.0,
a53 = 64448.0 / 6561.0,
a54 = -212.0 / 729.0,
a61 = 9017.0 / 3168.0,
a62 = -355.0 / 33.0,
a63 = 46732.0 / 5247.0,
a64 = 49.0 / 176.0,
a65 = -5103.0 / 18656.0,
a71 = 35.0 / 384.0,
a73 = 500.0 / 1113.0,
a74 = 125.0 / 192.0,
a75 = -2187.0 / 6784.0,
a76 = 11.0 / 84.0;
double e1 = 71.0 / 57600.0,
e3 = -71.0 / 16695.0,
e4 = 71.0 / 1920.0,
e5 = -17253.0 / 339200.0,
e6 = 22.0 / 525.0,
e7 = -1.0 / 40.0;
double tnew, h, hmax, hnew, ytol, err, sk, fac, fac11 = 1.0;
// parameters for step size control
double UROUND = 2.3e-16;
double SAFE = 0.90;
double fac1 = 0.2;
double fac2 = 10.0;
double beta = 0.04;
double facold = 1e-4;
double expo1 = 0.2 - beta * 0.75;
double facc1 = 1.0 / fac1;
double facc2 = 1.0 / fac2;
// various counters
int nstep = 1;
int nfcn = 0;
int naccpt = 0;
int nrejct = 0;
int reject = 0;
int adjust = this.Adjust;
// initial function evaluation
jInt.solve(t, y, n, this.K1, 0, op);
nfcn++;
// initial step size
h = htry[0];
hmax = tnext - t;
if (h == 0.0)
{
adjust = 1;
h = tnext - t;
for (int i = 1; i <= n; i++)
{
ytol = atol[i] + rtol[i] * Math.Abs(y[i]);
if (this.K1[i] != 0.0)
{
h = Math.Min(h, (ytol / Math.Abs(this.K1[i])));
}
}
}
h = Math.Max(1e-8, h);
// while not at end of time interval
while (t < tnext)
{
// --- check for zero step size
if (0.10 * Math.Abs(h) <= Math.Abs(t) * UROUND)
{
return(-2);
}
// --- adjust step size if interval exceeded
if ((t + 1.01 * h - tnext) > 0.0)
{
h = tnext - t;
}
tnew = t + c2 * h;
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i] + h * a21 * this.K1[i];
}
jInt.solve(tnew, this.Ynew, n, this.K1, this.K2off, op);
tnew = t + c3 * h;
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i] + h * (a31 * this.K1[i] + a32 * this.K1[this.K2off + i]);
}
// y[i] + h*(a31*K1[i] + a32*K2[i]
jInt.solve(tnew, this.Ynew, n, this.K1, this.K3off, op);
tnew = t + c4 * h;
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i]
+ h
* (a41 * this.K1[i] + a42 * this.K1[this.K2off + i] + a43 * this.K1[this.K3off + i]);
}
//a42*K2[i] + a43*K3[i]);
jInt.solve(tnew, this.Ynew, n, this.K1, this.K4off, op);
tnew = t + c5 * h;
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i]
+ h
* (a51 * this.K1[i] + a52 * this.K1[this.K2off + i] + a53 * this.K1[this.K3off + i]
+ a54 * this.K1[this.K4off + i]); //a52*K2[i] + a53*K3[i]+a54*K4[i]);
}
jInt.solve(tnew, this.Ynew, n, this.K1, this.K5off, op);
tnew = t + h;
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i] + h * (a61 * this.K1[i] + a62 * this.K1[i + this.K2off] +
a63 * this.K1[i + this.K3off] + a64 * this.K1[i + this.K4off]
+ a65 * this.K1[i + this.K5off]);
}
//Ynew[i] = y[i] + h*(a61*K1[i] + a62*K2[i] +
// a63*K3[i] + a64*K4[i] + a65*K5[i]);
jInt.solve(tnew, this.Ynew, n, this.K1, this.K6off, op);
for (int i = 1; i <= n; i++)
{
this.Ynew[i] = y[i] + h * (a71 * this.K1[i] + a73 * this.K1[i + this.K3off] +
a74 * this.K1[i + this.K4off] + a75 * this.K1[i + this.K5off]
+ a76 * this.K1[i + this.K6off]);
}
// Ynew[i] = y[i] + h*(a71*K1[i] + a73*K3[i] +
// a74*K4[i] + a75*K5[i] + a76*K6[i]);
jInt.solve(tnew, this.Ynew, n, this.K1, this.K2off, op);
nfcn += 6;
// step size adjustment
err = 0.0;
hnew = h;
if (adjust != 0)
{
for (int i = 1; i <= n; i++)
{
this.K1[i + this.K4off] = (e1 * this.K1[i] + e3 * this.K1[i + this.K3off]
+ e4 * this.K1[i + this.K4off] + e5 * this.K1[i + this.K5off] +
e6 * this.K1[i + this.K6off] + e7 * this.K1[i + this.K2off]) * h;
}
//K4[i] = (e1*K1[i] + e3*K3[i] + e4*K4[i] + e5*K5[i] +
// e6*K6[i] + e7*K2[i])*h;
for (int i = 1; i <= n; i++)
{
sk = atol[i] + rtol[i] * Math.Max(Math.Abs(y[i]), Math.Abs(this.Ynew[i]));
sk = this.K1[i + this.K4off] / sk; //K4[i]/sk;
err = err + (sk * sk);
}
err = Math.Sqrt(err / n);
// computation of hnew
fac11 = Math.Pow(err, expo1);
fac = fac11 / Math.Pow(facold, beta); // LUND-stabilization
fac = Math.Max(facc2, Math.Min(facc1, (fac / SAFE))); // must have FAC1 <= HNEW/H <= FAC2
hnew = h / fac;
}
// step is accepted
if (err <= 1.0)
{
facold = Math.Max(err, 1.0e-4);
naccpt++;
for (int i = 1; i <= n; i++)
{
this.K1[i] = this.K1[i + this.K2off]; //K2[i];
y[i] = this.Ynew[i];
}
t = t + h;
if (adjust != 0 && t <= tnext)
{
htry[0] = h;
}
if (Math.Abs(hnew) > hmax)
{
hnew = hmax;
}
if (reject != 0)
{
hnew = Math.Min(Math.Abs(hnew), Math.Abs(h));
}
reject = 0;
//if (Report) Report(t, y, n);
}
// step is rejected
else
{
if (adjust != 0)
{
hnew = h / Math.Min(facc1, (fac11 / SAFE));
}
reject = 1;
if (naccpt >= 1)
{
nrejct++;
}
}
// take another step
h = hnew;
if (adjust != 0)
{
htry[0] = h;
}
nstep++;
if (nstep >= this.Itmax)
{
return(-1);
}
}
return(nfcn);
}