1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#ifndef EIGEN_SPLINE_FITTING_H
11#define EIGEN_SPLINE_FITTING_H
12
13#include <numeric>
14
15#include "SplineFwd.h"
16
17#include <Eigen/QR>
18
19namespace Eigen
20{
21  /**
22   * \brief Computes knot averages.
23   * \ingroup Splines_Module
24   *
25   * The knots are computed as
26   * \f{align*}
27   *  u_0 & = \hdots = u_p = 0 \\
28   *  u_{m-p} & = \hdots = u_{m} = 1 \\
29   *  u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p
30   * \f}
31   * where \f$p\f$ is the degree and \f$m+1\f$ the number knots
32   * of the desired interpolating spline.
33   *
34   * \param[in] parameters The input parameters. During interpolation one for each data point.
35   * \param[in] degree The spline degree which is used during the interpolation.
36   * \param[out] knots The output knot vector.
37   *
38   * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
39   **/
40  template <typename KnotVectorType>
41  void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
42  {
43    typedef typename KnotVectorType::Scalar Scalar;
44
45    knots.resize(parameters.size()+degree+1);
46
47    for (DenseIndex j=1; j<parameters.size()-degree; ++j)
48      knots(j+degree) = parameters.segment(j,degree).mean();
49
50    knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
51    knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
52  }
53
54  /**
55   * \brief Computes chord length parameters which are required for spline interpolation.
56   * \ingroup Splines_Module
57   *
58   * \param[in] pts The data points to which a spline should be fit.
59   * \param[out] chord_lengths The resulting chord lenggth vector.
60   *
61   * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
62   **/
63  template <typename PointArrayType, typename KnotVectorType>
64  void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
65  {
66    typedef typename KnotVectorType::Scalar Scalar;
67
68    const DenseIndex n = pts.cols();
69
70    // 1. compute the column-wise norms
71    chord_lengths.resize(pts.cols());
72    chord_lengths[0] = 0;
73    chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
74
75    // 2. compute the partial sums
76    std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
77
78    // 3. normalize the data
79    chord_lengths /= chord_lengths(n-1);
80    chord_lengths(n-1) = Scalar(1);
81  }
82
83  /**
84   * \brief Spline fitting methods.
85   * \ingroup Splines_Module
86   **/
87  template <typename SplineType>
88  struct SplineFitting
89  {
90    typedef typename SplineType::KnotVectorType KnotVectorType;
91
92    /**
93     * \brief Fits an interpolating Spline to the given data points.
94     *
95     * \param pts The points for which an interpolating spline will be computed.
96     * \param degree The degree of the interpolating spline.
97     *
98     * \returns A spline interpolating the initially provided points.
99     **/
100    template <typename PointArrayType>
101    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
102
103    /**
104     * \brief Fits an interpolating Spline to the given data points.
105     *
106     * \param pts The points for which an interpolating spline will be computed.
107     * \param degree The degree of the interpolating spline.
108     * \param knot_parameters The knot parameters for the interpolation.
109     *
110     * \returns A spline interpolating the initially provided points.
111     **/
112    template <typename PointArrayType>
113    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
114  };
115
116  template <typename SplineType>
117  template <typename PointArrayType>
118  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
119  {
120    typedef typename SplineType::KnotVectorType::Scalar Scalar;
121    typedef typename SplineType::BasisVectorType BasisVectorType;
122    typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
123
124    typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
125
126    KnotVectorType knots;
127    KnotAveraging(knot_parameters, degree, knots);
128
129    DenseIndex n = pts.cols();
130    MatrixType A = MatrixType::Zero(n,n);
131    for (DenseIndex i=1; i<n-1; ++i)
132    {
133      const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
134
135      // The segment call should somehow be told the spline order at compile time.
136      A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
137    }
138    A(0,0) = 1.0;
139    A(n-1,n-1) = 1.0;
140
141    HouseholderQR<MatrixType> qr(A);
142
143    // Here, we are creating a temporary due to an Eigen issue.
144    ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
145
146    return SplineType(knots, ctrls);
147  }
148
149  template <typename SplineType>
150  template <typename PointArrayType>
151  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
152  {
153    KnotVectorType chord_lengths; // knot parameters
154    ChordLengths(pts, chord_lengths);
155    return Interpolate(pts, degree, chord_lengths);
156  }
157}
158
159#endif // EIGEN_SPLINE_FITTING_H
160