GCC Code Coverage Report

Directory: ./
File: include/ndcurves/sinusoidal.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 87 94 92.6%
Branches: 79 130 60.8%
Line Branch Exec Source
1 /**
2 * \file sinusoidal.h
3 * \brief class allowing to create a sinusoidal curve.
4 * \author Pierre Fernbach
5 * \version 0.4
6 * \date 29/04/2020
7 */
8
9 #ifndef _CLASS_SINUSOIDALCURVE
10 #define _CLASS_SINUSOIDALCURVE
11
12 #include <cmath>
13
14 #include "curve_abc.h"
15
16 namespace ndcurves {
17 /// \class sinusoidal.
18 /// \brief Represents a sinusoidal curve, evaluating the following equation:
19 /// p0 + amplitude * (sin(2pi/T + phi)
20 ///
21 template <typename Time = double, typename Numeric = Time, bool Safe = false,
22 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
23 struct sinusoidal : public curve_abc<Time, Numeric, Safe, Point> {
24 typedef Point point_t;
25 typedef Point point_derivate_t;
26 typedef Time time_t;
27 typedef Numeric num_t;
28 typedef sinusoidal<Time, Numeric, Safe, Point> sinusoidal_t;
29 typedef curve_abc<Time, Numeric, Safe, Point> curve_abc_t; // parent class
30
31 /* Constructors - destructors */
32 public:
33 /// \brief Empty constructor. Curve obtained this way can not perform other
34 /// class functions.
35 ///
36
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 sinusoidal() : T_min_(0), T_max_(0), dim_(0) {}
37
38 /// \brief Constructor
39 /// \param p0 : Offset of the sinusoidal
40 /// \param amplitude: Amplitude
41 /// \param T : The period
42 /// \param phi : the phase
43 /// \param T_min : lower bound of the time interval (default to 0)
44 /// \param T_max : upper bound of the time interval (default to +inf)
45 ///
46 61 sinusoidal(const Point& p0, const Point& amplitude, const time_t T,
47 const time_t phi, const time_t T_min = 0.,
48 const time_t T_max = std::numeric_limits<time_t>::max())
49 61 : p0_(p0),
50
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 61 amplitude_(amplitude),
51 61 T_(T),
52 61 phi_(std::fmod(phi, 2. * M_PI)),
53 61 T_min_(T_min),
54 61 T_max_(T_max),
55
1/2
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
 122 dim_(p0_.size()) {
56
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 59 times.
 61 if (Safe && T_min_ > T_max_) {
57
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
58 "can't create constant curve: min bound is higher than max bound");
59 }
60
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 57 times.
 59 if (T_ <= 0.)
61
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument("The period must be strictly positive");
62
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 56 times.
 57 if (static_cast<size_t>(amplitude_.size()) != dim_)
63
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 throw std::invalid_argument(
64 "The offset and the amplitude must have the same dimension");
65 71 }
66
67 /// \brief Constructor from stationary points
68 /// \param traj_time: duration to go from p_init to p_final (half a period)
69 /// \param p_init : first stationary point, either minimum or maximum
70 /// \param p_final : second stationary point, either minimum or maximum
71 /// \param T_min : lower bound of the time interval (default to 0)
72 /// \param T_max : upper bound of the time interval (default to +inf)
73 ///
74 9 sinusoidal(const time_t traj_time, const Point& p_init, const Point& p_final,
75 const time_t T_min = 0.,
76 const time_t T_max = std::numeric_limits<time_t>::max())
77 9 : T_(2. * traj_time),
78 9 phi_(M_PI / 2.),
79 9 T_min_(T_min),
80 9 T_max_(T_max),
81
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 9 dim_(p_init.size()) {
82
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
 9 if (Safe && T_min_ > T_max_) {
83
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 throw std::invalid_argument(
84 "can't create constant curve: min bound is higher than max bound");
85 }
86
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 6 times.
 8 if (T_ <= 0)
87
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument("The period must be strictly positive");
88
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 5 times.
 6 if (p_init.size() != p_final.size())
89
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 throw std::invalid_argument(
90 "The two stationary points must have the same dimension");
91
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 p0_ = (p_init + p_final) / 2.;
92
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 amplitude_ = (p_init - p_final) / 2.;
93 17 }
94
95 /// \brief Copy constructor
96 /// \param other
97 2 sinusoidal(const sinusoidal_t& other)
98 2 : p0_(other.p0_),
99
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 amplitude_(other.amplitude_),
100 2 T_(other.T_),
101 2 phi_(other.phi_),
102 2 T_min_(other.T_min_),
103 2 T_max_(other.T_max_),
104
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 dim_(other.dim_) {}
105
106 /// \brief Destructor.
107 194 virtual ~sinusoidal() {}
108 /* Constructors - destructors */
109
110 /*Operations*/
111 /// \brief Evaluation of the cubic spline at time t.
112 /// \param t : time when to evaluate the spine
113 /// \return \f$x(t)\f$, point corresponding on curve at time t.
114 188 virtual point_t operator()(const time_t t) const {
115
4/4
✓ Branch 0 taken 185 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 183 times.
 188 if (Safe && (t < T_min_ || t > T_max_)) {
116
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 throw std::invalid_argument(
117 "error in sinusoidal curve : time t to evaluate should be in range "
118 "[Tmin, Tmax] of the curve");
119 }
120
3/6
✓ Branch 2 taken 183 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 183 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 183 times.
✗ Branch 9 not taken.
 183 return p0_ + amplitude_ * sin(two_pi_f(t) + phi_);
121 }
122
123 /// \brief Evaluate the derivative of order N of curve at time t.
124 /// \param t : time when to evaluate the spline.
125 /// \param order : order of derivative.
126 /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative
127 /// curve of order N at time t.
128 438 virtual point_derivate_t derivate(const time_t t,
129 const std::size_t order) const {
130
4/4
✓ Branch 0 taken 436 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 434 times.
 438 if (Safe && (t < T_min_ || t > T_max_)) {
131
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 throw std::invalid_argument(
132 "error in constant curve : time t to derivate should be in range "
133 "[Tmin, Tmax] of the curve");
134 }
135
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 432 times.
 434 if (order <= 0)
136
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument("Order must be strictly positive");
137 ✗ return amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order)) *
138
3/6
✓ Branch 2 taken 432 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 432 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 432 times.
✗ Branch 9 not taken.
 432 sin(two_pi_f(t) + phi_ + (M_PI * static_cast<num_t>(order) / 2.));
