Avalonia/samples/ControlCatalog/CubicSpline.cs

// Copyright (c) 2009-2014 Math.NET Taken from http://github.com/mathnet/mathnet-numerics and modified for Wasabi Wallet

using System;

namespace MathNet
{
	/// <summary>
	/// Cubic Spline Interpolation.
	/// </summary>
	/// <remarks>Supports both differentiation and integration.</remarks>
	public class CubicSpline
	{
		private readonly double[] _x;
		private readonly double[] _c0;
		private readonly double[] _c1;
		private readonly double[] _c2;
		private readonly double[] _c3;
		private readonly Lazy<double[]> _indefiniteIntegral;

		/// <param name="x">Sample points (N+1), sorted ascending</param>
		/// <param name="c0">Zero order spline coefficients (N)</param>
		/// <param name="c1">First order spline coefficients (N)</param>
		/// <param name="c2">Second order spline coefficients (N)</param>
		/// <param name="c3">Third order spline coefficients (N)</param>
		public CubicSpline(double[] x, double[] c0, double[] c1, double[] c2, double[] c3)
		{
			if (x.Length != c0.Length + 1 || x.Length != c1.Length + 1 || x.Length != c2.Length + 1 || x.Length != c3.Length + 1)
			{
				throw new ArgumentException("All vectors must have the same dimensionality.");
			}

			if (x.Length < 2)
			{
				throw new ArgumentException("The given array is too small. It must be at least 2 long.", nameof(x));
			}

			_x = x;
			_c0 = c0;
			_c1 = c1;
			_c2 = c2;
			_c3 = c3;
			_indefiniteIntegral = new Lazy<double[]>(ComputeIndefiniteIntegral);
		}

		/// <summary>
		/// Create a Hermite cubic spline interpolation from a set of (x,y) value pairs and their slope (first derivative), sorted ascendingly by x.
		/// </summary>
		public static CubicSpline InterpolateHermiteSorted(double[] x, double[] y, double[] firstDerivatives)
		{
			if (x.Length != y.Length || x.Length != firstDerivatives.Length)
			{
				throw new ArgumentException("All vectors must have the same dimensionality.");
			}

			if (x.Length < 2)
			{
				throw new ArgumentException("The given array is too small. It must be at least 2 long.", nameof(x));
			}

			var c0 = new double[x.Length - 1];
			var c1 = new double[x.Length - 1];
			var c2 = new double[x.Length - 1];
			var c3 = new double[x.Length - 1];
			for (int i = 0; i < c1.Length; i++)
			{
				double w = x[i + 1] - x[i];
				double w2 = w * w;
				c0[i] = y[i];
				c1[i] = firstDerivatives[i];
				c2[i] = (3 * (y[i + 1] - y[i]) / w - 2 * firstDerivatives[i] - firstDerivatives[i + 1]) / w;
				c3[i] = (2 * (y[i] - y[i + 1]) / w + firstDerivatives[i] + firstDerivatives[i + 1]) / w2;
			}

			return new CubicSpline(x, c0, c1, c2, c3);
		}

		/// <summary>
		/// Create an Akima cubic spline interpolation from a set of (x,y) value pairs, sorted ascendingly by x.
		/// Akima splines are robust to outliers.
		/// </summary>
		public static CubicSpline InterpolateAkimaSorted(double[] x, double[] y)
		{
			if (x.Length != y.Length)
			{
				throw new ArgumentException("All vectors must have the same dimensionality.");
			}

			if (x.Length < 5)
			{
				throw new ArgumentException("The given array is too small. It must be at least 5 long.", nameof(x));
			}

			/* Prepare divided differences (diff) and weights (w) */

			var diff = new double[x.Length - 1];
			var weights = new double[x.Length - 1];

			for (int i = 0; i < diff.Length; i++)
			{
				diff[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]);
			}

			for (int i = 1; i < weights.Length; i++)
			{
				weights[i] = Math.Abs(diff[i] - diff[i - 1]);
			}

			/* Prepare Hermite interpolation scheme */

			var dd = new double[x.Length];

			for (int i = 2; i < dd.Length - 2; i++)
			{
				dd[i] = weights[i - 1].AlmostEqual(0.0) && weights[i + 1].AlmostEqual(0.0)
					? (((x[i + 1] - x[i]) * diff[i - 1]) + ((x[i] - x[i - 1]) * diff[i])) / (x[i + 1] - x[i - 1])
					: ((weights[i + 1] * diff[i - 1]) + (weights[i - 1] * diff[i])) / (weights[i + 1] + weights[i - 1]);
			}

