Spline.h
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 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_SPLINE_H
26 #define EIGEN_SPLINE_H
27 
28 #include "SplineFwd.h"
29 
30 namespace Eigen
31 {
49  template <typename _Scalar, int _Dim, int _Degree>
50  class Spline
51  {
52  public:
53  typedef _Scalar Scalar;
54  enum { Dimension = _Dim };
55  enum { Degree = _Degree };
56 
58  typedef typename SplineTraits<Spline>::PointType PointType;
59 
61  typedef typename SplineTraits<Spline>::KnotVectorType KnotVectorType;
62 
64  typedef typename SplineTraits<Spline>::BasisVectorType BasisVectorType;
65 
67  typedef typename SplineTraits<Spline>::ControlPointVectorType ControlPointVectorType;
68 
74  template <typename OtherVectorType, typename OtherArrayType>
75  Spline(const OtherVectorType& knots, const OtherArrayType& ctrls) : m_knots(knots), m_ctrls(ctrls) {}
76 
81  template <int OtherDegree>
83  m_knots(spline.knots()), m_ctrls(spline.ctrls()) {}
84 
88  const KnotVectorType& knots() const { return m_knots; }
89 
93  const ControlPointVectorType& ctrls() const { return m_ctrls; }
94 
106  PointType operator()(Scalar u) const;
107 
120  typename SplineTraits<Spline>::DerivativeType
121  derivatives(Scalar u, DenseIndex order) const;
122 
128  template <int DerivativeOrder>
129  typename SplineTraits<Spline,DerivativeOrder>::DerivativeType
130  derivatives(Scalar u, DenseIndex order = DerivativeOrder) const;
131 
148  typename SplineTraits<Spline>::BasisVectorType
149  basisFunctions(Scalar u) const;
150 
164  typename SplineTraits<Spline>::BasisDerivativeType
165  basisFunctionDerivatives(Scalar u, DenseIndex order) const;
166 
172  template <int DerivativeOrder>
173  typename SplineTraits<Spline,DerivativeOrder>::BasisDerivativeType
174  basisFunctionDerivatives(Scalar u, DenseIndex order = DerivativeOrder) const;
175 
179  DenseIndex degree() const;
180 
185  DenseIndex span(Scalar u) const;
186 
190  static DenseIndex Span(typename SplineTraits<Spline>::Scalar u, DenseIndex degree, const typename SplineTraits<Spline>::KnotVectorType& knots);
191 
204  static BasisVectorType BasisFunctions(Scalar u, DenseIndex degree, const KnotVectorType& knots);
205 
206 
207  private:
208  KnotVectorType m_knots;
209  ControlPointVectorType m_ctrls;
210  };
211 
212  template <typename _Scalar, int _Dim, int _Degree>
214  typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::Scalar u,
215  DenseIndex degree,
216  const typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::KnotVectorType& knots)
217  {
218  // Piegl & Tiller, "The NURBS Book", A2.1 (p. 68)
219  if (u <= knots(0)) return degree;
220  const Scalar* pos = std::upper_bound(knots.data()+degree-1, knots.data()+knots.size()-degree-1, u);
221  return static_cast<DenseIndex>( std::distance(knots.data(), pos) - 1 );
222  }
223 
224  template <typename _Scalar, int _Dim, int _Degree>
228  DenseIndex degree,
230  {
232 
233  const DenseIndex p = degree;
234  const DenseIndex i = Spline::Span(u, degree, knots);
235 
236  const KnotVectorType& U = knots;
237 
238  BasisVectorType left(p+1); left(0) = Scalar(0);
239  BasisVectorType right(p+1); right(0) = Scalar(0);
240 
241  VectorBlock<BasisVectorType,Degree>(left,1,p) = u - VectorBlock<const KnotVectorType,Degree>(U,i+1-p,p).reverse();
242  VectorBlock<BasisVectorType,Degree>(right,1,p) = VectorBlock<const KnotVectorType,Degree>(U,i+1,p) - u;
243 
244  BasisVectorType N(1,p+1);
245  N(0) = Scalar(1);
246  for (DenseIndex j=1; j<=p; ++j)
247  {
248  Scalar saved = Scalar(0);
249  for (DenseIndex r=0; r<j; r++)
250  {
251  const Scalar tmp = N(r)/(right(r+1)+left(j-r));
252  N[r] = saved + right(r+1)*tmp;
253  saved = left(j-r)*tmp;
254  }
255  N(j) = saved;
256  }
257  return N;
258  }
259 
260  template <typename _Scalar, int _Dim, int _Degree>
262  {
263  if (_Degree == Dynamic)
264  return m_knots.size() - m_ctrls.cols() - 1;
265  else
266  return _Degree;
267  }
268 
269  template <typename _Scalar, int _Dim, int _Degree>
271  {
272  return Spline::Span(u, degree(), knots());
273  }
274 
275  template <typename _Scalar, int _Dim, int _Degree>
277  {
278  enum { Order = SplineTraits<Spline>::OrderAtCompileTime };
279 
280  const DenseIndex span = this->span(u);
281  const DenseIndex p = degree();
282  const BasisVectorType basis_funcs = basisFunctions(u);
283 
284  const Replicate<BasisVectorType,Dimension,1> ctrl_weights(basis_funcs);
285  const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(ctrls(),0,span-p,Dimension,p+1);
286  return (ctrl_weights * ctrl_pts).rowwise().sum();
287  }
288 
289  /* --------------------------------------------------------------------------------------------- */
290 
291  template <typename SplineType, typename DerivativeType>
292  void derivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& der)
293  {
294  enum { Dimension = SplineTraits<SplineType>::Dimension };
295  enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
296  enum { DerivativeOrder = DerivativeType::ColsAtCompileTime };
297 
298  typedef typename SplineTraits<SplineType>::Scalar Scalar;
299 
300  typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
301  typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType;
302 
303  typedef typename SplineTraits<SplineType,DerivativeOrder>::BasisDerivativeType BasisDerivativeType;
304  typedef typename BasisDerivativeType::ConstRowXpr BasisDerivativeRowXpr;
305 
306  const DenseIndex p = spline.degree();
307  const DenseIndex span = spline.span(u);
308 
309  const DenseIndex n = (std::min)(p, order);
310 
311  der.resize(Dimension,n+1);
312 
313  // Retrieve the basis function derivatives up to the desired order...
314  const BasisDerivativeType basis_func_ders = spline.template basisFunctionDerivatives<DerivativeOrder>(u, n+1);
315 
316  // ... and perform the linear combinations of the control points.
317  for (DenseIndex der_order=0; der_order<n+1; ++der_order)
318  {
319  const Replicate<BasisDerivativeRowXpr,Dimension,1> ctrl_weights( basis_func_ders.row(der_order) );
320  const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(spline.ctrls(),0,span-p,Dimension,p+1);
321  der.col(der_order) = (ctrl_weights * ctrl_pts).rowwise().sum();
322  }
323  }
324 
325  template <typename _Scalar, int _Dim, int _Degree>
326  typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::DerivativeType
327  Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const
328  {
329  typename SplineTraits< Spline >::DerivativeType res;
330  derivativesImpl(*this, u, order, res);
331  return res;
332  }
333 
334  template <typename _Scalar, int _Dim, int _Degree>
335  template <int DerivativeOrder>
336  typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::DerivativeType
337  Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const
338  {
339  typename SplineTraits< Spline, DerivativeOrder >::DerivativeType res;
340  derivativesImpl(*this, u, order, res);
341  return res;
342  }
343 
344  template <typename _Scalar, int _Dim, int _Degree>
345  typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisVectorType
347  {
348  return Spline::BasisFunctions(u, degree(), knots());
349  }
350 
351  /* --------------------------------------------------------------------------------------------- */
352 
353  template <typename SplineType, typename DerivativeType>
354  void basisFunctionDerivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& N_)
355  {
356  enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
357 
358  typedef typename SplineTraits<SplineType>::Scalar Scalar;
359  typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
360  typedef typename SplineTraits<SplineType>::KnotVectorType KnotVectorType;
361  typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType;
362 
363  const KnotVectorType& U = spline.knots();
364 
365  const DenseIndex p = spline.degree();
366  const DenseIndex span = spline.span(u);
367 
368  const DenseIndex n = (std::min)(p, order);
369 
370  N_.resize(n+1, p+1);
371 
372  BasisVectorType left = BasisVectorType::Zero(p+1);
373  BasisVectorType right = BasisVectorType::Zero(p+1);
374 
375  Matrix<Scalar,Order,Order> ndu(p+1,p+1);
376 
377  double saved, temp;
378 
379  ndu(0,0) = 1.0;
380 
381  DenseIndex j;
382  for (j=1; j<=p; ++j)
383  {
384  left[j] = u-U[span+1-j];
385  right[j] = U[span+j]-u;
386  saved = 0.0;
387 
388  for (DenseIndex r=0; r<j; ++r)
389  {
390  /* Lower triangle */
391  ndu(j,r) = right[r+1]+left[j-r];
392  temp = ndu(r,j-1)/ndu(j,r);
393  /* Upper triangle */
394  ndu(r,j) = static_cast<Scalar>(saved+right[r+1] * temp);
395  saved = left[j-r] * temp;
396  }
397 
398  ndu(j,j) = static_cast<Scalar>(saved);
399  }
400 
401  for (j = p; j>=0; --j)
402  N_(0,j) = ndu(j,p);
403 
404  // Compute the derivatives
405  DerivativeType a(n+1,p+1);
406  DenseIndex r=0;
407  for (; r<=p; ++r)
408  {
409  DenseIndex s1,s2;
410  s1 = 0; s2 = 1; // alternate rows in array a
411  a(0,0) = 1.0;
412 
413  // Compute the k-th derivative
414  for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k)
415  {
416  double d = 0.0;
417  DenseIndex rk,pk,j1,j2;
418  rk = r-k; pk = p-k;
419 
420  if (r>=k)
421  {
422  a(s2,0) = a(s1,0)/ndu(pk+1,rk);
423  d = a(s2,0)*ndu(rk,pk);
424  }
425 
426  if (rk>=-1) j1 = 1;
427  else j1 = -rk;
428 
429  if (r-1 <= pk) j2 = k-1;
430  else j2 = p-r;
431 
432  for (j=j1; j<=j2; ++j)
433  {
434  a(s2,j) = (a(s1,j)-a(s1,j-1))/ndu(pk+1,rk+j);
435  d += a(s2,j)*ndu(rk+j,pk);
436  }
437 
438  if (r<=pk)
439  {
440  a(s2,k) = -a(s1,k-1)/ndu(pk+1,r);
441  d += a(s2,k)*ndu(r,pk);
442  }
443 
444  N_(k,r) = static_cast<Scalar>(d);
445  j = s1; s1 = s2; s2 = j; // Switch rows
446  }
447  }
448 
449  /* Multiply through by the correct factors */
450  /* (Eq. [2.9]) */
451  r = p;
452  for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k)
453  {
454  for (DenseIndex j=p; j>=0; --j) N_(k,j) *= r;
455  r *= p-k;
456  }
457  }
458 
459  template <typename _Scalar, int _Dim, int _Degree>
460  typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
462  {
463  typename SplineTraits< Spline >::BasisDerivativeType der;
464  basisFunctionDerivativesImpl(*this, u, order, der);
465  return der;
466  }
467 
468  template <typename _Scalar, int _Dim, int _Degree>
469  template <int DerivativeOrder>
470  typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType
471  Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
472  {
473  typename SplineTraits< Spline, DerivativeOrder >::BasisDerivativeType der;
474  basisFunctionDerivativesImpl(*this, u, order, der);
475  return der;
476  }
477 }
478 
479 #endif // EIGEN_SPLINE_H