runge-Kutta Method for Solving Differential Equations c#

Program.cs

using System;
using System.Collections.Generic;

namespace Orbital
{

	public class RK
	{

		public struct fValues
		{
			public double y1;
			public double y2;
		}

		double[] a = new double[] { 0, 1 / 4, 3 / 8, 12 / 13, 1, 1 / 2 };
		double[,] b = new double[,] {
			{ 0, 0, 0, 0, 0 },
			{ 1d / 4d, 0, 0, 0, 0 },
			{ 3d / 32d, 9d / 32d, 0, 0, 0 }, {
				1932d / 2197d,
				-7200d / 2197d,
				7296d / 2197d,
				0,
				0
			}, {
				439d / 216d,
				-8d,
				3680d / 513d,
				-845d / 4104d,
				0
			}, {
				-8d / 27d,
				2,
				-3544d / 2565d,
				1859d / 4104d,
				-11d / 40d
			},
		};
		double[] c4 = new double[] { 25d / 216d, 0, 1408d / 2565d, 2197d / 4104d, -1d / 5d, 0 };
		double[] c5 = new double[] { 16d / 135d, 0, 6656d / 12825d, 28561d / 56430d, -9d / 50d, 2d / 55d };

		public void rk45(Func<double, double[], double[]> mainFunc, double t0, double tf, double[] y0, List<double> tout, List<double[]> yout, double tol = 1E-8)
		{

			double t = t0;
			double[] y = y0;

			tout.Add(t);

			yout.Add(y0);

			double h = (tf - t0) / 100; // Assumed initial time step.
			while (t < tf)
			{
				double hmin = 16d * nextSmallest(t);
				double ti = t;
				double[] yi = y;
				List<double[]> f = new List<double[]>();
				//...Evaluate the time derivative(s) at six points within the current interval:
				for (int i = 0; i < 6; i++)
				{
					double t_inner = ti + a[i] * h;
					double[] y_inner = yi;
					for (int j = 0; j <= i - 1; j++)
					{
						y_inner = matrix1dSum(y_inner, scalar1d(h * b[i, j], f[j]));

					}
					f.Add(mainFunc(t_inner, y_inner));
				}
				//...Compute the maximum truncation error:
				double te_max = 0;
				double[] aux = scalar1d((c4[0] - c5[0]), f[0]);
				for (int i = 1; i < 6; i++)
				{
					aux=matrix1dSum(aux,scalar1d((c4[i] - c5[i]), f[i]));
				}
				aux=scalar1d(h, aux);

			   Array.ForEach(aux, (x) =>te_max = Math.Max(te_max, Math.Abs(x)));


				double ymax = 0;
				Array.ForEach(y, (x) => ymax = Math.Max(ymax, Math.Abs(x)));
				double te_allowed = tol * Math.Max(ymax, 1.0);

				//..Compute the fractional change in step size:
				double delta = Math.Pow((te_allowed / (te_max + nextSmallest(0))), 1d / 5d);

				//..If the truncation error is in bounds, then update the solution:
				if (te_max <= te_allowed)
				{
					h = Math.Min(h, tf - t);
					t = t + h;
					y = yi;
					for (int i = 0; i < 6; i++)
					{
						y = matrix1dSum(y, scalar1d(h * c5[i], f[i]));
					}

					tout.Add(t);
					yout.Add(y);
				}

				//%...Update the time step:
				h = Math.Min(delta * h, 4 * h);
				if (h < hmin)
				{
					//					fprintf(['\n\n Warning: Step size fell below its minimum\n'...
					//						' allowable value (%g) at time %g.\n\n'], hmin, t)
					return;
				}
			}

		}


		public double[] scalar1d(double number, double[] matrix)
		{
			int w = matrix.GetLength(0);
			double[] result = new double[w];
			for (int i = 0; i < w; i++)
			{

				result[i] = number * matrix[i];

			}

			return result;
		}

		public double[] matrix1dSum(double[] matrix1, double[] matrix2)
		{
			int w1 = matrix1.GetLength(0);



			double[] result = new double[w1];

			for (int i = 0; i < w1; i++)
			{

				result[i] = matrix1[i] + matrix2[i];

			}

			return result;
		}

		public double[,] matrixSum(double[,] matrix1, double[,] matrix2)
		{
			int w1 = matrix1.GetLength(0);
			int h1 = matrix1.GetLength(1);


			double[,] result = new double[h1, w1];

			for (int i = 0; i < w1; i++)
			{
				for (int j = 0; j < h1; j++)
				{
					result[j, i] = matrix1[i, j] + matrix2[i, j];
				}
			}

			return result;
		}


		public double nextSmallest(double value)
		{
			long bits = BitConverter.DoubleToInt64Bits(value);
			if (value > 0)
				return value-BitConverter.Int64BitsToDouble(bits - 1);
			else if (value < 0)
				return value-BitConverter.Int64BitsToDouble(bits + 1);
			else
				return -double.Epsilon;
		}

		static int Main(string[] args)
		{

			double minutes = 60;
			double x0 = 6500;
			double v0 = 7.8;
			double[] y0 = new double[] { x0, v0 };
			double t0 = 0;
			double tf = 70 * minutes;
			RK rk = new RK();
			List<double> times = new List<double>();
			List<double[]> values = new List<double[]>();
			rk.rk45(rates, t0, tf, y0, times, values);
			return 0;
		}

		static double[] rates(double t, double[] f)
		{
			double x = f[0];
			double Dx = f[1];
			double D2x = -398600 / (x * x);
			return new double[] { Dx, D2x };
		}
	}


}