			dd[0] = DifferentiateThreePoint(x, y, 0, 0, 1, 2);
			dd[1] = DifferentiateThreePoint(x, y, 1, 0, 1, 2);
			dd[x.Length - 2] = DifferentiateThreePoint(x, y, x.Length - 2, x.Length - 3, x.Length - 2, x.Length - 1);
			dd[x.Length - 1] = DifferentiateThreePoint(x, y, x.Length - 1, x.Length - 3, x.Length - 2, x.Length - 1);

			/* Build Akima spline using Hermite interpolation scheme */

			return InterpolateHermiteSorted(x, y, dd);
		}

		/// <summary>
		/// Create a piecewise cubic Hermite interpolating polynomial from an unsorted set of (x,y) value pairs.
		/// Monotone-preserving interpolation with continuous first derivative.
		/// </summary>
		public static CubicSpline InterpolatePchipSorted(double[] x, double[] y)
		{
			// Implementation based on "Numerical Computing with Matlab" (Moler, 2004).

			if (x.Length != y.Length)
			{
				throw new ArgumentException("All vectors must have the same dimensionality.");
			}

			if (x.Length < 3)
			{
				throw new ArgumentException("The given array is too small. It must be at least 3 long.", nameof(x));
			}

			var m = new double[x.Length - 1];

			for (int i = 0; i < m.Length; i++)
			{
				m[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]);
			}

			var dd = new double[x.Length];
			var hPrev = x[1] - x[0];

			// This check is quite costly as it usually involves a Math.Pow().
			var mPrevIs0 = m[0].AlmostEqual(0.0);

			for (var i = 1; i < x.Length - 1; ++i)
			{
				var h = x[i + 1] - x[i];
				var mIs0 = m[i].AlmostEqual(0.0);

				if (mIs0 || mPrevIs0 || Math.Sign(m[i]) != Math.Sign(m[i - 1]))
				{
					dd[i] = 0;
				}
				else
				{
					// Weighted harmonic mean of each slope.
					var w1 = 2 * h + hPrev;
					var w2 = h + 2 * hPrev;
					dd[i] = (w1 + w2) / (w1 / m[i - 1] + w2 / m[i]);
				}

				hPrev = h;
				mPrevIs0 = mIs0;
			}

			// Special case end-points.
			dd[0] = PchipEndPoints(x[1] - x[0], x[2] - x[1], m[0], m[1]);
			dd[dd.Length - 1] = PchipEndPoints(
				x[x.Length - 1] - x[x.Length - 2],
				x[x.Length - 2] - x[x.Length - 3],
				m[m.Length - 1],
				m[m.Length - 2]);

