Squashed 'third_party/eigen/' changes from 61d72f6..cf794d3
Change-Id: I9b814151b01f49af6337a8605d0c42a3a1ed4c72
git-subtree-dir: third_party/eigen
git-subtree-split: cf794d3b741a6278df169e58461f8529f43bce5d
diff --git a/unsupported/Eigen/src/Splines/SplineFitting.h b/unsupported/Eigen/src/Splines/SplineFitting.h
index 0265d53..c761a9b 100644
--- a/unsupported/Eigen/src/Splines/SplineFitting.h
+++ b/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 @@
}
/**
+ * \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 @@
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 @@
**/
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 @@
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