![]() |
Eigen-unsupported
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com> 00005 // 00006 // This Source Code Form is subject to the terms of the Mozilla 00007 // Public License v. 2.0. If a copy of the MPL was not distributed 00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00009 00010 #ifndef EIGEN_SPLINE_H 00011 #define EIGEN_SPLINE_H 00012 00013 #include "SplineFwd.h" 00014 00015 namespace Eigen 00016 { 00034 template <typename _Scalar, int _Dim, int _Degree> 00035 class Spline 00036 { 00037 public: 00038 typedef _Scalar Scalar; 00039 enum { Dimension = _Dim }; 00040 enum { Degree = _Degree }; 00041 00043 typedef typename SplineTraits<Spline>::PointType PointType; 00044 00046 typedef typename SplineTraits<Spline>::KnotVectorType KnotVectorType; 00047 00049 typedef typename SplineTraits<Spline>::ParameterVectorType ParameterVectorType; 00050 00052 typedef typename SplineTraits<Spline>::BasisVectorType BasisVectorType; 00053 00055 typedef typename SplineTraits<Spline>::BasisDerivativeType BasisDerivativeType; 00056 00058 typedef typename SplineTraits<Spline>::ControlPointVectorType ControlPointVectorType; 00059 00064 Spline() 00065 : m_knots(1, (Degree==Dynamic ? 2 : 2*Degree+2)) 00066 , m_ctrls(ControlPointVectorType::Zero(Dimension,(Degree==Dynamic ? 1 : Degree+1))) 00067 { 00068 // in theory this code can go to the initializer list but it will get pretty 00069 // much unreadable ... 00070 enum { MinDegree = (Degree==Dynamic ? 0 : Degree) }; 00071 m_knots.template segment<MinDegree+1>(0) = Array<Scalar,1,MinDegree+1>::Zero(); 00072 m_knots.template segment<MinDegree+1>(MinDegree+1) = Array<Scalar,1,MinDegree+1>::Ones(); 00073 } 00074 00080 template <typename OtherVectorType, typename OtherArrayType> 00081 Spline(const OtherVectorType& knots, const OtherArrayType& ctrls) : m_knots(knots), m_ctrls(ctrls) {} 00082 00087 template <int OtherDegree> 00088 Spline(const Spline<Scalar, Dimension, OtherDegree>& spline) : 00089 m_knots(spline.knots()), m_ctrls(spline.ctrls()) {} 00090 00094 const KnotVectorType& knots() const { return m_knots; } 00095 00099 const ControlPointVectorType& ctrls() const { return m_ctrls; } 00100 00112 PointType operator()(Scalar u) const; 00113 00126 typename SplineTraits<Spline>::DerivativeType 00127 derivatives(Scalar u, DenseIndex order) const; 00128 00134 template <int DerivativeOrder> 00135 typename SplineTraits<Spline,DerivativeOrder>::DerivativeType 00136 derivatives(Scalar u, DenseIndex order = DerivativeOrder) const; 00137 00154 typename SplineTraits<Spline>::BasisVectorType 00155 basisFunctions(Scalar u) const; 00156 00170 typename SplineTraits<Spline>::BasisDerivativeType 00171 basisFunctionDerivatives(Scalar u, DenseIndex order) const; 00172 00178 template <int DerivativeOrder> 00179 typename SplineTraits<Spline,DerivativeOrder>::BasisDerivativeType 00180 basisFunctionDerivatives(Scalar u, DenseIndex order = DerivativeOrder) const; 00181 00185 DenseIndex degree() const; 00186 00191 DenseIndex span(Scalar u) const; 00192 00196 static DenseIndex Span(typename SplineTraits<Spline>::Scalar u, DenseIndex degree, const typename SplineTraits<Spline>::KnotVectorType& knots); 00197 00210 static BasisVectorType BasisFunctions(Scalar u, DenseIndex degree, const KnotVectorType& knots); 00211 00217 static BasisDerivativeType BasisFunctionDerivatives( 00218 const Scalar u, const DenseIndex order, const DenseIndex degree, const KnotVectorType& knots); 00219 00220 private: 00221 KnotVectorType m_knots; 00222 ControlPointVectorType m_ctrls; 00224 template <typename DerivativeType> 00225 static void BasisFunctionDerivativesImpl( 00226 const typename Spline<_Scalar, _Dim, _Degree>::Scalar u, 00227 const DenseIndex order, 00228 const DenseIndex p, 00229 const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& U, 00230 DerivativeType& N_); 00231 }; 00232 00233 template <typename _Scalar, int _Dim, int _Degree> 00234 DenseIndex Spline<_Scalar, _Dim, _Degree>::Span( 00235 typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::Scalar u, 00236 DenseIndex degree, 00237 const typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::KnotVectorType& knots) 00238 { 00239 // Piegl & Tiller, "The NURBS Book", A2.1 (p. 68) 00240 if (u <= knots(0)) return degree; 00241 const Scalar* pos = std::upper_bound(knots.data()+degree-1, knots.data()+knots.size()-degree-1, u); 00242 return static_cast<DenseIndex>( std::distance(knots.data(), pos) - 1 ); 00243 } 00244 00245 template <typename _Scalar, int _Dim, int _Degree> 00246 typename Spline<_Scalar, _Dim, _Degree>::BasisVectorType 00247 Spline<_Scalar, _Dim, _Degree>::BasisFunctions( 00248 typename Spline<_Scalar, _Dim, _Degree>::Scalar u, 00249 DenseIndex degree, 00250 const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& knots) 00251 { 00252 typedef typename Spline<_Scalar, _Dim, _Degree>::BasisVectorType BasisVectorType; 00253 00254 const DenseIndex p = degree; 00255 const DenseIndex i = Spline::Span(u, degree, knots); 00256 00257 const KnotVectorType& U = knots; 00258 00259 BasisVectorType left(p+1); left(0) = Scalar(0); 00260 BasisVectorType right(p+1); right(0) = Scalar(0); 00261 00262 VectorBlock<BasisVectorType,Degree>(left,1,p) = u - VectorBlock<const KnotVectorType,Degree>(U,i+1-p,p).reverse(); 00263 VectorBlock<BasisVectorType,Degree>(right,1,p) = VectorBlock<const KnotVectorType,Degree>(U,i+1,p) - u; 00264 00265 BasisVectorType N(1,p+1); 00266 N(0) = Scalar(1); 00267 for (DenseIndex j=1; j<=p; ++j) 00268 { 00269 Scalar saved = Scalar(0); 00270 for (DenseIndex r=0; r<j; r++) 00271 { 00272 const Scalar tmp = N(r)/(right(r+1)+left(j-r)); 00273 N[r] = saved + right(r+1)*tmp; 00274 saved = left(j-r)*tmp; 00275 } 00276 N(j) = saved; 00277 } 00278 return N; 00279 } 00280 00281 template <typename _Scalar, int _Dim, int _Degree> 00282 DenseIndex Spline<_Scalar, _Dim, _Degree>::degree() const 00283 { 00284 if (_Degree == Dynamic) 00285 return m_knots.size() - m_ctrls.cols() - 1; 00286 else 00287 return _Degree; 00288 } 00289 00290 template <typename _Scalar, int _Dim, int _Degree> 00291 DenseIndex Spline<_Scalar, _Dim, _Degree>::span(Scalar u) const 00292 { 00293 return Spline::Span(u, degree(), knots()); 00294 } 00295 00296 template <typename _Scalar, int _Dim, int _Degree> 00297 typename Spline<_Scalar, _Dim, _Degree>::PointType Spline<_Scalar, _Dim, _Degree>::operator()(Scalar u) const 00298 { 00299 enum { Order = SplineTraits<Spline>::OrderAtCompileTime }; 00300 00301 const DenseIndex span = this->span(u); 00302 const DenseIndex p = degree(); 00303 const BasisVectorType basis_funcs = basisFunctions(u); 00304 00305 const Replicate<BasisVectorType,Dimension,1> ctrl_weights(basis_funcs); 00306 const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(ctrls(),0,span-p,Dimension,p+1); 00307 return (ctrl_weights * ctrl_pts).rowwise().sum(); 00308 } 00309 00310 /* --------------------------------------------------------------------------------------------- */ 00311 00312 template <typename SplineType, typename DerivativeType> 00313 void derivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& der) 00314 { 00315 enum { Dimension = SplineTraits<SplineType>::Dimension }; 00316 enum { Order = SplineTraits<SplineType>::OrderAtCompileTime }; 00317 enum { DerivativeOrder = DerivativeType::ColsAtCompileTime }; 00318 00319 typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType; 00320 typedef typename SplineTraits<SplineType,DerivativeOrder>::BasisDerivativeType BasisDerivativeType; 00321 typedef typename BasisDerivativeType::ConstRowXpr BasisDerivativeRowXpr; 00322 00323 const DenseIndex p = spline.degree(); 00324 const DenseIndex span = spline.span(u); 00325 00326 const DenseIndex n = (std::min)(p, order); 00327 00328 der.resize(Dimension,n+1); 00329 00330 // Retrieve the basis function derivatives up to the desired order... 00331 const BasisDerivativeType basis_func_ders = spline.template basisFunctionDerivatives<DerivativeOrder>(u, n+1); 00332 00333 // ... and perform the linear combinations of the control points. 00334 for (DenseIndex der_order=0; der_order<n+1; ++der_order) 00335 { 00336 const Replicate<BasisDerivativeRowXpr,Dimension,1> ctrl_weights( basis_func_ders.row(der_order) ); 00337 const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(spline.ctrls(),0,span-p,Dimension,p+1); 00338 der.col(der_order) = (ctrl_weights * ctrl_pts).rowwise().sum(); 00339 } 00340 } 00341 00342 template <typename _Scalar, int _Dim, int _Degree> 00343 typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::DerivativeType 00344 Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const 00345 { 00346 typename SplineTraits< Spline >::DerivativeType res; 00347 derivativesImpl(*this, u, order, res); 00348 return res; 00349 } 00350 00351 template <typename _Scalar, int _Dim, int _Degree> 00352 template <int DerivativeOrder> 00353 typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::DerivativeType 00354 Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const 00355 { 00356 typename SplineTraits< Spline, DerivativeOrder >::DerivativeType res; 00357 derivativesImpl(*this, u, order, res); 00358 return res; 00359 } 00360 00361 template <typename _Scalar, int _Dim, int _Degree> 00362 typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisVectorType 00363 Spline<_Scalar, _Dim, _Degree>::basisFunctions(Scalar u) const 00364 { 00365 return Spline::BasisFunctions(u, degree(), knots()); 00366 } 00367 00368 /* --------------------------------------------------------------------------------------------- */ 00369 00370 00371 template <typename _Scalar, int _Dim, int _Degree> 00372 template <typename DerivativeType> 00373 void Spline<_Scalar, _Dim, _Degree>::BasisFunctionDerivativesImpl( 00374 const typename Spline<_Scalar, _Dim, _Degree>::Scalar u, 00375 const DenseIndex order, 00376 const DenseIndex p, 00377 const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& U, 00378 DerivativeType& N_) 00379 { 00380 typedef Spline<_Scalar, _Dim, _Degree> SplineType; 00381 enum { Order = SplineTraits<SplineType>::OrderAtCompileTime }; 00382 00383 typedef typename SplineTraits<SplineType>::Scalar Scalar; 00384 typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType; 00385 00386 const DenseIndex span = SplineType::Span(u, p, U); 00387 00388 const DenseIndex n = (std::min)(p, order); 00389 00390 N_.resize(n+1, p+1); 00391 00392 BasisVectorType left = BasisVectorType::Zero(p+1); 00393 BasisVectorType right = BasisVectorType::Zero(p+1); 00394 00395 Matrix<Scalar,Order,Order> ndu(p+1,p+1); 00396 00397 Scalar saved, temp; // FIXME These were double instead of Scalar. Was there a reason for that? 00398 00399 ndu(0,0) = 1.0; 00400 00401 DenseIndex j; 00402 for (j=1; j<=p; ++j) 00403 { 00404 left[j] = u-U[span+1-j]; 00405 right[j] = U[span+j]-u; 00406 saved = 0.0; 00407 00408 for (DenseIndex r=0; r<j; ++r) 00409 { 00410 /* Lower triangle */ 00411 ndu(j,r) = right[r+1]+left[j-r]; 00412 temp = ndu(r,j-1)/ndu(j,r); 00413 /* Upper triangle */ 00414 ndu(r,j) = static_cast<Scalar>(saved+right[r+1] * temp); 00415 saved = left[j-r] * temp; 00416 } 00417 00418 ndu(j,j) = static_cast<Scalar>(saved); 00419 } 00420 00421 for (j = p; j>=0; --j) 00422 N_(0,j) = ndu(j,p); 00423 00424 // Compute the derivatives 00425 DerivativeType a(n+1,p+1); 00426 DenseIndex r=0; 00427 for (; r<=p; ++r) 00428 { 00429 DenseIndex s1,s2; 00430 s1 = 0; s2 = 1; // alternate rows in array a 00431 a(0,0) = 1.0; 00432 00433 // Compute the k-th derivative 00434 for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k) 00435 { 00436 Scalar d = 0.0; 00437 DenseIndex rk,pk,j1,j2; 00438 rk = r-k; pk = p-k; 00439 00440 if (r>=k) 00441 { 00442 a(s2,0) = a(s1,0)/ndu(pk+1,rk); 00443 d = a(s2,0)*ndu(rk,pk); 00444 } 00445 00446 if (rk>=-1) j1 = 1; 00447 else j1 = -rk; 00448 00449 if (r-1 <= pk) j2 = k-1; 00450 else j2 = p-r; 00451 00452 for (j=j1; j<=j2; ++j) 00453 { 00454 a(s2,j) = (a(s1,j)-a(s1,j-1))/ndu(pk+1,rk+j); 00455 d += a(s2,j)*ndu(rk+j,pk); 00456 } 00457 00458 if (r<=pk) 00459 { 00460 a(s2,k) = -a(s1,k-1)/ndu(pk+1,r); 00461 d += a(s2,k)*ndu(r,pk); 00462 } 00463 00464 N_(k,r) = static_cast<Scalar>(d); 00465 j = s1; s1 = s2; s2 = j; // Switch rows 00466 } 00467 } 00468 00469 /* Multiply through by the correct factors */ 00470 /* (Eq. [2.9]) */ 00471 r = p; 00472 for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k) 00473 { 00474 for (j=p; j>=0; --j) N_(k,j) *= r; 00475 r *= p-k; 00476 } 00477 } 00478 00479 template <typename _Scalar, int _Dim, int _Degree> 00480 typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType 00481 Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const 00482 { 00483 typename SplineTraits<Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType der; 00484 BasisFunctionDerivativesImpl(u, order, degree(), knots(), der); 00485 return der; 00486 } 00487 00488 template <typename _Scalar, int _Dim, int _Degree> 00489 template <int DerivativeOrder> 00490 typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType 00491 Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const 00492 { 00493 typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType der; 00494 BasisFunctionDerivativesImpl(u, order, degree(), knots(), der); 00495 return der; 00496 } 00497 00498 template <typename _Scalar, int _Dim, int _Degree> 00499 typename SplineTraits<Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType 00500 Spline<_Scalar, _Dim, _Degree>::BasisFunctionDerivatives( 00501 const typename Spline<_Scalar, _Dim, _Degree>::Scalar u, 00502 const DenseIndex order, 00503 const DenseIndex degree, 00504 const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& knots) 00505 { 00506 typename SplineTraits<Spline>::BasisDerivativeType der; 00507 BasisFunctionDerivativesImpl(u, order, degree, knots, der); 00508 return der; 00509 } 00510 } 00511 00512 #endif // EIGEN_SPLINE_H