SplineFitting.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 "./InternalHeaderCheck.h"
 
#include "SplineFwd.h"
 
#include "../../../../Eigen/LU"
#include "../../../../Eigen/QR"
 
 
namespace Eigen
{
  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);
  }
 
  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;
  }
 
  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);
  }
 
  template <typename SplineType>
  struct SplineFitting
  {
    typedef typename SplineType::KnotVectorType KnotVectorType;
    typedef typename SplineType::ParameterVectorType ParameterVectorType;
 
    template <typename PointArrayType>
    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
 
    template <typename PointArrayType>
    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
 
    template <typename PointArrayType, typename IndexArray>
    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
                                                 const PointArrayType& derivatives,
                                                 const IndexArray& derivativeIndices,
                                                 const unsigned int degree);
 
    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 (derivativeIndex < derivativeIndices.size() && 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(row++) = points.col(i);
      }
    }
    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