Skip to content

Commit

Permalink
Analytic coast integrations with PVG
Browse files Browse the repository at this point in the history
PVG is now fully analytic

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
lamont-granquist committed May 22, 2023
1 parent 02fab0d commit 4be1e06
Show file tree
Hide file tree
Showing 7 changed files with 182 additions and 53 deletions.
45 changes: 23 additions & 22 deletions MechJeb2/MechJebLib/Core/TwoBody/Shepperd.cs
Expand Up @@ -5,6 +5,10 @@ namespace MechJebLib.Core.TwoBody
{
public static class Shepperd
{
// NOTE: this isn't a faithful reproduction of Shepperd's method, it works
// better than Shepperd's method and uses different constants in the continued
// faction, although it otherwise has almost the same "shape" of algorithm.
// I don't yet understand exactly why or how it works.
public static (V3 rf, V3 vf) Solve(double mu, double tau, V3 ri, V3 vi)
{
double tolerance = 1.0e-12;
Expand Down Expand Up @@ -152,7 +156,8 @@ public static (V3 rf, V3 vf) Solve(double mu, double tau, V3 ri, V3 vi)
// [ 𝛿r ] = [ stm00 stm01 ] [ r ]
// [ 𝛿v ] = [ stm10 stm11 ] [ v ]
//
// NOTE: is it not at all clear to me why this version differs in several ways from the prior version.
// NOTE: this is a fairly faithful reproduction of Shepperd's method with a little added bisection method in
// case Newton's Method gets into trouble.
//
public static ( V3 rf, V3 vf, M3 stm00, M3 stm01, M3 stm10, M3 stm11) Solve2(double mu, double tau, V3 ri, V3 vi)

Expand Down Expand Up @@ -180,13 +185,9 @@ public static ( V3 rf, V3 vf, M3 stm00, M3 stm01, M3 stm10, M3 stm11) Solve2(dou

umin = -umax;

double delu;
double delu = 0.0;

if (beta <= 0.0)
{
delu = 0.0;
}
else
if (beta > 0.0)
{
double beta3 = beta * beta * beta;
double p = 2.0 * Math.PI * mu * 1 / Math.Sqrt(beta3);
Expand Down Expand Up @@ -301,7 +302,7 @@ public static ( V3 rf, V3 vf, M3 stm00, M3 stm01, M3 stm10, M3 stm11) Solve2(dou
V3 rf = f * ri + g * vi;
V3 vf = ff * ri + gg * vi;

uu = g * u2 + 3 * mu * uu;
double w = g * u2 + 3 * mu * uu;
double a0 = mu / (r0 * r0 * r0);
double a1 = mu / (r1 * r1 * r1);

Expand All @@ -319,31 +320,31 @@ public static ( V3 rf, V3 vf, M3 stm00, M3 stm01, M3 stm10, M3 stm11) Solve2(dou
m[2, 1] = fm * u2;
m[2, 2] = g * u2;

m[0, 0] -= a0 * a1 * uu;
m[0, 2] -= a1 * uu;
m[2, 0] -= a0 * uu;
m[2, 2] -= uu;
m[0, 0] -= a0 * a1 * w;
m[0, 2] -= a1 * w;
m[2, 0] -= a0 * w;
m[2, 2] -= w;

for (int i = 0; i < 3; i++)
{
double t001 = rf[i] * m[1, 0] + vf[i] * m[1, 1];
double t002 = rf[i] * m[2, 0] + vf[i] * m[2, 1];
double t001 = rf[i] * m[1, 0] + vf[i] * m[2, 0];
double t002 = rf[i] * m[1, 1] + vf[i] * m[2, 1];

double t011 = rf[i] * m[1, 1] + vf[i] * m[1, 2];
double t012 = rf[i] * m[2, 1] + vf[i] * m[2, 2];
double t011 = rf[i] * m[1, 1] + vf[i] * m[2, 1];
double t012 = rf[i] * m[1, 2] + vf[i] * m[2, 2];

double t101 = rf[i] * m[0, 0] + vf[i] * m[0, 1];
double t102 = rf[i] * m[1, 0] + vf[i] * m[1, 1];
double t101 = rf[i] * m[0, 0] + vf[i] * m[1, 0];
double t102 = rf[i] * m[0, 1] + vf[i] * m[1, 1];

double t111 = rf[i] * m[0, 1] + vf[i] * m[0, 2];
double t112 = rf[i] * m[1, 1] + vf[i] * m[1, 2];
double t111 = rf[i] * m[0, 1] + vf[i] * m[1, 1];
double t112 = rf[i] * m[0, 2] + vf[i] * m[1, 2];

for (int j = 0; j < 3; j++)
{
stm00[i, j] = t001 * ri[j] + t002 * vi[j];
stm01[i, j] = t011 * ri[j] + t012 * vi[j];
stm10[i, j] = -t101 * ri[j] + t102 * vi[j];
stm11[i, j] = -t111 * ri[j] + t112 * vi[j];
stm10[i, j] = -t101 * ri[j] - t102 * vi[j];
stm11[i, j] = -t111 * ri[j] - t112 * vi[j];
}
}

Expand Down
3 changes: 2 additions & 1 deletion MechJeb2/MechJebLib/PVG/Ascent.cs
Expand Up @@ -60,7 +60,8 @@ public void Run()
{
// FIXME: the analytic coast integrator is definitely buggy so the Shepperd solver must be buggy
if (phase.Coast)
phase.Integrator = new VacuumThrustIntegrator();
//phase.Integrator = new VacuumThrustIntegrator();
phase.Integrator = new VacuumCoastAnalytic();
else
// FIXME: make a debug setting to flip between these somewhere
phase.Integrator = new VacuumThrustAnalytic();
Expand Down
7 changes: 4 additions & 3 deletions MechJeb2/MechJebLib/PVG/Integrators/VacuumCoastAnalytic.cs
Expand Up @@ -18,14 +18,15 @@ public void Integrate(Vn yin, Vn yfout, Phase phase, double t0, double tf)
using var y0 = ArrayWrapper.Rent(yin);
using var yf = ArrayWrapper.Rent(yfout);

(V3 rf, V3 vf, M3 stm00, M3 stm01, M3 stm10, M3 stm11) = Shepperd.Solve2(1.0, tf - t0, y0.R, yf.V);
(V3 rf, V3 vf, M3 stm00, M3 stm01, M3 stm10, M3 stm11) = Shepperd.Solve2(1.0, tf - t0, y0.R, y0.V);

yf.R = rf;
yf.V = vf;

yf.PV = stm00 * yf.PV + stm01 * yf.PR;
yf.PR = stm10 * yf.PV + stm11 * yf.PR;
yf.PV = stm00 * y0.PV - stm01 * y0.PR;
yf.PR = -stm10 * y0.PV + stm11 * y0.PR ;

yf.M = y0.M;
yf.Pm = y0.Pm;

yf.DV = y0.DV;
Expand Down
10 changes: 10 additions & 0 deletions MechJeb2/MechJebLib/Primitives/M3.cs
Expand Up @@ -288,6 +288,16 @@ public bool Equals(M3 other)
return res;
}

/// <summary>
/// The additive inverse of the matrix.
/// </summary>
/// <param name="m">input matrix</param>
/// <returns>negative matrix</returns>
public static M3 operator -(M3 m)
{
return m * -1;
}

/// <summary>
/// This tests for strict equality between two matricies on all values. Returns false in the presence of NaN values.
/// </summary>
Expand Down
35 changes: 35 additions & 0 deletions MechJebLibTest/Maths/TwoBody/ShepperdTests.cs
Expand Up @@ -59,6 +59,41 @@ private void RandomForwardAndBack()
}
}

[Fact]
private void RandomForwardAndBack2()
{
const int NTRIALS = 5;

var random = new Random();

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 = 40 * random.NextDouble() - 20;

// XXX: this probably needs a test to reject random orbits that are near-parabolic. See the
// Farnocchia paper.

(V3 rf, V3 vf, _, _, _, _) = Shepperd.Solve2(1.0, dt, r0, v0);
(V3 rp, V3 vp, _, _, _, _) = Shepperd.Solve2(1.0, -dt, rf, vf);

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

if ((rp - r0).magnitude / r0.magnitude > 1e-8 || (vp - v0).magnitude / v0.magnitude > 1e-8)
{
_testOutputHelper.WriteLine(r0 + " " + v0);
}

rp.ShouldEqual(r0, 1e-8);
vp.ShouldEqual(vp, 1e-8);
}
}

private readonly VacuumKernel _ode = new VacuumKernel();

private class VacuumKernel
Expand Down
55 changes: 28 additions & 27 deletions MechJebLibTest/MechJebLibTest.csproj
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="4.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')"/>
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
<PropertyGroup>
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
Expand Down Expand Up @@ -38,10 +38,10 @@
<Reference Include="Assembly-CSharp, Version=0.0.0.0, Culture=neutral, PublicKeyToken=null">
<HintPath>..\..\..\..\..\Library\Application Support\Steam\steamapps\common\Kerbal Space Program\KSP.app\Contents\Resources\Data\Managed\Assembly-CSharp.dll</HintPath>
</Reference>
<Reference Include="System"/>
<Reference Include="System.Core"/>
<Reference Include="System.Data"/>
<Reference Include="System.Xml"/>
<Reference Include="System" />
<Reference Include="System.Core" />
<Reference Include="System.Data" />
<Reference Include="System.Xml" />
<Reference Include="xunit.abstractions, Version=2.0.0.0, Culture=neutral, PublicKeyToken=8d05b1bb7a6fdb6c">
<HintPath>..\packages\xunit.abstractions.2.0.0\lib\net35\xunit.abstractions.dll</HintPath>
</Reference>
Expand All @@ -56,35 +56,36 @@
</Reference>
</ItemGroup>
<ItemGroup>
<Compile Include="AssertionExtensions.cs"/>
<Compile Include="Control\PIDLoopTests.cs"/>
<Compile Include="Maths\BisectionTests.cs"/>
<Compile Include="Maths\BS3Tests.c.cs"/>
<Compile Include="Maths\DP5Tests.cs"/>
<Compile Include="Maths\FunctionsTests.cs"/>
<Compile Include="Maths\BrentRootTests.cs"/>
<Compile Include="Maths\GoodingTests.cs"/>
<Compile Include="Maths\TwoBody\FarnocchiaTests.cs"/>
<Compile Include="Maths\TwoBody\ShepperdTests.cs"/>
<Compile Include="Properties\AssemblyInfo.cs"/>
<Compile Include="PVG\AscentTests\BuggyTests.cs"/>
<Compile Include="PVG\AscentTests\TheStandardTests.cs"/>
<Compile Include="PVG\AscentTests\Titan2Tests.cs"/>
<Compile Include="PVG\Integrators\VacuumThrustIntegratorTests.cs"/>
<Compile Include="Structs\HTests.cs"/>
<Compile Include="Structs\M3Tests.cs"/>
<Compile Include="Structs\Q3Tests.cs"/>
<Compile Include="Structs\V3Tests.cs"/>
<Compile Include="TestInitialization.cs"/>
<Compile Include="Utils\StaticTests.cs"/>
<Compile Include="AssertionExtensions.cs" />
<Compile Include="Control\PIDLoopTests.cs" />
<Compile Include="Maths\BisectionTests.cs" />
<Compile Include="Maths\BS3Tests.c.cs" />
<Compile Include="Maths\DP5Tests.cs" />
<Compile Include="Maths\FunctionsTests.cs" />
<Compile Include="Maths\BrentRootTests.cs" />
<Compile Include="Maths\GoodingTests.cs" />
<Compile Include="Maths\TwoBody\FarnocchiaTests.cs" />
<Compile Include="Maths\TwoBody\ShepperdTests.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="PVG\AscentTests\BuggyTests.cs" />
<Compile Include="PVG\AscentTests\TheStandardTests.cs" />
<Compile Include="PVG\AscentTests\Titan2Tests.cs" />
<Compile Include="PVG\Integrators\VacuumCoastAnalyticTests.cs" />
<Compile Include="PVG\Integrators\VacuumThrustIntegratorTests.cs" />
<Compile Include="Structs\HTests.cs" />
<Compile Include="Structs\M3Tests.cs" />
<Compile Include="Structs\Q3Tests.cs" />
<Compile Include="Structs\V3Tests.cs" />
<Compile Include="TestInitialization.cs" />
<Compile Include="Utils\StaticTests.cs" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\MechJeb2\MechJeb2.csproj">
<Project>{9fc90ae6-c2e4-47f1-a6d1-db4731a40be3}</Project>
<Name>MechJeb2</Name>
</ProjectReference>
</ItemGroup>
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets"/>
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
<!-- To modify your build process, add your task inside one of the targets below and uncomment it.
Other similar extension points exist, see Microsoft.Common.targets.
<Target Name="BeforeBuild">
Expand Down
80 changes: 80 additions & 0 deletions MechJebLibTest/PVG/Integrators/VacuumCoastAnalyticTests.cs
@@ -0,0 +1,80 @@
using System;
using AssertExtensions;
using MechJebLib.Core.TwoBody;
using MechJebLib.Primitives;
using MechJebLib.PVG;
using MechJebLib.PVG.Integrators;
using Xunit;
using Xunit.Abstractions;
using static MechJebLib.Utils.Statics;

namespace MechJebLibTest.PVG
{
public class VacuumCoastAnalyticTests
{
private readonly ITestOutputHelper _testOutputHelper;

public VacuumCoastAnalyticTests(ITestOutputHelper testOutputHelper)
{
_testOutputHelper = testOutputHelper;
}

[Fact]
void RandomComparedToIntegrated()
{
const int NTRIALS = 50;

var random = new Random();

var integrated = new VacuumThrustIntegrator();
var analytic = new VacuumCoastAnalytic();

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);
var pr0 = new V3(4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2);
var pv0 = new V3(4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2);
double dt = 40 * random.NextDouble() - 20;

using var yin = Vn.Rent(ArrayWrapper.ARRAY_WRAPPER_LEN);
using var yout = Vn.Rent(ArrayWrapper.ARRAY_WRAPPER_LEN);
using var yout2 = Vn.Rent(ArrayWrapper.ARRAY_WRAPPER_LEN);

using var y0 = ArrayWrapper.Rent(yin);
using var yf = ArrayWrapper.Rent(yout);
using var yf2 = ArrayWrapper.Rent(yout2);

y0.R = r0;
y0.V = v0;
y0.PV = pv0;
y0.PR = pr0;
y0.M = 1;

var phase = Phase.NewFixedCoast(1.0, dt, 1);
phase.Normalized = true;

try
{
integrated.Integrate(yin, yout, phase, 0, dt);
}
catch (ArgumentException)
{
// the ODE integrator can throw iterations exceeded for certain initial
// conditions it inherently has issues with.
continue;
}

analytic.Integrate(yin, yout2, phase, 0, dt);

if (!NearlyEqual(yf.R, yf2.R, 1e-8) || !NearlyEqual(yf.V, yf2.V, 1e-8) || !NearlyEqual(yf.PV, yf2.PV, 1e-8) || !NearlyEqual(yf.PR, yf2.PR, 1e-8))
{
_testOutputHelper.WriteLine("r0 :" + r0 + " v0:" + v0 + " dt:" + dt + "\nrf:" + yf.R + " vf:" + yf.V + "\nrf2:" + yf2.R + " vf2:" +
yf2.V + "\n");
_testOutputHelper.WriteLine("pr0 :" + pr0 + " pv0:" + pv0 + " dt:" + dt + "\npv:" + yf.PV + " pr:" + yf.PR + "\npv2:" + yf2.PV + " pr2:" +
yf2.PR + "\n");
}
}
}
}
}

0 comments on commit 4be1e06

Please sign in to comment.