Skip to content

Commit

Permalink
add the Tsit5 method
Browse files Browse the repository at this point in the history 
no interpolant yet

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
@lamont-granquist
lamont-granquist committed Nov 21, 2023
1 parent 56031e9 commit 423d774
Show file tree
Hide file tree
Showing 3 changed files with 194 additions and 3 deletions.
1 change: 1 addition & 0 deletions MechJebLib/MechJebLib.csproj
View file
Expand Up @@ -80,6 +80,7 @@
<Compile Include="ODE\DP5.cs"/>
<Compile Include="ODE\DP8.cs"/>
<Compile Include="ODE\Event.cs"/>
<Compile Include="ODE\Tsit5.cs" />
<Compile Include="Primitives\Dual.cs"/>
<Compile Include="Primitives\DualV3.cs"/>
<Compile Include="Primitives\H1.cs"/>
Expand Down
122 changes: 122 additions & 0 deletions MechJebLib/ODE/Tsit5.cs
View file
@@ -0,0 +1,122 @@
/*
* Copyright Lamont Granquist, Sebastien Gaggini and the MechJeb contributors
* SPDX-License-Identifier: LicenseRef-PD-hp OR Unlicense OR CC0-1.0 OR 0BSD OR MIT-0 OR MIT OR LGPL-2.1+
*/

using System;
using System.Collections.Generic;
using MechJebLib.Primitives;
using static MechJebLib.Utils.Statics;
using static System.Math;

// ReSharper disable CompareOfFloatsByEqualityOperator
namespace MechJebLib.ODE
{
using IVPFunc = Action<IList<double>, double, IList<double>>;

public class Tsit5 : AbstractRungeKutta
{
protected override int Order => 5;
protected override int Stages => 6;
protected override int ErrorEstimatorOrder => 4;

#region IntegrationConstants

private const double A21 = 0.161;
private const double A31 = -0.008480655492356989;
private const double A32 = 0.335480655492357;
private const double A41 = 2.8971530571054935;
private const double A42 = -6.359448489975075;
private const double A43 = 4.3622954328695815;
private const double A51 = 5.325864828439257;
private const double A52 = -11.748883564062828;
private const double A53 = 7.4955393428898365;
private const double A54 = -0.09249506636175525;
private const double A61 = 5.86145544294642;
private const double A62 = -12.92096931784711;
private const double A63 = 8.159367898576159;
private const double A64 = -0.071584973281401;
private const double A65 = -0.028269050394068383;
private const double A71 = 0.09646076681806523;
private const double A72 = 0.01;
private const double A73 = 0.4798896504144996;
private const double A74 = 1.379008574103742;
private const double A75 = -3.290069515436081;
private const double A76 = 2.324710524099774;

private const double C2 = 0.161;
private const double C3 = 0.327;
private const double C4 = 0.9;
private const double C5 = 0.9800255409045097;

private const double E1 = 0.0017800110522257773;
private const double E2 = 0.0008164344596567463;
private const double E3 = -0.007880878010261994;
private const double E4 = 0.1447110071732629;
private const double E5 = -0.5823571654525552;
private const double E6 = 0.45808210592918686;
private const double E7 = -0.015151515151515152;

#endregion

protected override void RKStep(IVPFunc f)
{
double h = Habs * Direction;

K[1].CopyFrom(Dy);

for (int i = 0; i < N; i++)
Ynew[i] = Y[i] + h * (A21 * Dy[i]);
f(Ynew, T + C2 * h, K[2]);

for (int i = 0; i < N; i++)
Ynew[i] = Y[i] + h * (A31 * K[1][i] + A32 * K[2][i]);
f(Ynew, T + C3 * h, K[3]);

for (int i = 0; i < N; i++)
Ynew[i] = Y[i] + h * (A41 * K[1][i] + A42 * K[2][i] + A43 * K[3][i]);
f(Ynew, T + C4 * h, K[4]);

for (int i = 0; i < N; i++)
Ynew[i] = Y[i] + h * (A51 * K[1][i] + A52 * K[2][i] + A53 * K[3][i] + A54 * K[4][i]);
f(Ynew, T + C5 * h, K[5]);

for (int i = 0; i < N; i++)
Ynew[i] = Y[i] + h * (A61 * K[1][i] + A62 * K[2][i] + A63 * K[3][i] + A64 * K[4][i] + A65 * K[5][i]);
f(Ynew, T + h, K[6]);

for (int i = 0; i < N; i++)
Ynew[i] = Y[i] + h * (A71 * K[1][i] + A72 * K[2][i] + A73 * K[3][i] + A74 * K[4][i] + A75 * K[5][i] + A76 * K[6][i]);

f(Ynew, T + h, K[7]);


K[7].CopyTo(Dynew);
}

protected override double ScaledErrorNorm()
{
using var err = Vn.Rent(N);

for (int i = 0; i < N; i++)
err[i] = K[1][i] * E1 + K[2][i] * E2 + K[3][i] * E3 + K[4][i] * E4 + K[5][i] * E5 + K[6][i] * E6 + K[7][i] * E7;

double error = 0.0;

for (int i = 0; i < N; i++)
{
double scale = Atol + Rtol * Max(Abs(Y[i]), Abs(Ynew[i]));
error += Powi(err[i] / scale, 2);
}

return Sqrt(error / N);
}

protected override void InitInterpolant()
{
// intentionally left blank
}

protected override void Interpolate(double x, Vn yout) => throw new NotImplementedException();
}
}
74 changes: 71 additions & 3 deletions MechJebLibTest/TwoBodyTests/ShepperdTests.cs
View file
Expand Up @@ -121,7 +121,7 @@ public void dydt(IList<double> yin, double x, IList<double> dyout)
[Fact]
private void RandomComparedToDP5()
{
var solver = new DP5 { Rtol = 1e-13, Hmin = EPS, ThrowOnMaxIter = true, Maxiter = 2000 };
var solver = new DP5 { Rtol = 1e-6, Hmin = EPS, ThrowOnMaxIter = true, Maxiter = 2000 };

const int NTRIALS = 500;

Expand Down Expand Up @@ -183,7 +183,75 @@ private void RandomComparedToDP5()
}

