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 23 namespace 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> KnotAveraging(const KnotVectorType & parameters,DenseIndex degree,KnotVectorType & knots)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> KnotAveragingWithDerivatives(const ParameterVectorType & parameters,const unsigned int degree,const IndexArray & derivativeIndices,KnotVectorType & knots)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> ChordLengths(const PointArrayType & pts,KnotVectorType & chord_lengths)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> Interpolate(const PointArrayType & pts,DenseIndex degree,const KnotVectorType & knot_parameters)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> Interpolate(const PointArrayType & pts,DenseIndex degree)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 InterpolateWithDerivatives(const PointArrayType & points,const PointArrayType & derivatives,const IndexArray & derivativeIndices,const unsigned int degree,const ParameterVectorType & parameters)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 InterpolateWithDerivatives(const PointArrayType & points,const PointArrayType & derivatives,const IndexArray & derivativeIndices,const unsigned int degree)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