/
AbstractIVP.cs
165 lines (130 loc) · 5.29 KB
/
AbstractIVP.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
/*
* Copyright Lamont Granquist, Sebastien Gaggini and the MechJeb contributors
* SPDX-License-Identifier: MIT-0 OR LGPL-2.1+ OR CC0-1.0
*/
using System;
using System.Collections.Generic;
using System.Threading;
using MechJebLib.Primitives;
using static MechJebLib.Utils.Statics;
#nullable enable
// ReSharper disable CompareOfFloatsByEqualityOperator
namespace MechJebLib.Core.ODE
{
using IVPFunc = Action<Vn, double, Vn>;
using IVPEvent = Func<double, Vn, Vn, (double x, bool dir, bool stop)>;
public abstract class AbstractIVP
{
/// <summary>
/// Minimum h step (may be violated on the last step or before an event).
/// </summary>
public double Hmin { get; set; } = EPS;
/// <summary>
/// Maximum h step.
/// </summary>
public double Hmax { get; set; }
/// <summary>
/// Maximum number of steps.
/// </summary>
public double Maxiter { get; set; } = 2000;
/// <summary>
/// Desired relative tolerance.
/// </summary>
public double Rtol { get; set; } = 1e-9;
/// <summary>
/// Desired absolute tolerance.
/// </summary>
public double Atol { get; set; } = 1e-9;
/// <summary>
/// Starting step-size (can be zero for automatic guess).
/// </summary>
public double Hstart { get; set; }
/// <summary>
/// Interpolants are pulled on an evenly spaced grid
/// </summary>
public int Interpnum { get; set; } = 20;
/// <summary>
/// Throw exception when MaxIter is hit (PVG optimizer works better with this set to false).
/// </summary>
public bool ThrowOnMaxIter { get; set; } = true;
public CancellationToken CancellationToken { get; set; }
/// <summary>
/// Dormand Prince 5(4)7FM ODE integrator (aka DOPRI5 aka ODE45)
/// </summary>
/// <param name="f"></param>
/// <param name="y0"></param>
/// <param name="yf"></param>
/// <param name="t0"></param>
/// <param name="tf"></param>
/// <param name="interpolant"></param>
/// <param name="events"></param>
/// <exception cref="ArgumentException"></exception>
public void Solve(IVPFunc f, IReadOnlyList<double> y0, IList<double> yf, double t0, double tf, Hn? interpolant = null,
List<IVPEvent>? events = null)
{
int n = y0.Count;
using var ynew = Vn.Rent(n);
using var dynew = Vn.Rent(n);
using var dy = Vn.Rent(n);
using var y = Vn.Rent(n);
using IDisposable data = SetupData(n);
int direction = t0 != tf ? Math.Sign(tf - t0) : 1;
double habs = SelectInitialStep(t0, tf);
double t = t0;
y.CopyFrom(y0);
double niter = 0;
int interpCount = 1;
f(y, t, dy);
interpolant?.Add(t, y, dy);
while (t != tf)
{
CancellationToken.ThrowIfCancellationRequested();
double h = habs * direction;
double tnew = t + h;
if (direction * (tnew - tf) > 0)
tnew = tf;
h = tnew - t;
habs = Math.Abs(h);
habs = Step(f, t, habs, direction, y, dy, ynew, dynew, data);
if (events != null)
{
}
// extract a low fidelity interpolant
if (interpolant != null)
{
while (interpCount < Interpnum)
{
double tinterp = t0 + (tf - t0) * interpCount / Interpnum;
if (!tinterp.IsWithin(t, tnew))
break;
using var yinterp = Vn.Rent(n);
using var finterp = Vn.Rent(n);
PrepareInterpolant(habs, direction, y, dy, ynew, dynew, data);
Interpolate(tinterp, t, h, y, yinterp, data);
f(yinterp, tinterp, finterp);
interpolant?.Add(tinterp, yinterp, finterp);
interpCount++;
}
}
// take a step
ynew.CopyTo(y);
dynew.CopyTo(dy);
t = tnew;
// handle max iterations
if (Maxiter > 0 && niter++ > Maxiter)
{
if (ThrowOnMaxIter)
throw new ArgumentException("maximum iterations exceeded");
break;
}
}
interpolant?.Add(t, y, dy);
y.CopyTo(yf);
}
protected abstract double Step(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, object data);
protected abstract double SelectInitialStep(double t0, double tf);
protected abstract void PrepareInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, object data);
protected abstract void Interpolate(double x, double t, double h, Vn y, Vn yout, object data);
protected abstract IDisposable SetupData(int n);
}
}