Added Spline interpolation with derivatives.

4 files changed, 363 insertions, 13 deletions

diff --git a/unsupported/Eigen/src/Splines/Spline.h b/unsupported/Eigen/src/Splines/Spline.h
index 1b47992d6..c46f728bc 100644
--- a/unsupported/Eigen/src/Splines/Spline.h
+++ b/unsupported/Eigen/src/Splines/Spline.h
@@ -44,9 +44,15 @@ namespace Eigen
     
     /** \brief The data type used to store knot vectors. */
     typedef typename SplineTraits<Spline>::KnotVectorType KnotVectorType;
+
+    /** \brief The data type used to store parameter vectors. */
+    typedef typename SplineTraits<Spline>::ParameterVectorType ParameterVectorType;
     
     /** \brief The data type used to store non-zero basis functions. */
     typedef typename SplineTraits<Spline>::BasisVectorType BasisVectorType;
+
+    /** \brief The data type used to store the values of the basis function derivatives. */
+    typedef typename SplineTraits<Spline>::BasisDerivativeType BasisDerivativeType;
     
     /** \brief The data type representing the spline's control points. */
     typedef typename SplineTraits<Spline>::ControlPointVectorType ControlPointVectorType;
@@ -203,10 +209,25 @@ namespace Eigen
      **/
     static BasisVectorType BasisFunctions(Scalar u, DenseIndex degree, const KnotVectorType& knots);
 
+    /**
+     * \copydoc Spline::basisFunctionDerivatives
+     * \param degree The degree of the underlying spline
+     * \param knots The underlying spline's knot vector.
+     **/    
+    static BasisDerivativeType BasisFunctionDerivatives(
+      const Scalar u, const DenseIndex order, const DenseIndex degree, const KnotVectorType& knots);
 
   private:
     KnotVectorType m_knots; /*!< Knot vector. */
     ControlPointVectorType  m_ctrls; /*!< Control points. */
+
+    template <typename DerivativeType>
+    static void BasisFunctionDerivativesImpl(
+      const typename Spline<_Scalar, _Dim, _Degree>::Scalar u,
+      const DenseIndex order,
+      const DenseIndex p, 
+      const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& U,
+      DerivativeType& N_);
   };
 
   template <typename _Scalar, int _Dim, int _Degree>
@@ -345,20 +366,24 @@ namespace Eigen
   }
 
   /* --------------------------------------------------------------------------------------------- */
-
-  template <typename SplineType, typename DerivativeType>
-  void basisFunctionDerivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& N_)
+  
+  
+  template <typename _Scalar, int _Dim, int _Degree>
+  template <typename DerivativeType>
+  void Spline<_Scalar, _Dim, _Degree>::BasisFunctionDerivativesImpl(
+    const typename Spline<_Scalar, _Dim, _Degree>::Scalar u,
+    const DenseIndex order,
+    const DenseIndex p, 
+    const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& U,
+    DerivativeType& N_)
   {
+    typedef Spline<_Scalar, _Dim, _Degree> SplineType;
     enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
 
     typedef typename SplineTraits<SplineType>::Scalar Scalar;
     typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
-    typedef typename SplineTraits<SplineType>::KnotVectorType KnotVectorType;
-
-    const KnotVectorType& U = spline.knots();
-
-    const DenseIndex p = spline.degree();
-    const DenseIndex span = spline.span(u);
+  
+    const DenseIndex span = SplineType::Span(u, p, U);
 
     const DenseIndex n = (std::min)(p, order);
 
@@ -455,8 +480,8 @@ namespace Eigen
   typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
     Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
   {
-    typename SplineTraits< Spline >::BasisDerivativeType der;
-    basisFunctionDerivativesImpl(*this, u, order, der);
+    typename SplineTraits<Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType der;
+    BasisFunctionDerivativesImpl(u, order, degree(), knots(), der);
     return der;
   }
 
@@ -465,8 +490,21 @@ namespace Eigen
   typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType
     Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
   {
-    typename SplineTraits< Spline, DerivativeOrder >::BasisDerivativeType der;
-    basisFunctionDerivativesImpl(*this, u, order, der);
+    typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType der;
+    BasisFunctionDerivativesImpl(u, order, degree(), knots(), der);
+    return der;
+  }
+
+  template <typename _Scalar, int _Dim, int _Degree>
+  typename SplineTraits<Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
+  Spline<_Scalar, _Dim, _Degree>::BasisFunctionDerivatives(
+    const typename Spline<_Scalar, _Dim, _Degree>::Scalar u,
+    const DenseIndex order,
+    const DenseIndex degree,
+    const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& knots)
+  {
+    typename SplineTraits<Spline>::BasisDerivativeType der;
+    BasisFunctionDerivativesImpl(u, order, degree, knots, der);
     return der;
   }
 }
