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 <algorithm>
14#include <functional>
15#include <numeric>
16#include <vector>
17
18#include "SplineFwd.h"
19
20#include <Eigen/LU>
21#include <Eigen/QR>
22
23namespace Eigen
24{
25  /**
26   * \brief Computes knot averages.
27   * \ingroup Splines_Module
28   *
29   * The knots are computed as
30   * \f{align*}
31   *  u_0 & = \hdots = u_p = 0 \\
32   *  u_{m-p} & = \hdots = u_{m} = 1 \\
33   *  u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p
34   * \f}
35   * where \f$p\f$ is the degree and \f$m+1\f$ the number knots
36   * of the desired interpolating spline.
37   *
38   * \param[in] parameters The input parameters. During interpolation one for each data point.
39   * \param[in] degree The spline degree which is used during the interpolation.
40   * \param[out] knots The output knot vector.
41   *
42   * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
43   **/
44  template <typename KnotVectorType>
45  void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
46  {
47    knots.resize(parameters.size()+degree+1);
48
49    for (DenseIndex j=1; j<parameters.size()-degree; ++j)
50      knots(j+degree) = parameters.segment(j,degree).mean();
51
52    knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
53    knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
54  }
55
56  /**
57   * \brief Computes knot averages when derivative constraints are present.
58   * Note that this is a technical interpretation of the referenced article
59   * since the algorithm contained therein is incorrect as written.
60   * \ingroup Splines_Module
61   *
62   * \param[in] parameters The parameters at which the interpolation B-Spline
63   *            will intersect the given interpolation points. The parameters
64   *            are assumed to be a non-decreasing sequence.
65   * \param[in] degree The degree of the interpolating B-Spline. This must be
66   *            greater than zero.
67   * \param[in] derivativeIndices The indices corresponding to parameters at
68   *            which there are derivative constraints. The indices are assumed
69   *            to be a non-decreasing sequence.
70   * \param[out] knots The calculated knot vector. These will be returned as a
71   *             non-decreasing sequence
72   *
73   * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
74   * Curve interpolation with directional constraints for engineering design.
75   * Engineering with Computers
76   **/
77  template <typename KnotVectorType, typename ParameterVectorType, typename IndexArray>
78  void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
79                                    const unsigned int degree,
80                                    const IndexArray& derivativeIndices,
81                                    KnotVectorType& knots)
82  {
83    typedef typename ParameterVectorType::Scalar Scalar;
84
85    DenseIndex numParameters = parameters.size();
86    DenseIndex numDerivatives = derivativeIndices.size();
87
88    if (numDerivatives < 1)
89    {
90      KnotAveraging(parameters, degree, knots);
91      return;
92    }
93
94    DenseIndex startIndex;
95    DenseIndex endIndex;
96
97    DenseIndex numInternalDerivatives = numDerivatives;
98
99    if (derivativeIndices[0] == 0)
100    {
101      startIndex = 0;
102      --numInternalDerivatives;
103    }
104    else
105    {
106      startIndex = 1;
107    }
108    if (derivativeIndices[numDerivatives - 1] == numParameters - 1)
109    {
110      endIndex = numParameters - degree;
111      --numInternalDerivatives;
112    }
113    else
114    {
115      endIndex = numParameters - degree - 1;
116    }
117
118    // There are (endIndex - startIndex + 1) knots obtained from the averaging
119    // and 2 for the first and last parameters.
120    DenseIndex numAverageKnots = endIndex - startIndex + 3;
121    KnotVectorType averageKnots(numAverageKnots);
122    averageKnots[0] = parameters[0];
123
124    int newKnotIndex = 0;
125    for (DenseIndex i = startIndex; i <= endIndex; ++i)
126      averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
127    averageKnots[++newKnotIndex] = parameters[numParameters - 1];
128
129    newKnotIndex = -1;
130
131    ParameterVectorType temporaryParameters(numParameters + 1);
132    KnotVectorType derivativeKnots(numInternalDerivatives);
133    for (DenseIndex i = 0; i < numAverageKnots - 1; ++i)
134    {
135      temporaryParameters[0] = averageKnots[i];
136      ParameterVectorType parameterIndices(numParameters);
137      int temporaryParameterIndex = 1;
138      for (DenseIndex j = 0; j < numParameters; ++j)
139      {
140        Scalar parameter = parameters[j];
141        if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1])
142        {
143          parameterIndices[temporaryParameterIndex] = j;
144          temporaryParameters[temporaryParameterIndex++] = parameter;
145        }
146      }
147      temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1];
148
149      for (int j = 0; j <= temporaryParameterIndex - 2; ++j)
150      {
151        for (DenseIndex k = 0; k < derivativeIndices.size(); ++k)
152        {
153          if (parameterIndices[j + 1] == derivativeIndices[k]
154              && parameterIndices[j + 1] != 0
155              && parameterIndices[j + 1] != numParameters - 1)
156          {
157            derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean();
158            break;
159          }
160        }
161      }
162    }
163
164    KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size());
165
166    std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(),
167               derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(),
168               temporaryKnots.data());
169
170    // Number of knots (one for each point and derivative) plus spline order.
171    DenseIndex numKnots = numParameters + numDerivatives + degree + 1;
172    knots.resize(numKnots);
173
174    knots.head(degree).fill(temporaryKnots[0]);
175    knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]);
176    knots.segment(degree, temporaryKnots.size()) = temporaryKnots;
177  }
178
179  /**
180   * \brief Computes chord length parameters which are required for spline interpolation.
181   * \ingroup Splines_Module
182   *
183   * \param[in] pts The data points to which a spline should be fit.
184   * \param[out] chord_lengths The resulting chord lenggth vector.
185   *
186   * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
187   **/
188  template <typename PointArrayType, typename KnotVectorType>
189  void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
190  {
191    typedef typename KnotVectorType::Scalar Scalar;
192
193    const DenseIndex n = pts.cols();
194
195    // 1. compute the column-wise norms
196    chord_lengths.resize(pts.cols());
197    chord_lengths[0] = 0;
198    chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
199
200    // 2. compute the partial sums
201    std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
202
203    // 3. normalize the data
204    chord_lengths /= chord_lengths(n-1);
205    chord_lengths(n-1) = Scalar(1);
206  }
207
208  /**
209   * \brief Spline fitting methods.
210   * \ingroup Splines_Module
211   **/
212  template <typename SplineType>
213  struct SplineFitting
214  {
215    typedef typename SplineType::KnotVectorType KnotVectorType;
216    typedef typename SplineType::ParameterVectorType ParameterVectorType;
217
218    /**
219     * \brief Fits an interpolating Spline to the given data points.
220     *
221     * \param pts The points for which an interpolating spline will be computed.
222     * \param degree The degree of the interpolating spline.
223     *
224     * \returns A spline interpolating the initially provided points.
225     **/
226    template <typename PointArrayType>
227    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
228
229    /**
230     * \brief Fits an interpolating Spline to the given data points.
231     *
232     * \param pts The points for which an interpolating spline will be computed.
233     * \param degree The degree of the interpolating spline.
234     * \param knot_parameters The knot parameters for the interpolation.
235     *
236     * \returns A spline interpolating the initially provided points.
237     **/
238    template <typename PointArrayType>
239    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
240
241    /**
242     * \brief Fits an interpolating spline to the given data points and
243     * derivatives.
244     *
245     * \param points The points for which an interpolating spline will be computed.
246     * \param derivatives The desired derivatives of the interpolating spline at interpolation
247     *                    points.
248     * \param derivativeIndices An array indicating which point each derivative belongs to. This
249     *                          must be the same size as @a derivatives.
250     * \param degree The degree of the interpolating spline.
251     *
252     * \returns A spline interpolating @a points with @a derivatives at those points.
253     *
254     * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
255     * Curve interpolation with directional constraints for engineering design.
256     * Engineering with Computers
257     **/
258    template <typename PointArrayType, typename IndexArray>
259    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
260                                                 const PointArrayType& derivatives,
261                                                 const IndexArray& derivativeIndices,
262                                                 const unsigned int degree);
263
264    /**
265     * \brief Fits an interpolating spline to the given data points and derivatives.
266     *
267     * \param points The points for which an interpolating spline will be computed.
268     * \param derivatives The desired derivatives of the interpolating spline at interpolation points.
269     * \param derivativeIndices An array indicating which point each derivative belongs to. This
270     *                          must be the same size as @a derivatives.
271     * \param degree The degree of the interpolating spline.
272     * \param parameters The parameters corresponding to the interpolation points.
273     *
274     * \returns A spline interpolating @a points with @a derivatives at those points.
275     *
276     * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
277     * Curve interpolation with directional constraints for engineering design.
278     * Engineering with Computers
279     */
280    template <typename PointArrayType, typename IndexArray>
281    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
282                                                 const PointArrayType& derivatives,
283                                                 const IndexArray& derivativeIndices,
284                                                 const unsigned int degree,
285                                                 const ParameterVectorType& parameters);
286  };
287
288  template <typename SplineType>
289  template <typename PointArrayType>
290  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
291  {
292    typedef typename SplineType::KnotVectorType::Scalar Scalar;
293    typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
294
295    typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
296
297    KnotVectorType knots;
298    KnotAveraging(knot_parameters, degree, knots);
299
300    DenseIndex n = pts.cols();
301    MatrixType A = MatrixType::Zero(n,n);
302    for (DenseIndex i=1; i<n-1; ++i)
303    {
304      const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
305
306      // The segment call should somehow be told the spline order at compile time.
307      A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
308    }
309    A(0,0) = 1.0;
310    A(n-1,n-1) = 1.0;
311
312    HouseholderQR<MatrixType> qr(A);
313
314    // Here, we are creating a temporary due to an Eigen issue.
315    ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
316
317    return SplineType(knots, ctrls);
318  }
319
320  template <typename SplineType>
321  template <typename PointArrayType>
322  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
323  {
324    KnotVectorType chord_lengths; // knot parameters
325    ChordLengths(pts, chord_lengths);
326    return Interpolate(pts, degree, chord_lengths);
327  }
328
329  template <typename SplineType>
330  template <typename PointArrayType, typename IndexArray>
331  SplineType
332  SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
333                                                        const PointArrayType& derivatives,
334                                                        const IndexArray& derivativeIndices,
335                                                        const unsigned int degree,
336                                                        const ParameterVectorType& parameters)
337  {
338    typedef typename SplineType::KnotVectorType::Scalar Scalar;
339    typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
340
341    typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType;
342
343    const DenseIndex n = points.cols() + derivatives.cols();
344
345    KnotVectorType knots;
346
347    KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots);
348
349    // fill matrix
350    MatrixType A = MatrixType::Zero(n, n);
351
352    // Use these dimensions for quicker populating, then transpose for solving.
353    MatrixType b(points.rows(), n);
354
355    DenseIndex startRow;
356    DenseIndex derivativeStart;
357
358    // End derivatives.
359    if (derivativeIndices[0] == 0)
360    {
361      A.template block<1, 2>(1, 0) << -1, 1;
362
363      Scalar y = (knots(degree + 1) - knots(0)) / degree;
364      b.col(1) = y*derivatives.col(0);
365
366      startRow = 2;
367      derivativeStart = 1;
368    }
369    else
370    {
371      startRow = 1;
372      derivativeStart = 0;
373    }
374    if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1)
375    {
376      A.template block<1, 2>(n - 2, n - 2) << -1, 1;
377
378      Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree;
379      b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1);
380    }
381
382    DenseIndex row = startRow;
383    DenseIndex derivativeIndex = derivativeStart;
384    for (DenseIndex i = 1; i < parameters.size() - 1; ++i)
385    {
386      const DenseIndex span = SplineType::Span(parameters[i], degree, knots);
387
388      if (derivativeIndices[derivativeIndex] == i)
389      {
390        A.block(row, span - degree, 2, degree + 1)
391          = SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots);
392
393        b.col(row++) = points.col(i);
394        b.col(row++) = derivatives.col(derivativeIndex++);
395      }
396      else
397      {
398        A.row(row++).segment(span - degree, degree + 1)
399          = SplineType::BasisFunctions(parameters[i], degree, knots);
400      }
401    }
402    b.col(0) = points.col(0);
403    b.col(b.cols() - 1) = points.col(points.cols() - 1);
404    A(0,0) = 1;
405    A(n - 1, n - 1) = 1;
406
407    // Solve
408    FullPivLU<MatrixType> lu(A);
409    ControlPointVectorType controlPoints = lu.solve(MatrixType(b.transpose())).transpose();
410
411    SplineType spline(knots, controlPoints);
412
413    return spline;
414  }
415
416  template <typename SplineType>
417  template <typename PointArrayType, typename IndexArray>
418  SplineType
419  SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
420                                                        const PointArrayType& derivatives,
421                                                        const IndexArray& derivativeIndices,
422                                                        const unsigned int degree)
423  {
424    ParameterVectorType parameters;
425    ChordLengths(points, parameters);
426    return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
427  }
428}
429
430#endif // EIGEN_SPLINE_FITTING_H
431