mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
cyclic.h
Go to the documentation of this file.
1// clang: MatousFormat
7#ifndef CYCLIC_H
8#define CYCLIC_H
9
10#include <cmath>
11#include <ostream>
12
13namespace mrs_lib
14{
15 namespace geometry
16 {
47 template <typename flt, class spec>
48 struct cyclic
49 {
55 cyclic() : val(minimum){};
61 cyclic(const flt val) : val(wrap(val)){};
67 cyclic(const cyclic& other) : val(other.val){};
73 cyclic(const spec& other) : val(other.val){};
74
80 flt value() const
81 {
82 return val;
83 };
84
85 static constexpr flt minimum = spec::minimum;
86 static constexpr flt supremum = spec::supremum;
87 static constexpr flt range =
89 static constexpr flt half_range =
90 range / flt(2);
92 /* static_assert((supremum > minimum), "cyclic value: Range not valid"); */
93
100 static bool inRange(const flt val)
101 {
102 return val >= minimum && val < supremum;
103 }
104
114 static flt wrap(const flt val)
115 {
116 // these few ifs should cover most cases, improving speed and precision
117 if (val >= minimum)
118 {
119 if (val < supremum) // value is actually in range and doesn't need to be wrapped
120 return val;
121 else if (val < supremum + range)
122 return val - range; // to avoid unnecessary costly fmod operation for this case (assumed to be significantly more common than the general case)
123 } else
124 {
125 if (val >= minimum - range)
126 return val + range; // to avoid unnecessary costly fmod operation for this case (assumed to be significantly more common than the general case)
127 }
128
129 // general case
130 const flt rem = std::fmod(val - minimum, range);
131 const flt wrapped = rem + minimum + std::signbit(rem) * range;
132 return wrapped;
133 }
134
157 static flt unwrap(const flt what, const flt from)
158 {
159 return from + diff(what, from);
160 }
161
171 static flt pdist(const flt from, const flt to)
172 {
173 return pdist(cyclic(from), cyclic(to));
174 }
175
176 static flt pdist(const cyclic from, const cyclic to)
177 {
178 const flt tmp = to.val - from.val;
179 const flt dist = tmp + std::signbit(tmp) * range;
180 return dist;
181 }
182
193 static flt diff(const flt minuend, const flt subtrahend)
194 {
196 }
197
198 static flt diff(const cyclic minuend, const cyclic subtrahend)
199 {
200 const flt d = minuend.val - subtrahend.val;
201 if (d < -half_range)
202 return d + range;
203 if (d >= half_range)
204 return d - range;
205 return d;
206 }
207
219 static flt dist(const flt from, const flt to)
220 {
221 return dist(cyclic(from), cyclic(to));
222 }
223
224 static flt dist(const cyclic from, const cyclic to)
225 {
226 return std::abs(diff(from, to));
227 }
228
242 static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
243 {
245 }
246
247 static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
248 {
249 const flt dang = diff(to, from);
250 const flt intp = from.val + coeff * dang;
251 return intp;
252 }
253
264 static flt interp(const flt from, const flt to, const flt coeff)
265 {
266 return wrap(interpUnwrapped(from, to, coeff));
267 }
268
269 static flt interp(const cyclic from, const cyclic to, const flt coeff)
270 {
271 return wrap(interpUnwrapped(from, to, coeff));
272 }
273
288 static flt pinterpUnwrapped(const flt from, const flt to, const flt coeff)
289 {
291 }
292
293 static flt pinterpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
294 {
295 const flt dang = pdist(to, from);
296 const flt intp = from.val + coeff * dang;
297 return intp;
298 }
299
311 static flt pinterp(const flt from, const flt to, const flt coeff)
312 {
314 }
315
316 static flt pinterp(const cyclic from, const cyclic to, const flt coeff)
317 {
318 return wrap(pinterpUnwrapped(from, to, coeff));
319 }
320
333 template <class other_t>
334 static other_t convert(const cyclic& what)
335 {
336 return other_t(what.val / range * other_t::range);
337 }
338
350 template <class other_t>
352 {
353 return other_t(val / range * other_t::range);
354 }
355
356 // | ------------------------ Operators ----------------------- |
364 {
365 val = wrap(nval);
366 return *this;
367 };
375 {
376 val = other.val;
377 return *this;
378 };
386 {
387 val = other.val;
388 return *this;
389 };
390
398 {
399 val = wrap(val + other.val);
400 return *this;
401 };
402
410 {
411 val = diff(val, other.val);
412 return *this;
413 };
414
422 friend spec operator+(const cyclic& lhs, const cyclic& rhs)
423 {
424 return wrap(lhs.val + rhs.val);
425 }
426
434 friend flt operator-(const cyclic& lhs, const cyclic& rhs)
435 {
436 return diff(lhs, rhs);
437 }
438
439 protected:
440 flt val;
441 };
442
452 template <typename flt, class spec>
453 bool operator<(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
454 {
455 return cyclic<flt, spec>::dist(lhs, 0) < cyclic<flt, spec>::dist(rhs, 0);
456 }
457
467 template <typename flt, class spec>
468 bool operator>(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
469 {
470 return cyclic<flt, spec>::dist(lhs, 0) > cyclic<flt, spec>::dist(rhs, 0);
471 }
472
480 template <typename flt, class spec>
481 std::ostream& operator<<(std::ostream& out, const cyclic<flt, spec>& ang)
482 {
483 return (out << ang.value());
484 }
485
489 struct radians : public cyclic<double, radians>
490 {
491 using cyclic<double, radians>::cyclic; // necessary to inherit constructors
492 static constexpr double minimum = 0;
493 static constexpr double supremum = 2 * M_PI;
494 };
495
499 struct sradians : public cyclic<double, sradians>
500 {
501 using cyclic<double, sradians>::cyclic; // necessary to inherit constructors
502 static constexpr double minimum = -M_PI;
503 static constexpr double supremum = M_PI;
504 };
505
509 struct degrees : public cyclic<double, degrees>
510 {
511 using cyclic<double, degrees>::cyclic; // necessary to inherit constructors
512 static constexpr double minimum = 0;
513 static constexpr double supremum = 360;
514 };
515
519 struct sdegrees : public cyclic<double, sdegrees>
520 {
521 using cyclic<double, sdegrees>::cyclic; // necessary to inherit constructors
522 static constexpr double minimum = -180;
523 static constexpr double supremum = 180;
524 };
525 } // namespace geometry
526} // namespace mrs_lib
527
528#endif // CYCLIC_H
bool operator>(const cyclic< flt, spec > &lhs, const cyclic< flt, spec > &rhs)
Implementation of the comparison operation between two angles.
Definition cyclic.h:468
bool operator<(const cyclic< flt, spec > &lhs, const cyclic< flt, spec > &rhs)
Implementation of the comparison operation between two angles.
Definition cyclic.h:453
std::ostream & operator<<(std::ostream &out, const cyclic< flt, spec > &ang)
Implementation of the stream output operator.
Definition cyclic.h:481
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
Implementation of the a general cyclic value (such as angles in radians/degrees etc).
Definition cyclic.h:49
flt value() const
Getter for val.
Definition cyclic.h:80
static flt pinterpUnwrapped(const flt from, const flt to, const flt coeff)
Interpolation between two circular quantities in the positive direction without wrapping of the resul...
Definition cyclic.h:288
static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
Interpolation between two circular quantities without wrapping of the result.
Definition cyclic.h:242
static flt unwrap(const flt what, const flt from)
Returns value of the parameter what modified so that there is no "jump" between from and \t what.
Definition cyclic.h:157
cyclic & operator-=(const cyclic &other)
Subtraction compound operator.
Definition cyclic.h:409
static constexpr flt range
Range of the valid interval of wrapped values (also the period of the cyclic quantity).
Definition cyclic.h:87
static flt pdist(const flt from, const flt to)
Returns length of the shortest walk in the positive direction from the first parameter to the second ...
Definition cyclic.h:171
friend spec operator+(const cyclic &lhs, const cyclic &rhs)
Addition operator.
Definition cyclic.h:422
static constexpr flt half_range
Half of the range of the valid interval of wrapped values (used for some calculations).
Definition cyclic.h:89
static other_t convert(const cyclic &what)
Conversion between two different circular quantities.
Definition cyclic.h:334
cyclic & operator=(cyclic &&other)
Move operator.
Definition cyclic.h:385
static flt pinterp(const flt from, const flt to, const flt coeff)
Interpolation between two circular quantities in the positive direction.
Definition cyclic.h:311
cyclic(const cyclic &other)
Copy constructor.
Definition cyclic.h:67
friend flt operator-(const cyclic &lhs, const cyclic &rhs)
Subtraction operator (uses the diff() method).
Definition cyclic.h:434
static flt dist(const flt from, const flt to)
Returns the distance between the two circular values.
Definition cyclic.h:219
cyclic & operator+=(const cyclic &other)
Addition compound operator.
Definition cyclic.h:397
static flt diff(const flt minuend, const flt subtrahend)
Returns the difference between the two circular values.
Definition cyclic.h:193
other_t convert() const
Conversion between two different circular quantities.
Definition cyclic.h:351
cyclic & operator=(const flt nval)
Assignment operator.
Definition cyclic.h:363
cyclic & operator=(const cyclic &other)
Assignment operator.
Definition cyclic.h:374
cyclic()
Default constructor.
Definition cyclic.h:55
cyclic(const spec &other)
Copy constructor.
Definition cyclic.h:73
static flt wrap(const flt val)
Returns val, converted to the valid interval of values.
Definition cyclic.h:114
static constexpr flt minimum
Minimum of the valid interval of wrapped values .
Definition cyclic.h:85
static constexpr flt supremum
Supremum of the valid interval of wrapped values .
Definition cyclic.h:86
cyclic(const flt val)
Constructor overload.
Definition cyclic.h:61
static bool inRange(const flt val)
Checks if val is within the valid interval of wrapped values.
Definition cyclic.h:100
static flt interp(const flt from, const flt to, const flt coeff)
Interpolation between two circular quantities.
Definition cyclic.h:264
Convenience specialization of the cyclic class for unsigned degrees (from $0$ to $360$).
Definition cyclic.h:510
Convenience specialization of the cyclic class for unsigned radians (from $0$ to $2\pi$).
Definition cyclic.h:490
Convenience specialization of the cyclic class for signed degrees (from $-180$ to $180$).
Definition cyclic.h:520
Convenience specialization of the cyclic class for signed radians (from $-\pi$ to $\pi$).
Definition cyclic.h:500