diff --git a/unsupported/Eigen/src/Splines/SplineFitting.h b/unsupported/Eigen/src/Splines/SplineFitting.h
index 0265d532c..7c2484e0d 100644
--- a/unsupported/Eigen/src/Splines/SplineFitting.h
+++ b/unsupported/Eigen/src/Splines/SplineFitting.h
@@ -10,7 +10,10 @@
 #ifndef EIGEN_SPLINE_FITTING_H
 #define EIGEN_SPLINE_FITTING_H
 
+#include <algorithm>
+#include <functional>
 #include <numeric>
+#include <vector>
 
 #include "SplineFwd.h"
 
@@ -50,6 +53,129 @@ namespace Eigen
   }
 
   /**
+   * \brief Computes knot averages when derivative constraints are present.
+   * Note that this is a technical interpretation of the referenced article
+   * since the algorithm contained therein is incorrect as written.
+   * \ingroup Splines_Module
+   *
+   * \param[in] parameters The parameters at which the interpolation B-Spline
+   *            will intersect the given interpolation points. The parameters
+   *            are assumed to be a non-decreasing sequence.
+   * \param[in] degree The degree of the interpolating B-Spline. This must be
+   *            greater than zero.
+   * \param[in] derivativeIndices The indices corresponding to parameters at
+   *            which there are derivative constraints. The indices are assumed
+   *            to be a non-decreasing sequence.
+   * \param[out] knots The calculated knot vector. These will be returned as a
+   *             non-decreasing sequence
+   *
+   * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
+   * Curve interpolation with directional constraints for engineering design. 
+   * Engineering with Computers
+   **/
+  template <typename KnotVectorType, typename ParameterVectorType>
+  void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
+				    const unsigned int degree,
+				    const IndexArray& derivativeIndices,
+				    KnotVectorType& knots)
+  {
+    typedef typename ParameterVectorType::Scalar Scalar;
+
+    const unsigned int numParameters = parameters.size();
+    const unsigned int numDerivatives = derivativeIndices.size();
+
+    if (numDerivatives < 1)
+    {
+      KnotAveraging(parameters, degree, knots);
+      return;
+    }
+
+    uint startIndex;
+    uint endIndex;
+  
+    unsigned int numInternalDerivatives = numDerivatives;
+    
+    if (derivativeIndices[0] == 0)
+    {
+      startIndex = 0;
+      --numInternalDerivatives;
+    }
+    else
+    {
+      startIndex = 1;
+    }
+    if (derivativeIndices[numDerivatives - 1] == numParameters - 1)
+    {
+      endIndex = numParameters - degree;
+      --numInternalDerivatives;
+    }
+    else
+    {
+      endIndex = numParameters - degree - 1;
+    }
+
+    // There are (endIndex - startIndex + 1) knots obtained from the averaging
+    // and 2 for the first and last parameters.
+    unsigned int numAverageKnots = endIndex - startIndex + 3;
+    KnotVectorType averageKnots(numAverageKnots);
+    averageKnots[0] = parameters[0];
+
+    int newKnotIndex = 0;
+    for (unsigned int i = startIndex; i <= endIndex; ++i)
+      averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
+    averageKnots[++newKnotIndex] = parameters[numParameters - 1];
+
+    newKnotIndex = -1;
+  
+    ParameterVectorType temporaryParameters(numParameters);
+    KnotVectorType derivativeKnots(numInternalDerivatives);
+    for (unsigned int i = 0; i < numAverageKnots - 1; ++i)
+    {
+      temporaryParameters[0] = averageKnots[i];
+      ParameterVectorType parameterIndices(numParameters);
+      int temporaryParameterIndex = 1;
+      for (size_t j = 0; j < numParameters; ++j)
+      {
+	Scalar parameter = parameters[j];
+	if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1])
+	{
+	  parameterIndices[temporaryParameterIndex] = j;
+	  temporaryParameters[temporaryParameterIndex++] = parameter;
+	}
+      }
+      temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1];
+
+      for (int j = 0; j <= temporaryParameterIndex - 2; ++j)
+      {
+	for (DenseIndex k = 0; k < derivativeIndices.size(); ++k)
+	{
+	  if (parameterIndices[j + 1] == derivativeIndices[k]
+	      && parameterIndices[j + 1] != 0
+	      && parameterIndices[j + 1] != numParameters - 1)
+	  {
+	    derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean();
+	    break;
+	  }
+	}
+      }
+    }
+    
+    KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size());
+
+    std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(),
+	       derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(),
+	       temporaryKnots.data());
+
+    // Number of control points (one for each point and derivative) plus spline order.
+    DenseIndex numKnots = numParameters + numDerivatives + degree + 1;
+    knots.resize(numKnots);
+
+    knots.head(degree).fill(temporaryKnots[0]);
+    knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]);
+    knots.segment(degree, temporaryKnots.size()) = temporaryKnots;
+  }
+
+  /**
    * \brief Computes chord length parameters which are required for spline interpolation.
    * \ingroup Splines_Module
    *
@@ -86,6 +212,7 @@ namespace Eigen
   struct SplineFitting
   {
     typedef typename SplineType::KnotVectorType KnotVectorType;
+    typedef typename SplineType::ParameterVectorType ParameterVectorType;
 
     /**
      * \brief Fits an interpolating Spline to the given data points.
@@ -109,6 +236,52 @@ namespace Eigen
      **/
     template <typename PointArrayType>
     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
