- Main Page
- Classes
- Files

- File List

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

    00001 ﻿//-----------------------------------------------------------------------------
    00002 // CubicSpline.cs
    00003 // (A. Schiffler, 2010)
    00004 //-----------------------------------------------------------------------------
    00005
    00006 using System;
    00007 using System.Collections.Generic;
    00008 using System.Linq;
    00009
    00010 namespace NewGamePhysics.Mathematics
    00011 {
    00015     public class CubicSpline
    00016     {
    00020         private SortedList<double, double[]> data;
    00021
    00025         private int dimensions;
    00026
    00030         private double[] xData;
    00031
    00035         private double[][] yData;
    00036
    00040         private double[] xIntervals;
    00041
    00045         private double[][] bCoeff;
    00046
    00050         private double[][] cCoeff;
    00051
    00055         private double[][] dCoeff;
    00056
    00060         private bool needUpdate;
    00061
    00065         public CubicSpline()
    00066             : this(1)
    00067         {
    00068         }
    00069
    00074         public CubicSpline(int dimensions)
    00075         {
    00076             if (dimensions < 1)
    00077             {
    00078                 throw new ArgumentOutOfRangeException(
    00079                     "dimensions",
    00080                     "The number of dimensions must be 1 or more");
    00081             }
    00082
    00083             this.dimensions = dimensions;
    00084             this.data = new SortedList<double, double[]>();
    00085             this.bCoeff = new double[dimensions][];
    00086             this.cCoeff = new double[dimensions][];
    00087             this.dCoeff = new double[dimensions][];
    00088             this.needUpdate = false;
    00089         }
    00090
    00094         public double Dimensions
    00095         {
    00096             get
    00097             {
    00098                 return this.dimensions;
    00099             }
    00100         }
    00101
    00105         public double Count
    00106         {
    00107             get
    00108             {
    00109                 return this.data.Count;
    00110             }
    00111         }
    00112
    00117         public bool CanInterpolate
    00118         {
    00119             get
    00120             {
    00121                 return (this.data.Count > 3);
    00122             }
    00123         }
    00124
    00128         public void Clear()
    00129         {
    00130             this.data.Clear();
    00131             this.xData = null;
    00132             this.xIntervals = null;
    00133             this.yData = null;
    00134             for (int i = 0; i < this.dimensions; i++)
    00135             {
    00136                 this.bCoeff[i] = null;
    00137                 this.cCoeff[i] = null;
    00138                 this.dCoeff[i] = null;
    00139             }
    00140
    00141             this.needUpdate = false;
    00142         }
    00143
    00155         public bool AddDataPoint(double x, double[] y)
    00156         {
    00157             if (y.Length != this.dimensions)
    00158             {
    00159                 throw new ArgumentOutOfRangeException(
    00160                     "y",
    00161                     "The array must contain " + this.dimensions + " values.");
    00162             }
    00163
    00164             if (this.data.ContainsKey(x))
    00165             {
    00166                 return false;
    00167             }
    00168             else
    00169             {
    00170                 this.data.Add(x, y);
    00171                 this.needUpdate = true;
    00172                 return true;
    00173             }
    00174         }
    00175
    00182         public double[] Interpolate(double x)
    00183         {
    00184             if (!this.CanInterpolate)
    00185             {
    00186                 throw new InvalidOperationException(
    00187                     "Insufficient number of points. Add at least 4 points before calling Interpolate.");
    00188             }
    00189
    00190             if (this.needUpdate)
    00191             {
    00192                 this.CalculateCoefficients();
    00193             }
    00194
    00195             int index = BinaryIndexSearch(this.xData, x);
    00196             double dx = x - this.xData[index];
    00197             double[] y = new double[this.dimensions];
    00198             for (int i = 0; i < this.dimensions; i++)
    00199             {
    00200                 y[i] = ((this.dCoeff[i][index] * dx + this.cCoeff[i][index]) * dx + this.bCoeff[i][index]) * dx + this.yData[i][index];
    00201             }
    00202
    00203             return y;
    00204         }
    00205
    00206         #region private_methods
    00207
    00215         private static int BinaryIndexSearch(double[] values, double value)
    00216         {
    00217             // special cases
    00218             if (value <= values[0])
    00219             {
    00220                 return 0;
    00221             }
    00222
    00223             int max = values.Length - 1;
    00224
    00225             if (value >= values[max])
    00226             {
    00227                 return max - 1;
    00228             }
    00229
    00230             // binary search
    00231             int low = 0;
    00232             int high = max;
    00233             while (low < high)
    00234             {
    00235                 int mid = low + (high - low) / 2;
    00236                 if (values[mid] < value)
    00237                 {
    00238                     low = mid + 1;
    00239                 }
    00240                 else
    00241                 {
    00242                     high = mid;
    00243                 }
    00244             }
    00245
    00246             // found
    00247             return low - 1;
    00248         }
    00249
    00255         private static double[] CalculateIntervals(double[] values)
    00256         {
    00257             if (values == null)
    00258             {
    00259                 throw new ArgumentNullException();
    00260             }
    00261
    00262             if (values.Length < 2)
    00263             {
    00264                 throw new ArgumentOutOfRangeException();
    00265             }
    00266
    00267             int inputLength = values.Length;
    00268             int outputLength = inputLength - 1;
    00269             double[] intervals = new double[outputLength];
    00270             for (int i = 0, ip = 1; i < outputLength; i++, ip++)
    00271             {
    00272                 intervals[i] = values[ip] - values[i];
    00273             }
    00274
    00275             return intervals;
    00276         }
    00277
    00285         private static double[] CalculateC(double[] a, double[] xdata, double[] intervals)
    00286         {
    00287             int dataLength = xdata.Length;
    00288             double[] c = new double[dataLength];
    00289             double[] alpha = new double[dataLength];
    00290             double[] l = new double[dataLength];
    00291             double[] mu = new double[dataLength];
    00292             double[] z = new double[dataLength];
    00293
    00294             // Solve tridiagonal matrix
    00295             for (int i = 1, im = 0, ip = 2; i < dataLength - 2; i++, im++, ip++)
    00296             {
    00297                 alpha[i] = 3 * ((a[ip] * intervals[im]) - (a[i] * (xdata[ip] - xdata[im]))
    00298                     + (a[im] * intervals[i])) / (intervals[im] * intervals[i]);
    00299             }
    00300
    00301             l[0] = 0.0;
    00302             mu[0] = 0.0;
    00303             z[0] = 0.0;
    00304             for (int i = 1, im = 0, ip = 2; i < dataLength - 2; i++, im++, ip++)
    00305             {
    00306                 l[i] = 2 * (xdata[ip] - xdata[im]) - intervals[im] * mu[im];
    00307                 mu[i] = intervals[i] / l[i];
    00308                 z[i] = (alpha[i] - intervals[im] * z[im]) / l[i];
    00309             }
    00310
    00311             // Calculate c's
    00312             c[dataLength - 1] = 0;
    00313             for (int i = dataLength - 2, ip = dataLength - 1; i >= 0; i--, ip--)
    00314             {
    00315                 c[i] = z[i] - mu[i] * c[ip];
    00316             }
    00317
    00318             return c;
    00319         }
    00320
    00328         private static double[] CalculateB(double[] a, double[] c, double[] interval)
    00329         {
    00330             int dataLength = a.Length;
    00331             double[] b = new double[dataLength];
    00332
    00333             for (int i = dataLength - 2, ip = dataLength - 1; i >= 0; i--, ip--)
    00334             {
    00335                 b[i] = (a[ip] - a[i]) / interval[i] - interval[i] * (c[ip] + 2 * c[i]) / 3;
    00336             }
    00337
    00338             return b;
    00339         }
    00340
    00347         private static double[] CalculateD(double[] c, double[] interval)
    00348         {
    00349             int dataLength = c.Length;
    00350             double[] d = new double[dataLength];
    00351
    00352             for (int i = dataLength - 2; i >= 0; i--)
    00353             {
    00354                 d[i] = (c[i + 1] - c[i]) / (3 * interval[i]);
    00355             }
    00356
    00357             return d;
    00358         }
    00359
    00363         private void CalculateCoefficients()
    00364         {
    00365             // create all data arrays
    00366             this.xData = this.data.Keys.ToArray();
    00367             this.xIntervals = CalculateIntervals(this.xData);
    00368             double[][] yArray = this.data.Values.ToArray();
    00369             this.yData = new double[this.dimensions][];
    00370             for (int i = 0; i < this.dimensions; i++)
    00371             {
    00372                 // y data array needs to be rotated in its order from the list array
    00373                 this.yData[i] = new double[this.data.Count];
    00374                 for (int j = 0; j < this.data.Count; j++)
    00375                 {
    00376                     this.yData[i][j] = yArray[j][i];
    00377                 }
    00378
    00379                 // calculate coefficients
    00380                 this.cCoeff[i] = CalculateC(this.yData[i], this.xData, this.xIntervals);
    00381                 this.bCoeff[i] = CalculateB(this.yData[i], this.cCoeff[i], this.xIntervals);
    00382                 this.dCoeff[i] = CalculateD(this.cCoeff[i], this.xIntervals);
    00383             }
    00384
    00385             this.needUpdate = false;
    00386         }
    00387
    00388         #endregion
    00389     }
    00390 }