Aikido
BSpline.hpp
Go to the documentation of this file.
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 // The AIKIDO development team has extended this class as follows:
11 // - Enable to modify control points even after construction
12 
13 #ifndef AIKIDO_COMMON_BSPLINE_HPP_
14 #define AIKIDO_COMMON_BSPLINE_HPP_
15 
16 #include "aikido/common/SplineFwd.hpp"
17 
18 namespace aikido {
19 namespace common {
20 
38 template <typename _Scalar, int _Dim, int _Degree>
39 class BSpline
40 {
41 public:
42  typedef _Scalar Scalar;
43  enum
44  {
45  Dimension = _Dim
46  };
47  enum
48  {
49  Degree = _Degree
50  };
51 
53  typedef typename SplineTraits<BSpline>::PointType PointType;
54 
56  typedef typename SplineTraits<BSpline>::KnotVectorType KnotVectorType;
57 
59  typedef
60  typename SplineTraits<BSpline>::ParameterVectorType ParameterVectorType;
61 
63  typedef typename SplineTraits<BSpline>::BasisVectorType BasisVectorType;
64 
67  typedef
68  typename SplineTraits<BSpline>::BasisDerivativeType BasisDerivativeType;
69 
71  typedef typename SplineTraits<BSpline>::ControlPointVectorType
72  ControlPointVectorType;
73 
78  BSpline()
79  : m_knots(1, (Degree == Eigen::Dynamic ? 2 : 2 * Degree + 2))
80  , m_ctrls(ControlPointVectorType::Zero(
81  Dimension, (Degree == Eigen::Dynamic ? 1 : Degree + 1)))
82  {
83  // in theory this code can go to the initializer list but it will get pretty
84  // much unreadable ...
85  enum
86  {
87  MinDegree = (Degree == Eigen::Dynamic ? 0 : Degree)
88  };
89  m_knots.template segment<MinDegree + 1>(0)
90  = Eigen::Array<Scalar, 1, MinDegree + 1>::Zero();
91  m_knots.template segment<MinDegree + 1>(MinDegree + 1)
92  = Eigen::Array<Scalar, 1, MinDegree + 1>::Ones();
93  }
94 
100  template <typename OtherVectorType, typename OtherArrayType>
101  BSpline(const OtherVectorType& knots, const OtherArrayType& ctrls)
102  : m_knots(knots), m_ctrls(ctrls)
103  {
104  }
105 
110  template <int OtherDegree>
111  BSpline(const BSpline<Scalar, Dimension, OtherDegree>& spline)
112  : m_knots(spline.knots()), m_ctrls(spline.ctrls())
113  {
114  }
115 
119  const KnotVectorType& knots() const
120  {
121  return m_knots;
122  }
123 
127  ControlPointVectorType& ctrls()
128  {
129  return m_ctrls;
130  }
131  // Note: Added by AIKIDO to be able to change the control points.
132 
136  const ControlPointVectorType& ctrls() const
137  {
138  return m_ctrls;
139  }
140 
152  PointType operator()(Scalar u) const;
153 
167  typename SplineTraits<BSpline>::DerivativeType derivatives(
168  Scalar u, Eigen::DenseIndex order) const;
169 
175  template <int DerivativeOrder>
176  typename SplineTraits<BSpline, DerivativeOrder>::DerivativeType derivatives(
177  Scalar u, Eigen::DenseIndex order = DerivativeOrder) const;
178 
195  typename SplineTraits<BSpline>::BasisVectorType basisFunctions(
196  Scalar u) const;
197 
213  typename SplineTraits<BSpline>::BasisDerivativeType basisFunctionDerivatives(
214  Scalar u, Eigen::DenseIndex order) const;
215 
221  template <int DerivativeOrder>
222  typename SplineTraits<BSpline, DerivativeOrder>::BasisDerivativeType
223  basisFunctionDerivatives(
224  Scalar u, Eigen::DenseIndex order = DerivativeOrder) const;
225 
229  Eigen::DenseIndex degree() const;
230 
235  Eigen::DenseIndex span(Scalar u) const;
236 
241  static Eigen::DenseIndex Span(
242  typename SplineTraits<BSpline>::Scalar u,
243  Eigen::DenseIndex degree,
244  const typename SplineTraits<BSpline>::KnotVectorType& knots);
245 
258  static BasisVectorType BasisFunctions(
259  Scalar u, Eigen::DenseIndex degree, const KnotVectorType& knots);
260 
266  static BasisDerivativeType BasisFunctionDerivatives(
267  const Scalar u,
268  const Eigen::DenseIndex order,
269  const Eigen::DenseIndex degree,
270  const KnotVectorType& knots);
271 
272 private:
273  KnotVectorType m_knots;
274  ControlPointVectorType m_ctrls;
276  template <typename DerivativeType>
277  static void BasisFunctionDerivativesImpl(
278  const typename BSpline<_Scalar, _Dim, _Degree>::Scalar u,
279  const Eigen::DenseIndex order,
280  const Eigen::DenseIndex p,
281  const typename BSpline<_Scalar, _Dim, _Degree>::KnotVectorType& U,
282  DerivativeType& N_);
283 };
284 
285 template <typename _Scalar, int _Dim, int _Degree>
286 Eigen::DenseIndex BSpline<_Scalar, _Dim, _Degree>::Span(
287  typename SplineTraits<BSpline<_Scalar, _Dim, _Degree> >::Scalar u,
288  Eigen::DenseIndex degree,
289  const typename SplineTraits<
290  BSpline<_Scalar, _Dim, _Degree> >::KnotVectorType& knots)
291 {
292  // Piegl & Tiller, "The NURBS Book", A2.1 (p. 68)
293  if (u <= knots(0))
294  return degree;
295  const Scalar* pos = std::upper_bound(
296  knots.data() + degree - 1, knots.data() + knots.size() - degree - 1, u);
297  return static_cast<Eigen::DenseIndex>(std::distance(knots.data(), pos) - 1);
298 }
299 
300 template <typename _Scalar, int _Dim, int _Degree>
301 typename BSpline<_Scalar, _Dim, _Degree>::BasisVectorType
302 BSpline<_Scalar, _Dim, _Degree>::BasisFunctions(
303  typename BSpline<_Scalar, _Dim, _Degree>::Scalar u,
304  Eigen::DenseIndex degree,
305  const typename BSpline<_Scalar, _Dim, _Degree>::KnotVectorType& knots)
306 {
307  typedef
308  typename BSpline<_Scalar, _Dim, _Degree>::BasisVectorType BasisVectorType;
309 
310  const Eigen::DenseIndex p = degree;
311  const Eigen::DenseIndex i = BSpline::Span(u, degree, knots);
312 
313  const KnotVectorType& U = knots;
314 
315  BasisVectorType left(p + 1);
316  left(0) = Scalar(0);
317  BasisVectorType right(p + 1);
318  right(0) = Scalar(0);
319 
320  Eigen::VectorBlock<BasisVectorType, Degree>(left, 1, p)
321  = u
322  - Eigen::VectorBlock<const KnotVectorType, Degree>(U, i + 1 - p, p)
323  .reverse();
324  Eigen::VectorBlock<BasisVectorType, Degree>(right, 1, p)
325  = Eigen::VectorBlock<const KnotVectorType, Degree>(U, i + 1, p) - u;
326 
327  BasisVectorType N(1, p + 1);
328  N(0) = Scalar(1);
329  for (Eigen::DenseIndex j = 1; j <= p; ++j)
330  {
331  Scalar saved = Scalar(0);
332  for (Eigen::DenseIndex r = 0; r < j; r++)
333  {
334  const Scalar tmp = N(r) / (right(r + 1) + left(j - r));
335  N[r] = saved + right(r + 1) * tmp;
336  saved = left(j - r) * tmp;
337  }
338  N(j) = saved;
339  }
340  return N;
341 }
342 
343 template <typename _Scalar, int _Dim, int _Degree>
344 Eigen::DenseIndex BSpline<_Scalar, _Dim, _Degree>::degree() const
345 {
346  if (_Degree == Eigen::Dynamic)
347  return m_knots.size() - m_ctrls.cols() - 1;
348  else
349  return _Degree;
350 }
351 
352 template <typename _Scalar, int _Dim, int _Degree>
353 Eigen::DenseIndex BSpline<_Scalar, _Dim, _Degree>::span(Scalar u) const
354 {
355  return BSpline::Span(u, degree(), knots());
356 }
357 
358 template <typename _Scalar, int _Dim, int _Degree>
359 typename BSpline<_Scalar, _Dim, _Degree>::PointType
360 BSpline<_Scalar, _Dim, _Degree>::operator()(Scalar u) const
361 {
362  enum
363  {
364  Order = SplineTraits<BSpline>::OrderAtCompileTime
365  };
366 
367  const Eigen::DenseIndex span = this->span(u);
368  const Eigen::DenseIndex p = degree();
369  const BasisVectorType basis_funcs = basisFunctions(u);
370 
371  const Eigen::Replicate<BasisVectorType, Dimension, 1> ctrl_weights(
372  basis_funcs);
373  const Eigen::Block<const ControlPointVectorType, Dimension, Order> ctrl_pts(
374  ctrls(), 0, span - p, Dimension, p + 1);
375  return (ctrl_weights * ctrl_pts).rowwise().sum();
376 }
377 
378 /* ---------------------------------------------------------------------------------------------
379  */
380 
381 template <typename SplineType, typename DerivativeType>
382 void derivativesImpl(
383  const SplineType& spline,
384  typename SplineType::Scalar u,
385  Eigen::DenseIndex order,
386  DerivativeType& der)
387 {
388  enum
389  {
390  Dimension = SplineTraits<SplineType>::Dimension
391  };
392  enum
393  {
394  Order = SplineTraits<SplineType>::OrderAtCompileTime
395  };
396  enum
397  {
398  DerivativeOrder = DerivativeType::ColsAtCompileTime
399  };
400 
401  typedef typename SplineTraits<SplineType>::ControlPointVectorType
402  ControlPointVectorType;
403  typedef
404  typename SplineTraits<SplineType, DerivativeOrder>::BasisDerivativeType
405  BasisDerivativeType;
406  typedef typename BasisDerivativeType::ConstRowXpr BasisDerivativeRowXpr;
407 
408  const Eigen::DenseIndex p = spline.degree();
409  const Eigen::DenseIndex span = spline.span(u);
410 
411  const Eigen::DenseIndex n = (std::min)(p, order);
412 
413  der.resize(Dimension, n + 1);
414 
415  // Retrieve the basis function derivatives up to the desired order...
416  const BasisDerivativeType basis_func_ders
417  = spline.template basisFunctionDerivatives<DerivativeOrder>(u, n + 1);
418 
419  // ... and perform the linear combinations of the control points.
420  for (Eigen::DenseIndex der_order = 0; der_order < n + 1; ++der_order)
421  {
422  const Eigen::Replicate<BasisDerivativeRowXpr, Dimension, 1> ctrl_weights(
423  basis_func_ders.row(der_order));
424  const Eigen::Block<const ControlPointVectorType, Dimension, Order> ctrl_pts(
425  spline.ctrls(), 0, span - p, Dimension, p + 1);
426  der.col(der_order) = (ctrl_weights * ctrl_pts).rowwise().sum();
427  }
428 }
429 
430 template <typename _Scalar, int _Dim, int _Degree>
431 typename SplineTraits<BSpline<_Scalar, _Dim, _Degree> >::DerivativeType
432 BSpline<_Scalar, _Dim, _Degree>::derivatives(
433  Scalar u, Eigen::DenseIndex order) const
434 {
435  typename SplineTraits<BSpline>::DerivativeType res;
436  derivativesImpl(*this, u, order, res);
437  return res;
438 }
439 
440 template <typename _Scalar, int _Dim, int _Degree>
441 template <int DerivativeOrder>
442 typename SplineTraits<BSpline<_Scalar, _Dim, _Degree>, DerivativeOrder>::
443  DerivativeType
444  BSpline<_Scalar, _Dim, _Degree>::derivatives(
445  Scalar u, Eigen::DenseIndex order) const
446 {
447  typename SplineTraits<BSpline, DerivativeOrder>::DerivativeType res;
448  derivativesImpl(*this, u, order, res);
449  return res;
450 }
451 
452 template <typename _Scalar, int _Dim, int _Degree>
453 typename SplineTraits<BSpline<_Scalar, _Dim, _Degree> >::BasisVectorType
454 BSpline<_Scalar, _Dim, _Degree>::basisFunctions(Scalar u) const
455 {
456  return BSpline::BasisFunctions(u, degree(), knots());
457 }
458 
459 /* ---------------------------------------------------------------------------------------------
460  */
461 
462 template <typename _Scalar, int _Dim, int _Degree>
463 template <typename DerivativeType>
464 void BSpline<_Scalar, _Dim, _Degree>::BasisFunctionDerivativesImpl(
465  const typename BSpline<_Scalar, _Dim, _Degree>::Scalar u,
466  const Eigen::DenseIndex order,
467  const Eigen::DenseIndex p,
468  const typename BSpline<_Scalar, _Dim, _Degree>::KnotVectorType& U,
469  DerivativeType& N_)
470 {
471  typedef BSpline<_Scalar, _Dim, _Degree> SplineType;
472  enum
473  {
474  Order = SplineTraits<SplineType>::OrderAtCompileTime
475  };
476 
477  typedef typename SplineTraits<SplineType>::Scalar Scalar;
478  typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
479 
480  const Eigen::DenseIndex span = SplineType::Span(u, p, U);
481 
482  const Eigen::DenseIndex n = (std::min)(p, order);
483 
484  N_.resize(n + 1, p + 1);
485 
486  BasisVectorType left = BasisVectorType::Zero(p + 1);
487  BasisVectorType right = BasisVectorType::Zero(p + 1);
488 
489  Eigen::Matrix<Scalar, Order, Order> ndu(p + 1, p + 1);
490 
491  Scalar saved, temp; // FIXME These were double instead of Scalar. Was there a
492  // reason for that?
493 
494  ndu(0, 0) = 1.0;
495 
496  Eigen::DenseIndex j;
497  for (j = 1; j <= p; ++j)
498  {
499  left[j] = u - U[span + 1 - j];
500  right[j] = U[span + j] - u;
501  saved = 0.0;
502 
503  for (Eigen::DenseIndex r = 0; r < j; ++r)
504  {
505  /* Lower triangle */
506  ndu(j, r) = right[r + 1] + left[j - r];
507  temp = ndu(r, j - 1) / ndu(j, r);
508  /* Upper triangle */
509  ndu(r, j) = static_cast<Scalar>(saved + right[r + 1] * temp);
510  saved = left[j - r] * temp;
511  }
512 
513  ndu(j, j) = static_cast<Scalar>(saved);
514  }
515 
516  for (j = p; j >= 0; --j)
517  N_(0, j) = ndu(j, p);
518 
519  // Compute the derivatives
520  DerivativeType a(n + 1, p + 1);
521  Eigen::DenseIndex r = 0;
522  for (; r <= p; ++r)
523  {
524  Eigen::DenseIndex s1, s2;
525  s1 = 0;
526  s2 = 1; // alternate rows in array a
527  a(0, 0) = 1.0;
528 
529  // Compute the k-th derivative
530  for (Eigen::DenseIndex k = 1; k <= static_cast<Eigen::DenseIndex>(n); ++k)
531  {
532  Scalar d = 0.0;
533  Eigen::DenseIndex rk, pk, j1, j2;
534  rk = r - k;
535  pk = p - k;
536 
537  if (r >= k)
538  {
539  a(s2, 0) = a(s1, 0) / ndu(pk + 1, rk);
540  d = a(s2, 0) * ndu(rk, pk);
541  }
542 
543  if (rk >= -1)
544  j1 = 1;
545  else
546  j1 = -rk;
547 
548  if (r - 1 <= pk)
549  j2 = k - 1;
550  else
551  j2 = p - r;
552 
553  for (j = j1; j <= j2; ++j)
554  {
555  a(s2, j) = (a(s1, j) - a(s1, j - 1)) / ndu(pk + 1, rk + j);
556  d += a(s2, j) * ndu(rk + j, pk);
557  }
558 
559  if (r <= pk)
560  {
561  a(s2, k) = -a(s1, k - 1) / ndu(pk + 1, r);
562  d += a(s2, k) * ndu(r, pk);
563  }
564 
565  N_(k, r) = static_cast<Scalar>(d);
566  j = s1;
567  s1 = s2;
568  s2 = j; // Switch rows
569  }
570  }
571 
572  /* Multiply through by the correct factors */
573  /* (Eq. [2.9]) */
574  r = p;
575  for (Eigen::DenseIndex k = 1; k <= static_cast<Eigen::DenseIndex>(n); ++k)
576  {
577  for (j = p; j >= 0; --j)
578  N_(k, j) *= r;
579  r *= p - k;
580  }
581 }
582 
583 template <typename _Scalar, int _Dim, int _Degree>
584 typename SplineTraits<BSpline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
585 BSpline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(
586  Scalar u, Eigen::DenseIndex order) const
587 {
588  typename SplineTraits<BSpline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
589  der;
590  BasisFunctionDerivativesImpl(u, order, degree(), knots(), der);
591  return der;
592 }
593 
594 template <typename _Scalar, int _Dim, int _Degree>
595 template <int DerivativeOrder>
596 typename SplineTraits<BSpline<_Scalar, _Dim, _Degree>, DerivativeOrder>::
597  BasisDerivativeType
598  BSpline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(
599  Scalar u, Eigen::DenseIndex order) const
600 {
601  typename SplineTraits<BSpline<_Scalar, _Dim, _Degree>, DerivativeOrder>::
602  BasisDerivativeType der;
603  BasisFunctionDerivativesImpl(u, order, degree(), knots(), der);
604  return der;
605 }
606 
607 template <typename _Scalar, int _Dim, int _Degree>
608 typename SplineTraits<BSpline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
609 BSpline<_Scalar, _Dim, _Degree>::BasisFunctionDerivatives(
610  const typename BSpline<_Scalar, _Dim, _Degree>::Scalar u,
611  const Eigen::DenseIndex order,
612  const Eigen::DenseIndex degree,
613  const typename BSpline<_Scalar, _Dim, _Degree>::KnotVectorType& knots)
614 {
615  typename SplineTraits<BSpline>::BasisDerivativeType der;
616  BasisFunctionDerivativesImpl(u, order, degree, knots, der);
617  return der;
618 }
619 
620 } // namespace common
621 } // namespace aikido
622 
623 #endif // AIKIDO_COMMON_BSPLINE_HPP_
aikido::common::BSpline::basisFunctions
SplineTraits< BSpline >::BasisVectorType basisFunctions(Scalar u) const
Computes the non-zero basis functions at the given site.
Definition: BSpline.hpp:454
aikido::common::BSpline::ParameterVectorType
SplineTraits< BSpline >::ParameterVectorType ParameterVectorType
The data type used to store parameter vectors.
Definition: BSpline.hpp:60
aikido::common::BSpline::BasisFunctionDerivatives
static BasisDerivativeType BasisFunctionDerivatives(const Scalar u, const Eigen::DenseIndex order, const Eigen::DenseIndex degree, const KnotVectorType &knots)
Definition: BSpline.hpp:609
aikido::common::BSpline::operator()
PointType operator()(Scalar u) const
Returns the spline value at a given site .
Definition: BSpline.hpp:360
aikido::common::BSpline::BasisFunctionDerivativesImpl
static void BasisFunctionDerivativesImpl(const typename BSpline< _Scalar, _Dim, _Degree >::Scalar u, const Eigen::DenseIndex order, const Eigen::DenseIndex p, const typename BSpline< _Scalar, _Dim, _Degree >::KnotVectorType &U, DerivativeType &N_)
Definition: BSpline.hpp:464
aikido::common::BSpline::basisFunctionDerivatives
SplineTraits< BSpline >::BasisDerivativeType basisFunctionDerivatives(Scalar u, Eigen::DenseIndex order) const
Computes the non-zero spline basis function derivatives up to given order.
Definition: BSpline.hpp:585
aikido::common::BSpline::derivatives
SplineTraits< BSpline >::DerivativeType derivatives(Scalar u, Eigen::DenseIndex order) const
Evaluation of spline derivatives of up-to given order.
Definition: BSpline.hpp:432
aikido
Format of serialized trajectory in YAML.
Definition: algorithm.hpp:4
aikido::common::BSpline::Degree
@ Degree
Definition: BSpline.hpp:49
aikido::common::BSpline::ctrls
const ControlPointVectorType & ctrls() const
Returns the ctrls of the underlying spline.
Definition: BSpline.hpp:136
aikido::common::BSpline::BSpline
BSpline()
Creates a (constant) zero spline.
Definition: BSpline.hpp:78
aikido::common::derivativesImpl
void derivativesImpl(const SplineType &spline, typename SplineType::Scalar u, Eigen::DenseIndex order, DerivativeType &der)
Definition: BSpline.hpp:382
aikido::common::BSpline::BSpline
BSpline(const OtherVectorType &knots, const OtherArrayType &ctrls)
Creates a spline from a knot vector and control points.
Definition: BSpline.hpp:101
aikido::common::SplineTraits
Definition: SplineFwd.hpp:22
aikido::common::BSpline::BasisVectorType
SplineTraits< BSpline >::BasisVectorType BasisVectorType
The data type used to store non-zero basis functions.
Definition: BSpline.hpp:63
aikido::common::BSpline
Definition: BSpline.hpp:39
aikido::common::BSpline::Scalar
_Scalar Scalar
Definition: BSpline.hpp:42
aikido::common::BSpline::m_ctrls
ControlPointVectorType m_ctrls
Definition: BSpline.hpp:274
aikido::common::BSpline::KnotVectorType
SplineTraits< BSpline >::KnotVectorType KnotVectorType
The data type used to store knot vectors.
Definition: BSpline.hpp:56
aikido::common::BSpline::ControlPointVectorType
SplineTraits< BSpline >::ControlPointVectorType ControlPointVectorType
The data type representing the spline's control points.
Definition: BSpline.hpp:72
aikido::common::BSpline::BSpline
BSpline(const BSpline< Scalar, Dimension, OtherDegree > &spline)
Copy constructor for splines.
Definition: BSpline.hpp:111
aikido::common::BSpline::Dimension
@ Dimension
Definition: BSpline.hpp:45
aikido::common::BSpline::BasisDerivativeType
SplineTraits< BSpline >::BasisDerivativeType BasisDerivativeType
The data type used to store the values of the basis function derivatives.
Definition: BSpline.hpp:68
SplineFwd.hpp
aikido::common::BSpline::knots
const KnotVectorType & knots() const
Returns the knots of the underlying spline.
Definition: BSpline.hpp:119
aikido::common::BSpline::ctrls
ControlPointVectorType & ctrls()
Returns the ctrls of the underlying spline.
Definition: BSpline.hpp:127
aikido::common::BSpline::span
Eigen::DenseIndex span(Scalar u) const
Returns the span within the knot vector in which u is falling.
Definition: BSpline.hpp:353
aikido::common::BSpline::BasisFunctions
static BasisVectorType BasisFunctions(Scalar u, Eigen::DenseIndex degree, const KnotVectorType &knots)
Returns the spline's non-zero basis functions.
Definition: BSpline.hpp:302
aikido::common::BSpline::Span
static Eigen::DenseIndex Span(typename SplineTraits< BSpline >::Scalar u, Eigen::DenseIndex degree, const typename SplineTraits< BSpline >::KnotVectorType &knots)
Computes the spang within the provided knot vector in which u is falling.
Definition: BSpline.hpp:286
aikido::common::BSpline::m_knots
KnotVectorType m_knots
Definition: BSpline.hpp:273
aikido::common::BSpline::degree
Eigen::DenseIndex degree() const
Returns the spline degree.
Definition: BSpline.hpp:344
aikido::common::BSpline::PointType
SplineTraits< BSpline >::PointType PointType
The point type the spline is representing.
Definition: BSpline.hpp:53