_testOutputHelper.WriteLine($"Successful: {count}");
Assert.True(count > 450);
Assert.True(count > NTRIALS * 0.90);
}

[Fact]
private void RandomComparedToTsit5()
{
var solver = new Tsit5 { Rtol = 1e-6, Hmin = EPS, ThrowOnMaxIter = true, Maxiter = 2000 };

const int NTRIALS = 500;

var random = new Random();

int count = 0;

for (int i = 0; i < NTRIALS; i++)
{
var r0 = new V3(4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2);
var v0 = new V3(4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2);
double dt = 10 * random.NextDouble() - 5;

(double _, double ecc, double _, double _, double _, double _, double l) =
Astro.KeplerianFromStateVectors(1.0, r0, v0);

// near-parabolic orbits are difficult for Shepperd, see the Farnocchia paper.
if (ecc < 1.01 && ecc > 0.99)
continue;

// RK methods have issue with small SLRs
if (l < 0.1)
continue;

(V3 rf, V3 vf) = Shepperd.Solve(1.0, dt, r0, v0);

V3 rf2, vf2;

using (var y0 = Vn.Rent(6))
using (var yf = Vn.Rent(6))
{
y0.Set(0, r0);
y0.Set(3, v0);

try
{
solver.Solve(_ode.dydt, y0, yf, 0, dt);
}
catch (ArgumentException) // sometimes RK method still throws
{
continue;
}

rf2 = yf.Get(0);
vf2 = yf.Get(3);
}

count++;

if (!NearlyEqual(rf, rf2, 1e-5) || !NearlyEqual(vf, vf2, 1e-5))
{
_testOutputHelper.WriteLine("r0 :" + r0 + " v0:" + v0 + " dt:" + dt + " ecc:" + ecc + "\nrf:" + rf + " vf:" + vf + "\nrf2:" +
rf2 + " vf2:" +
vf2 + "\n");
}

rf.ShouldEqual(rf2, 1e-5);
vf.ShouldEqual(vf2, 1e-5);
}

_testOutputHelper.WriteLine($"Successful: {count}");
Assert.True(count > NTRIALS * 0.90);
}

[Fact]
Expand Down Expand Up @@ -254,7 +322,7 @@ private void RandomComparedToDP8()

_testOutputHelper.WriteLine($"Successful: {count}");

Assert.True(count > 450);
Assert.True(count > NTRIALS * 0.90);
}
}
}

0 comments on commit 423d774

Please sign in to comment.