base
Spline.hpp
Go to the documentation of this file.
1 #ifndef _BASE_SPLINE_HPP_INC
2 #define _BASE_SPLINE_HPP_INC
3 
4 #include <vector>
5 #include <base/Eigen.hpp>
6 #include <stdexcept>
7 #include <algorithm>
8 
9 struct SISLCurve;
10 
11 namespace base {
12 namespace geometry {
20  class SplineBase
21  {
22  public:
23  SplineBase(SplineBase const& source);
24  ~SplineBase();
25 
26  explicit SplineBase(int dimension,
27  double geometric_resolution = 0.1, int order = 3);
28  explicit SplineBase(double geometric_resolution, SISLCurve* curve);
29 
31  void setGeometricResolution(double _geores) { geometric_resolution = _geores; }
32  double getGeometricResolution() const { return geometric_resolution; };
33 
35  bool isEmpty() const
36  { return !getSISLCurve() && singleton.empty(); }
37 
39  bool isSingleton() const
40  { return !singleton.empty(); }
41 
43  int getDimension() const { return dimension; }
45  int getPointCount() const;
49  void setCurveOrder(int value) { curve_order = value; }
51  int getCurveOrder() const { return curve_order; }
52 
58  double getCurveLength(double relative_resolution = 0.01) const;
59  double getCurveLength(double startParam, double relative_resolution) const;
60  double getCurveLength(double startParam, double endParam, double relative_resolution) const;
61 
62 
64  double getCurvatureMax();
65  double getStartParam() const { return start_param; };
66  double getEndParam() const { return end_param; };
67 
73  SISLCurve const* getSISLCurve() const;
74 
80  SISLCurve* getSISLCurve();
81 
87  double getCurvature(double _param) const;
88 
95  double getVariationOfCurvature(double _param); // Variation of Curvature
96 
97  std::vector<double> getCoordinates() const;
98 
99  std::vector<double> getKnots() const;
100 
101  int getSISLCurveType() const;
102 
104  void printCurveProperties(std::ostream& io);
105 
110  {
119  };
120 
134  void interpolate(std::vector<double> const& coordinates,
135  std::vector<double> const& parameters = std::vector<double>(),
136  std::vector<CoordinateType> const& coord_types = std::vector<CoordinateType>() );
137 
138  void interpolate(std::vector<double> const& coordinates,
139  std::vector<double> &parameterOut,
140  std::vector<double> const& parameterIn = std::vector<double>(),
141  std::vector<CoordinateType> const& coord_types = std::vector<CoordinateType>() );
142 
145  bool testIntersection(SplineBase const& other, double resolution = 0.01) const;
146 
148  void clear();
149 
150  std::vector<double> simplify();
151  std::vector<double> simplify(double tolerance);
152 
153  SplineBase const& operator = (SplineBase const& base);
154 
155  bool isNURBS() const;
156 
162  void reset(std::vector<double> const& coordinates, std::vector<double> const& knots, int kind = -1);
163 
166  void reverse();
167 
168  void append(SplineBase const& other);
169 
177  void append(SplineBase const& other, double tolerance);
178 
194  double join(SplineBase const& other, double tolerance, bool with_tangents);
195 
201  void split(SplineBase& second_part, double parameter);
202 
208  void crop(double start_t, double end_t);
209 
214  SplineBase *getSubSpline(double start_t, double end_t) const;
215 
217  {
218  if (isNURBS()) return dimension + 1;
219  else return dimension;
220  }
221 
222  void setSingleton(double const* coordinates);
223 
226  void derive(unsigned int order, SplineBase& result) const;
227 
228  protected:
237  bool checkAndNormalizeParam(double &param, double equalDistance = 0.001) const;
238 
246  void reset(SISLCurve* curve);
247  void getPoint(double* result, double _param) const;
248  void getPointAndTangent(double* result, double _param) const;
249 
250  void findPointIntersections(double const* _point,
251  std::vector<double>& _result_points,
252  std::vector< std::pair<double, double> >& _result_curves,
253  double _geores) const;
254 
255  std::pair<double, bool> findOneLineIntersection(double const* _point,
256  double const* _normal, double _guess, double _geores) const;
257  void findLineIntersections(double const* _point, double const* _normal,
258  std::vector<double>& _result_points,
259  std::vector< std::pair<double, double> >& _result_curves,
260  double _geores) const;
261 
262  void findSphereIntersections(double const* _center, double radius,
263  std::vector<double>& points,
264  std::vector< std::pair<double, double> >& segments, double _geores) const;
265 
270  double findOneClosestPoint(double const* _pt, double _guess, double _geores) const;
271  void findClosestPoints(double const* ref_point,
272  std::vector<double>& _result_points,
273  std::vector< std::pair<double, double> >& _result_curves,
274  double _geores) const;
275 
276  double localClosestPointSearch(double const* ref_point,
277  double _guess, double _start, double _end,
278  double _geores) const;
279 
280  void getPointAndTangentHelper(double* result, double _param, bool with_tangent) const;
281 
289  double getResultClosestToGuess(double _guess, std::vector<double> points,
290  std::vector< std::pair<double, double> > curves) const;
291 
293  base::Matrix3d getFrenetFrame(double _param);
295  double getHeading(double _param);
297  double headingError(double _actHeading, double _param);
299  double distanceError(base::Vector3d _pt, double _param);
301  base::Vector3d poseError(base::Vector3d _pt, double _actZRot, double _st_para);
303  base::Vector3d poseError(base::Vector3d _pt, double _actZRot, double _st_para, double minParam);
304  private:
305  std::vector<double> singleton;
306 
307  int dimension;
309  SISLCurve *curve;
310 
312  double geometric_resolution;
314  int curve_order;
316  double start_param;
318  double end_param;
319 
321  bool has_curvature_max;
323  double curvature_max;
324  };
325 
329  class Spline3Base : public SplineBase
330  {
331  public:
335  explicit Spline3Base(int dimension, double geometric_resolution, int order)
336  : SplineBase(3, geometric_resolution, order) {}
340  explicit Spline3Base(double geometric_resolution, SISLCurve* curve)
341  : SplineBase(geometric_resolution, curve) {}
342  Spline3Base(SplineBase const& source)
343  : SplineBase(source) {}
344 
346  { return SplineBase::getFrenetFrame(_param); }
347 
348  double getHeading(double _param)
349  { return SplineBase::getHeading(_param); }
350 
351  double headingError(double _actZRot, double _param)
352  { return SplineBase::headingError(_actZRot, _param); }
353 
354  double distanceError(base::Vector3d _pt, double _param)
355  { return SplineBase::distanceError(_pt, _param); }
356 
364  base::Vector3d poseError(base::Vector3d const& _position, double _heading, double _guess)
365  { return SplineBase::poseError(_position, _heading, _guess); }
366 
377  base::Vector3d poseError(base::Vector3d const& _position, double _heading, double _guess, double minParam)
378  { return SplineBase::poseError(_position, _heading, _guess, minParam); }
379  };
380 
381  template<int DIM> struct SplineBaseClass
382  { typedef SplineBase type; };
383  template<> struct SplineBaseClass<3>
384  { typedef Spline3Base type; };
385 
386  template<int DIM>
387  class Spline : public SplineBaseClass<DIM>::type
388  {
389  public:
391  typedef Eigen::Matrix<double, DIM, 1, Eigen::DontAlign> vector_t;
392  typedef Eigen::Matrix<double, DIM, 1, Eigen::AutoAlign> vector_ta;
393  typedef Eigen::Transform<double, DIM, Eigen::Affine> transform_t;
394 
395  explicit Spline(double geometric_resolution = 0.1, int order = 3)
396  : base_t(DIM, geometric_resolution, order) {}
397  explicit Spline(double geometric_resolution, SISLCurve* curve)
398  : base_t(geometric_resolution, curve)
399  {
400  if (this->getDimension() != DIM)
401  throw std::runtime_error("trying to initialize a Spline<> class with a curve of wrong dimension");
402  }
404  : base_t(base) {}
405 
407  vector_t getStartPoint() const
408  { return getPoint(this->getStartParam()); }
409 
411  vector_t getEndPoint() const
412  { return getPoint(this->getEndParam()); }
413 
415  void setSingleton(vector_t const& v)
416  {
417  SplineBase::setSingleton(v.data());
418  }
419 
422  vector_t getPoint(double _param) const
423  {
424  vector_t result;
425  SplineBase::getPoint(result.data(), _param);
426  return result;
427  }
428 
429  std::vector<vector_t> getPoints(std::vector<double> const& parameters) const
430  {
431  std::vector<vector_t> result;
432  result.reserve(parameters.size());
433  for (unsigned int i = 0; i < parameters.size(); ++i)
434  result.push_back(getPoint(parameters[i]));
435  return result;
436  }
437 
438  Spline derive(int order) const
439  {
440  Spline result;
441  SplineBase::derive(order, result);
442  return result;
443  }
444 
460  bool doAdvance(double& result, double& cur_length, double target, double start, vector_t const& start_p, double end, vector_t const& end_p, double _geores) const
461  {
462  double d = (start_p - end_p).norm();
463  if (d < _geores)
464  {
465  if(target < 0)
466  {
467  cur_length -= d;
468  if (cur_length < target)
469  {
470  result = end;
471  return true;
472  }
473  }
474  else
475  {
476  cur_length += d;
477  if (cur_length > target)
478  {
479  result = end;
480  return true;
481  }
482  }
483 
484  return false;
485  }
486 
487  double middle = (start + end) / 2;
488  vector_t middle_p = getPoint(middle);
489 
490  if (doAdvance(result, cur_length, target, start, start_p, middle, middle_p, _geores))
491  return true;
492  if (doAdvance(result, cur_length, target, middle, middle_p, end, end_p, _geores))
493  return true;
494 
495  return false;
496  }
497 
506  std::pair<double, double> advance(double t, double length, double _geores) const
507  {
508  double result_t = 0;
509  double result_d = 0;
510  if(length < 0)
511  {
512  if (!doAdvance(result_t, result_d, length, t, getPoint(t), this->getStartParam(), getPoint(this->getStartParam()), _geores))
513  return std::make_pair(this->getStartParam(), result_d);
514  }
515  else
516  {
517  if (!doAdvance(result_t, result_d, length, t, getPoint(t), this->getEndParam(), getPoint(this->getEndParam()), _geores))
518  return std::make_pair(this->getEndParam(), result_d);
519  }
520  return std::make_pair(result_t, result_d);
521  }
522 
523 
529  double length(double start, double end, double _geores) const
530  {
531  double result_t = 0;
532  double result_d = 0;
533  doAdvance(result_t, result_d, std::numeric_limits<double>::infinity(), start, getPoint(start), end, getPoint(end), _geores);
534  return result_d;
535  }
536 
539  std::pair<vector_t, vector_t> getPointAndTangent(double _param) const
540  {
541  double result[DIM * 2];
542  SplineBase::getPointAndTangent(result, _param);
543  vector_t point(result);
544  vector_t tangent(result + DIM);
545  return std::make_pair(point, tangent);
546  }
547 
551  std::vector<vector_t> sample(double _geores, std::vector<double>* parameters = 0, int max_recursion = 20) const
552  {
553  std::vector<vector_t> result;
554  sample(result, _geores, parameters, max_recursion);
555  return result;
556  }
557 
561  void sample(std::vector<vector_t>& result, double _geores, std::vector<double>* parameters = 0, int max_recursion = 20) const
562  {
563  double start = this->getStartParam();
564  vector_t start_p = this->getPoint(start);
565  if (parameters)
566  parameters->push_back(start);
567  result.push_back(start_p);
568 
569  double end = this->getEndParam();
570  vector_t end_p = this->getPoint(end);
571  sample(result, start, start_p, end, end_p, _geores, parameters, max_recursion);
572  }
573 
576  void sample(std::vector<vector_t>& result, double start, vector_t const& start_p, double end, vector_t const& end_p, double _geores, std::vector<double>* parameters, int max_recursion = 20) const
577  {
578  if (max_recursion == 0 || (start_p - end_p).norm() < _geores)
579  {
580  if (parameters)
581  parameters->push_back(end);
582 
583  result.push_back(end_p);
584  return;
585  }
586 
587  double middle = (start + end) / 2;
588  vector_t middle_p = getPoint(middle);
589  sample(result, start, start_p, middle, middle_p, _geores, parameters, max_recursion - 1);
590  sample(result, middle, middle_p, end, end_p, _geores, parameters, max_recursion - 1);
591  }
592 
594  void interpolate(std::vector<vector_t> const& points,
595  std::vector<double> const& parameters = std::vector<double>(),
596  std::vector<SplineBase::CoordinateType> const& coord_types = std::vector<SplineBase::CoordinateType>() )
597  {
598  std::vector<double> coordinates;
599  for (size_t i = 0; i < points.size(); ++i)
600  coordinates.insert(coordinates.end(), points[i].data(), points[i].data() + DIM);
601  SplineBase::interpolate(coordinates, parameters, coord_types);
602  }
603 
604  void interpolate(std::vector<vector_t> const& points,
605  std::vector<double> &parametersOut,
606  std::vector<double> const& parametersIn = std::vector<double>(),
607  std::vector<SplineBase::CoordinateType> const& coord_types = std::vector<SplineBase::CoordinateType>() )
608  {
609  std::vector<double> coordinates;
610  for (size_t i = 0; i < points.size(); ++i)
611  coordinates.insert(coordinates.end(), points[i].data(), points[i].data() + DIM);
612  SplineBase::interpolate(coordinates, parametersOut, parametersIn, coord_types);
613  }
614 
615  void interpolate(std::vector<vector_ta> const& points,
616  std::vector<double> &parametersOut,
617  std::vector<double> const& parametersIn = std::vector<double>(),
618  std::vector<SplineBase::CoordinateType> const& coord_types = std::vector<SplineBase::CoordinateType>() )
619  {
620  std::vector<double> coordinates;
621  for (size_t i = 0; i < points.size(); ++i)
622  coordinates.insert(coordinates.end(), points[i].data(), points[i].data() + DIM);
623  SplineBase::interpolate(coordinates, parametersOut, parametersIn, coord_types);
624  }
625 
627  void interpolate(std::vector<vector_ta> const& points,
628  std::vector<double> const& parameters = std::vector<double>(),
629  std::vector<SplineBase::CoordinateType> const& coord_types = std::vector<SplineBase::CoordinateType>() )
630  {
631  std::vector<double> coordinates;
632  for (size_t i = 0; i < points.size(); ++i)
633  coordinates.insert(coordinates.end(), points[i].data(), points[i].data() + DIM);
634  SplineBase::interpolate(coordinates, parameters, coord_types);
635  }
636 
637  void interpolate(std::vector<double> const& coordinates,
638  std::vector<double> const& parameters = std::vector<double>(),
639  std::vector<SplineBase::CoordinateType> const& coord_types = std::vector<SplineBase::CoordinateType>() )
640  {
641  return SplineBase::interpolate(coordinates, parameters, coord_types);
642  };
643 
644  void interpolate(std::vector<double> const& coordinates,
645  std::vector<double> &parameterOut,
646  std::vector<double> const& parameterIn = std::vector<double>(),
647  std::vector<SplineBase::CoordinateType> const& coord_types = std::vector<SplineBase::CoordinateType>() )
648  {
649  return SplineBase::interpolate(coordinates, parameterOut, parameterIn, coord_types);
650  };
651 
652 
655  double distanceTo(vector_t const& _pt) const
656  {
657  double closest = findOneClosestPoint(_pt);
658  vector_t curve_p = getPoint(closest);
659  return (_pt - curve_p).norm();
660  }
661 
662  template<typename Test>
663  std::pair<double, double> dichotomic_search(double start_t, double end_t, Test test, double resolution, double parameter_threshold) const
664  {
665  return this->dichotomic_search(
666  start_t, this->getPoint(start_t),
667  end_t, this->getPoint(end_t),
668  test, resolution, parameter_threshold);
669  }
670 
677  template<typename Test>
678  std::pair<double, double> dichotomic_search(double start_t, vector_t const& start_p, double end_t, vector_t const& end_p,
679  Test test, double resolution, double parameter_threshold) const
680  {
681  std::pair<bool, double> test_result =
682  test(start_t, end_t, *this);
683  if (!test_result.first)
684  return std::make_pair(0, 0);
685 
686  if (fabs(end_t - start_t) < parameter_threshold || test_result.second < resolution)
687  return std::make_pair(start_t, end_t);
688 
689  double middle_t = (start_t + end_t) / 2;
690  vector_t middle_p = getPoint(middle_t);
691  std::pair<double, double> result;
692 
693  result = dichotomic_search(start_t, start_p, middle_t, middle_p, test, resolution, parameter_threshold);
694  if (result.first != result.second)
695  return result;
696 
697  result = dichotomic_search(middle_t, middle_p, end_t, end_p, test, resolution, parameter_threshold);
698  if (result.first != result.second)
699  return result;
700 
701  throw std::runtime_error("cannot find a solution, but we should have. Is the provided function stable ?");
702  }
703 
704  void findSphereIntersections(vector_t const& _center, double _radius,
705  std::vector<double>& points, std::vector< std::pair<double, double> >& segments) const
706  { return findSphereIntersections(_center, _radius, points, segments, SplineBase::getGeometricResolution()); }
707 
708  void findSphereIntersections(vector_t const& _center, double _radius,
709  std::vector<double>& points, std::vector< std::pair<double, double> >& segments, double _geores) const
710  { return SplineBase::findSphereIntersections(_center.data(), _radius,
711  points, segments, _geores); }
712 
713  void findPointIntersections(vector_t const& _point,
714  std::vector<double>& _result_points,
715  std::vector< std::pair<double, double> >& _result_curves,
716  double _geores) const
717  { return SplineBase::findPointIntersections(_point.data(),
718  _result_points, _result_curves, _geores); }
719 
722  std::pair<double, bool> findOneLineIntersection(vector_t const& _pt, vector_t const& _normal) const
724 
731  std::pair<double, bool> findOneLineIntersection(vector_t const& _pt, vector_t const& _normal, double geometric_resolution) const
732  { return findOneLineIntersection(_pt, _normal, SplineBase::getStartParam(), geometric_resolution); }
733 
744  std::pair<double, bool> findOneLineIntersection(vector_t const& _pt, vector_t const& _normal, double _guess, double _geometric_resolution) const
745  { return SplineBase::findOneLineIntersection(_pt.data(), _normal.data(), _guess, _geometric_resolution); }
746 
749  std::pair<double, bool> findOneLineIntersection(vector_t const& _pt, vector_t const& _normal, vector_t const& _guess, double _geometric_resolution) const
750  {
751  std::vector<double> points;
752  std::vector< std::pair<double, double> > curves;
753  findLineIntersections(_pt, _normal, points, curves, _geometric_resolution);
754 
755  if (points.empty() && curves.empty())
756  return std::make_pair(0, false);
757 
758  for (unsigned int i = 0; i < curves.size(); ++i)
759  {
760  points.push_back(curves[i].first);
761  points.push_back(curves[i].second);
762  }
763 
764  double result_t = points[0];
765  vector_t result_p = getPoint(result_t);
766  double min_d = (result_p - _guess).norm();
767  for (unsigned int i = 1; i < points.size(); ++i)
768  {
769  vector_t p = getPoint(points[i]);
770  double d = (p - _guess).norm();
771  if (d < min_d)
772  {
773  result_t = points[i];
774  result_p = p;
775  min_d = d;
776  }
777  }
778  return std::make_pair(result_t, true);
779  }
780 
781  void findLineIntersections(vector_t const& _point, vector_t const& _normal,
782  std::vector<double>& _result_points,
783  std::vector< std::pair<double, double> >& _result_curves,
784  double _geores) const
785  {
786  return SplineBase::findLineIntersections(_point.data(), _normal.data(),
787  _result_points, _result_curves, _geores);
788  }
789 
790 
791  void findSegmentIntersections(vector_t const& _p0, vector_t const& _p1,
792  std::vector<double>& _result_points,
793  std::vector< std::pair<double, double> >& _result_curves,
794  double _geores) const
795  {
796  std::vector<double> points;
797  std::vector< std::pair<double, double> > curves;
798 
799  vector_t p0p1 = (_p1 - _p0);
800  double p0p1_length = p0p1.norm();
801  p0p1.normalize();
802 
803  vector_t normal;
804  if (p0p1.x() < p0p1.y() && p0p1.x() < p0p1.z())
805  normal = p0p1.cross(Eigen::Vector3d::UnitX());
806  else if (p0p1.y() < p0p1.x() && p0p1.y() < p0p1.z())
807  normal = p0p1.cross(Eigen::Vector3d::UnitY());
808  else if (p0p1.z() < p0p1.x() && p0p1.z() < p0p1.y())
809  normal = p0p1.cross(Eigen::Vector3d::UnitY());
810  findLineIntersections(_p0, normal, points, curves, _geores);
811 
812  for (unsigned int i = 0; i < points.size(); ++i)
813  {
814  vector_t p = getPoint(points[i]);
815  double p_t = (p - _p0).dot(p0p1);
816  if (p_t >=0 && p_t <= p0p1_length && fabs((p - _p0).dot(normal)) < _geores)
817  _result_points.push_back(points[i]);
818  }
819 
820  for (unsigned int curve_idx = 0; curve_idx < curves.size(); ++curve_idx)
821  {
822  double curve_t[2] =
823  { curves[curve_idx].first, curves[curve_idx].second };
824  vector_t p[2];
825  double segment_t[2];
826  for (int i = 0; i < 2; ++i)
827  {
828  p[i] = getPoint(curve_t[i]);
829  segment_t[i] = (p[i] - _p0).dot(p0p1);
830  }
831 
832  double* min_t = std::min_element(segment_t, segment_t + 2);
833  double* max_t = std::max_element(segment_t, segment_t + 2);
834  if (*min_t <= p0p1_length && *max_t >= 0)
835  {
836  std::pair<double, double> segment;
837  if (*min_t < 0)
838  segment.first = findOneClosestPoint(_p0);
839  else
840  segment.first = curve_t[min_t - segment_t];
841 
842  if (*max_t > p0p1_length)
843  segment.second = findOneClosestPoint(_p1);
844  else
845  segment.second = curve_t[max_t - segment_t];
846 
847  if (segment.first > segment.second)
848  std::swap(segment.first, segment.second);
849  _result_curves.push_back(segment);
850  }
851  }
852  }
853 
855  bool isIntersectingSegment(vector_t const& _p0, vector_t const& _p1, double _geores)
856  {
857  std::vector<double> points;
858  std::vector< std::pair<double, double> > curves;
859  findSegmentIntersections(_p0, _p1, points, curves, _geores);
860  return !points.empty() || !curves.empty();
861  }
862 
865  double findOneClosestPoint(vector_t const& _pt) const
867 
868  bool isCloser(const vector_t &p, const double &squaredDist, const double param, vector_t &pOfParam, double &squaredDistOfParam) const
869  {
870  vector_t curPoint = getPoint(param);
871  double curSquaredDist = (curPoint - p).squaredNorm();
872  if( curSquaredDist < squaredDist )
873  {
874  pOfParam = curPoint;
875  squaredDistOfParam = curSquaredDist;
876  return true;
877  }
878 
879  return false;
880  }
881 
886  double findOneClosestPoint(vector_t const& _pt, double _geometric_resolution) const
887  {
889  return SplineBase::getStartParam();
890 
891  std::vector<double> points;
892  std::vector< std::pair<double, double> > curves;
893  findClosestPoints(_pt, points, curves, _geometric_resolution);
894 
895  vector_t closestPoint;
896  double closestParam;
897  double closestSquaredDist = std::numeric_limits< double >::max();
898  if (points.empty())
899  {
900  if (curves.empty())
901  throw std::logic_error("no closest point returned by findClosestPoints");
902  else
903  {
904  closestPoint = getPoint(curves.front().first);
905  closestParam = curves.front().first;
906  closestSquaredDist = (_pt - closestPoint).squaredNorm();
907  }
908  }
909  else
910  {
911  closestPoint = getPoint(points.front());
912  closestParam = points.front();
913  closestSquaredDist = (_pt - closestPoint).squaredNorm();
914  for(std::vector<double>::iterator it = points.begin() + 1; it != points.end(); ++it)
915  {
916  if(isCloser(_pt, closestSquaredDist, *it, closestPoint, closestSquaredDist))
917  closestParam = *it;
918  }
919  }
920 
921  for (std::vector< std::pair<double, double> >::const_iterator it = curves.begin();
922  it != curves.end(); ++it)
923  {
924  if(isCloser(_pt, closestSquaredDist, it->first, closestPoint, closestSquaredDist))
925  closestParam = it->first;
926 
927  if(isCloser(_pt, closestSquaredDist, it->second, closestPoint, closestSquaredDist))
928  closestParam = it->second;
929  }
930  return closestParam;
931  }
932 
943  double findOneClosestPoint(vector_t const& _pt, double _guess, double _geometric_resolution) const
944  { return findOneClosestPoint(_pt, _geometric_resolution); }
945 
948  void findClosestPoints(vector_t const& _pt,
949  std::vector<double>& _points,
950  std::vector< std::pair<double, double> >& _curves) const
951  { return findClosestPoints(_pt, _points, _curves, SplineBase::getGeometricResolution()); }
952 
958  void findClosestPoints(vector_t const& _pt,
959  std::vector<double>& _points,
960  std::vector< std::pair<double, double> >& _curves,
961  double _geores) const
962  { return SplineBase::findClosestPoints(_pt.data(), _points, _curves, _geores); }
963 
966  double localClosestPointSearch(vector_t const& _pt, double _guess, double _start, double _end)
967  { return localClosestPointSearch(_pt, _guess, _start, _end, SplineBase::getGeometricResolution()); }
968 
973  double localClosestPointSearch(vector_t const& _pt, double _guess, double _start, double _end, double _geores)
974  { return SplineBase::localClosestPointSearch(_pt.data(), _guess, _start, _end, _geores); }
975 
976  template<typename Transform>
977  void transform(Transform const& t)
978  {
979  if (SplineBase::isEmpty())
980  return;
981  else if (SplineBase::isSingleton())
982  {
983  vector_t v = getPoint(0);
984  v = t * v;
985  std::vector<double> coordinates(v.data(), v.data() + DIM);
986  SplineBase::reset(coordinates, std::vector<double>());
987  }
988  else
989  {
990  std::vector<double> const& current_coordinates = SplineBase::getCoordinates();
991  std::vector<double> coordinates(current_coordinates.begin(), current_coordinates.end());
992  int stride = SplineBase::getCoordinatesStride();
993 
994  vector_t v;
995  for (unsigned int i = 0; i < current_coordinates.size(); i += stride)
996  {
997  memcpy(v.data(), &coordinates[i], sizeof(double) * DIM);
998  v = t * v;
999  memcpy(&coordinates[i], v.data(), sizeof(double) * DIM);
1000  }
1001  SplineBase::reset(coordinates, SplineBase::getKnots());
1002  }
1003  }
1004  };
1005 
1006  // This is for GCCXML parsing
1011  };
1015 
1016  inline std::ostream& operator << (std::ostream& io, base::geometry::Spline<3> const& s)
1017  {
1018  io << "Length " << s.getCurveLength()
1019  << " start " << s.getStartPoint().transpose() << " end " << s.getEndPoint().transpose()
1020  << " startParam " << s.getStartParam() << " endParam " << s.getEndParam();
1021 
1022  return io;
1023  }
1024 
1025 } // geometry
1026 } // base
1027 #endif
int getCoordinatesStride() const
Definition: Spline.hpp:216
void interpolate(std::vector< vector_ta > const &points, std::vector< double > &parametersOut, std::vector< double > const &parametersIn=std::vector< double >(), std::vector< SplineBase::CoordinateType > const &coord_types=std::vector< SplineBase::CoordinateType >())
Definition: Spline.hpp:615
bool isCloser(const vector_t &p, const double &squaredDist, const double param, vector_t &pOfParam, double &squaredDistOfParam) const
Definition: Spline.hpp:868
std::pair< double, bool > findOneLineIntersection(double const *_point, double const *_normal, double _guess, double _geores) const
Definition: Spline.cpp:600
~SplineBase()
Definition: Spline.cpp:54
void crop(double start_t, double end_t)
Definition: Spline.cpp:1059
int getDimension() const
Definition: Spline.hpp:43
void findClosestPoints(double const *ref_point, std::vector< double > &_result_points, std::vector< std::pair< double, double > > &_result_curves, double _geores) const
Definition: Spline.cpp:513
void findLineIntersections(vector_t const &_point, vector_t const &_normal, std::vector< double > &_result_points, std::vector< std::pair< double, double > > &_result_curves, double _geores) const
Definition: Spline.hpp:781
double getStartParam() const
Definition: Spline.hpp:65
SplineBase(SplineBase const &source)
Definition: Spline.cpp:61
double localClosestPointSearch(double const *ref_point, double _guess, double _start, double _end, double _geores) const
Definition: Spline.cpp:541
Definition: Spline.hpp:381
void findSphereIntersections(vector_t const &_center, double _radius, std::vector< double > &points, std::vector< std::pair< double, double > > &segments) const
Definition: Spline.hpp:704
void findSegmentIntersections(vector_t const &_p0, vector_t const &_p1, std::vector< double > &_result_points, std::vector< std::pair< double, double > > &_result_curves, double _geores) const
Definition: Spline.hpp:791
Spline3Base(SplineBase const &source)
Definition: Spline.hpp:342
double getResultClosestToGuess(double _guess, std::vector< double > points, std::vector< std::pair< double, double > > curves) const
Definition: Spline.cpp:476
bool isEmpty() const
Definition: Spline.hpp:35
void findLineIntersections(double const *_point, double const *_normal, std::vector< double > &_result_points, std::vector< std::pair< double, double > > &_result_curves, double _geores) const
Definition: Spline.cpp:614
void findSphereIntersections(vector_t const &_center, double _radius, std::vector< double > &points, std::vector< std::pair< double, double > > &segments, double _geores) const
Definition: Spline.hpp:708
double getCurveLength(double relative_resolution=0.01) const
Definition: Spline.cpp:190
double distanceError(base::Vector3d _pt, double _param)
available only in Spline<3>
Definition: Spline.cpp:1022
double distanceTo(vector_t const &_pt) const
Definition: Spline.hpp:655
Spline3Base(int dimension, double geometric_resolution, int order)
Definition: Spline.hpp:335
void findClosestPoints(vector_t const &_pt, std::vector< double > &_points, std::vector< std::pair< double, double > > &_curves, double _geores) const
Definition: Spline.hpp:958
void getPointAndTangent(double *result, double _param) const
Definition: Spline.cpp:102
double getHeading(double _param)
available only in Spline<3>
Definition: Spline.cpp:1004
Spline3Base type
Definition: Spline.hpp:384
void getPoint(double *result, double _param) const
Definition: Spline.cpp:99
Eigen::Matrix< double, DIM, 1, Eigen::DontAlign > vector_t
Definition: Spline.hpp:391
vector_t getEndPoint() const
Definition: Spline.hpp:411
void findPointIntersections(double const *_point, std::vector< double > &_result_points, std::vector< std::pair< double, double > > &_result_curves, double _geores) const
Definition: Spline.cpp:567
vector_t getPoint(double _param) const
Definition: Spline.hpp:422
SISLCurve const * getSISLCurve() const
Definition: Spline.cpp:975
bool checkAndNormalizeParam(double &param, double equalDistance=0.001) const
Definition: Spline.cpp:105
double findOneClosestPoint(vector_t const &_pt, double _geometric_resolution) const
Definition: Spline.hpp:886
std::pair< double, bool > findOneLineIntersection(vector_t const &_pt, vector_t const &_normal, double _guess, double _geometric_resolution) const
Definition: Spline.hpp:744
Spline3Base(double geometric_resolution, SISLCurve *curve)
Definition: Spline.hpp:340
SplineBaseClass< DIM >::type base_t
Definition: Spline.hpp:390
SplineBase const & operator=(SplineBase const &base)
Definition: Spline.cpp:71
Spline derive(int order) const
Definition: Spline.hpp:438
Definition: Spline.hpp:329
void setCurveOrder(int value)
Definition: Spline.hpp:49
Spline(SplineBase const &base)
Definition: Spline.hpp:403
std::pair< double, bool > findOneLineIntersection(vector_t const &_pt, vector_t const &_normal, vector_t const &_guess, double _geometric_resolution) const
Definition: Spline.hpp:749
base::geometry::Spline< 2 > instanciation2
Definition: Spline.hpp:1009
SplineBase type
Definition: Spline.hpp:382
void reset(std::vector< double > const &coordinates, std::vector< double > const &knots, int kind=-1)
Definition: Spline.cpp:424
void reverse()
Definition: Spline.cpp:912
void getPointAndTangentHelper(double *result, double _param, bool with_tangent) const
Definition: Spline.cpp:120
std::vector< vector_t > getPoints(std::vector< double > const &parameters) const
Definition: Spline.hpp:429
std::pair< vector_t, vector_t > getPointAndTangent(double _param) const
Definition: Spline.hpp:539
void derive(unsigned int order, SplineBase &result) const
Definition: Spline.cpp:1160
double findOneClosestPoint(vector_t const &_pt, double _guess, double _geometric_resolution) const
Definition: Spline.hpp:943
std::pair< double, double > dichotomic_search(double start_t, vector_t const &start_p, double end_t, vector_t const &end_p, Test test, double resolution, double parameter_threshold) const
Definition: Spline.hpp:678
double headingError(double _actHeading, double _param)
available only in Spline<3>
Definition: Spline.cpp:1016
double getHeading(double _param)
Definition: Spline.hpp:348
Spline(double geometric_resolution=0.1, int order=3)
Definition: Spline.hpp:395
void interpolate(std::vector< double > const &coordinates, std::vector< double > const &parameters=std::vector< double >(), std::vector< SplineBase::CoordinateType > const &coord_types=std::vector< SplineBase::CoordinateType >())
Definition: Spline.hpp:637
std::pair< double, double > dichotomic_search(double start_t, double end_t, Test test, double resolution, double parameter_threshold) const
Definition: Spline.hpp:663
int getSISLCurveType() const
Definition: Spline.cpp:400
std::vector< double > getCoordinates() const
Definition: Spline.cpp:380
vector_t getStartPoint() const
Definition: Spline.hpp:407
double join(SplineBase const &other, double tolerance, bool with_tangents)
Definition: Spline.cpp:754
std::pair< double, bool > findOneLineIntersection(vector_t const &_pt, vector_t const &_normal) const
Definition: Spline.hpp:722
base::geometry::Spline< 2 > Spline2
Definition: Spline.hpp:1013
std::pair< double, bool > findOneLineIntersection(vector_t const &_pt, vector_t const &_normal, double geometric_resolution) const
Definition: Spline.hpp:731
void transform(Transform const &t)
Definition: Spline.hpp:977
std::pair< double, double > advance(double t, double length, double _geores) const
Definition: Spline.hpp:506
double findOneClosestPoint(vector_t const &_pt) const
Definition: Spline.hpp:865
double localClosestPointSearch(vector_t const &_pt, double _guess, double _start, double _end, double _geores)
Definition: Spline.hpp:973
void findSphereIntersections(double const *_center, double radius, std::vector< double > &points, std::vector< std::pair< double, double > > &segments, double _geores) const
Definition: Spline.cpp:654
bool isIntersectingSegment(vector_t const &_p0, vector_t const &_p1, double _geores)
Definition: Spline.hpp:855
void setSingleton(vector_t const &v)
Definition: Spline.hpp:415
Definition: Spline.hpp:20
int getPointCount() const
Definition: Spline.cpp:91
void append(SplineBase const &other)
Definition: Spline.cpp:700
base::geometry::Spline< 1 > instanciation1
Definition: Spline.hpp:1008
void sample(std::vector< vector_t > &result, double _geores, std::vector< double > *parameters=0, int max_recursion=20) const
Definition: Spline.hpp:561
Eigen::Transform< double, DIM, Eigen::Affine > transform_t
Definition: Spline.hpp:393
void split(SplineBase &second_part, double parameter)
Definition: Spline.cpp:1092
bool isSingleton() const
Definition: Spline.hpp:39
SplineBase * getSubSpline(double start_t, double end_t) const
Definition: Spline.cpp:1148
base::geometry::Spline< 1 > Spline1
Definition: Spline.hpp:1012
Eigen::Matrix< double, DIM, 1, Eigen::AutoAlign > vector_ta
Definition: Spline.hpp:392
base::Matrix3d getFrenetFrame(double _param)
available only in Spline<3>
Definition: Spline.cpp:985
Definition: LinearAngular6DCommand.hpp:8
base::Vector3d poseError(base::Vector3d const &_position, double _heading, double _guess)
Definition: Spline.hpp:364
void findPointIntersections(vector_t const &_point, std::vector< double > &_result_points, std::vector< std::pair< double, double > > &_result_curves, double _geores) const
Definition: Spline.hpp:713
base::Vector3d poseError(base::Vector3d const &_position, double _heading, double _guess, double minParam)
Definition: Spline.hpp:377
bool doAdvance(double &result, double &cur_length, double target, double start, vector_t const &start_p, double end, vector_t const &end_p, double _geores) const
Definition: Spline.hpp:460
double length(double start, double end, double _geores) const
Definition: Spline.hpp:529
base::geometry::Spline< 3 > instanciation3
Definition: Spline.hpp:1010
base::Matrix3d getFrenetFrame(double _param)
Definition: Spline.hpp:345
T infinity()
Definition: Float.hpp:14
void sample(std::vector< vector_t > &result, double start, vector_t const &start_p, double end, vector_t const &end_p, double _geores, std::vector< double > *parameters, int max_recursion=20) const
Definition: Spline.hpp:576
bool testIntersection(SplineBase const &other, double resolution=0.01) const
Definition: Spline.cpp:918
int getCurveOrder() const
Definition: Spline.hpp:51
Eigen::Matrix< double, 3, 1, Eigen::DontAlign > Vector3d
Definition: Eigen.hpp:20
void interpolate(std::vector< double > const &coordinates, std::vector< double > const &parameters=std::vector< double >(), std::vector< CoordinateType > const &coord_types=std::vector< CoordinateType >())
Definition: Spline.cpp:361
double getGeometricResolution() const
Definition: Spline.hpp:32
double getEndParam() const
Definition: Spline.hpp:66
void interpolate(std::vector< vector_t > const &points, std::vector< double > const &parameters=std::vector< double >(), std::vector< SplineBase::CoordinateType > const &coord_types=std::vector< SplineBase::CoordinateType >())
Definition: Spline.hpp:594
void interpolate(std::vector< vector_ta > const &points, std::vector< double > const &parameters=std::vector< double >(), std::vector< SplineBase::CoordinateType > const &coord_types=std::vector< SplineBase::CoordinateType >())
Definition: Spline.hpp:627
CoordinateType
Definition: Spline.hpp:109
double localClosestPointSearch(vector_t const &_pt, double _guess, double _start, double _end)
Definition: Spline.hpp:966
void setSingleton(double const *coordinates)
Definition: Spline.cpp:1083
bool isNURBS() const
Definition: Spline.cpp:256
Definition: Spline.hpp:387
void printCurveProperties(std::ostream &io)
Definition: Spline.cpp:369
void clear()
Definition: Spline.cpp:902
double findOneClosestPoint(double const *_pt, double _guess, double _geores) const
Definition: Spline.cpp:464
void findClosestPoints(vector_t const &_pt, std::vector< double > &_points, std::vector< std::pair< double, double > > &_curves) const
Definition: Spline.hpp:948
Eigen::Matrix< double, 3, 3, Eigen::DontAlign > Matrix3d
Definition: Eigen.hpp:27
double getCurvature(double _param) const
Definition: Spline.cpp:153
base::Vector3d poseError(base::Vector3d _pt, double _actZRot, double _st_para)
available only in Spline<3>
Definition: Spline.cpp:1051
double getCurvatureMax()
Definition: Spline.cpp:227
base::geometry::Spline< 3 > Spline3
Definition: Spline.hpp:1014
double getVariationOfCurvature(double _param)
Definition: Spline.cpp:172
std::vector< vector_t > sample(double _geores, std::vector< double > *parameters=0, int max_recursion=20) const
Definition: Spline.hpp:551
std::vector< double > getKnots() const
Definition: Spline.cpp:392
Spline(double geometric_resolution, SISLCurve *curve)
Definition: Spline.hpp:397
double distanceError(base::Vector3d _pt, double _param)
Definition: Spline.hpp:354
double headingError(double _actZRot, double _param)
Definition: Spline.hpp:351
void setGeometricResolution(double _geores)
Definition: Spline.hpp:31
void interpolate(std::vector< vector_t > const &points, std::vector< double > &parametersOut, std::vector< double > const &parametersIn=std::vector< double >(), std::vector< SplineBase::CoordinateType > const &coord_types=std::vector< SplineBase::CoordinateType >())
Definition: Spline.hpp:604
std::vector< double > simplify()
Definition: Spline.cpp:942
void interpolate(std::vector< double > const &coordinates, std::vector< double > &parameterOut, std::vector< double > const &parameterIn=std::vector< double >(), std::vector< SplineBase::CoordinateType > const &coord_types=std::vector< SplineBase::CoordinateType >())
Definition: Spline.hpp:644