1 #ifndef _BASE_SPLINE_HPP_INC 2 #define _BASE_SPLINE_HPP_INC 5 #include <base/Eigen.hpp> 27 double geometric_resolution = 0.1,
int order = 3);
28 explicit SplineBase(
double geometric_resolution, SISLCurve* curve);
40 {
return !singleton.empty(); }
59 double getCurveLength(
double startParam,
double relative_resolution)
const;
60 double getCurveLength(
double startParam,
double endParam,
double relative_resolution)
const;
99 std::vector<double>
getKnots()
const;
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>() );
138 void interpolate(std::vector<double>
const& coordinates,
139 std::vector<double> ¶meterOut,
140 std::vector<double>
const& parameterIn = std::vector<double>(),
141 std::vector<CoordinateType>
const& coord_types = std::vector<CoordinateType>() );
151 std::vector<double>
simplify(
double tolerance);
162 void reset(std::vector<double>
const& coordinates, std::vector<double>
const& knots,
int kind = -1);
194 double join(
SplineBase const& other,
double tolerance,
bool with_tangents);
208 void crop(
double start_t,
double end_t);
218 if (
isNURBS())
return dimension + 1;
219 else return dimension;
246 void reset(SISLCurve* curve);
247 void getPoint(
double* result,
double _param)
const;
251 std::vector<double>& _result_points,
252 std::vector< std::pair<double, double> >& _result_curves,
253 double _geores)
const;
256 double const* _normal,
double _guess,
double _geores)
const;
258 std::vector<double>& _result_points,
259 std::vector< std::pair<double, double> >& _result_curves,
260 double _geores)
const;
263 std::vector<double>& points,
264 std::vector< std::pair<double, double> >& segments,
double _geores)
const;
272 std::vector<double>& _result_points,
273 std::vector< std::pair<double, double> >& _result_curves,
274 double _geores)
const;
277 double _guess,
double _start,
double _end,
278 double _geores)
const;
290 std::vector< std::pair<double, double> > curves)
const;
305 std::vector<double> singleton;
312 double geometric_resolution;
321 bool has_curvature_max;
323 double curvature_max;
335 explicit Spline3Base(
int dimension,
double geometric_resolution,
int order)
336 :
SplineBase(3, geometric_resolution, order) {}
340 explicit Spline3Base(
double geometric_resolution, SISLCurve* curve)
391 typedef Eigen::Matrix<double, DIM, 1, Eigen::DontAlign>
vector_t;
392 typedef Eigen::Matrix<double, DIM, 1, Eigen::AutoAlign>
vector_ta;
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)
401 throw std::runtime_error(
"trying to initialize a Spline<> class with a curve of wrong dimension");
429 std::vector<vector_t>
getPoints(std::vector<double>
const& parameters)
const 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]));
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 462 double d = (start_p - end_p).norm();
468 if (cur_length < target)
477 if (cur_length > target)
487 double middle = (start + end) / 2;
488 vector_t middle_p =
getPoint(middle);
490 if (doAdvance(result, cur_length, target, start, start_p, middle, middle_p, _geores))
492 if (doAdvance(result, cur_length, target, middle, middle_p, end, end_p, _geores))
506 std::pair<double, double>
advance(
double t,
double length,
double _geores)
const 518 return std::make_pair(this->
getEndParam(), result_d);
520 return std::make_pair(result_t, result_d);
529 double length(
double start,
double end,
double _geores)
const 541 double result[DIM * 2];
543 vector_t point(result);
544 vector_t tangent(result + DIM);
545 return std::make_pair(point, tangent);
551 std::vector<vector_t>
sample(
double _geores, std::vector<double>* parameters = 0,
int max_recursion = 20)
const 553 std::vector<vector_t> result;
554 sample(result, _geores, parameters, max_recursion);
561 void sample(std::vector<vector_t>& result,
double _geores, std::vector<double>* parameters = 0,
int max_recursion = 20)
const 564 vector_t start_p = this->
getPoint(start);
566 parameters->push_back(start);
567 result.push_back(start_p);
570 vector_t end_p = this->
getPoint(end);
571 sample(result, start, start_p, end, end_p, _geores, parameters, max_recursion);
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 578 if (max_recursion == 0 || (start_p - end_p).norm() < _geores)
581 parameters->push_back(end);
583 result.push_back(end_p);
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);
595 std::vector<double>
const& parameters = std::vector<double>(),
596 std::vector<SplineBase::CoordinateType>
const& coord_types = std::vector<SplineBase::CoordinateType>() )
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);
605 std::vector<double> ¶metersOut,
606 std::vector<double>
const& parametersIn = std::vector<double>(),
607 std::vector<SplineBase::CoordinateType>
const& coord_types = std::vector<SplineBase::CoordinateType>() )
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);
616 std::vector<double> ¶metersOut,
617 std::vector<double>
const& parametersIn = std::vector<double>(),
618 std::vector<SplineBase::CoordinateType>
const& coord_types = std::vector<SplineBase::CoordinateType>() )
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);
628 std::vector<double>
const& parameters = std::vector<double>(),
629 std::vector<SplineBase::CoordinateType>
const& coord_types = std::vector<SplineBase::CoordinateType>() )
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);
638 std::vector<double>
const& parameters = std::vector<double>(),
639 std::vector<SplineBase::CoordinateType>
const& coord_types = std::vector<SplineBase::CoordinateType>() )
645 std::vector<double> ¶meterOut,
646 std::vector<double>
const& parameterIn = std::vector<double>(),
647 std::vector<SplineBase::CoordinateType>
const& coord_types = std::vector<SplineBase::CoordinateType>() )
658 vector_t curve_p =
getPoint(closest);
659 return (_pt - curve_p).norm();
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 665 return this->dichotomic_search(
668 test, resolution, parameter_threshold);
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 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);
686 if (fabs(end_t - start_t) < parameter_threshold || test_result.second < resolution)
687 return std::make_pair(start_t, end_t);
689 double middle_t = (start_t + end_t) / 2;
690 vector_t middle_p =
getPoint(middle_t);
691 std::pair<double, double> result;
693 result = dichotomic_search(start_t, start_p, middle_t, middle_p, test, resolution, parameter_threshold);
694 if (result.first != result.second)
697 result = dichotomic_search(middle_t, middle_p, end_t, end_p, test, resolution, parameter_threshold);
698 if (result.first != result.second)
701 throw std::runtime_error(
"cannot find a solution, but we should have. Is the provided function stable ?");
705 std::vector<double>& points, std::vector< std::pair<double, double> >& segments)
const 709 std::vector<double>& points, std::vector< std::pair<double, double> >& segments,
double _geores)
const 711 points, segments, _geores); }
714 std::vector<double>& _result_points,
715 std::vector< std::pair<double, double> >& _result_curves,
716 double _geores)
const 718 _result_points, _result_curves, _geores); }
731 std::pair<double, bool>
findOneLineIntersection(vector_t
const& _pt, vector_t
const& _normal,
double geometric_resolution)
const 744 std::pair<double, bool>
findOneLineIntersection(vector_t
const& _pt, vector_t
const& _normal,
double _guess,
double _geometric_resolution)
const 749 std::pair<double, bool>
findOneLineIntersection(vector_t
const& _pt, vector_t
const& _normal, vector_t
const& _guess,
double _geometric_resolution)
const 751 std::vector<double> points;
752 std::vector< std::pair<double, double> > curves;
755 if (points.empty() && curves.empty())
756 return std::make_pair(0,
false);
758 for (
unsigned int i = 0; i < curves.size(); ++i)
760 points.push_back(curves[i].first);
761 points.push_back(curves[i].second);
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)
770 double d = (p - _guess).norm();
773 result_t = points[i];
778 return std::make_pair(result_t,
true);
782 std::vector<double>& _result_points,
783 std::vector< std::pair<double, double> >& _result_curves,
784 double _geores)
const 787 _result_points, _result_curves, _geores);
792 std::vector<double>& _result_points,
793 std::vector< std::pair<double, double> >& _result_curves,
794 double _geores)
const 796 std::vector<double> points;
797 std::vector< std::pair<double, double> > curves;
799 vector_t p0p1 = (_p1 - _p0);
800 double p0p1_length = p0p1.norm();
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());
812 for (
unsigned int i = 0; i < points.size(); ++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]);
820 for (
unsigned int curve_idx = 0; curve_idx < curves.size(); ++curve_idx)
823 { curves[curve_idx].first, curves[curve_idx].second };
826 for (
int i = 0; i < 2; ++i)
829 segment_t[i] = (p[i] - _p0).dot(p0p1);
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)
836 std::pair<double, double> segment;
840 segment.first = curve_t[min_t - segment_t];
842 if (*max_t > p0p1_length)
845 segment.second = curve_t[max_t - segment_t];
847 if (segment.first > segment.second)
848 std::swap(segment.first, segment.second);
849 _result_curves.push_back(segment);
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();
868 bool isCloser(
const vector_t &p,
const double &squaredDist,
const double param, vector_t &pOfParam,
double &squaredDistOfParam)
const 870 vector_t curPoint =
getPoint(param);
871 double curSquaredDist = (curPoint - p).squaredNorm();
872 if( curSquaredDist < squaredDist )
875 squaredDistOfParam = curSquaredDist;
891 std::vector<double> points;
892 std::vector< std::pair<double, double> > curves;
895 vector_t closestPoint;
897 double closestSquaredDist = std::numeric_limits< double >::max();
901 throw std::logic_error(
"no closest point returned by findClosestPoints");
904 closestPoint =
getPoint(curves.front().first);
905 closestParam = curves.front().first;
906 closestSquaredDist = (_pt - closestPoint).squaredNorm();
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)
916 if(isCloser(_pt, closestSquaredDist, *it, closestPoint, closestSquaredDist))
921 for (std::vector< std::pair<double, double> >::const_iterator it = curves.begin();
922 it != curves.end(); ++it)
924 if(isCloser(_pt, closestSquaredDist, it->first, closestPoint, closestSquaredDist))
925 closestParam = it->first;
927 if(isCloser(_pt, closestSquaredDist, it->second, closestPoint, closestSquaredDist))
928 closestParam = it->second;
949 std::vector<double>& _points,
950 std::vector< std::pair<double, double> >& _curves)
const 959 std::vector<double>& _points,
960 std::vector< std::pair<double, double> >& _curves,
961 double _geores)
const 976 template<
typename Transform>
985 std::vector<double> coordinates(v.data(), v.data() + DIM);
991 std::vector<double> coordinates(current_coordinates.begin(), current_coordinates.end());
995 for (
unsigned int i = 0; i < current_coordinates.size(); i += stride)
997 memcpy(v.data(), &coordinates[i],
sizeof(double) * DIM);
999 memcpy(&coordinates[i], v.data(),
sizeof(double) * DIM);
1016 inline std::ostream& operator << (std::ostream& io, base::geometry::Spline<3>
const& s)
1019 <<
" start " << s.getStartPoint().transpose() <<
" end " << s.getEndPoint().transpose()
1020 <<
" startParam " << s.getStartParam() <<
" endParam " << s.getEndParam();
int getCoordinatesStride() const
Definition: Spline.hpp:216
void interpolate(std::vector< vector_ta > const &points, std::vector< double > ¶metersOut, std::vector< double > const ¶metersIn=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
Definition: Spline.hpp:114
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
Definition: Spline.hpp:118
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 ¶m, 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 ¶meters) const
Definition: Spline.hpp:429
std::pair< vector_t, vector_t > getPointAndTangent(double _param) const
Definition: Spline.hpp:539
Definition: Spline.hpp:117
Definition: Spline.hpp:113
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 ¶meters=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
Definition: Spline.hpp:112
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
Definition: Spline.hpp:111
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
Definition: Spline.hpp:1007
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 ¶meters=std::vector< double >(), std::vector< CoordinateType > const &coord_types=std::vector< CoordinateType >())
Definition: Spline.cpp:361
double getGeometricResolution() const
Definition: Spline.hpp:32
Definition: Spline.hpp:116
double getEndParam() const
Definition: Spline.hpp:66
void interpolate(std::vector< vector_t > const &points, std::vector< double > const ¶meters=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 ¶meters=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
Definition: Spline.hpp:115
void interpolate(std::vector< vector_t > const &points, std::vector< double > ¶metersOut, std::vector< double > const ¶metersIn=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 > ¶meterOut, std::vector< double > const ¶meterIn=std::vector< double >(), std::vector< SplineBase::CoordinateType > const &coord_types=std::vector< SplineBase::CoordinateType >())
Definition: Spline.hpp:644