private void GetDailyTemps()
{
double[] tDays = new double[500];
double[] tTemps = new double[500];
CubicSpline minInterpolator = CubicSpline.InterpolateNatural(tDays, tTemps);
//minInterpolator.Interpolate(blah);
}
static void Main(string[] args)
{
var xvec = new DenseVector(new double[] { 0.0, 1.0, 2.0, 3.0, 4.0 });
var yvec = new DenseVector(new double[] { 3.0, 2.7, 2.3, 1.6, 0.2 });
Debug.WriteLine("Input Data Table");
Debug.WriteLine($"{"x",column_width} {"y",column_width}");
for (int i = 0; i < xvec.Count; i++)
{
Debug.WriteLine($"{xvec[i],column_width:G5} {yvec[i],column_width:G5}");
}
Debug.WriteLine(" ");
var cs = CubicSpline.InterpolateNatural(xvec, yvec);
var x = new DenseVector(15);
var y = new DenseVector(x.Count);
var dydx = new DenseVector(x.Count);
Debug.WriteLine("Interpoaltion Results Table");
Debug.WriteLine($"{"x",column_width} {"y",column_width} {"dy/dx",column_width}");
for (int i = 0; i < x.Count; i++)
{
x[i] = (4.0 * i) / (x.Count - 1);
y[i] = cs.Interpolate(x[i]);
dydx[i] = cs.Differentiate(x[i]);
Debug.WriteLine($"{x[i],column_width:G5} {y[i],column_width:G5} {dydx[i],column_width:G5}");
}
}
public RTandInt Interpolate(double[] starttimes, double[] intensities)
{
List <double> intensityList = new List <double>();
List <double> starttimesList = new List <double>();
//First step is interpolating a spline, then choosing the pointsat which to add values:
CubicSpline interpolation = CubicSpline.InterpolateNatural(starttimes, intensities);
//Now we work out how many to add so we reach at least 100 datapoints and add them:
double stimesinterval = starttimes.Last() - starttimes[0];
int numNeeded = 100 - starttimes.Count();
double intervals = stimesinterval / numNeeded;
intensityList = intensities.OfType <double>().ToList();
starttimesList = starttimes.OfType <double>().ToList();
for (int i = 0; i < numNeeded; i++)
{
double placetobe = starttimes[0] + (intervals * i);
//insert newIntensity into the correct spot in the array
for (int currentintensity = 0; currentintensity < 100; currentintensity++)
{
if (starttimesList[currentintensity] < placetobe)
{
continue;
}
else
{
if (currentintensity > 0)
{
if (starttimesList[currentintensity] == placetobe)
{
placetobe = placetobe + 0.01;
}
double newIntensity = interpolation.Interpolate(placetobe);
intensityList.Insert(currentintensity, newIntensity);
starttimesList.Insert(currentintensity, placetobe);
}
else
{
if (starttimesList[currentintensity] == placetobe)
{
placetobe = placetobe - 0.01;
}
double newIntensity = interpolation.Interpolate(placetobe);
intensityList.Insert(currentintensity, newIntensity);
starttimesList.Insert(currentintensity, placetobe);
}
break;
}
}
}
RTandInt ri = new RTandInt();
ri.intensities = intensityList.Select(item => Convert.ToDouble(item)).ToArray();
ri.starttimes = starttimesList.Select(item => Convert.ToDouble(item)).ToArray();
return(ri);
}
public Func <double, double> CreateInterpolator(IEnumerable <double> xCoords, IEnumerable <double> yCoords)
{
if (xCoords == null)
{
throw new ArgumentNullException(nameof(xCoords));
}
if (yCoords == null)
{
throw new ArgumentNullException(nameof(yCoords));
}
if (xCoords.Count() != yCoords.Count())
{
throw new ArgumentException("xCoords and yCoords must have the same number of elements.");
}
if (xCoords.Count() == 1) // Trivial case of a single point
{
double singleY = yCoords.Single();
return(x => singleY);
}
var cubicSpline = CubicSpline.InterpolateNatural(xCoords, yCoords);
return(cubicSpline.Interpolate);
}
protected void UpdateInterpolation()
{
int count = _keyFrames.Count;
var xs = new double[count];
var ys = new double[count];
for (int i = 0; i < count; ++i)
{
xs[i] = (double)_keyFrames[i].time;
ys[i] = (double)_keyFrames[i].value;
}
if (count <= 1)
{
_interpolation = StepInterpolation.Interpolate(xs, ys);
}
else if (count <= 2)
{
_interpolation = LinearSpline.Interpolate(xs, ys);
}
else if (count <= 3)
{
_interpolation = MathNet.Numerics.Interpolate.Polynomial(xs, ys);
}
else if (count <= 4)
{
_interpolation = CubicSpline.InterpolateNatural(xs, ys);
}
else
{
_interpolation = CubicSpline.InterpolateAkima(xs, ys);
}
}
public void NaturalFitsAtSamplePoints()
{
IInterpolation it = CubicSpline.InterpolateNatural(_t, _y);
for (int i = 0; i < _y.Length; i++)
{
Assert.AreEqual(_y[i], it.Interpolate(_t[i]), "A Exact Point " + i);
}
}
public void NaturalSupportsLinearCase(int samples)
{
LinearInterpolationCase.Build(out var x, out var y, out var xtest, out var ytest, samples);
IInterpolation it = CubicSpline.InterpolateNatural(x, y);
for (int i = 0; i < xtest.Length; i++)
{
Assert.AreEqual(ytest[i], it.Interpolate(xtest[i]), 1e-15, "Linear with {0} samples, sample {1}", samples, i);
}
}
public InterpolateFromCSVStrategy()
{
airfoils = new Dictionary <string, AirfoilData>();
// Read data (drag and lift coefficient) from files
foreach (var airfoil in airfoilsArr)
{
string dragCoefPath = Path.Combine(Application.streamingAssetsPath, airfoil + "_drag.csv");
string liftCoefPath = Path.Combine(Application.streamingAssetsPath, airfoil + "_lift.csv");
if (File.Exists(dragCoefPath) && File.Exists(liftCoefPath))
{
string[] dragLines = File.ReadAllLines(dragCoefPath);
double[] anglesForDrag = new double[dragLines.Length];
double[] coefsForDrag = new double[dragLines.Length];
for (int i = 0; i < dragLines.Length; i++)
{
var line = dragLines[i];
string[] arr = line.Split(',');
anglesForDrag[i] = double.Parse(arr[0], System.Globalization.CultureInfo.InvariantCulture);
coefsForDrag[i] = double.Parse(arr[1], System.Globalization.CultureInfo.InvariantCulture);
}
CubicSpline dragCubicSpline = CubicSpline.InterpolateNatural(anglesForDrag, coefsForDrag);
// Interpolating lift coefficient from tables in file
string[] liftLines = File.ReadAllLines(liftCoefPath);
double[] anglesForLift = new double[liftLines.Length];
double[] coefsForLift = new double[liftLines.Length];
for (int i = 0; i < liftLines.Length; i++)
{
var line = liftLines[i];
string[] arr = line.Split(',');
anglesForLift[i] = double.Parse(arr[0], System.Globalization.CultureInfo.InvariantCulture);
coefsForLift[i] = double.Parse(arr[1], System.Globalization.CultureInfo.InvariantCulture);
}
CubicSpline liftCubicSpline = CubicSpline.InterpolateNatural(anglesForLift, coefsForLift);
airfoils.Add(airfoil, new AirfoilData(liftCubicSpline, dragCubicSpline));
}
else
{
Debug.LogError("Could not load drag or coefficient table file for " + airfoil);
}
}
selectedAirfoil = "naca0012";
selectedLiftInterpolation = airfoils[selectedAirfoil].liftCubicSpline;
selectedDragInterpolation = airfoils[selectedAirfoil].dragCubicSpline;
}
private decimal PlotValue(decimal[] DataPoints, decimal Position)
{
CubicSpline oCs = CubicSpline.InterpolateNatural(
DataPoints.Select((s, i2) => new { i2, s })
.Select(t => Convert.ToDouble(t.i2)).ToArray(),
DataPoints.Select(s => Convert.ToDouble(s)).ToArray());
return(Convert.ToDecimal(oCs.Interpolate(Convert.ToDouble(Position))));
}
/// <summary>
/// Interpolation of the interface points onto the equidistant Fourier points
/// </summary>
protected void InterpolateOntoFourierPoints(MultidimensionalArray interP, double[] samplP)
{
int numP = interP.Lengths[0];
// set interpolation data
double[] independentVal = new double[numP + 2];
double[] dependentVal = new double[numP + 2];
for (int sp = 1; sp <= numP; sp++)
{
independentVal[sp] = interP[sp - 1, 0];
dependentVal[sp] = interP[sp - 1, 1];
}
// extend the interpolation data for sample points at the boundary of the domain
independentVal[0] = interP[numP - 1, 0] - DomainSize;
dependentVal[0] = interP[numP - 1, 1];
independentVal[numP + 1] = interP[0, 0] + DomainSize;
dependentVal[numP + 1] = interP[0, 1];
switch (this.InterpolationType)
{
case Interpolationtype.LinearSplineInterpolation:
//LinearSplineInterpolation LinSpline = new LinearSplineInterpolation();
//LinSpline.Initialize(independentVal, dependentVal);
var LinSpline = LinearSpline.Interpolate(independentVal, dependentVal);
for (int sp = 0; sp < numFp; sp++)
{
samplP[sp] = LinSpline.Interpolate(FourierP[sp]);
//invDFT_coeff[sp] = (Complex)samplP[sp];
}
break;
case Interpolationtype.CubicSplineInterpolation:
//CubicSplineInterpolation CubSpline = new CubicSplineInterpolation();
//CubSpline.Initialize(independentVal, dependentVal);
var CubSpline = CubicSpline.InterpolateNatural(independentVal, dependentVal);
for (int sp = 0; sp < numFp; sp++)
{
samplP[sp] = CubSpline.Interpolate(FourierP[sp]);
//invDFT_coeff[sp] = (Complex)samplP[sp];
}
break;
default:
throw new NotImplementedException();
}
}
public double Interpolate(double x)
{
//double value = interpolation.Interpolate(x);
if (_xRange.Length < 3 || _yRange.Length < 3)
{
//return LinearSpline.InterpolateSorted(_xRange, _yRange).Interpolate(x);
return(LinearSpline.Interpolate(_xRange, _yRange).Interpolate(x));
}
else
{
return(CubicSpline.InterpolateNatural(_xRange, _yRange).Interpolate(x));
}
}
static void Main(string[] args)
{
// Type code here.
var xvec = new DenseVector(new double[] { 0.0, 1.0, 2.0, 3.0, 4.0 });
var yvec = new DenseVector(new double[] { 3.0, 2.7, 2.2, 1.6, 0.2 });
CubicSpline cs = CubicSpline.InterpolateNatural(xvec, yvec);
Console.WriteLine($"{"x",column_width} {"y",column_width} {"dy/dx",column_width}");
for (int i = 0; i < xvec.Count; i++)
{
double dydx = cs.Differentiate(xvec[i]);
Console.WriteLine($"{xvec[i],column_width:G5} {yvec[i],column_width:G5} {dydx,column_width:G5}");
}
}
private static float[] Interpolate(float[] samples, int newSize, int cubicSplineFactor)
{
var xCoordinates = Array.ConvertAll(Enumerable.Range(0, samples.Length).ToArray(), Convert.ToDouble);
var yCoordinates = Array.ConvertAll(samples, Convert.ToDouble);
var interpolatedSamples = new List <float>();
var cubicSpline = CubicSpline.InterpolateNatural(xCoordinates, yCoordinates);
for (var n = 0; n < newSize * cubicSplineFactor; n++)
{
interpolatedSamples.Add((float)cubicSpline.Interpolate((double)n * samples.Length / (cubicSplineFactor * newSize)));
}
return(interpolatedSamples.ToArray());
}
public void InterpolateCurveNatural()
{
List <SensitivityPoint> smoothCurve = new List <SensitivityPoint>();
double[] timestamp = sensCurve.Select(sensCurve => sensCurve.timeStamp).ToArray();
double[] randomsense = sensCurve.Select(sensCurve => sensCurve.sensitivity).ToArray();
CubicSpline spline = CubicSpline.InterpolateNatural(timestamp, randomsense);
for (double timecode = 0; timecode < this.lenght; timecode += timestep)
{
SensitivityPoint sensPoint = new SensitivityPoint(timecode, spline.Interpolate(timecode));
smoothCurve.Add(sensPoint);
}
sensCurve = smoothCurve;
}
private void FitSpline(FreeformPointLineSeries fpls)
{
m_chart.BeginUpdate();
int iMarkerCount = fpls.SeriesEventMarkers.Count;
double[] aMarkerValuesX = new double[iMarkerCount];
double[] aMarkerValuesY = new double[iMarkerCount];
for (int i = 0; i < iMarkerCount; i++)
{
SeriesEventMarker marker = fpls.SeriesEventMarkers[i];
aMarkerValuesX[i] = marker.XValue;
aMarkerValuesY[i] = marker.YValue;
}
//One solved point for each pixel in X-dimension
double[] aXValues = new double[100];
double dXMin = aMarkerValuesX[0];
double dXMax = aMarkerValuesX[iMarkerCount - 1];
double dXStep = (dXMax - dXMin) / (double)(100 - 1);
for (int i = 0; i < 100; i++)
{
aXValues[i] = dXMin + dXStep * (double)i;
}
int iOrder = iMarkerCount - 1;
//double[] aYValues = MathRoutines.PolynomialRegression(aMarkerValuesX, aMarkerValuesY, aXValues, iOrder);
double[] aYValues = new double[100];
CubicSpline naturalSpline = CubicSpline.InterpolateNatural(aMarkerValuesX, aMarkerValuesY);
for (int i = 0; i < 100; i++)
{
aYValues[i] = naturalSpline.Interpolate(aXValues[i]);
}
if (aYValues != null)
{
fpls.Clear();
fpls.AddPoints(aXValues, aYValues, false);
}
m_chart.EndUpdate();
}
public static double Differentiate(List <double> points, List <double> values, double diffPoint, int degree)
{
double recDifferentiation(List <double> newValues, int newDegree)
{
if (newDegree == 0)
{
return(CubicSpline.InterpolateNatural(points, newValues).Differentiate(diffPoint));
}
else
{
var cs = CubicSpline.InterpolateNatural(points, newValues);
newValues = points.Select(point => cs.Differentiate(point)).ToList();
return(recDifferentiation(newValues, newDegree - 1));
}
}
return(recDifferentiation(values, degree));
}
protected override void DrawCore(DrawingContext context, DrawingAttributes overrides)
{
var points = DrawingAttributes.FitToCurve ? GetBezierStylusPoints() : StylusPoints;
var reducedPoints = GeometryHelper.DouglasPeuckerReduction(points.Select(sp => sp.ToPoint()).ToList(), 10d);
var spline = CubicSpline.InterpolateNatural(reducedPoints.Select(p => p.X), reducedPoints.Select(p => p.Y));
Point?prevPoint = null;
Points = new List <Point>();
for (var i = points.Min(sp => sp.X); i <= points.Max(sp => sp.X); i++)
{
var curPoint = new Point(i, spline.Interpolate(i));
Points.Add(curPoint);
if (prevPoint != null)
{
context.DrawLine(new Pen(Brushes.Black, 2), prevPoint.Value, curPoint);
}
prevPoint = curPoint;
}
}
public Trajectory GenerateTrajectory()
{
var xSpline = CubicSpline.InterpolateNatural(_tData, _xData);
var ySpline = CubicSpline.InterpolateNatural(_tData, _yData);
var zSpline = CubicSpline.InterpolateNatural(_tData, _zData);
var latSpline = CubicSpline.InterpolateNatural(_tData, _latData);
var longSpline = CubicSpline.InterpolateNatural(_tData, _longData);
var speedSpline = CubicSpline.InterpolateNatural(_tData, _speedData);
var thrustSpline = CubicSpline.InterpolateNatural(_tData, _thrustData);
var trajectory = new Trajectory(xSpline, ySpline, zSpline, longSpline, latSpline, speedSpline, thrustSpline, _aircraft);
trajectory.LowerLeftPoint = new Point3D(_xData.Min(), _yData.Min(), 0, CoordinateUnit.metric);
trajectory.UpperRightPoint = new Point3D(_xData.Max(), _yData.Max(), 0, CoordinateUnit.metric);
trajectory.ReferencePoint = _referencePoint;
trajectory.Duration = (int)_tData.Last();
return(trajectory);
}
//CubicSpline lookX, lookY, lookZ;
/// <summary>
/// Initialize our path with a sequence of locations and lookAt directions.
/// </summary>
public void Initialize(IEnumerable <Vector3D> locations, IEnumerable <Vector3D> lookAts)
{
int count = locations.Count();
//if( lookAts.Count() != count )
// throw new System.ArgumentException();
double[] times = Enumerable.Range(0, count).Select(i => (double)i / (count - 1)).ToArray();
//.6, 0, -.8
locX = CubicSpline.InterpolateBoundaries(times, locations.Select(v => v.X), SplineBoundaryCondition.FirstDerivative, .6, SplineBoundaryCondition.FirstDerivative, .6);
locY = CubicSpline.InterpolateBoundaries(times, locations.Select(v => v.Y), SplineBoundaryCondition.FirstDerivative, 0, SplineBoundaryCondition.FirstDerivative, 0);
locZ = CubicSpline.InterpolateBoundaries(times, locations.Select(v => v.Z), SplineBoundaryCondition.FirstDerivative, -.8, SplineBoundaryCondition.FirstDerivative, -.8);
locX = CubicSpline.InterpolateNatural(times, locations.Select(v => v.X));
locY = CubicSpline.InterpolateNatural(times, locations.Select(v => v.Y));
locZ = CubicSpline.InterpolateNatural(times, locations.Select(v => v.Z));
/*lookX = CubicSpline.InterpolateNatural( times, lookAts.Select( v => v.X ) );
* lookY = CubicSpline.InterpolateNatural( times, lookAts.Select( v => v.Y ) );
* lookZ = CubicSpline.InterpolateNatural( times, lookAts.Select( v => v.Z ) );*/
}
public bool Interpolate(double[] xIn, double[] yIn, out CubicSpline spline, out string errMsg)
{
bool success = true;
errMsg = "";
spline = null;
if (xIn.Length == yIn.Length)
{
var xvec = new DenseVector(xIn);
var yvec = new DenseVector(yIn);
spline = CubicSpline.InterpolateNatural(xvec, yvec);
}
else
{
errMsg = "x and y vectors must be the same length";
success = false;
}
return(success);
}
private ChartValues <ObservablePoint> GetPlotData()
{
ChartValues <ObservablePoint> chartdata = new ChartValues <ObservablePoint>();
CubicSpline cs = new CubicSpline(dataptArray, c0, c1, c2, c3);
cs = CubicSpline.InterpolateNatural(xdata, ydata);
c0 = HelperLibrary.PropertyHelper.GetPrivateFieldValue <double[]>(cs, "_c0");
c1 = HelperLibrary.PropertyHelper.GetPrivateFieldValue <double[]>(cs, "_c1");
c2 = HelperLibrary.PropertyHelper.GetPrivateFieldValue <double[]>(cs, "_c2");
c3 = HelperLibrary.PropertyHelper.GetPrivateFieldValue <double[]>(cs, "_c3");
for (int i = 0; i < c0.Length; i++)
{
List <double[]> tmp = new List <double[]>();
tmp = CubicIntervalData(xdata[i], xdata[i + 1], c3[i], c2[i], c1[i], c0[i]);
foreach (double[] pt in tmp)
{
chartdata.Add(new ObservablePoint(pt[0], pt[1]));
}
}
return(chartdata);
}
/// <summary>
/// Interpolation of no equidistant sample point onto equidistant Fourier points
/// </summary>
public static void InterpolateOntoFourierPoints(MultidimensionalArray samplP, double DomainSize, double[] samplFp)
{
int numSp = samplP.Lengths[0];
// set interpolation data (delete multiple independent values)
ArrayList independentList = new ArrayList();
ArrayList dependentList = new ArrayList();
for (int sp = 0; sp < numSp; sp++)
{
if (independentList.Contains(samplP[sp, 0]) == false)
{
independentList.Add(samplP[sp, 0]);
dependentList.Add(samplP[sp, 1]);
}
}
// extend the interpolation data for sample points at the boundary of the domain
independentList.Insert(0, samplP[numSp - 1, 0] - DomainSize);
independentList.Insert(independentList.Count, samplP[0, 0] + DomainSize);
dependentList.Insert(0, samplP[numSp - 1, 1]);
dependentList.Insert(dependentList.Count, samplP[0, 1]);
double[] independentVal = (double[])independentList.ToArray(typeof(double));
double[] dependentVal = (double[])dependentList.ToArray(typeof(double));
// set Fourier points
int numSFp = samplFp.Length;
double[] FourierP = new double[numSFp];
for (int i = 0; i < numSFp; i++)
{
FourierP[i] = (DomainSize / numSFp) * i;
}
switch (InterpolationType)
{
case Interpolationtype.LinearSplineInterpolation:
//LinearSplineInterpolation LinSpline = new LinearSplineInterpolation();
//LinSpline.Initialize(independentVal, dependentVal);
var LinSpline = LinearSpline.Interpolate(independentVal, dependentVal);
for (int Fp = 0; Fp < numSFp; Fp++)
{
samplFp[Fp] = LinSpline.Interpolate(FourierP[Fp]);
}
break;
case Interpolationtype.CubicSplineInterpolation:
//CubicSplineInterpolation CubSpline = new CubicSplineInterpolation();
//CubSpline.Initialize(independentVal, dependentVal);
var CubSpline = CubicSpline.InterpolateNatural(independentVal, dependentVal);
for (int Fp = 0; Fp < numSFp; Fp++)
{
samplFp[Fp] = CubSpline.Interpolate(FourierP[Fp]);
}
break;
default:
throw new NotImplementedException();
}
}
public void NaturalFitsAtArbitraryPoints(double t, double x, double maxAbsoluteError)
{
IInterpolation it = CubicSpline.InterpolateNatural(_t, _y);
Assert.AreEqual(x, it.Interpolate(t), maxAbsoluteError, "Interpolation at {0}", t);
}
public void FewSamples()
{
Assert.That(() => CubicSpline.InterpolateNatural(Array.Empty <double>(), Array.Empty <double>()), Throws.ArgumentException);
Assert.That(() => CubicSpline.InterpolateNatural(new double[1], new double[1]), Throws.ArgumentException);
Assert.That(CubicSpline.InterpolateNatural(new[] { 1.0, 2.0 }, new[] { 2.0, 2.0 }).Interpolate(1.0), Is.EqualTo(2.0));
}
public Vector <double>[] bvpsolver_zzz(Func <double, Vector <double>, Vector <double> > system,
double ya, double yb,
double xa, double xb,
int N)
{
//先求解方程得到初始斜率的估计值，再进行打靶法迭代。--zzz
Vector <double>[] res;
Vector <double> y0;
double s_guess, s_guess_pre;
double fais, fais_pre;
double dfai, ds;
int count = 0;
//计算20组s_guess和fais，然后样条插值得到连续函数，再通过解方程，得到使fais=0的初始s_guess
int M = 50;
double[] sLst = Vector <double> .Build.Dense(M, i => yb / M *i).ToArray();
double[] faisLst = new double[M];
count = 0;
while (count < M)
{
y0 = Vector <double> .Build.DenseOfArray(new[] { ya, sLst[count] });
// observer_pr ob_pr = this->write_targetFunc_end;
res = RungeKutta.FourthOrder(y0, xa, xb, N, system);
faisLst[count] = res[N - 1][0] - yb;
count++;
}
//样条插值得到连续函数
var cubicSpl = CubicSpline.InterpolateNatural(sLst, faisLst);
/*如果初始值离解太远，牛顿法会不收敛。故采用Mathnet包中的RobustNewtonRaphson
* double s_cur = 0, s_next;
* count = 0;
* while (count < 1000)
* {
* fais = cubicSpl.Interpolate(s_cur);
* dfaids = cubicSpl.Differentiate(s_cur);
* if (fais < 1e-5 && fais > -1e-5)
* {
* break;
* }
*
* s_next = s_cur - fais / dfaids;
* s_cur = s_next;
* count += 1;
* }*/
//解方程fais=0,得到初始斜率s_guess。该法先尝试牛顿法，如失败会采用二分法（bisection）。
s_guess = RobustNewtonRaphson.FindRoot(cubicSpl.Interpolate, cubicSpl.Differentiate, 0, yb, 1e-5);
//利用解得的s_guess，构造s_guess_pre，目的是求导数dfai/ds。
if (s_guess == 0)
{
s_guess_pre = 1e-2;
}
else
{
s_guess_pre = s_guess * 0.99;
}
//求s_guess_pre对应的fais_pre，目的是求导数dfai/ds。
y0 = Vector <double> .Build.DenseOfArray(new[] { ya, s_guess_pre });
res = RungeKutta.FourthOrder(y0, xa, xb, N, system);
fais_pre = res[N - 1][0] - yb;
count = 0;
while (count < 50)
{
y0 = Vector <double> .Build.DenseOfArray(new[] { ya, s_guess });
res = RungeKutta.FourthOrder(y0, xa, xb, N, system);
fais = res[N - 1][0] - yb;
dfai = fais - fais_pre;
ds = s_guess - s_guess_pre;
if (fais < 1e-5 && fais > -1e-5)
{
break;
}
fais_pre = fais;
s_guess_pre = s_guess;
s_guess = s_guess - fais * ds / dfai;
count++;
}
return(res);
}
internal static void Departing(this FlightContext context)
{
/*
* First check plausible scenarios. The easiest to track is an aerotow.
*
* If not, wait until the launch is completed.
*/
if (context.Flight.LaunchMethod == LaunchMethods.None)
{
var departure = context.Flight.PositionUpdates
.Where(q => q.Heading != 0 && !double.IsNaN(q.Heading))
.OrderBy(q => q.TimeStamp)
.Take(5)
.ToList();
if (departure.Count > 4)
{
context.Flight.DepartureHeading = Convert.ToInt16(departure.Average(q => q.Heading));
if (context.Flight.DepartureHeading == 0)
{
context.Flight.DepartureHeading = 360;
}
// Only start method recognition after the heading has been determined
context.Flight.LaunchMethod = LaunchMethods.Unknown | LaunchMethods.Aerotow | LaunchMethods.Winch | LaunchMethods.Self;
}
else
{
return;
}
}
if (context.Flight.DepartureTime != null &&
(context.CurrentPosition.TimeStamp - (context.Flight.PositionUpdates.FirstOrDefault(q => q.Speed > 30)?.TimeStamp ?? context.CurrentPosition.TimeStamp)).TotalSeconds < 10)
{
return;
}
// We can safely try to extract the correct path
if (context.Flight.LaunchMethod.HasFlag(LaunchMethods.Unknown | LaunchMethods.Aerotow))
{
var encounters = context.TowEncounter().ToList();
if (encounters.Count(q => q?.Type == EncounterType.Tug || q?.Type == EncounterType.Tow) > 1)
{
return;
}
var encounter = encounters.SingleOrDefault(q => q?.Type == EncounterType.Tug || q?.Type == EncounterType.Tow);
if (encounter != null)
{
context.Flight.LaunchMethod = LaunchMethods.Aerotow
| (encounter.Type == EncounterType.Tug
? LaunchMethods.OnTow
: LaunchMethods.TowPlane
);
context.Flight.Encounters.Add(encounter);
context.StateMachine.Fire(FlightContext.Trigger.TrackAerotow);
return;
}
else if (encounters.Any(q => q == null))
{
return;
}
context.Flight.LaunchMethod &= ~LaunchMethods.Aerotow;
}
if (context.Flight.LaunchMethod.HasFlag(LaunchMethods.Unknown))
{
var x = new DenseVector(context.Flight.PositionUpdates.Select(w => (w.TimeStamp - context.Flight.DepartureTime.Value).TotalSeconds).ToArray());
var y = new DenseVector(context.Flight.PositionUpdates.Select(w => w.Altitude).ToArray());
var interpolation = CubicSpline.InterpolateNatural(x, y);
var r = new List <double>();
var r2 = new List <double>();
for (var i = 0; i < (context.CurrentPosition.TimeStamp - context.Flight.DepartureTime.Value).TotalSeconds; i++)
{
r.Add(interpolation.Differentiate(i));
r2.Add(interpolation.Differentiate2(i));
}
// When the initial climb has completed
if (interpolation.Differentiate((context.CurrentPosition.TimeStamp - context.Flight.DepartureTime.Value).TotalSeconds) < 0)
{
// Skip the first element because heading is 0 when in rest
var averageHeading = context.Flight.PositionUpdates.Skip(1).Average(q => q.Heading);
// ToDo: Add check to see whether there is another aircraft nearby
if (context.Flight.PositionUpdates
.Skip(1)
.Where(q => interpolation.Differentiate((context.CurrentPosition.TimeStamp - context.Flight.DepartureTime.Value).TotalSeconds) > 0)
.Select(q => Geo.GetHeadingError(averageHeading, q.Heading))
.Any(q => q > 20) ||
Geo.DistanceTo(
context.Flight.PositionUpdates.First().Location,
context.CurrentPosition.Location) > 3000)
{
context.Flight.LaunchMethod = LaunchMethods.Self;
}
else
{
context.Flight.LaunchMethod = LaunchMethods.Winch;
}
context.Flight.LaunchFinished = context.CurrentPosition.TimeStamp;
context.InvokeOnLaunchCompletedEvent();
context.StateMachine.Fire(FlightContext.Trigger.LaunchCompleted);
}
}
}