git.gag.com Git - debian/openrocket/blob - core/src/net/sf/openrocket/util/PolyInterpolator.java

   1 package net.sf.openrocket.util;
   2
   3 import java.util.Arrays;
   4
   5 /**
   6  * A class for polynomial interpolation.  The interpolation constraints can be specified
   7  * either as function values or values of the n'th derivative of the function.
   8  * Using an interpolation consists of three steps:
   9  * <p>
  10  * 1. constructing a <code>PolyInterpolator</code> using the interpolation x coordinates <br>
  11  * 2. generating the interpolation polynomial using the function and derivative values <br>
  12  * 3. evaluating the polynomial at the desired point
  13  * <p>
  14  * The constructor takes an array of double arrays.  The first array defines x coordinate
  15  * values for the function values, the second array x coordinate values for the function's
  16  * derivatives, the third array for second derivatives and so on.  Constructing the
  17  * <code>PolyInterpolator</code> is relatively slow, O(n^3) where n is the order of the
  18  * polynomial.  (It contains calculation of the inverse of an n x n matrix.)
  19  * <p>
  20  * Generating the interpolation polynomial is performed by the method
  21  * {@link #interpolator(double...)}, which takes as an argument the function and
  22  * derivative values.  This operation takes O(n^2) time.
  23  * <p>
  24  * Finally, evaluating the polynomial at different positions takes O(n) time.
  25  *
  26  * @author Sampo Niskanen <sampo.niskanen@iki.fi>
  27  */
  28 public class PolyInterpolator {
  29
  30         // Order of the polynomial
  31         private final int count;
  32
  33         private final double[][] interpolationMatrix;
  34
  35
  36         /**
  37          * Construct a polynomial interpolation generator.  All arguments to the constructor
  38          * are the x coordinates of the interpolated function.  The first array correspond to
  39          * the function values, the second to function derivatives, the third to second
  40          * derivatives and so forth.  The order of the polynomial is automatically calculated
  41          * from the total number of constraints.
  42          * <p>
  43          * The construction takes O(n^3) time.
  44          *
  45          * @param points        an array of constraints, the first corresponding to function value
  46          *                                      constraints, the second to derivative constraints etc.
  47          */
  48         public PolyInterpolator(double[] ... points) {
  49                 int count = 0;
  50                 for (int i=0; i < points.length; i++) {
  51                         count += points[i].length;
  52                 }
  53                 if (count == 0) {
  54                         throw new IllegalArgumentException("No interpolation points defined.");
  55                 }
  56
  57                 this.count = count;
  58
  59                 int[] mul = new int[count];
  60                 Arrays.fill(mul, 1);
  61
  62                 double[][] matrix = new double[count][count];
  63                 int row = 0;
  64                 for (int j=0; j < points.length; j++) {
  65
  66                         for (int i=0; i < points[j].length; i++) {
  67                                 double x = 1;
  68                                 for (int col = count-1-j; col>= 0; col--) {
  69                                         matrix[row][col] = x*mul[col];
  70                                         x *= points[j][i];
  71                                 }
  72                                 row++;
  73                         }
  74
  75                         for (int i=0; i < count; i++) {
  76                                 mul[i] *= (count-i-j-1);
  77                         }
  78                 }
  79                 assert(row == count);
  80
  81                 interpolationMatrix = inverse(matrix);
  82         }
  83
  84
  85         /**
  86          * Generates an interpolation polynomial.  The arguments supplied to this method
  87          * are the function values, derivatives, second derivatives etc. in the order
  88          * specified in the constructor (i.e. values first, then derivatives etc).
  89          * <p>
  90          * This method takes O(n^2) time.
  91          *
  92          * @param values        the function values, derivatives etc. at positions defined in the
  93          *                                      constructor.
  94          * @return              the coefficients of the interpolation polynomial, the highest order
  95          *                                      term first and the constant last.
  96          */
  97         public double[] interpolator(double... values) {
  98                 if (values.length != count) {
  99                         throw new IllegalArgumentException("Wrong number of arguments "+values.length+
 100                                         " expected "+count);
 101                 }
 102
 103                 double[] ret = new double[count];
 104
 105                 for (int j=0; j < count; j++) {
 106                         for (int i=0; i < count; i++) {
 107                                 ret[j] += interpolationMatrix[j][i] * values[i];
 108                         }
 109                 }
 110
 111                 return ret;
 112         }
 113
 114
 115         /**
 116          * Interpolate the given values at the point <code>x</code>.  This is equivalent
 117          * to generating an interpolation polynomial and evaluating the polynomial at the
 118          * specified point.
 119          *
 120          * @param x                     point at which to evaluate the interpolation polynomial.
 121          * @param values        the function, derivatives etc. at position defined in the
 122          *                                      constructor.
 123          * @return                      the value of the interpolation.
 124          */
 125         public double interpolate(double x, double... values) {
 126                 return eval(x, interpolator(values));
 127         }
 128
 129
 130         /**
 131          * Evaluate a polynomial at the specified point <code>x</code>.  The coefficients are
 132          * assumed to have the highest order coefficient first and the constant term last.
 133          *
 134          * @param x                             position at which to evaluate the polynomial.
 135          * @param coefficients  polynomial coefficients, highest term first and constant last.
 136          * @return                              the value of the polynomial.
 137          */
 138         public static double eval(double x, double[] coefficients) {
 139                 double v = 1;
 140                 double result = 0;
 141                 for (int i = coefficients.length-1; i >= 0; i--) {
 142                         result += coefficients[i] * v;
 143                         v *= x;
 144                 }
 145                 return result;
 146         }
 147
 148
 149
 150
 151         private static double[][] inverse(double[][] matrix) {
 152                 int n = matrix.length;
 153
 154                 double x[][] = new double[n][n];
 155                 double b[][] = new double[n][n];
 156                 int index[] = new int[n];
 157                 for (int i=0; i<n; ++i)
 158                         b[i][i] = 1;
 159
 160                 // Transform the matrix into an upper triangle
 161                 gaussian(matrix, index);
 162
 163                 // Update the matrix b[i][j] with the ratios stored
 164                 for (int i=0; i<n-1; ++i)
 165                         for (int j=i+1; j<n; ++j)
 166                                 for (int k=0; k<n; ++k)
 167                                         b[index[j]][k] -= matrix[index[j]][i]*b[index[i]][k];
 168
 169                 // Perform backward substitutions
 170                 for (int i=0; i<n; ++i) {
 171                         x[n-1][i] = b[index[n-1]][i]/matrix[index[n-1]][n-1];
 172                         for (int j=n-2; j>=0; --j) {
 173                                 x[j][i] = b[index[j]][i];
 174                                 for (int k=j+1; k<n; ++k) {
 175                                         x[j][i] -= matrix[index[j]][k]*x[k][i];
 176                                 }
 177                                 x[j][i] /= matrix[index[j]][j];
 178                         }
 179                 }
 180                 return x;
 181         }
 182
 183         private static void gaussian(double a[][],
 184                         int index[]) {
 185                 int n = index.length;
 186                 double c[] = new double[n];
 187
 188                 // Initialize the index
 189                 for (int i=0; i<n; ++i) index[i] = i;
 190
 191                 // Find the rescaling factors, one from each row
 192                 for (int i=0; i<n; ++i) {
 193                         double c1 = 0;
 194                         for (int j=0; j<n; ++j) {
 195                                 double c0 = Math.abs(a[i][j]);
 196                                 if (c0 > c1) c1 = c0;
 197                         }
 198                         c[i] = c1;
 199                 }
 200
 201                 // Search the pivoting element from each column
 202                 int k = 0;
 203                 for (int j=0; j<n-1; ++j) {
 204                         double pi1 = 0;
 205                         for (int i=j; i<n; ++i) {
 206                                 double pi0 = Math.abs(a[index[i]][j]);
 207                                 pi0 /= c[index[i]];
 208                                 if (pi0 > pi1) {
 209                                         pi1 = pi0;
 210                                         k = i;
 211                                 }
 212                         }
 213
 214                         // Interchange rows according to the pivoting order
 215                         int itmp = index[j];
 216                         index[j] = index[k];
 217                         index[k] = itmp;
 218                         for (int i=j+1; i<n; ++i) {
 219                                 double pj = a[index[i]][j]/a[index[j]][j];
 220
 221                                 // Record pivoting ratios below the diagonal
 222                                 a[index[i]][j] = pj;
 223
 224                                 // Modify other elements accordingly
 225                                 for (int l=j+1; l<n; ++l)
 226                                         a[index[i]][l] -= pj*a[index[j]][l];
 227                         }
 228                 }
 229         }
 230
 231 }