+
+    /**
+     * \brief Fits an interpolating spline to the given data points and
+     * derivatives.
+     * 
+     * \param points The points for which an interpolating spline will be computed.
+     * \param derivatives The desired derivatives of the interpolating spline at interpolation
+     *                    points.
+     * \param derivativeIndices An array indicating which point each derivative belongs to. This
+     *                          must be the same size as @a derivatives.
+     * \param degree The degree of the interpolating spline.
+     *
+     * \returns A spline interpolating @a points with @a derivatives at those points.
+     *
+     * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
+     * Curve interpolation with directional constraints for engineering design. 
+     * Engineering with Computers
+     **/
+    template <typename PointArrayType>
+    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
+						 const PointArrayType& derivatives,
+						 const IndexArray& derivativeIndices,
+						 const unsigned int degree);
+
+    /**
+     * \brief Fits an interpolating spline to the given data points and derivatives.
+     * 
+     * \param points The points for which an interpolating spline will be computed.
+     * \param derivatives The desired derivatives of the interpolating spline at interpolation points.
+     * \param derivativeIndices An array indicating which point each derivative belongs to. This
+     *                          must be the same size as @a derivatives.
+     * \param degree The degree of the interpolating spline.
+     * \param parameters The parameters corresponding to the interpolation points.
+     *
+     * \returns A spline interpolating @a points with @a derivatives at those points.
+     *
+     * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
+     * Curve interpolation with directional constraints for engineering design. 
+     * Engineering with Computers
+     */
+    template <typename PointArrayType>
+    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
+						 const PointArrayType& derivatives,
+						 const IndexArray& derivativeIndices,
+						 const unsigned int degree,
+						 const ParameterVectorType& parameters);
   };
 
   template <typename SplineType>
@@ -151,6 +324,106 @@ namespace Eigen
     ChordLengths(pts, chord_lengths);
     return Interpolate(pts, degree, chord_lengths);
   }
+  
+  template <typename SplineType>
+  template <typename PointArrayType>
+  SplineType 
+  SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
+							const PointArrayType& derivatives,
+							const IndexArray& derivativeIndices,
+							const unsigned int degree,
+							const ParameterVectorType& parameters)
+  {
+    typedef typename SplineType::KnotVectorType::Scalar Scalar;      
+    typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
+
+    typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType;
+
+    const DenseIndex n = points.cols() + derivatives.cols();
+    
+    KnotVectorType knots;
+
+    KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots);
+    
+    // fill matrix
+    MatrixType A = MatrixType::Zero(n, n);
+
+    // Use these dimensions for quicker populating, then transpose for solving.
+    MatrixType b(points.rows(), n);
+
+    DenseIndex startRow;
+    DenseIndex derivativeStart;
+
+    // End derivatives.
+    if (derivativeIndices[0] == 0)
+    {
+      A.template block<1, 2>(1, 0) << -1, 1;
+      
+      Scalar y = (knots(degree + 1) - knots(0)) / degree;
+      b.col(1) = y*derivatives.col(0);
+      
+      startRow = 2;
+      derivativeStart = 1;
+    }
+    else
+    {
+      startRow = 1;
+      derivativeStart = 0;
+    }
+    if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1)
+    {
+      A.template block<1, 2>(n - 2, n - 2) << -1, 1;
+
+      Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree;
+      b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1);
+    }
+    
+    DenseIndex row = startRow;
+    DenseIndex derivativeIndex = derivativeStart;
+    for (DenseIndex i = 1; i < parameters.size() - 1; ++i)
+    {
+      const DenseIndex span = SplineType::Span(parameters[i], degree, knots);
+
+      if (derivativeIndices[derivativeIndex] == i)
+      {
+	A.block(row, span - degree, 2, degree + 1)
+	  = SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots);
+
+	b.col(row++) = points.col(i);
+	b.col(row++) = derivatives.col(derivativeIndex++);
+      }
+      else
+      {
+	A.row(row++).segment(span - degree, degree + 1)
+	  = SplineType::BasisFunctions(parameters[i], degree, knots);
+      }
+    }
+    b.col(0) = points.col(0);
+    b.col(b.cols() - 1) = points.col(points.cols() - 1);
+    A(0,0) = 1;
+    A(n - 1, n - 1) = 1;
+    
+    // Solve
+    HouseholderQR<MatrixType> qr(A);
+    ControlPointVectorType controlPoints = qr.solve(MatrixType(b.transpose())).transpose();
+
+    SplineType spline(knots, controlPoints);
+    
+    return spline;
+  }
+  
+  template <typename SplineType>
+  template <typename PointArrayType>
+  SplineType
+  SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
+							const PointArrayType& derivatives,
+							const IndexArray& derivativeIndices,
+							const unsigned int degree)
+  {
+    ParameterVectorType parameters;
+    ChordLengths(points, parameters);
+    return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
+  }
 }
 
 #endif // EIGEN_SPLINE_FITTING_H
