Spline.h
Go to the documentation of this file.
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>::BasisVectorType BasisVectorType;
00050     
00052     typedef typename SplineTraits<Spline>::ControlPointVectorType ControlPointVectorType;
00053     
00058     Spline() 
00059     : m_knots(1, (Degree==Dynamic ? 2 : 2*Degree+2))
00060     , m_ctrls(ControlPointVectorType::Zero(2,(Degree==Dynamic ? 1 : Degree+1))) 
00061     {
00062       // in theory this code can go to the initializer list but it will get pretty
00063       // much unreadable ...
00064       enum { MinDegree = (Degree==Dynamic ? 0 : Degree) };
00065       m_knots.template segment<MinDegree+1>(0) = Array<Scalar,1,MinDegree+1>::Zero();
00066       m_knots.template segment<MinDegree+1>(MinDegree+1) = Array<Scalar,1,MinDegree+1>::Ones();
00067     }
00068 
00074     template <typename OtherVectorType, typename OtherArrayType>
00075     Spline(const OtherVectorType& knots, const OtherArrayType& ctrls) : m_knots(knots), m_ctrls(ctrls) {}
00076 
00081     template <int OtherDegree>
00082     Spline(const Spline<Scalar, Dimension, OtherDegree>& spline) : 
00083     m_knots(spline.knots()), m_ctrls(spline.ctrls()) {}
00084 
00088     const KnotVectorType& knots() const { return m_knots; }
00089     
00093     const ControlPointVectorType& ctrls() const { return m_ctrls; }
00094 
00106     PointType operator()(Scalar u) const;
00107 
00120     typename SplineTraits<Spline>::DerivativeType
00121       derivatives(Scalar u, DenseIndex order) const;
00122 
00128     template <int DerivativeOrder>
00129     typename SplineTraits<Spline,DerivativeOrder>::DerivativeType
00130       derivatives(Scalar u, DenseIndex order = DerivativeOrder) const;
00131 
00148     typename SplineTraits<Spline>::BasisVectorType
00149       basisFunctions(Scalar u) const;
00150 
00164     typename SplineTraits<Spline>::BasisDerivativeType
00165       basisFunctionDerivatives(Scalar u, DenseIndex order) const;
00166 
00172     template <int DerivativeOrder>
00173     typename SplineTraits<Spline,DerivativeOrder>::BasisDerivativeType
00174       basisFunctionDerivatives(Scalar u, DenseIndex order = DerivativeOrder) const;
00175 
00179     DenseIndex degree() const;
00180 
00185     DenseIndex span(Scalar u) const;
00186 
00190     static DenseIndex Span(typename SplineTraits<Spline>::Scalar u, DenseIndex degree, const typename SplineTraits<Spline>::KnotVectorType& knots);
00191     
00204     static BasisVectorType BasisFunctions(Scalar u, DenseIndex degree, const KnotVectorType& knots);
00205 
00206 
00207   private:
00208     KnotVectorType m_knots; 
00209     ControlPointVectorType  m_ctrls; 
00210   };
00211 
00212   template <typename _Scalar, int _Dim, int _Degree>
00213   DenseIndex Spline<_Scalar, _Dim, _Degree>::Span(
00214     typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::Scalar u,
00215     DenseIndex degree,
00216     const typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::KnotVectorType& knots)
00217   {
00218     // Piegl & Tiller, "The NURBS Book", A2.1 (p. 68)
00219     if (u <= knots(0)) return degree;
00220     const Scalar* pos = std::upper_bound(knots.data()+degree-1, knots.data()+knots.size()-degree-1, u);
00221     return static_cast<DenseIndex>( std::distance(knots.data(), pos) - 1 );
00222   }
00223 
00224   template <typename _Scalar, int _Dim, int _Degree>
00225   typename Spline<_Scalar, _Dim, _Degree>::BasisVectorType
00226     Spline<_Scalar, _Dim, _Degree>::BasisFunctions(
00227     typename Spline<_Scalar, _Dim, _Degree>::Scalar u,
00228     DenseIndex degree,
00229     const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& knots)
00230   {
00231     typedef typename Spline<_Scalar, _Dim, _Degree>::BasisVectorType BasisVectorType;
00232 
00233     const DenseIndex p = degree;
00234     const DenseIndex i = Spline::Span(u, degree, knots);
00235 
00236     const KnotVectorType& U = knots;
00237 
00238     BasisVectorType left(p+1); left(0) = Scalar(0);
00239     BasisVectorType right(p+1); right(0) = Scalar(0);        
00240 
00241     VectorBlock<BasisVectorType,Degree>(left,1,p) = u - VectorBlock<const KnotVectorType,Degree>(U,i+1-p,p).reverse();
00242     VectorBlock<BasisVectorType,Degree>(right,1,p) = VectorBlock<const KnotVectorType,Degree>(U,i+1,p) - u;
00243 
00244     BasisVectorType N(1,p+1);
00245     N(0) = Scalar(1);
00246     for (DenseIndex j=1; j<=p; ++j)
00247     {
00248       Scalar saved = Scalar(0);
00249       for (DenseIndex r=0; r<j; r++)
00250       {
00251         const Scalar tmp = N(r)/(right(r+1)+left(j-r));
00252         N[r] = saved + right(r+1)*tmp;
00253         saved = left(j-r)*tmp;
00254       }
00255       N(j) = saved;
00256     }
00257     return N;
00258   }
00259 
00260   template <typename _Scalar, int _Dim, int _Degree>
00261   DenseIndex Spline<_Scalar, _Dim, _Degree>::degree() const
00262   {
00263     if (_Degree == Dynamic)
00264       return m_knots.size() - m_ctrls.cols() - 1;
00265     else
00266       return _Degree;
00267   }
00268 
00269   template <typename _Scalar, int _Dim, int _Degree>
00270   DenseIndex Spline<_Scalar, _Dim, _Degree>::span(Scalar u) const
00271   {
00272     return Spline::Span(u, degree(), knots());
00273   }
00274 
00275   template <typename _Scalar, int _Dim, int _Degree>
00276   typename Spline<_Scalar, _Dim, _Degree>::PointType Spline<_Scalar, _Dim, _Degree>::operator()(Scalar u) const
00277   {
00278     enum { Order = SplineTraits<Spline>::OrderAtCompileTime };
00279 
00280     const DenseIndex span = this->span(u);
00281     const DenseIndex p = degree();
00282     const BasisVectorType basis_funcs = basisFunctions(u);
00283 
00284     const Replicate<BasisVectorType,Dimension,1> ctrl_weights(basis_funcs);
00285     const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(ctrls(),0,span-p,Dimension,p+1);
00286     return (ctrl_weights * ctrl_pts).rowwise().sum();
00287   }
00288 
00289   /* --------------------------------------------------------------------------------------------- */
00290 
00291   template <typename SplineType, typename DerivativeType>
00292   void derivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& der)
00293   {    
00294     enum { Dimension = SplineTraits<SplineType>::Dimension };
00295     enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
00296     enum { DerivativeOrder = DerivativeType::ColsAtCompileTime };
00297 
00298     typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType;
00299     typedef typename SplineTraits<SplineType,DerivativeOrder>::BasisDerivativeType BasisDerivativeType;
00300     typedef typename BasisDerivativeType::ConstRowXpr BasisDerivativeRowXpr;    
00301 
00302     const DenseIndex p = spline.degree();
00303     const DenseIndex span = spline.span(u);
00304 
00305     const DenseIndex n = (std::min)(p, order);
00306 
00307     der.resize(Dimension,n+1);
00308 
00309     // Retrieve the basis function derivatives up to the desired order...    
00310     const BasisDerivativeType basis_func_ders = spline.template basisFunctionDerivatives<DerivativeOrder>(u, n+1);
00311 
00312     // ... and perform the linear combinations of the control points.
00313     for (DenseIndex der_order=0; der_order<n+1; ++der_order)
00314     {
00315       const Replicate<BasisDerivativeRowXpr,Dimension,1> ctrl_weights( basis_func_ders.row(der_order) );
00316       const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(spline.ctrls(),0,span-p,Dimension,p+1);
00317       der.col(der_order) = (ctrl_weights * ctrl_pts).rowwise().sum();
00318     }
00319   }
00320 
00321   template <typename _Scalar, int _Dim, int _Degree>
00322   typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::DerivativeType
00323     Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const
00324   {
00325     typename SplineTraits< Spline >::DerivativeType res;
00326     derivativesImpl(*this, u, order, res);
00327     return res;
00328   }
00329 
00330   template <typename _Scalar, int _Dim, int _Degree>
00331   template <int DerivativeOrder>
00332   typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::DerivativeType
00333     Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const
00334   {
00335     typename SplineTraits< Spline, DerivativeOrder >::DerivativeType res;
00336     derivativesImpl(*this, u, order, res);
00337     return res;
00338   }
00339 
00340   template <typename _Scalar, int _Dim, int _Degree>
00341   typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisVectorType
00342     Spline<_Scalar, _Dim, _Degree>::basisFunctions(Scalar u) const
00343   {
00344     return Spline::BasisFunctions(u, degree(), knots());
00345   }
00346 
00347   /* --------------------------------------------------------------------------------------------- */
00348 
00349   template <typename SplineType, typename DerivativeType>
00350   void basisFunctionDerivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& N_)
00351   {
00352     enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
00353 
00354     typedef typename SplineTraits<SplineType>::Scalar Scalar;
00355     typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
00356     typedef typename SplineTraits<SplineType>::KnotVectorType KnotVectorType;
00357 
00358     const KnotVectorType& U = spline.knots();
00359 
00360     const DenseIndex p = spline.degree();
00361     const DenseIndex span = spline.span(u);
00362 
00363     const DenseIndex n = (std::min)(p, order);
00364 
00365     N_.resize(n+1, p+1);
00366 
00367     BasisVectorType left = BasisVectorType::Zero(p+1);
00368     BasisVectorType right = BasisVectorType::Zero(p+1);
00369 
00370     Matrix<Scalar,Order,Order> ndu(p+1,p+1);
00371 
00372     double saved, temp;
00373 
00374     ndu(0,0) = 1.0;
00375 
00376     DenseIndex j;
00377     for (j=1; j<=p; ++j)
00378     {
00379       left[j] = u-U[span+1-j];
00380       right[j] = U[span+j]-u;
00381       saved = 0.0;
00382 
00383       for (DenseIndex r=0; r<j; ++r)
00384       {
00385         /* Lower triangle */
00386         ndu(j,r) = right[r+1]+left[j-r];
00387         temp = ndu(r,j-1)/ndu(j,r);
00388         /* Upper triangle */
00389         ndu(r,j) = static_cast<Scalar>(saved+right[r+1] * temp);
00390         saved = left[j-r] * temp;
00391       }
00392 
00393       ndu(j,j) = static_cast<Scalar>(saved);
00394     }
00395 
00396     for (j = p; j>=0; --j) 
00397       N_(0,j) = ndu(j,p);
00398 
00399     // Compute the derivatives
00400     DerivativeType a(n+1,p+1);
00401     DenseIndex r=0;
00402     for (; r<=p; ++r)
00403     {
00404       DenseIndex s1,s2;
00405       s1 = 0; s2 = 1; // alternate rows in array a
00406       a(0,0) = 1.0;
00407 
00408       // Compute the k-th derivative
00409       for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k)
00410       {
00411         double d = 0.0;
00412         DenseIndex rk,pk,j1,j2;
00413         rk = r-k; pk = p-k;
00414 
00415         if (r>=k)
00416         {
00417           a(s2,0) = a(s1,0)/ndu(pk+1,rk);
00418           d = a(s2,0)*ndu(rk,pk);
00419         }
00420 
00421         if (rk>=-1) j1 = 1;
00422         else        j1 = -rk;
00423 
00424         if (r-1 <= pk) j2 = k-1;
00425         else           j2 = p-r;
00426 
00427         for (j=j1; j<=j2; ++j)
00428         {
00429           a(s2,j) = (a(s1,j)-a(s1,j-1))/ndu(pk+1,rk+j);
00430           d += a(s2,j)*ndu(rk+j,pk);
00431         }
00432 
00433         if (r<=pk)
00434         {
00435           a(s2,k) = -a(s1,k-1)/ndu(pk+1,r);
00436           d += a(s2,k)*ndu(r,pk);
00437         }
00438 
00439         N_(k,r) = static_cast<Scalar>(d);
00440         j = s1; s1 = s2; s2 = j; // Switch rows
00441       }
00442     }
00443 
00444     /* Multiply through by the correct factors */
00445     /* (Eq. [2.9])                             */
00446     r = p;
00447     for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k)
00448     {
00449       for (DenseIndex j=p; j>=0; --j) N_(k,j) *= r;
00450       r *= p-k;
00451     }
00452   }
00453 
00454   template <typename _Scalar, int _Dim, int _Degree>
00455   typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
00456     Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
00457   {
00458     typename SplineTraits< Spline >::BasisDerivativeType der;
00459     basisFunctionDerivativesImpl(*this, u, order, der);
00460     return der;
00461   }
00462 
00463   template <typename _Scalar, int _Dim, int _Degree>
00464   template <int DerivativeOrder>
00465   typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType
00466     Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
00467   {
00468     typename SplineTraits< Spline, DerivativeOrder >::BasisDerivativeType der;
00469     basisFunctionDerivativesImpl(*this, u, order, der);
00470     return der;
00471   }
00472 }
00473 
00474 #endif // EIGEN_SPLINE_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:39:43