			return InterpolateHermiteSorted(x, y, dd);
		}

		private static double PchipEndPoints(double h0, double h1, double m0, double m1)
		{
			// One-sided, shape-preserving, three-point estimate for the derivative.
			var d = ((2 * h0 + h1) * m0 - h0 * m1) / (h0 + h1);

			if (Math.Sign(d) != Math.Sign(m0))
			{
				return 0.0;
			}

			if (Math.Sign(m0) != Math.Sign(m1) && (Math.Abs(d) > 3 * Math.Abs(m0)))
			{
				return 3 * m0;
			}

			return d;
		}

		/// <summary>
		/// Create a cubic spline interpolation from a set of (x,y) value pairs, sorted ascendingly by x,
		/// and custom boundary/termination conditions.
		/// </summary>
		public static CubicSpline InterpolateBoundariesSorted(
			double[] x,
			double[] y,
			SplineBoundaryCondition leftBoundaryCondition,
			double leftBoundary,
			SplineBoundaryCondition rightBoundaryCondition,
			double rightBoundary)
		{
			if (x.Length != y.Length)
			{
				throw new ArgumentException("All vectors must have the same dimensionality.");
			}

			if (x.Length < 2)
			{
				throw new ArgumentException("The given array is too small. It must be at least 2 long.", nameof(x));
			}

			int n = x.Length;

			// normalize special cases
			if ((n == 2)
				&& (leftBoundaryCondition == SplineBoundaryCondition.ParabolicallyTerminated)
				&& (rightBoundaryCondition == SplineBoundaryCondition.ParabolicallyTerminated))
			{
				leftBoundaryCondition = SplineBoundaryCondition.SecondDerivative;
				leftBoundary = 0d;
				rightBoundaryCondition = SplineBoundaryCondition.SecondDerivative;
				rightBoundary = 0d;
			}

			if (leftBoundaryCondition == SplineBoundaryCondition.Natural)
			{
				leftBoundaryCondition = SplineBoundaryCondition.SecondDerivative;
				leftBoundary = 0d;
			}

			if (rightBoundaryCondition == SplineBoundaryCondition.Natural)
			{
				rightBoundaryCondition = SplineBoundaryCondition.SecondDerivative;
				rightBoundary = 0d;
			}

			var a1 = new double[n];
			var a2 = new double[n];
			var a3 = new double[n];
			var b = new double[n];

			// Left Boundary
			switch (leftBoundaryCondition)
			{
				case SplineBoundaryCondition.ParabolicallyTerminated:
					a1[0] = 0;
					a2[0] = 1;
					a3[0] = 1;
					b[0] = 2 * (y[1] - y[0]) / (x[1] - x[0]);
					break;
				case SplineBoundaryCondition.FirstDerivative:
					a1[0] = 0;
					a2[0] = 1;
					a3[0] = 0;
					b[0] = leftBoundary;
					break;
				case SplineBoundaryCondition.SecondDerivative:
					a1[0] = 0;
					a2[0] = 2;
					a3[0] = 1;
					b[0] = (3 * ((y[1] - y[0]) / (x[1] - x[0]))) - (0.5 * leftBoundary * (x[1] - x[0]));
					break;
				default:
					throw new NotSupportedException("Invalid Left Boundary Condition.");
			}

			// Central Conditions
			for (int i = 1; i < x.Length - 1; i++)
			{
				a1[i] = x[i + 1] - x[i];
				a2[i] = 2 * (x[i + 1] - x[i - 1]);
				a3[i] = x[i] - x[i - 1];
				b[i] = (3 * (y[i] - y[i - 1]) / (x[i] - x[i - 1]) * (x[i + 1] - x[i])) + (3 * (y[i + 1] - y[i]) / (x[i + 1] - x[i]) * (x[i] - x[i - 1]));
			}

			// Right Boundary
			switch (rightBoundaryCondition)
			{
				case SplineBoundaryCondition.ParabolicallyTerminated:
					a1[n - 1] = 1;
					a2[n - 1] = 1;
					a3[n - 1] = 0;
					b[n - 1] = 2 * (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]);
					break;
				case SplineBoundaryCondition.FirstDerivative:
					a1[n - 1] = 0;
					a2[n - 1] = 1;
					a3[n - 1] = 0;
					b[n - 1] = rightBoundary;
					break;
				case SplineBoundaryCondition.SecondDerivative:
					a1[n - 1] = 1;
					a2[n - 1] = 2;
					a3[n - 1] = 0;
					b[n - 1] = (3 * (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2])) + (0.5 * rightBoundary * (x[n - 1] - x[n - 2]));
					break;
				default:
					throw new NotSupportedException("Invalid Right Boundary Condition.");
			}

			// Build Spline
			double[] dd = SolveTridiagonal(a1, a2, a3, b);
			return InterpolateHermiteSorted(x, y, dd);
		}

		/// <summary>
		/// Create a natural cubic spline interpolation from a set of (x,y) value pairs
		/// and zero second derivatives at the two boundaries, sorted ascendingly by x.
		/// </summary>
		public static CubicSpline InterpolateNaturalSorted(double[] x, double[] y)
		{
			return InterpolateBoundariesSorted(x, y, SplineBoundaryCondition.SecondDerivative, 0.0, SplineBoundaryCondition.SecondDerivative, 0.0);
		}

		/// <summary>
		/// Three-Point Differentiation Helper.
		/// </summary>
		/// <param name="xx">Sample Points t.</param>
		/// <param name="yy">Sample Values x(t).</param>
		/// <param name="indexT">Index of the point of the differentiation.</param>
		/// <param name="index0">Index of the first sample.</param>
		/// <param name="index1">Index of the second sample.</param>
		/// <param name="index2">Index of the third sample.</param>
		/// <returns>The derivative approximation.</returns>
		private static double DifferentiateThreePoint(double[] xx, double[] yy, int indexT, int index0, int index1, int index2)
		{
			double x0 = yy[index0];
			double x1 = yy[index1];
			double x2 = yy[index2];

			double t = xx[indexT] - xx[index0];
			double t1 = xx[index1] - xx[index0];
			double t2 = xx[index2] - xx[index0];

			double a = (x2 - x0 - (t2 / t1 * (x1 - x0))) / (t2 * (t2 - t1));
			double b = (x1 - x0 - a * t1 * t1) / t1;
			return (2 * a * t) + b;
		}

		/// <summary>
		/// Tridiagonal Solve Helper.
		/// </summary>
		/// <param name="a">The a-vector[n].</param>
		/// <param name="b">The b-vector[n], will be modified by this function.</param>
		/// <param name="c">The c-vector[n].</param>
		/// <param name="d">The d-vector[n], will be modified by this function.</param>
		/// <returns>The x-vector[n]</returns>
		private static double[] SolveTridiagonal(double[] a, double[] b, double[] c, double[] d)
		{
			for (int k = 1; k < a.Length; k++)
			{
				double t = a[k] / b[k - 1];
				b[k] = b[k] - (t * c[k - 1]);
				d[k] = d[k] - (t * d[k - 1]);
			}

			var x = new double[a.Length];
			x[x.Length - 1] = d[d.Length - 1] / b[b.Length - 1];
			for (int k = x.Length - 2; k >= 0; k--)
			{
				x[k] = (d[k] - (c[k] * x[k + 1])) / b[k];
			}

			return x;
		}

		/// <summary>
		/// Interpolate at point t.
		/// </summary>
		/// <param name="t">Point t to interpolate at.</param>
		/// <returns>Interpolated value x(t).</returns>
		public double Interpolate(double t)
		{
			int k = LeftSegmentIndex(t);
			var x = t - _x[k];
			return _c0[k] + x * (_c1[k] + x * (_c2[k] + x * _c3[k]));
		}

		/// <summary>
		/// Differentiate at point t.
		/// </summary>
		/// <param name="t">Point t to interpolate at.</param>
		/// <returns>Interpolated first derivative at point t.</returns>
		public double Differentiate(double t)
		{
			int k = LeftSegmentIndex(t);
			var x = t - _x[k];
			return _c1[k] + x * (2 * _c2[k] + x * 3 * _c3[k]);
		}

		/// <summary>
		/// Differentiate twice at point t.
		/// </summary>
		/// <param name="t">Point t to interpolate at.</param>
		/// <returns>Interpolated second derivative at point t.</returns>
		public double Differentiate2(double t)
		{
			int k = LeftSegmentIndex(t);
			var x = t - _x[k];
			return 2 * _c2[k] + x * 6 * _c3[k];
		}

		/// <summary>
		/// Indefinite integral at point t.
		/// </summary>
		/// <param name="t">Point t to integrate at.</param>
		public double Integrate(double t)
		{
			int k = LeftSegmentIndex(t);
			var x = t - _x[k];
			return _indefiniteIntegral.Value[k] + x * (_c0[k] + x * (_c1[k] / 2 + x * (_c2[k] / 3 + x * _c3[k] / 4)));
		}

		/// <summary>
		/// Definite integral between points a and b.
		/// </summary>
		/// <param name="a">Left bound of the integration interval [a,b].</param>
		/// <param name="b">Right bound of the integration interval [a,b].</param>
		public double Integrate(double a, double b)
		{
			return Integrate(b) - Integrate(a);
		}

		private double[] ComputeIndefiniteIntegral()
		{
			var integral = new double[_c1.Length];
			for (int i = 0; i < integral.Length - 1; i++)
			{
				double w = _x[i + 1] - _x[i];
				integral[i + 1] = integral[i] + w * (_c0[i] + w * (_c1[i] / 2 + w * (_c2[i] / 3 + w * _c3[i] / 4)));
			}

			return integral;
		}

		/// <summary>
		/// Find the index of the greatest sample point smaller than t,
		/// or the left index of the closest segment for extrapolation.
		/// </summary>
		private int LeftSegmentIndex(double t)
		{
			int index = Array.BinarySearch(_x, t);
			if (index < 0)
			{
				index = ~index - 1;
			}

			return Math.Min(Math.Max(index, 0), _x.Length - 2);
		}
	}
}