diff --git a/unsupported/Eigen/src/Splines/SplineFwd.h b/unsupported/Eigen/src/Splines/SplineFwd.h
index 9ea23a9a1..ec3dc0ff5 100644
--- a/unsupported/Eigen/src/Splines/SplineFwd.h
+++ b/unsupported/Eigen/src/Splines/SplineFwd.h
@@ -48,6 +48,9 @@ namespace Eigen
       
       /** \brief The data type used to store knot vectors. */
       typedef Array<Scalar,1,Dynamic> KnotVectorType;
+
+      /** \brief The data type used to store parameter vectors. */
+      typedef Array<Scalar,1,Dynamic> ParameterVectorType;
       
       /** \brief The data type representing the spline's control points. */
       typedef Array<Scalar,Dimension,Dynamic> ControlPointVectorType;
@@ -85,6 +88,8 @@ namespace Eigen
     
     /** \brief 3D double B-spline with dynamic degree. */
     typedef Spline<double,3> Spline3d;
+
+    typedef Array<DenseIndex, 1, Dynamic> IndexArray;
 }
 
 #endif // EIGEN_SPLINES_FWD_H
diff --git a/unsupported/test/splines.cpp b/unsupported/test/splines.cpp
index a7eb3e0c4..1f3856143 100644
--- a/unsupported/test/splines.cpp
+++ b/unsupported/test/splines.cpp
@@ -234,6 +234,39 @@ void check_global_interpolation2d()
   }
 }
 
+void check_global_interpolation_with_derivatives2d()
+{
+  typedef Spline2d::PointType PointType;
+  typedef Spline2d::KnotVectorType KnotVectorType;
+
+  const unsigned int numPoints = 100;
+  const unsigned int dimension = 2;
+  const unsigned int degree = 3;
+
+  ArrayXXd points = ArrayXXd::Random(dimension, numPoints);
+
+  KnotVectorType knots;
+  Eigen::ChordLengths(points, knots);
+
+  ArrayXXd derivatives = ArrayXXd::Random(dimension, numPoints);
+  Eigen::IndexArray derivativeIndices(numPoints);
+
+  for (Eigen::DenseIndex i = 0; i < numPoints; ++i)
+    derivativeIndices(i) = i;
+
+  const Spline2d spline = SplineFitting<Spline2d>::InterpolateWithDerivatives(
+    points, derivatives, derivativeIndices, degree);  
+    
+  for (Eigen::DenseIndex i = 0; i < points.cols(); ++i)
+  {
+    PointType point = spline(knots(i));
+    PointType referencePoint = points.col(i);
+    VERIFY((point - referencePoint).matrix().norm() < 1e-12);
+    PointType derivative = spline.derivatives(knots(i), 1).col(1);
+    PointType referenceDerivative = derivatives.col(i);
+    VERIFY((derivative - referenceDerivative).matrix().norm() < 1e-10);
+  }
+}
 
 void test_splines()
 {
@@ -241,4 +274,5 @@ void test_splines()
   CALL_SUBTEST( eval_spline3d_onbrks() );
   CALL_SUBTEST( eval_closed_spline2d() );
   CALL_SUBTEST( check_global_interpolation2d() );
+  CALL_SUBTEST( check_global_interpolation_with_derivatives2d() );
 }