unsupported/Eigen/src/Splines/SplineFitting.h - eigen/fuchsia - Git at Google

 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
 //
 // Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

 #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
 {
   /**
    * \brief Computes knot averages.
    * \ingroup Splines_Module
    *
    * The knots are computed as
    * \f{align*}
    *  u_0 & = \hdots = u_p = 0 \\
    *  u_{m-p} & = \hdots = u_{m} = 1 \\
    *  u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p
    * \f}
    * where \f$p\f$ is the degree and \f$m+1\f$ the number knots
    * of the desired interpolating spline.
    *
    * \param[in] parameters The input parameters. During interpolation one for each data point.
    * \param[in] degree The spline degree which is used during the interpolation.
    * \param[out] knots The output knot vector.
    *
    * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
    **/
   template <typename KnotVectorType>
   void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
   {
     knots.resize(parameters.size()+degree+1);

     for (DenseIndex j=1; j<parameters.size()-degree; ++j)
       knots(j+degree) = parameters.segment(j,degree).mean();

     knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
     knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
   }

   /**
    * \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
    *
    * \param[in] pts The data points to which a spline should be fit.
    * \param[out] chord_lengths The resulting chord lenggth vector.
    *
    * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
    **/
   template <typename PointArrayType, typename KnotVectorType>
   void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
   {
     typedef typename KnotVectorType::Scalar Scalar;

     const DenseIndex n = pts.cols();

     // 1. compute the column-wise norms
     chord_lengths.resize(pts.cols());
     chord_lengths[0] = 0;
     chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();

     // 2. compute the partial sums
     std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());

     // 3. normalize the data
     chord_lengths /= chord_lengths(n-1);
     chord_lengths(n-1) = Scalar(1);
   }

   /**
    * \brief Spline fitting methods.
    * \ingroup Splines_Module
    **/
   template <typename SplineType>
   struct SplineFitting
   {
     typedef typename SplineType::KnotVectorType KnotVectorType;
     typedef typename SplineType::ParameterVectorType ParameterVectorType;

     /**
      * \brief Fits an interpolating Spline to the given data points.
      *
      * \param pts The points for which an interpolating spline will be computed.
      * \param degree The degree of the interpolating spline.
      *
      * \returns A spline interpolating the initially provided points.
      **/
     template <typename PointArrayType>
     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);

     /**
      * \brief Fits an interpolating Spline to the given data points.
      *
      * \param pts The points for which an interpolating spline will be computed.
      * \param degree The degree of the interpolating spline.
      * \param knot_parameters The knot parameters for the interpolation.
      *
      * \returns A spline interpolating the initially provided points.
      **/
     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>
   template <typename PointArrayType>
   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
   {
     typedef typename SplineType::KnotVectorType::Scalar Scalar;
     typedef typename SplineType::ControlPointVectorType ControlPointVectorType;

     typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;

     KnotVectorType knots;
     KnotAveraging(knot_parameters, degree, knots);

     DenseIndex n = pts.cols();
     MatrixType A = MatrixType::Zero(n,n);
     for (DenseIndex i=1; i<n-1; ++i)
     {
       const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);

       // The segment call should somehow be told the spline order at compile time.
       A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
     }
     A(0,0) = 1.0;
     A(n-1,n-1) = 1.0;

     HouseholderQR<MatrixType> qr(A);

     // Here, we are creating a temporary due to an Eigen issue.
     ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();

     return SplineType(knots, ctrls);
   }

   template <typename SplineType>
   template <typename PointArrayType>
   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
   {
     KnotVectorType chord_lengths; // knot parameters
     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
	// This file is part of Eigen, a lightweight C++ template library
	// for linear algebra.
	//
	// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
	//
	// This Source Code Form is subject to the terms of the Mozilla
	// Public License v. 2.0. If a copy of the MPL was not distributed
	// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

	#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
	{
	/**
	* \brief Computes knot averages.
	* \ingroup Splines_Module
	*
	* The knots are computed as
	* \f{align*}
	* u_0 & = \hdots = u_p = 0 \\
	* u_{m-p} & = \hdots = u_{m} = 1 \\
	* u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p
	* \f}
	* where \f$p\f$ is the degree and \f$m+1\f$ the number knots
	* of the desired interpolating spline.
	*
	* \param[in] parameters The input parameters. During interpolation one for each data point.
	* \param[in] degree The spline degree which is used during the interpolation.
	* \param[out] knots The output knot vector.
	*
	* \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
	**/
	template <typename KnotVectorType>
	void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
	{
	knots.resize(parameters.size()+degree+1);

	for (DenseIndex j=1; j<parameters.size()-degree; ++j)
	knots(j+degree) = parameters.segment(j,degree).mean();

	knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
	knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
	}

	/**
	* \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
	*
	* \param[in] pts The data points to which a spline should be fit.
	* \param[out] chord_lengths The resulting chord lenggth vector.
	*
	* \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
	**/
	template <typename PointArrayType, typename KnotVectorType>
	void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
	{
	typedef typename KnotVectorType::Scalar Scalar;

	const DenseIndex n = pts.cols();

	// 1. compute the column-wise norms
	chord_lengths.resize(pts.cols());
	chord_lengths[0] = 0;
	chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();

	// 2. compute the partial sums
	std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());

	// 3. normalize the data
	chord_lengths /= chord_lengths(n-1);
	chord_lengths(n-1) = Scalar(1);
	}

	/**
	* \brief Spline fitting methods.
	* \ingroup Splines_Module
	**/
	template <typename SplineType>
	struct SplineFitting
	{
	typedef typename SplineType::KnotVectorType KnotVectorType;
	typedef typename SplineType::ParameterVectorType ParameterVectorType;

	/**
	* \brief Fits an interpolating Spline to the given data points.
	*
	* \param pts The points for which an interpolating spline will be computed.
	* \param degree The degree of the interpolating spline.
	*
	* \returns A spline interpolating the initially provided points.
	**/
	template <typename PointArrayType>
	static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);

	/**
	* \brief Fits an interpolating Spline to the given data points.
	*
	* \param pts The points for which an interpolating spline will be computed.
	* \param degree The degree of the interpolating spline.
	* \param knot_parameters The knot parameters for the interpolation.
	*
	* \returns A spline interpolating the initially provided points.
	**/
	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>
	template <typename PointArrayType>
	SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
	{
	typedef typename SplineType::KnotVectorType::Scalar Scalar;
	typedef typename SplineType::ControlPointVectorType ControlPointVectorType;

	typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;

	KnotVectorType knots;
	KnotAveraging(knot_parameters, degree, knots);

	DenseIndex n = pts.cols();
	MatrixType A = MatrixType::Zero(n,n);
	for (DenseIndex i=1; i<n-1; ++i)
	{
	const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);

	// The segment call should somehow be told the spline order at compile time.
	A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
	}
	A(0,0) = 1.0;
	A(n-1,n-1) = 1.0;

	HouseholderQR<MatrixType> qr(A);

	// Here, we are creating a temporary due to an Eigen issue.
	ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();

	return SplineType(knots, ctrls);
	}

	template <typename SplineType>
	template <typename PointArrayType>
	SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
	{
	KnotVectorType chord_lengths; // knot parameters
	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