update

1 files changed, 274 insertions, 0 deletions

diff --git a/eigen/unsupported/Eigen/src/Splines/SplineFitting.h b/eigen/unsupported/Eigen/src/Splines/SplineFitting.h
index 0265d53..c761a9b 100644
--- a/eigen/unsupported/Eigen/src/Splines/SplineFitting.h
+++ b/eigen/unsupported/Eigen/src/Splines/SplineFitting.h
@@ -10,10 +10,14 @@
 #ifndef EIGEN_SPLINE_FITTING_H
 #define EIGEN_SPLINE_FITTING_H
 
+#include <algorithm>
+#include <functional>
 #include <numeric>
+#include <vector>
 
 #include "SplineFwd.h"
 
+#include <Eigen/LU>
 #include <Eigen/QR>
 
 namespace Eigen
@@ -50,6 +54,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, typename IndexArray>
+  void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
+                                    const unsigned int degree,
+                                    const IndexArray& derivativeIndices,
+                                    KnotVectorType& knots)
+  {
+    typedef typename ParameterVectorType::Scalar Scalar;
+
+    DenseIndex numParameters = parameters.size();
+    DenseIndex numDerivatives = derivativeIndices.size();
+
+    if (numDerivatives < 1)
+    {
+      KnotAveraging(parameters, degree, knots);
+      return;
+    }
+
+    DenseIndex startIndex;
+    DenseIndex endIndex;
+  
+    DenseIndex 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.
+    DenseIndex numAverageKnots = endIndex - startIndex + 3;
+    KnotVectorType averageKnots(numAverageKnots);
+    averageKnots[0] = parameters[0];
+
+    int newKnotIndex = 0;
+    for (DenseIndex i = startIndex; i <= endIndex; ++i)
+      averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
+    averageKnots[++newKnotIndex] = parameters[numParameters - 1];
+
+    newKnotIndex = -1;
+  
+    ParameterVectorType temporaryParameters(numParameters + 1);
+    KnotVectorType derivativeKnots(numInternalDerivatives);
+    for (DenseIndex i = 0; i < numAverageKnots - 1; ++i)
+    {
+      temporaryParameters[0] = averageKnots[i];
+      ParameterVectorType parameterIndices(numParameters);
+      int temporaryParameterIndex = 1;
+      for (DenseIndex 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 knots (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 +213,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 +237,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, typename IndexArray>
+    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, typename IndexArray>
+    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
+                                                 const PointArrayType& derivatives,
+                                                 const IndexArray& derivativeIndices,
+                                                 const unsigned int degree,
+                                                 const ParameterVectorType& parameters);
   };
 
   template <typename SplineType>
@@ -151,6 +325,106 @@ namespace Eigen
     ChordLengths(pts, chord_lengths);
     return Interpolate(pts, degree, chord_lengths);
   }
+  
+  template <typename SplineType>
+  template <typename PointArrayType, typename IndexArray>
+  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
+    FullPivLU<MatrixType> lu(A);
+    ControlPointVectorType controlPoints = lu.solve(MatrixType(b.transpose())).transpose();
+
+    SplineType spline(knots, controlPoints);
+    
+    return spline;
+  }
+  
+  template <typename SplineType>
+  template <typename PointArrayType, typename IndexArray>
+  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