139 }
140
141 /// \brief Compute the derived curve at order N.
142 /// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier
143 /// curve of parametric equation x(t). \param order : order of derivative.
144 /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
145 19 sinusoidal_t compute_derivate(const std::size_t order) const {
146
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 19 times.
 19 if (order <= 0)
147 ✗ throw std::invalid_argument("Order must be strictly positive");
148
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 const point_t amplitude =
149
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order));
150 19 const time_t phi = phi_ + (M_PI * static_cast<num_t>(order) / 2.);
151 19 return sinusoidal_t(point_t::Zero(dim_), amplitude, T_, phi, T_min_,
152
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 38 T_max_);
153 19 }
154
155 /// \brief Compute the derived curve at orderN.
156 /// \param order : order of derivative.
157 /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
158 /// curve.
159 10 virtual sinusoidal_t* compute_derivate_ptr(const std::size_t order) const {
160
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 return new sinusoidal_t(compute_derivate(order));
161 }
162
163 /**
164 * @brief isApprox check if other and *this are approximately equals given a
165 * precision threshold Only two curves of the same class can be approximately
166 * equals, for comparison between different type of curves see isEquivalent.
167 * @param other the other curve to check
168 * @param prec the precision threshold, default
169 * Eigen::NumTraits<Numeric>::dummy_precision()
170 * @return true is the two curves are approximately equals
171 */
172 26 virtual bool isApprox(
173 const sinusoidal_t& other,
174 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
175
2/2
✓ Branch 2 taken 20 times.
✓ Branch 3 taken 4 times.
 50 return ndcurves::isApprox<time_t>(T_min_, other.min()) &&
176 24 ndcurves::isApprox<time_t>(T_max_, other.max()) &&
177
3/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✓ Branch 5 taken 2 times.
 20 dim_ == other.dim() && p0_.isApprox(other.p0_, prec) &&
178
4/4
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 15 times.
✓ Branch 4 taken 1 times.
 34 amplitude_.isApprox(other.amplitude_, prec) &&
179
4/4
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 2 times.
✓ Branch 3 taken 13 times.
✓ Branch 4 taken 2 times.
 66 ndcurves::isApprox<time_t>(T_, other.T_) &&
180 41 ndcurves::isApprox<time_t>(phi_, other.phi_);
181 }
182
183 ✗ virtual bool isApprox(
184 const curve_abc_t* other,
185 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
186 ✗ const sinusoidal_t* other_cast = dynamic_cast<const sinusoidal_t*>(other);
187 ✗ if (other_cast)
188 ✗ return isApprox(*other_cast, prec);
189 else
190 ✗ return false;
191 }
192
193 24 virtual bool operator==(const sinusoidal_t& other) const {
194 24 return isApprox(other);
195 }
196
197 12 virtual bool operator!=(const sinusoidal_t& other) const {
198 12 return !(*this == other);
199 }
200
201 /*Helpers*/
202 /// \brief Get dimension of curve.
203 /// \return dimension of curve.
204 74 std::size_t virtual dim() const { return dim_; }
205 /// \brief Get the minimum time for which the curve is defined
206 /// \return \f$t_{min}\f$ lower bound of time range.
207 101 num_t virtual min() const { return T_min_; }
208 /// \brief Get the maximum time for which the curve is defined.
209 /// \return \f$t_{max}\f$ upper bound of time range.
210 285 num_t virtual max() const { return T_max_; }
211 /// \brief Get the degree of the curve.
212 /// \return \f$degree\f$, the degree of the curve.
213 10 virtual std::size_t degree() const { return 1; }
214 /*Helpers*/
215
216 /*Attributes*/
217 Point p0_; // offset
218 Point amplitude_;
219 time_t T_; // period
220 time_t phi_; // phase
221 time_t T_min_, T_max_; // const
222 std::size_t dim_; // const
223 /*Attributes*/
224
225 // Serialization of the class
226 friend class boost::serialization::access;
227
228 template <class Archive>
229 24 void serialize(Archive& ar, const unsigned int version) {
230 if (version) {
231 // Do something depending on version ?
232 }
233
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 24 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
234
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& boost::serialization::make_nvp("p0", p0_);
235
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& boost::serialization::make_nvp("amplitude_", amplitude_);
236
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& boost::serialization::make_nvp("T_", T_);
237
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& boost::serialization::make_nvp("phi_", phi_);
238
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& boost::serialization::make_nvp("T_min", T_min_);
239
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& boost::serialization::make_nvp("T_max", T_max_);
240
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& boost::serialization::make_nvp("dim", dim_);
241 24 }
242
243 private:
244 615 inline const num_t two_pi_f(const time_t& t) const {
245 615 return (2 * M_PI / T_) * t;
246 }
247
248 }; // struct sinusoidal
249 } // namespace ndcurves
250
251 DEFINE_CLASS_TEMPLATE_VERSION(
252 SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
253 SINGLE_ARG(ndcurves::sinusoidal<Time, Numeric, Safe, Point>))
254
255 #endif // _CLASS_SINUSOIDALCURVE
256