8 #ifndef _CLASS_CUBICHERMITESPLINE 9 #define _CLASS_CUBICHERMITESPLINE 22 #include <boost/serialization/utility.hpp> 34 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
35 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
59 template <
typename In>
61 :
size_(std::distance(PairsBegin, PairsEnd)),
degree_(3) {
63 if (Safe &&
size_ < 1) {
64 throw std::length_error(
"can not create cubic_hermite_spline, number of pairs is inferior to 2.");
67 dim_ = PairsBegin->first.size();
70 for (; it != PairsEnd; ++it) {
71 if(Safe && (static_cast<size_t>(it->first.size()) !=
dim_ || static_cast<size_t>(it->second.size()) !=
dim_))
72 throw std::invalid_argument(
"All the control points and their derivatives must have the same dimension.");
101 throw std::invalid_argument(
"can't evaluate cubic hermite spline, out of range");
106 const bezier_t bezier = buildCurrentBezier(t);
120 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
121 bool equal = ndcurves::isApprox<num_t>(
T_min_, other.
min()) && ndcurves::isApprox<num_t>(
T_max_, other.
max()) &&
124 if (!equal)
return false;
125 for (std::size_t i = 0; i <
size_; ++i) {
134 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
135 const cubic_hermite_spline_t* other_cast =
dynamic_cast<const cubic_hermite_spline_t*
>(other);
144 virtual bool operator!=(
const cubic_hermite_spline_t& other)
const {
return !(*
this == other); }
154 throw std::invalid_argument(
"can't derivate cubic hermite spline, out of range");
159 const bezier_t bezier = buildCurrentBezier(t);
165 piecewise_bezier_t res;
166 for(
size_t i = 0 ; i <
size_ - 1 ; ++i){
185 void setTime(
const vector_time_t& time_control_points) {
189 if (time_control_points.size() !=
size()) {
190 throw std::length_error(
"size of time control points should be equal to number of control points");
192 computeDurationSplines();
193 if (!checkDurationSplines()) {
194 throw std::invalid_argument(
"time_splines not monotonous, all spline duration should be superior to 0");
223 std::size_t findInterval(
const time_t t)
const {
232 std::size_t left_id = 0;
233 std::size_t right_id =
size_ - 1;
234 while (left_id <= right_id) {
235 const std::size_t middle_id = left_id + (right_id - left_id) / 2;
237 left_id = middle_id + 1;
239 right_id = middle_id - 1;
253 bezier_t buildCurrentBezier(
const time_t t)
const{
254 size_t id_interval = findInterval(t);
256 const pair_point_tangent_t pair1 =
control_points_.at(id_interval + 1);
259 t_point_t control_points;
260 control_points.reserve(4);
261 control_points.push_back(pair0.first);
262 control_points.push_back(pair0.first + pair0.second / 3. * (t1 - t0));
263 control_points.push_back(pair1.first - pair1.second / 3. * (t1 - t0));
264 control_points.push_back(pair1.first);
265 return bezier_t(control_points.begin(), control_points.end(), t0, t1);
270 void check_conditions()
const {
272 throw std::runtime_error(
273 "Error in cubic hermite : there is no control points set / did you use empty constructor ?");
274 }
else if (
dim_ == 0) {
275 throw std::runtime_error(
276 "Error in cubic hermite : Dimension of points is zero / did you use empty constructor ?");
284 void computeDurationSplines() {
289 for (i = 0; i <
size() - 1; i++) {
292 prev_time = actual_time;
299 bool checkDurationSplines()
const {
301 bool is_positive =
true;
314 std::size_t
virtual dim()
const {
return dim_; }
353 template <
class Archive>
354 void serialize(Archive& ar,
const unsigned int version) {
358 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
359 ar& boost::serialization::make_nvp(
"dim", dim_);
360 ar& boost::serialization::make_nvp(
"control_points", control_points_);
361 ar& boost::serialization::make_nvp(
"time_control_points", time_control_points_);
362 ar& boost::serialization::make_nvp(
"duration_splines", duration_splines_);
363 ar& boost::serialization::make_nvp(
"T_min", T_min_);
364 ar& boost::serialization::make_nvp(
"T_max", T_max_);
365 ar& boost::serialization::make_nvp(
"size", size_);
366 ar& boost::serialization::make_nvp(
"degree", degree_);
371 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point),
373 #endif //_CLASS_CUBICHERMITESPLINE Definition: bernstein.h:20
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: cubic_hermite_spline.h:133
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluate the derivative order N of curve at time t. If derivative is to be evaluated several times...
Definition: bezier_curve.h:275
t_pair_point_tangent_t control_points_
Vector of pair < Point, Tangent >.
Definition: cubic_hermite_spline.h:330
bezier_curve_t compute_derivate(const std::size_t order) const
Compute the derived curve at order N. Computes the derivative order N, of bezier curve of parametric...
Definition: bezier_curve.h:184
Numeric num_t
Definition: cubic_hermite_spline.h:42
vector_time_t duration_splines_
Definition: cubic_hermite_spline.h:339
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: cubic_hermite_spline.h:320
friend class boost::serialization::access
Definition: cubic_hermite_spline.h:351
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: cubic_hermite_spline.h:317
Definition: cubic_hermite_spline.h:36
Time T_max_
Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
Definition: cubic_hermite_spline.h:343
bezier_curve< Time, Numeric, Safe, point_t > bezier_t
Definition: cubic_hermite_spline.h:45
virtual Point derivate(const time_t t, const std::size_t order) const
Evaluate the derivative of order N of spline at time t.
Definition: cubic_hermite_spline.h:151
cubic_hermite_spline< Time, Numeric, Safe, point_t > cubic_hermite_spline_t
Definition: cubic_hermite_spline.h:44
std::size_t size() const
Get number of control points contained in the trajectory.
Definition: cubic_hermite_spline.h:211
interface for a Curve of arbitrary dimension.
Time time_t
Definition: cubic_hermite_spline.h:41
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_t > piecewise_bezier_t
Definition: cubic_hermite_spline.h:47
t_pair_point_tangent_t getControlPoints()
Get vector of pair (positition, derivative) corresponding to control points.
Definition: cubic_hermite_spline.h:201
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:40
std::vector< Time > vector_time_t
Definition: cubic_hermite_spline.h:40
virtual std::size_t dim() const
Get dimension of curve.
Definition: cubic_hermite_spline.h:314
virtual std::size_t degree() const
Get the degree of the curve.
Definition: cubic_hermite_spline.h:323
std::size_t degree_
Degree (Cubic so degree 3)
Definition: cubic_hermite_spline.h:347
double Time
Definition: effector_spline.h:27
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t &time_control_points)
Constructor.
Definition: cubic_hermite_spline.h:60
piecewise_bezier_t compute_derivate(const std::size_t order) const
Definition: cubic_hermite_spline.h:164
virtual bool operator!=(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:144
Time T_min_
Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
Definition: cubic_hermite_spline.h:341
virtual ~cubic_hermite_spline()
Destructor.
Definition: cubic_hermite_spline.h:90
cubic_hermite_spline()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: cubic_hermite_spline.h:52
cubic_hermite_spline(const cubic_hermite_spline &other)
Definition: cubic_hermite_spline.h:79
std::size_t size_
Number of control points (pairs).
Definition: cubic_hermite_spline.h:345
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Definition: bezier_curve.h:33
std::size_t numIntervals() const
Get number of intervals (subsplines) contained in the trajectory.
Definition: cubic_hermite_spline.h:216
std::pair< Point, Point > pair_point_tangent_t
Definition: cubic_hermite_spline.h:38
std::vector< pair_point_tangent_t, Eigen::aligned_allocator< Point > > t_pair_point_tangent_t
Definition: cubic_hermite_spline.h:39
bool isApprox(const cubic_hermite_spline_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: cubic_hermite_spline.h:119
double Numeric
Definition: effector_spline.h:26
class allowing to create a piecewise curve.
virtual bool operator==(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:142
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:142
std::size_t dim_
Dim of curve.
Definition: cubic_hermite_spline.h:328
virtual Point operator()(const time_t t) const
Evaluation of the cubic hermite spline at time t.
Definition: cubic_hermite_spline.h:98
void setTime(const vector_time_t &time_control_points)
Set time of each control point of cubic hermite spline. Set duration of each spline, Exemple : with values corresponding to times for respectively.
Definition: cubic_hermite_spline.h:185
void serialize(Archive &ar, const unsigned int version)
Definition: cubic_hermite_spline.h:354
vector_time_t getTime()
Get vector of Time corresponding to Time for each control point.
Definition: cubic_hermite_spline.h:206
piecewise_bezier_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: cubic_hermite_spline.h:176
Point point_t
Definition: cubic_hermite_spline.h:37
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: cubic_hermite_spline.h:43
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
bezier_t::t_point_t t_point_t
Definition: cubic_hermite_spline.h:46
vector_time_t time_control_points_
Definition: cubic_hermite_spline.h:334