private static IEnumerable <AXPoint> InterpolateGapSpline(AXPoint p0, AXPoint p1, AXPoint p2, AXPoint p3, int interpolationInterval, int maxAllowedGap)
{
// calculate the number of points to be interpolated
var numberOfPoints = ((int)Math.Floor((p2.Time - p1.Time).TotalSeconds / interpolationInterval)) - 1;
// don't interpolate if it's not needed or the gap is too large
if (numberOfPoints > 0 && numberOfPoints <= maxAllowedGap)
{
// define interpolator
// "convert" time to double
var x0 = 0;
var x1 = (p1.Time - p0.Time).TotalSeconds;
var x2 = (p2.Time - p0.Time).TotalSeconds;
var x3 = (p3.Time - p0.Time).TotalSeconds;
var interpolator = CubicInterpolator.CatmullRom(x0, x1, x2, x3);
var deltat = 1.0 / (numberOfPoints + 1);
for (int i = 1; i <= numberOfPoints; i++)
{
// perform interpolation
var p = new AXPoint(
p1.Time.AddSeconds(i * interpolationInterval),
interpolator.Interpolate(p0.Easting, p1.Easting, p2.Easting, p3.Easting, i * deltat),
interpolator.Interpolate(p0.Northing, p1.Northing, p2.Northing, p3.Northing, i * deltat),
interpolator.Interpolate(p0.Altitude, p1.Altitude, p2.Altitude, p3.Altitude, i * deltat));
yield return(p);
}
}
}
