using System;
namespace Science.Mathematics.OrdinaryDifferentialEquation
{
///
/// Ordinary Differential Equation is solved.
///
public class EquationWithInitialCondition
{
private Function.ToLastType fdf;
private InitialCondition ic;
private double to;
private int hm;
public EquationWithInitialCondition(Function.ToLastType firstDerivativeFunction,
InitialCondition initialCondition, double interval, int howMany)
{
fdf = firstDerivativeFunction;
ic = initialCondition;
to = ic.At + interval;
hm = howMany;
}
public void Solve()
{
double[] y = ic.Value;
double[] dydx = fdf(ic.At, y);
StepperBase s = new StepperDopr5(ic.Value.Length);
s.derivs = fdf;
Output outt = new Output(hm);
Odeint dd = new Odeint(y, ic.At, to, 0.00000001, 0.00000001, 0.000001, 0.0, outt, s);
dd.integrate();
x = outt.xsave;
sol = outt.ysave;
}
private double[] x;
public double[] IndependentVariable
{
get { return x; }
}
private double[,] sol;
public double[,] Solution
{
get { return sol; }
}
}
// private class
class Odeint
{
private int MAXSTP = 50000;
private double EPS = 1.0e-6;
private int nvar;
private double x1, x2, hmin;
private bool dense;
private double[] y, dydx;
private double[] ystart;
private Output Out;
private StepperBase s;
private int nstp;
private double x, h;
public int nok;
public int nbad;
public Odeint(double[] ystartt, double xx1, double xx2, double atol, double rtol, double h1,
double hminn, Output outt, StepperBase s1)
{
nvar = ystartt.Length;
y = new Double[nvar];
dydx = new Double[nvar];
ystart = new Double[nvar];
for (int i = 0; i < nvar; i++) ystart[i] = ystartt[i];
x = xx1;
nok = 0;
nbad = 0;
x1 = xx1;
x2 = xx2;
hmin = hminn;
dense = outt.dense;
Out = outt;
s1.atol = atol;
s1.rtol = rtol;
s1.dense = outt.dense;
s = s1;
h = SIGN(h1, x2 - x1);
for (int i = 0; i < nvar; i++) y[i] = ystart[i];
Out.init(s1.neqn, x1, x2);
}
private double SIGN(double a, double b)
{
return ((b) >= 0.0 ? Math.Abs(a) : -Math.Abs(a));
}
public void integrate()
{
dydx = s.derivs(x, y);
if (dense) Out.Out(-1, x, y, s, h);
else Out.save(x, y);
for (nstp = 0; nstp < MAXSTP; nstp++)
{
if ((x + h * 1.0001 - x2) * (x2 - x1) > 0.0) h = x2 - x;
s.step(ref x, y, dydx, h);
if (s.hdid == h) ++nok; else ++nbad;
if (dense) Out.Out(nstp, x, y, s, s.hdid);
else Out.save(x, y);
if ((x - x2) * (x2 - x1) >= 0.0)
{
for (int i = 0; i < nvar; i++) ystart[i] = y[i];
if (Out.kmax > 0 && Math.Abs(Out.xsave[Out.count - 1] - x2) >
100.0 * Math.Abs(x2) * EPS) Out.save(x, y);
return;
}
if (Math.Abs(s.hnext) <= hmin)
try { throw new Science.Error(); }
catch (Science.Error e)
{
e.Write("Step size too small in Odeint");
}
h = s.hnext;
}
try { throw new Science.Error(); }
catch (Science.Error e)
{
e.Write("Too many steps in routine Odeint");
}
}
}
class Output
{
public bool dense;
public int count;
public int kmax;
public double[] xsave;
public double[,] ysave;
private int nvar;
private int nsave;
private double x1, x2, xout, dxout;
public Output()
{
kmax = -1;
dense = false;
count = 0;
}
public Output(int nsavee)
{
kmax = 500;
nsave = nsavee;
count = 0;
xsave = new double[kmax];
dense = (nsave > 0 ? true : false);
}
public void init(int neqn, double xlo, double xhi)
{
nvar = neqn;
if (kmax == -1) return;
ysave = new double[nvar, kmax];
if (dense)
{
x1 = xlo;
x2 = xhi;
xout = x1;
dxout = (x2 - x1) / nsave;
}
}
private void resize()
{
int kold = kmax;
kmax *= 2;
double[] tempvec = new double[xsave.Length];
for (int i = 0; i < xsave.Length; i++) tempvec[i] = xsave[i];
xsave = new double[kmax];
for (int i = 0; i < kold; i++) xsave[i] = tempvec[i];
double[,] tempmat = new double[ysave.GetLength(0), ysave.GetLength(1)];
for (int i = 0; i < ysave.GetLength(0); i++)
for (int j = 0; j < ysave.GetLength(1); j++) tempmat[i, j] = ysave[i, j];
ysave = new double[nvar, kmax];
for (int i = 0; i < nvar; i++)
for (int j = 0; j < kold; j++) ysave[i, j] = tempmat[i, j];
}
private void save_dense(StepperBase s, double xout, double h)
{
if (count == kmax) resize();
for (int i = 0; i < nvar; i++)
ysave[i, count] = s.dense_out(i, xout, h);
xsave[count++] = xout;
}
public void save(double x, double[] y)
{
if (kmax <= 0) return;
if (count == kmax) resize();
for (int i = 0; i < nvar; i++) ysave[i, count] = y[i];
xsave[count++] = x;
}
public void Out(int nstp, double x, double[] y, StepperBase s, double h)
{
if (!dense) try { throw new Science.Error(); }
catch (Science.Error e)
{
e.Write("dense output not set in Output!");
}
if (nstp == -1)
{
save(x, y);
xout += dxout;
}
else
{
while ((x - xout) * (x2 - x1) > 0.0)
{
save_dense(s, xout, h);
xout += dxout;
}
}
}
}
class StepperBase
{
public double atol, rtol;
public bool dense;
public double hdid;
public double hnext;
public Function.ToLastType derivs;
// public Function.ToLastType partial; // stiff case
// public Function.ToLastType jacobian; // stiff case
protected double EPS = 1.0e-6;
protected double xold;
protected double[] yout, yerr;
protected int n;
public int neqn;
public StepperBase(int nvar)
{
n = nvar;
}
public virtual void step(ref double x, double[] y, double[] dydx, double htry)
{
}
public virtual double dense_out(int i, double x, double h)
{
return 0.0;
}
}
class StepperDopr5 : StepperBase
{
double[] k2, k3, k4, k5, k6;
double[] rcont1, rcont2, rcont3, rcont4, rcont5;
double[] dydxnew;
public StepperDopr5(int nvar)
: base(nvar)
{
neqn = n;
yout = new double[n];
yerr = new double[n];
k2 = new double[n];
k3 = new double[n];
k4 = new double[n];
k5 = new double[n];
k6 = new double[n];
rcont1 = new double[n];
rcont2 = new double[n];
rcont3 = new double[n];
rcont4 = new double[n];
rcont5 = new double[n];
dydxnew = new double[n];
}
public override void step(ref double x, double[] y, double[] dydx, double htry)
{
double h = htry;
for (; ; )
{
dy(x, y, dydx, h);
double err = error(y);
if (success(err, ref h)) break;
if (Math.Abs(h) <= Math.Abs(x) * EPS) try { throw new Science.Error(); }
catch (Science.Error e)
{
e.Write("stepsize underflow in Stepperdopr5");
}
}
if (dense) prepare_dense(y, dydx, h);
for (int i = 0; i < n; i++)
{
dydx[i] = dydxnew[i];
y[i] = yout[i];
}
xold = x;
x += (hdid = h);
hnext = chnext;
}
private void dy(double x, double[] y, double[] dydx, double h)
{
int i, n = y.Length;
double c2 = 0.2, c3 = 0.3, c4 = 0.8, c5 = 8.0 / 9.0,
a21 = 0.2,
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 / 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,
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[] ytemp = new Double[n];
for (i = 0; i < n; i++)
ytemp[i] = y[i] + h * a21 * dydx[i];
k2 = derivs(x + c2 * h, ytemp);
for (i = 0; i < n; i++)
ytemp[i] = y[i] + h * (a31 * dydx[i] + a32 * k2[i]);
k3 = derivs(x + c3 * h, ytemp);
for (i = 0; i < n; i++)
ytemp[i] = y[i] + h * (a41 * dydx[i] + a42 * k2[i] + a43 * k3[i]);
k4 = derivs(x + c4 * h, ytemp);
for (i = 0; i < n; i++)
ytemp[i] = y[i] + h * (a51 * dydx[i] + a52 * k2[i] + a53 * k3[i] + a54 * k4[i]);
k5 = derivs(x + c5 * h, ytemp);
for (i = 0; i < n; i++)
ytemp[i] = y[i] + h * (a61 * dydx[i] + a62 * k2[i] + a63 * k3[i] + a64 * k4[i] + a65 * k5[i]);
double xph = x + h;
k6 = derivs(xph, ytemp);
for (i = 0; i < n; i++)
yout[i] = y[i] + h * (a71 * dydx[i] + a73 * k3[i] + a74 * k4[i] + a75 * k5[i] + a76 * k6[i]);
dydxnew = derivs(xph, yout);
for (i = 0; i < n; i++)
yerr[i] = h * (e1 * dydx[i] + e3 * k3[i] + e4 * k4[i] + e5 * k5[i] + e6 * k6[i] + e7 * dydxnew[i]);
}
private void prepare_dense(double[] y, double[] dydx, double h)
{
int n = y.Length;
double[] ytemp = new double[n];
double d1 = -12715105075.0 / 11282082432.0;
double d3 = 87487479700.0 / 32700410799.0;
double d4 = -10690763975.0 / 1880347072.0;
double d5 = 701980252875.0 / 199316789632.0;
double d6 = -1453857185.0 / 822651844.0;
double d7 = 69997945.0 / 29380423.0;
for (int i = 0; i < n; i++)
{
rcont1[i] = y[i];
double ydiff = yout[i] - y[i];
rcont2[i] = ydiff;
double bspl = h * dydx[i] - ydiff;
rcont3[i] = bspl;
rcont4[i] = ydiff - h * dydxnew[i] - bspl;
rcont5[i] = h * (d1 * dydx[i] + d3 * k3[i] + d4 * k4[i] + d5 * k5[i] + d6 * k6[i] + d7 * dydxnew[i]);
}
}
public override double dense_out(int i, double x, double h)
{
double s = (x - xold) / h;
double s1 = 1.0 - s;
return rcont1[i] + s * (rcont2[i] + s1 * (rcont3[i] + s * (rcont4[i] + s1 * rcont5[i])));
}
private double error(double[] y)
{
int n = y.Length;
double err = 0.0, sk;
for (int i = 0; i < n; i++)
{
sk = atol + rtol * Math.Max(Math.Abs(y[i]), Math.Abs(yout[i]));
err += yerr[i] / sk * yerr[i] / sk;
}
return Math.Sqrt(err / n);
}
private double chnext, errold = 1.0e-4;
private bool reject = false;
private bool success(double err, ref double h)
{
double beta = 0.0, alpha = 0.2 - beta * 0.75, safe = 0.9;
double minscale = 0.2, maxscale = 10.0;
double scale;
if (err <= 1.0)
{
if (err == 0.0) scale = maxscale;
else
{
scale = safe * Math.Pow(err, -alpha) * Math.Pow(errold, beta);
if (scale < minscale) scale = minscale;
if (scale > maxscale) scale = maxscale;
}
if (reject) chnext = h * Math.Min(scale, 1.0);
else chnext = h * scale;
errold = Math.Max(err, 1.0e-4);
reject = false;
return true;
}
else
{
scale = Math.Max(safe * Math.Pow(err, -alpha), minscale);
h *= scale;
reject = true;
return false;
}
}
}
}