BSpline.java

/*
 * Class:        BSpline
 * Description:  B-spline
 * Environment:  Java
 * Software:     SSJ 
 * Copyright (C) 2001  Pierre L'Ecuyer and Universite de Montreal
 * Organization: DIRO, Universite de Montreal
 * @author       
 * @since
 *
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
package umontreal.ssj.functionfit;
 
import umontreal.ssj.util.Misc;
import umontreal.ssj.util.RootFinder;
import umontreal.ssj.functions.MathFunction;
import umontreal.ssj.functions.MathFunctionWithIntegral;
import umontreal.ssj.functions.MathFunctionWithDerivative;
import umontreal.ssj.functions.MathFunctionWithFirstDerivative;
import umontreal.ssj.functions.MathFunctionUtil;
import cern.colt.matrix.linalg.Algebra;
import cern.colt.matrix.DoubleMatrix2D;
import cern.colt.matrix.DoubleFactory2D;
import java.util.Arrays;
import java.util.Random;
import java.io.*;

public class BSpline implements MathFunction
 
      , MathFunctionWithIntegral, MathFunctionWithDerivative, MathFunctionWithFirstDerivative {
   // Formula taken from http://www.ibiblio.org/e-notes/Splines/Basis.htm
   // and http://en.wikipedia.org/wiki/De_Boor_algorithm
   private double[] x; // x original
   private double[] y; // y original
 
   private int degree;
 
   // variables sur lesquelles on travaille
   private double[] myX;
   private double[] myY;
   private double[] knots;

   public BSpline(final double[] x, final double[] y, final int degree) {
      if (x.length != y.length)
         throw new IllegalArgumentException("The arrays x and y must share the same length");
      this.degree = degree;
      this.x = x.clone();
      this.y = y.clone();
      init(x, y, null);
   }

   public BSpline(final double[] x, final double[] y, final double[] knots) {
      if (x.length != y.length)
         throw new IllegalArgumentException("The arrays x and y must share the same length");
      if (knots.length <= x.length + 1)
         throw new IllegalArgumentException("The number of knots must be at least n+2");
 
      this.x = x.clone();
      this.y = y.clone();
      this.knots = knots.clone();
      init(x, y, knots);
   }

   public double[] getX() {
      return myX.clone();
   }

   public double[] getY() {
      return myY.clone();
   }

   public double getMaxKnot() {
      return knots[knots.length - 1];
   }

   public double getMinKnot() {
      return knots[0];
   }

   public double[] getKnots() {
      return knots.clone();
   }

   public static BSpline createInterpBSpline(double[] x, double[] y, int degree) {
      if (x.length != y.length)
         throw new IllegalArgumentException("The arrays x and y must share the same length");
      if (x.length <= degree)
         throw new IllegalArgumentException("The arrays length must be greater than degree");
 
      int n = x.length - 1;
      // compute t : parameters vector uniformly from 0 to 1
      double[] t = new double[x.length];
      for (int i = 0; i < t.length; i++) {
         t[i] = (double) i / (t.length - 1);
      }
 
      // compute U : clamped knots vector uniformly from 0 to 1
      double U[] = new double[x.length + degree + 1];
      int m = U.length - 1;
      for (int i = 0; i <= degree; i++)
         U[i] = 0;
      for (int i = 1; i < x.length - degree; i++)
         U[i + degree] = (double) i / (x.length - degree);
      for (int i = U.length - 1 - degree; i < U.length; i++)
         U[i] = 1;
 
      // compute matrix N : made of BSpline coefficients
      double[][] N = new double[x.length][x.length];
      for (int i = 0; i < x.length; i++) {
         N[i] = computeN(U, degree, t[i], x.length);
      }
 
      // initialize D : initial points matrix
      double[][] D = new double[x.length][2];
      for (int i = 0; i < x.length; i++) {
         D[i][0] = x[i];
         D[i][1] = y[i];
      }
 
      // solve the linear equation system using colt library
      DoubleMatrix2D coltN = DoubleFactory2D.dense.make(N);
      DoubleMatrix2D coltD = DoubleFactory2D.dense.make(D);
      DoubleMatrix2D coltP;
      coltP = Algebra.ZERO.solve(coltN, coltD);
 
      return new BSpline(coltP.viewColumn(0).toArray(), coltP.viewColumn(1).toArray(), U);
   }

   public static BSpline createApproxBSpline(double[] x, double[] y, int degree, int hp1) {
      if (x.length != y.length)
         throw new IllegalArgumentException("The arrays x and y must share the same length");
      if (x.length <= degree)
         throw new IllegalArgumentException("The arrays length must be greater than degree");
 
      int h = hp1 - 1;
      int n = x.length - 1;
 
      // compute t : parameters vector uniformly from 0 to 1
      double[] t = new double[x.length];
      for (int i = 0; i < t.length; i++) {
         t[i] = (double) i / n;
      }
 
      // compute U : clamped knots vector uniformly from 0 to 1
      int m = h + degree + 1;
      double U[] = new double[m + 1];
      for (int i = 0; i <= degree; i++)
         U[i] = 0;
      for (int i = 1; i < hp1 - degree; i++)
         U[i + degree] = (double) i / (hp1 - degree);
      for (int i = m - degree; i <= m; i++)
         U[i] = 1;
 
      // compute matrix N : composed of BSpline coefficients
      double[][] N = new double[n + 1][h + 1];
      for (int i = 0; i < N.length; i++) {
         N[i] = computeN(U, degree, t[i], h + 1);
      }
 
      // initialize D : initial points matrix
      double[][] D = new double[x.length][2];
      for (int i = 0; i < x.length; i++) {
         D[i][0] = x[i];
         D[i][1] = y[i];
      }
 
      // compute Q :
      double[][] tempQ = new double[x.length][2];
      for (int k = 1; k < n; k++) {
         tempQ[k][0] = D[k][0] - N[k][0] * D[0][0] - N[k][h] * D[D.length - 1][0];
         tempQ[k][1] = D[k][1] - N[k][0] * D[0][1] - N[k][h] * D[D.length - 1][1];
      }
      double[][] Q = new double[h - 1][2];
      for (int i = 1; i < h; i++) {
         for (int k = 1; k < n; k++) {
            Q[i - 1][0] += N[k][i] * tempQ[k][0];
            Q[i - 1][1] += N[k][i] * tempQ[k][1];
         }
      }
 
      // compute N matrix for computation:
      double[][] N2 = new double[n - 1][h - 1];
      for (int i = 0; i < N2.length; i++) {
         for (int j = 0; j < h - 1; j++)
            N2[i][j] = N[i + 1][j + 1];
      }
 
      // solve the linear equation system using colt library
      DoubleMatrix2D coltQ = DoubleFactory2D.dense.make(Q);
      DoubleMatrix2D coltN = DoubleFactory2D.dense.make(N2);
      DoubleMatrix2D coltM = Algebra.ZERO.mult(Algebra.ZERO.transpose(coltN), coltN);
      DoubleMatrix2D coltP = Algebra.ZERO.solve(coltM, coltQ);
      double[] pxTemp = coltP.viewColumn(0).toArray();
      double[] pyTemp = coltP.viewColumn(1).toArray();
      double[] px = new double[hp1];
      double[] py = new double[hp1];
      px[0] = D[0][0];
      py[0] = D[0][1];
      px[h] = D[D.length - 1][0];
      py[h] = D[D.length - 1][1];
      for (int i = 0; i < pxTemp.length; i++) {
         px[i + 1] = pxTemp[i];
         py[i + 1] = pyTemp[i];
      }
 
      return new BSpline(px, py, U);
      // return new BSpline(px, py, degree);
   }

   public BSpline derivativeBSpline() {
      double xTemp[] = new double[this.myX.length - 1];
      double yTemp[] = new double[this.myY.length - 1];
      for (int i = 0; i < xTemp.length; i++) {
         xTemp[i] = (myX[i + 1] - myX[i]) * degree / (knots[i + degree + 1] - knots[i + 1]);
         yTemp[i] = (myY[i + 1] - myY[i]) * degree / (knots[i + degree + 1] - knots[i + 1]);
      }
 
      double[] newKnots = new double[knots.length - 2];
      for (int i = 0; i < newKnots.length; i++) {
         newKnots[i] = knots[i + 1];
      }
 
      // tri pas optimise du tout
      double xTemp2[] = new double[this.myX.length - 1];
      double yTemp2[] = new double[this.myY.length - 1];
      for (int i = 0; i < xTemp.length; i++) {
         int k = 0;
         for (int j = 0; j < xTemp.length; j++) {
            if (xTemp[i] > xTemp[j])
               k++;
         }
         while (xTemp2[k] != 0)
            k++;
         xTemp2[k] = xTemp[i];
         yTemp2[k] = yTemp[i];
      }
 
      return new BSpline(xTemp2, yTemp2, newKnots);
   }

   public BSpline derivativeBSpline(int i) {
      BSpline bs = this;
      while (i > 0) {
         i--;
         bs = bs.derivativeBSpline();
      }
      return bs;
   }
 
   public double evaluate(final double u) {
      final MathFunction xFunction = new MathFunction() {
         public double evaluate(double t) {
            return evalX(t) - u;
         }
      };
      // brentDekker may be unstable; using bisection instead
      // final double t = RootFinder.brentDekker (0, 1, xFunction, 1e-6);
      final double t = RootFinder.bisection(0, 1, xFunction, 1e-6);
      return evalY(t);
   }
 
   public double integral(double a, double b) {
      return MathFunctionUtil.simpsonIntegral(this, a, b, 500);
   }
 
   public double derivative(double u) {
      return derivativeBSpline().evaluate(u);
   }
 
   public double derivative(double u, int n) {
      return derivativeBSpline(n).evaluate(u);
   }
 
   private void init(double[] x, double[] y, double[] initialKnots) {
      if (initialKnots == null) {
         // Cree un vecteur de noeud uniforme entre 0 et 1
         knots = new double[x.length + degree + 1];
         for (int i = degree; i < this.knots.length - degree; i++)
            this.knots[i] = (double) (i - degree) / (knots.length - (2.0 * degree) - 1);
         for (int i = this.knots.length - degree; i < this.knots.length; i++)
            this.knots[i] = this.knots[i - 1];
         for (int i = degree; i > 0; i--)
            this.knots[i - 1] = this.knots[i];
 
         // cree notre vecteur interne de Points de controle
         // ici, aucune modification a faire sur les tableaux originaux
         myX = x;
         myY = y;
      } else {
         degree = initialKnots.length - x.length - 1;
 
         // on adapte le tableau des noeuds a notre algorithme
         // le tableau knot necessite d'avoir degree fois la meme valeur en debut et en
         // fin de tableau
         // on adapte la taille des tableau X et Y en consequence afin de continuer a
         // respecter la condition :
         // x.length + degree + 1 = this.knots.length
         // Cette modification n'influence pas le resultat et permet de fermer notre
         // courbe
 
         // Compute the number of values that need to be added
         int iBorneInf = 1, iBorneSup = initialKnots.length - 2;
         // while(initialKnots[iBorneInf] == initialKnots[0])
         while (areEqual(initialKnots[iBorneInf], initialKnots[0], 1e-10))
            iBorneInf++;
         if (iBorneInf <= degree)
            iBorneInf = degree - iBorneInf + 1;
         else
            iBorneInf = 0;// on a alors iBorneInf valeurs a rajouter en debut de tableau
 
         // while(initialKnots[iBorneSup] == initialKnots[initialKnots.length-1])
         while (areEqual(initialKnots[iBorneSup], initialKnots[initialKnots.length - 1], 1e-10))
            iBorneSup--;
         if (iBorneSup >= initialKnots.length - 1 - degree)
            iBorneSup = degree + 1 - (initialKnots.length - 1 - iBorneSup);
         else
            iBorneSup = 0; // on a alors iBorneSup valeurs a rajouter en fin de tableau
 
         // add computed values
         this.knots = new double[initialKnots.length + iBorneInf + iBorneSup];
         myX = new double[x.length + iBorneInf + iBorneSup];
         myY = new double[y.length + iBorneInf + iBorneSup];
         for (int i = 0; i < iBorneInf; i++) {
            this.knots[i] = initialKnots[0];
            myX[i] = x[0];
            myY[i] = y[0];
         }
         for (int i = 0; i < initialKnots.length; i++)
            this.knots[iBorneInf + i] = initialKnots[i];
         for (int i = 0; i < x.length; i++) {
            myX[iBorneInf + i] = x[i];
            myY[iBorneInf + i] = y[i];
         }
         for (int i = 0; i < iBorneSup; i++) {
            this.knots[this.knots.length - 1 - i] = initialKnots[initialKnots.length - 1];
            myX[myX.length - 1 - i] = x[x.length - 1];
            myY[myY.length - 1 - i] = y[y.length - 1];
         }
      }
   }
 
   public double evalX(double u) {
      int k = Misc.getTimeInterval(knots, 0, knots.length - 1, u);
      if (k >= myX.length) // set to last control point
         k = myX.length - 1;
 
      double[][] X = new double[degree + 1][myX.length];
 
      for (int i = k - degree; i <= k; i++)
         X[0][i] = myX[i];
 
      for (int j = 1; j <= degree; j++) {
         for (int i = k - degree + j; i <= k; i++) {
            double aij = (u - this.knots[i]) / (this.knots[i + 1 + degree - j] - this.knots[i]);
            X[j][i] = (1 - aij) * X[j - 1][i - 1] + aij * X[j - 1][i];
         }
      }
      return X[degree][k];
   }
 
   public double evalY(double u) {
      int k = Misc.getTimeInterval(knots, 0, knots.length - 1, u);
      if (k >= myY.length) // set to last control point
         k = myY.length - 1;
 
      double[][] Y = new double[degree + 1][myX.length];
 
      for (int i = k - degree; i <= k; i++)
         Y[0][i] = myY[i];
      for (int j = 1; j <= degree; j++) {
         for (int i = k - degree + j; i <= k; i++) {
            double aij = (u - this.knots[i]) / (this.knots[i + 1 + degree - j] - this.knots[i]);
            Y[j][i] = (1 - aij) * Y[j - 1][i - 1] + aij * Y[j - 1][i];
         }
      }
      return Y[degree][k];
   }

   private static boolean areEqual(double a, double b, double tol) {
      return Math.abs(a - b) < tol;
   }
 
   private static double[] computeN(double[] U, int degree, double u, int np1) {
      double[] N = new double[np1];
 
      // special cases at bounds
      if (areEqual(u, U[0], 1e-10)) {
         N[0] = 1.0;
         return N;
      } else if (areEqual(u, U[U.length - 1], 1e-10)) {
         N[N.length - 1] = 1.0;
         return N;
      }
 
      // find the knot index k such that U[k]<= u < U[k+1]
      int k = Misc.getTimeInterval(U, 0, U.length - 1, u);
 
      N[k] = 1.0;
      for (int d = 1; d <= degree; d++) {
         N[k - d] = N[k - d + 1] * (U[k + 1] - u) / (U[k + 1] - U[k - d + 1]);
         for (int i = k - d + 1; i <= k - 1; i++)
            N[i] = (u - U[i]) / (U[i + d] - U[i]) * N[i] + ((U[i + d + 1] - u) / (U[i + d + 1] - U[i + 1])) * N[i + 1];
         N[k] = (u - U[k]) / (U[k + d] - U[k]) * N[k];
      }
      return N;
   }
 
}