Spline.h
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 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #ifndef EIGEN_SPLINE_H
00026 #define EIGEN_SPLINE_H
00027 
00028 #include "SplineFwd.h"
00029 
00030 namespace Eigen
00031 {
00049   template <typename _Scalar, int _Dim, int _Degree>
00050   class Spline
00051   {
00052   public:
00053     typedef _Scalar Scalar; 
00054     enum { Dimension = _Dim  };
00055     enum { Degree = _Degree  };
00056 
00058     typedef typename SplineTraits<Spline>::PointType PointType;
00059     
00061     typedef typename SplineTraits<Spline>::KnotVectorType KnotVectorType;
00062     
00064     typedef typename SplineTraits<Spline>::BasisVectorType BasisVectorType;
00065     
00067     typedef typename SplineTraits<Spline>::ControlPointVectorType ControlPointVectorType;
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>::Scalar Scalar;
00299 
00300     typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
00301     typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType;
00302 
00303     typedef typename SplineTraits<SplineType,DerivativeOrder>::BasisDerivativeType BasisDerivativeType;
00304     typedef typename BasisDerivativeType::ConstRowXpr BasisDerivativeRowXpr;    
00305 
00306     const DenseIndex p = spline.degree();
00307     const DenseIndex span = spline.span(u);
00308 
00309     const DenseIndex n = (std::min)(p, order);
00310 
00311     der.resize(Dimension,n+1);
00312 
00313     // Retrieve the basis function derivatives up to the desired order...    
00314     const BasisDerivativeType basis_func_ders = spline.template basisFunctionDerivatives<DerivativeOrder>(u, n+1);
00315 
00316     // ... and perform the linear combinations of the control points.
00317     for (DenseIndex der_order=0; der_order<n+1; ++der_order)
00318     {
00319       const Replicate<BasisDerivativeRowXpr,Dimension,1> ctrl_weights( basis_func_ders.row(der_order) );
00320       const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(spline.ctrls(),0,span-p,Dimension,p+1);
00321       der.col(der_order) = (ctrl_weights * ctrl_pts).rowwise().sum();
00322     }
00323   }
00324 
00325   template <typename _Scalar, int _Dim, int _Degree>
00326   typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::DerivativeType
00327     Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const
00328   {
00329     typename SplineTraits< Spline >::DerivativeType res;
00330     derivativesImpl(*this, u, order, res);
00331     return res;
00332   }
00333 
00334   template <typename _Scalar, int _Dim, int _Degree>
00335   template <int DerivativeOrder>
00336   typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::DerivativeType
00337     Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const
00338   {
00339     typename SplineTraits< Spline, DerivativeOrder >::DerivativeType res;
00340     derivativesImpl(*this, u, order, res);
00341     return res;
00342   }
00343 
00344   template <typename _Scalar, int _Dim, int _Degree>
00345   typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisVectorType
00346     Spline<_Scalar, _Dim, _Degree>::basisFunctions(Scalar u) const
00347   {
00348     return Spline::BasisFunctions(u, degree(), knots());
00349   }
00350 
00351   /* --------------------------------------------------------------------------------------------- */
00352 
00353   template <typename SplineType, typename DerivativeType>
00354   void basisFunctionDerivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& N_)
00355   {
00356     enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
00357 
00358     typedef typename SplineTraits<SplineType>::Scalar Scalar;
00359     typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
00360     typedef typename SplineTraits<SplineType>::KnotVectorType KnotVectorType;
00361     typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType;
00362 
00363     const KnotVectorType& U = spline.knots();
00364 
00365     const DenseIndex p = spline.degree();
00366     const DenseIndex span = spline.span(u);
00367 
00368     const DenseIndex n = (std::min)(p, order);
00369 
00370     N_.resize(n+1, p+1);
00371 
00372     BasisVectorType left = BasisVectorType::Zero(p+1);
00373     BasisVectorType right = BasisVectorType::Zero(p+1);
00374 
00375     Matrix<Scalar,Order,Order> ndu(p+1,p+1);
00376 
00377     double saved, temp;
00378 
00379     ndu(0,0) = 1.0;
00380 
00381     DenseIndex j;
00382     for (j=1; j<=p; ++j)
00383     {
00384       left[j] = u-U[span+1-j];
00385       right[j] = U[span+j]-u;
00386       saved = 0.0;
00387 
00388       for (DenseIndex r=0; r<j; ++r)
00389       {
00390         /* Lower triangle */
00391         ndu(j,r) = right[r+1]+left[j-r];
00392         temp = ndu(r,j-1)/ndu(j,r);
00393         /* Upper triangle */
00394         ndu(r,j) = static_cast<Scalar>(saved+right[r+1] * temp);
00395         saved = left[j-r] * temp;
00396       }
00397 
00398       ndu(j,j) = static_cast<Scalar>(saved);
00399     }
00400 
00401     for (j = p; j>=0; --j) 
00402       N_(0,j) = ndu(j,p);
00403 
00404     // Compute the derivatives
00405     DerivativeType a(n+1,p+1);
00406     DenseIndex r=0;
00407     for (; r<=p; ++r)
00408     {
00409       DenseIndex s1,s2;
00410       s1 = 0; s2 = 1; // alternate rows in array a
00411       a(0,0) = 1.0;
00412 
00413       // Compute the k-th derivative
00414       for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k)
00415       {
00416         double d = 0.0;
00417         DenseIndex rk,pk,j1,j2;
00418         rk = r-k; pk = p-k;
00419 
00420         if (r>=k)
00421         {
00422           a(s2,0) = a(s1,0)/ndu(pk+1,rk);
00423           d = a(s2,0)*ndu(rk,pk);
00424         }
00425 
00426         if (rk>=-1) j1 = 1;
00427         else        j1 = -rk;
00428 
00429         if (r-1 <= pk) j2 = k-1;
00430         else           j2 = p-r;
00431 
00432         for (j=j1; j<=j2; ++j)
00433         {
00434           a(s2,j) = (a(s1,j)-a(s1,j-1))/ndu(pk+1,rk+j);
00435           d += a(s2,j)*ndu(rk+j,pk);
00436         }
00437 
00438         if (r<=pk)
00439         {
00440           a(s2,k) = -a(s1,k-1)/ndu(pk+1,r);
00441           d += a(s2,k)*ndu(r,pk);
00442         }
00443 
00444         N_(k,r) = static_cast<Scalar>(d);
00445         j = s1; s1 = s2; s2 = j; // Switch rows
00446       }
00447     }
00448 
00449     /* Multiply through by the correct factors */
00450     /* (Eq. [2.9])                             */
00451     r = p;
00452     for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k)
00453     {
00454       for (DenseIndex j=p; j>=0; --j) N_(k,j) *= r;
00455       r *= p-k;
00456     }
00457   }
00458 
00459   template <typename _Scalar, int _Dim, int _Degree>
00460   typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
00461     Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
00462   {
00463     typename SplineTraits< Spline >::BasisDerivativeType der;
00464     basisFunctionDerivativesImpl(*this, u, order, der);
00465     return der;
00466   }
00467 
00468   template <typename _Scalar, int _Dim, int _Degree>
00469   template <int DerivativeOrder>
00470   typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType
00471     Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
00472   {
00473     typename SplineTraits< Spline, DerivativeOrder >::BasisDerivativeType der;
00474     basisFunctionDerivativesImpl(*this, u, order, der);
00475     return der;
00476   }
00477 }
00478 
00479 #endif // EIGEN_SPLINE_H