C:/Users/Andreas Schiffler/Desktop/SVN/NewGamePhysics/NewGamePhysics/Mathematics/CubicSpline.cs

//-----------------------------------------------------------------------------
// CubicSpline.cs
// (A. Schiffler, 2010)
//-----------------------------------------------------------------------------

using System;
using System.Collections.Generic;
using System.Linq;

namespace NewGamePhysics.Mathematics
{
    public class CubicSpline
    {
        private SortedList<double, double[]> data;

        private int dimensions;

        private double[] xData;

        private double[][] yData;

        private double[] xIntervals;

        private double[][] bCoeff;

        private double[][] cCoeff;

        private double[][] dCoeff;

        private bool needUpdate;

        public CubicSpline()
            : this(1)
        {
        }

        public CubicSpline(int dimensions)
        {
            if (dimensions < 1)
            {
                throw new ArgumentOutOfRangeException(
                    "dimensions", 
                    "The number of dimensions must be 1 or more");
            }

            this.dimensions = dimensions;
            this.data = new SortedList<double, double[]>();
            this.bCoeff = new double[dimensions][];
            this.cCoeff = new double[dimensions][];
            this.dCoeff = new double[dimensions][];
            this.needUpdate = false;
        }

        public double Dimensions
        {
            get
            {
                return this.dimensions;
            }
        }

        public double Count
        {
            get
            {
                return this.data.Count;
            }
        }

        public bool CanInterpolate
        {
            get
            {
                return (this.data.Count > 3);
            }
        }

        public void Clear()
        {
            this.data.Clear();
            this.xData = null;
            this.xIntervals = null;
            this.yData = null;
            for (int i = 0; i < this.dimensions; i++)
            {
                this.bCoeff[i] = null;
                this.cCoeff[i] = null;
                this.dCoeff[i] = null;
            }

            this.needUpdate = false;
        }

        public bool AddDataPoint(double x, double[] y)
        {
            if (y.Length != this.dimensions)
            {
                throw new ArgumentOutOfRangeException(
                    "y",
                    "The array must contain " + this.dimensions + " values.");
            }

            if (this.data.ContainsKey(x))
            {
                return false;
            }
            else
            {
                this.data.Add(x, y);
                this.needUpdate = true;
                return true;
            }
        }

        public double[] Interpolate(double x)
        {
            if (!this.CanInterpolate)
            {
                throw new InvalidOperationException(
                    "Insufficient number of points. Add at least 4 points before calling Interpolate.");
            }

            if (this.needUpdate)
            {
                this.CalculateCoefficients();
            }

            int index = BinaryIndexSearch(this.xData, x);
            double dx = x - this.xData[index];
            double[] y = new double[this.dimensions];
            for (int i = 0; i < this.dimensions; i++)
            {
                y[i] = ((this.dCoeff[i][index] * dx + this.cCoeff[i][index]) * dx + this.bCoeff[i][index]) * dx + this.yData[i][index];
            }

            return y;
        }

        #region private_methods

        private static int BinaryIndexSearch(double[] values, double value)
        {
            // special cases
            if (value <= values[0])
            {
                return 0;
            }

            int max = values.Length - 1;

            if (value >= values[max])
            {
                return max - 1;
            }

            // binary search
            int low = 0;
            int high = max;
            while (low < high)
            {
                int mid = low + (high - low) / 2;
                if (values[mid] < value)
                {
                    low = mid + 1;
                }
                else
                {
                    high = mid;
                }
            }

            // found
            return low - 1;
        }

        private static double[] CalculateIntervals(double[] values)
        {
            if (values == null)
            {
                throw new ArgumentNullException();
            }

            if (values.Length < 2)
            {
                throw new ArgumentOutOfRangeException();
            }

            int inputLength = values.Length;
            int outputLength = inputLength - 1;
            double[] intervals = new double[outputLength];
            for (int i = 0, ip = 1; i < outputLength; i++, ip++)
            {
                intervals[i] = values[ip] - values[i];
            }

            return intervals;
        }

        private static double[] CalculateC(double[] a, double[] xdata, double[] intervals)
        {
            int dataLength = xdata.Length;
            double[] c = new double[dataLength];
            double[] alpha = new double[dataLength];
            double[] l = new double[dataLength];
            double[] mu = new double[dataLength];
            double[] z = new double[dataLength];

            // Solve tridiagonal matrix
            for (int i = 1, im = 0, ip = 2; i < dataLength - 2; i++, im++, ip++)
            {
                alpha[i] = 3 * ((a[ip] * intervals[im]) - (a[i] * (xdata[ip] - xdata[im]))
                    + (a[im] * intervals[i])) / (intervals[im] * intervals[i]);
            }

            l[0] = 0.0;
            mu[0] = 0.0;
            z[0] = 0.0;
            for (int i = 1, im = 0, ip = 2; i < dataLength - 2; i++, im++, ip++)
            {
                l[i] = 2 * (xdata[ip] - xdata[im]) - intervals[im] * mu[im];
                mu[i] = intervals[i] / l[i];
                z[i] = (alpha[i] - intervals[im] * z[im]) / l[i];
            }

            // Calculate c's
            c[dataLength - 1] = 0;
            for (int i = dataLength - 2, ip = dataLength - 1; i >= 0; i--, ip--)
            {
                c[i] = z[i] - mu[i] * c[ip];
            }

            return c;
        }

        private static double[] CalculateB(double[] a, double[] c, double[] interval)
        {
            int dataLength = a.Length;
            double[] b = new double[dataLength];

            for (int i = dataLength - 2, ip = dataLength - 1; i >= 0; i--, ip--)
            {
                b[i] = (a[ip] - a[i]) / interval[i] - interval[i] * (c[ip] + 2 * c[i]) / 3;
            }

            return b;
        }

        private static double[] CalculateD(double[] c, double[] interval)
        {
            int dataLength = c.Length;
            double[] d = new double[dataLength];

            for (int i = dataLength - 2; i >= 0; i--)
            {
                d[i] = (c[i + 1] - c[i]) / (3 * interval[i]);
            }

            return d;
        }

        private void CalculateCoefficients()
        {
            // create all data arrays
            this.xData = this.data.Keys.ToArray();
            this.xIntervals = CalculateIntervals(this.xData);
            double[][] yArray = this.data.Values.ToArray();
            this.yData = new double[this.dimensions][];
            for (int i = 0; i < this.dimensions; i++)
            {
                // y data array needs to be rotated in its order from the list array
                this.yData[i] = new double[this.data.Count];
                for (int j = 0; j < this.data.Count; j++)
                {
                    this.yData[i][j] = yArray[j][i];
                }

                // calculate coefficients
                this.cCoeff[i] = CalculateC(this.yData[i], this.xData, this.xIntervals);
                this.bCoeff[i] = CalculateB(this.yData[i], this.cCoeff[i], this.xIntervals);
                this.dCoeff[i] = CalculateD(this.cCoeff[i], this.xIntervals);
            }

            this.needUpdate = false;
        }

        #endregion
    }
}

---