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);
}
}
}
/// <summary>
/// Calculates a value of the difficulty correction function for a given movement.
/// </summary>
/// <param name="currentMovementDistance">The distance between the last note and the target note.</param>
/// <param name="otherMovementDistance">
/// The distance between the target note and the next note
/// (in the case of <see cref="FLOW_NEXT"/> and <see cref="SNAP_NEXT"/>),
/// or the distance between the second-to-last note and the last note
/// (in the case of <see cref="FLOW_SECONDLAST"/> and <see cref="SNAP_SECONDLAST"/>).
/// </param>
/// <param name="angles">The angle between the note pairs that are part of this movement.</param>
/// <param name="values">Values of the correction function at the interpolation nodes.</param>
/// <param name="minimumCorrection">
/// The lower bound for the correction, as a function of the first movement length.
/// Defaults to the constant function y(x) = 0 if not given.
/// </param>
/// <param name="maximumCorrection">
/// The upper bound for the correction, as a function of the first movement length.
/// Defaults to the constant function y(x) = 1 if not given.
/// </param>
/// <param name="otherMovementScalingFactor">Optional scaling factor to apply to <paramref name="otherMovementDistance"/>.</param>
private AngleCorrection(
double[] currentMovementDistance,
double[] otherMovementDistance,
double[] angles,
double[,,] values,
CubicInterpolator minimumCorrection = null,
CubicInterpolator maximumCorrection = null,
Func <double, double> otherMovementScalingFactor = null)
{
this.otherMovementScalingFactor = otherMovementScalingFactor;
this.minimumCorrection = minimumCorrection;
this.maximumCorrection = maximumCorrection;
correctionInterpolator = new TricubicInterpolator(currentMovementDistance, otherMovementDistance, angles, values, dzLower: 0, dzUpper: 0);
}
/// <summary>
/// Creates a new <see cref="BicubicInterpolator"/> using the supplied data.
/// </summary>
/// <remarks>
/// Marginal partial derivative values, if not supplied in
/// <paramref name="dxLower"/>, <paramref name="dxUpper"/>,
/// <paramref name="dyLower"/>, <paramref name="dyUpper"/>,
/// will be calculated independently for each point of the grid.
/// </remarks>
/// <param name="xs">1D array with the <c>x</c> coordinates of the interpolation grid nodes.</param>
/// <param name="ys">1D array with the <c>y</c> coordinates of the interpolation grid nodes.</param>
/// <param name="vs">
/// 2D array with the values of the function for each node of the grid specified
/// by <paramref name="xs"/> and <paramref name="ys"/>.
/// </param>
/// <param name="dxLower">The value of the partial derivative over <c>x</c> for all points with the smallest <c>x</c> coordinate value on the 2D grid.</param>
/// <param name="dxUpper">The value of the partial derivative over <c>x</c> for all points with the largest <c>x</c> coordinate value on the 2D grid.</param>
/// <param name="dyLower">The value of the partial derivative over <c>y</c> for all points with the smallest <c>y</c> coordinate value on the 2D grid.</param>
/// <param name="dyUpper">The value of the partial derivative over <c>y</c> for all points with the largest <c>y</c> coordinate value on the 2D grid.</param>
public BicubicInterpolator(double[] xs, double[] ys, double[][] vs,
double?dxLower = null, double?dxUpper = null,
double?dyLower = null, double?dyUpper = null)
{
this.xs = xs;
this.dxLower = dxLower;
this.dxUpper = dxUpper;
cubicInterpolators = new CubicInterpolator[xs.Length];
for (int i = 0; i < xs.Length; ++i)
{
cubicInterpolators[i] = new CubicInterpolator(ys, vs[i], dyLower, dyUpper);
}
}
