Gazebo Math

API Reference

7.4.0
gz/math/Helpers.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef GZ_MATH_FUNCTIONS_HH_
18#define GZ_MATH_FUNCTIONS_HH_
19
20#include <algorithm>
21#include <chrono>
22#include <cmath>
23#include <cstdint>
24#include <ostream>
25#include <limits>
26#include <string>
27#include <tuple>
28#include <utility>
29#include <vector>
30
31#include <gz/math/config.hh>
32#include "gz/math/Export.hh"
33
36template <typename T>
38
39// TODO(CH3): Deprecated. Remove on tock.
40template <typename T>
42
45#ifdef M_PI
46#define GZ_PI M_PI
47#define GZ_PI_2 M_PI_2
48#define GZ_PI_4 M_PI_4
49#define GZ_SQRT2 M_SQRT2
50#else
51#define GZ_PI 3.14159265358979323846
52#define GZ_PI_2 1.57079632679489661923
53#define GZ_PI_4 0.78539816339744830962
54#define GZ_SQRT2 1.41421356237309504880
55#endif
56
60#if defined __FLT_EVAL_METHOD__ && __FLT_EVAL_METHOD__ == 2
61#define GZ_FP_VOLATILE volatile
62#else
63#define GZ_FP_VOLATILE
64#endif
65
68#define GZ_SPHERE_VOLUME(_radius) (4.0*GZ_PI*std::pow(_radius, 3)/3.0)
69
73#define GZ_CYLINDER_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2))
74
79#define GZ_BOX_VOLUME(_x, _y, _z) (_x *_y * _z)
80
83#define GZ_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())
84
85namespace gz
86{
88 namespace math
89 {
90 // Inline bracket to help doxygen filtering.
91 inline namespace GZ_MATH_VERSION_NAMESPACE {
92 //
94 static const size_t GZ_ZERO_SIZE_T = 0u;
95
97 static const size_t GZ_ONE_SIZE_T = 1u;
98
100 static const size_t GZ_TWO_SIZE_T = 2u;
101
103 static const size_t GZ_THREE_SIZE_T = 3u;
104
106 static const size_t GZ_FOUR_SIZE_T = 4u;
107
109 static const size_t GZ_FIVE_SIZE_T = 5u;
110
112 static const size_t GZ_SIX_SIZE_T = 6u;
113
115 static const size_t GZ_SEVEN_SIZE_T = 7u;
116
118 static const size_t GZ_EIGHT_SIZE_T = 8u;
119
121 static const size_t GZ_NINE_SIZE_T = 9u;
122
123 // TODO(CH3): Deprecated. Remove on tock.
124 constexpr auto GZ_DEPRECATED(7) IGN_ZERO_SIZE_T = &GZ_ZERO_SIZE_T;
125 constexpr auto GZ_DEPRECATED(7) IGN_ONE_SIZE_T = &GZ_ONE_SIZE_T;
126 constexpr auto GZ_DEPRECATED(7) IGN_TWO_SIZE_T = &GZ_TWO_SIZE_T;
127 constexpr auto GZ_DEPRECATED(7) IGN_THREE_SIZE_T = &GZ_THREE_SIZE_T;
128 constexpr auto GZ_DEPRECATED(7) IGN_FOUR_SIZE_T = &GZ_FOUR_SIZE_T;
129 constexpr auto GZ_DEPRECATED(7) IGN_FIVE_SIZE_T = &GZ_FIVE_SIZE_T;
130 constexpr auto GZ_DEPRECATED(7) IGN_SIX_SIZE_T = &GZ_SIX_SIZE_T;
131 constexpr auto GZ_DEPRECATED(7) IGN_SEVEN_SIZE_T = &GZ_SEVEN_SIZE_T;
132 constexpr auto GZ_DEPRECATED(7) IGN_EIGHT_SIZE_T = &GZ_EIGHT_SIZE_T;
133 constexpr auto GZ_DEPRECATED(7) IGN_NINE_SIZE_T = &GZ_NINE_SIZE_T;
134
136 static const double MAX_D = std::numeric_limits<double>::max();
137
139 static const double MIN_D = std::numeric_limits<double>::min();
140
142 static const double LOW_D = std::numeric_limits<double>::lowest();
143
145 static const double INF_D = std::numeric_limits<double>::infinity();
146
148 static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
149
151 static const float MAX_F = std::numeric_limits<float>::max();
152
154 static const float MIN_F = std::numeric_limits<float>::min();
155
157 static const float LOW_F = std::numeric_limits<float>::lowest();
158
160 static const float INF_F = std::numeric_limits<float>::infinity();
161
163 static const float NAN_F = std::numeric_limits<float>::quiet_NaN();
164
166 static const uint16_t MAX_UI16 = std::numeric_limits<uint16_t>::max();
167
169 static const uint16_t MIN_UI16 = std::numeric_limits<uint16_t>::min();
170
173 static const uint16_t LOW_UI16 = std::numeric_limits<uint16_t>::lowest();
174
176 static const uint16_t INF_UI16 = std::numeric_limits<uint16_t>::infinity();
177
179 static const int16_t MAX_I16 = std::numeric_limits<int16_t>::max();
180
182 static const int16_t MIN_I16 = std::numeric_limits<int16_t>::min();
183
186 static const int16_t LOW_I16 = std::numeric_limits<int16_t>::lowest();
187
189 static const int16_t INF_I16 = std::numeric_limits<int16_t>::infinity();
190
192 static const uint32_t MAX_UI32 = std::numeric_limits<uint32_t>::max();
193
195 static const uint32_t MIN_UI32 = std::numeric_limits<uint32_t>::min();
196
199 static const uint32_t LOW_UI32 = std::numeric_limits<uint32_t>::lowest();
200
202 static const uint32_t INF_UI32 = std::numeric_limits<uint32_t>::infinity();
203
205 static const int32_t MAX_I32 = std::numeric_limits<int32_t>::max();
206
208 static const int32_t MIN_I32 = std::numeric_limits<int32_t>::min();
209
212 static const int32_t LOW_I32 = std::numeric_limits<int32_t>::lowest();
213
215 static const int32_t INF_I32 = std::numeric_limits<int32_t>::infinity();
216
218 static const uint64_t MAX_UI64 = std::numeric_limits<uint64_t>::max();
219
221 static const uint64_t MIN_UI64 = std::numeric_limits<uint64_t>::min();
222
225 static const uint64_t LOW_UI64 = std::numeric_limits<uint64_t>::lowest();
226
228 static const uint64_t INF_UI64 = std::numeric_limits<uint64_t>::infinity();
229
231 static const int64_t MAX_I64 = std::numeric_limits<int64_t>::max();
232
234 static const int64_t MIN_I64 = std::numeric_limits<int64_t>::min();
235
238 static const int64_t LOW_I64 = std::numeric_limits<int64_t>::lowest();
239
241 static const int64_t INF_I64 = std::numeric_limits<int64_t>::infinity();
242
244 static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
245
253 template<typename T>
254 inline T clamp(T _v, T _min, T _max)
255 {
256 return std::max(std::min(_v, _max), _min);
257 }
258
262 inline bool isnan(float _v)
263 {
264 return (std::isnan)(_v);
265 }
266
270 inline bool isnan(double _v)
271 {
272 return (std::isnan)(_v);
273 }
274
278 inline float fixnan(float _v)
279 {
280 return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
281 }
282
286 inline double fixnan(double _v)
287 {
288 return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
289 }
290
294 inline bool isEven(const int _v)
295 {
296 return !(_v % 2);
297 }
298
302 inline bool isEven(const unsigned int _v)
303 {
304 return !(_v % 2);
305 }
306
310 inline bool isOdd(const int _v)
311 {
312 return (_v % 2) != 0;
313 }
314
318 inline bool isOdd(const unsigned int _v)
319 {
320 return (_v % 2) != 0;
321 }
322
329 template<typename T>
330 inline int sgn(T _value)
331 {
332 return (T(0) < _value) - (_value < T(0));
333 }
334
341 template<typename T>
342 inline int signum(T _value)
343 {
344 return sgn(_value);
345 }
346
350 template<typename T>
351 inline T mean(const std::vector<T> &_values)
352 {
353 T sum = 0;
354 for (unsigned int i = 0; i < _values.size(); ++i)
355 sum += _values[i];
356 return sum / static_cast<T>(_values.size());
357 }
358
362 template<typename T>
363 inline T variance(const std::vector<T> &_values)
364 {
365 T avg = mean<T>(_values);
366
367 T sum = 0;
368 for (unsigned int i = 0; i < _values.size(); ++i)
369 sum += (_values[i] - avg) * (_values[i] - avg);
370 return sum / static_cast<T>(_values.size());
371 }
372
376 template<typename T>
377 inline T max(const std::vector<T> &_values)
378 {
379 return *std::max_element(std::begin(_values), std::end(_values));
380 }
381
385 template<typename T>
386 inline T min(const std::vector<T> &_values)
387 {
388 return *std::min_element(std::begin(_values), std::end(_values));
389 }
390
396 template<typename T>
397 inline bool equal(const T &_a, const T &_b,
398 const T &_epsilon = T(1e-6))
399 {
400 GZ_FP_VOLATILE T diff = std::abs(_a - _b);
401 return diff <= _epsilon;
402 }
403
409 template<typename T>
410 inline bool lessOrNearEqual(const T &_a, const T &_b,
411 const T &_epsilon = 1e-6)
412 {
413 return _a < _b + _epsilon;
414 }
415
421 template<typename T>
422 inline bool greaterOrNearEqual(const T &_a, const T &_b,
423 const T &_epsilon = 1e-6)
424 {
425 return _a > _b - _epsilon;
426 }
427
432 template<typename T>
433 inline T precision(const T &_a, const unsigned int &_precision)
434 {
435 auto p = std::pow(10, _precision);
436 return static_cast<T>(std::round(_a * p) / p);
437 }
438
444 template<typename T>
445 inline void sort2(T &_a, T &_b)
446 {
447 if (_b < _a)
448 std::swap(_a, _b);
449 }
450
458 template<typename T>
459 inline void sort3(T &_a, T &_b, T &_c)
460 {
461 // _a <= _b
462 sort2(_a, _b);
463 // _a <= _c, _b <= _c
464 sort2(_b, _c);
465 // _a <= _b <= _c
466 sort2(_a, _b);
467 }
468
472 template<typename T>
473 inline void appendToStream(std::ostream &_out, T _number)
474 {
475 if (std::fpclassify(_number) == FP_ZERO)
476 {
477 _out << 0;
478 }
479 else
480 {
481 _out << _number;
482 }
483 }
484
488 template<>
489 inline void appendToStream(std::ostream &_out, int _number)
490 {
491 _out << _number;
492 }
493
497 inline bool isPowerOfTwo(unsigned int _x)
498 {
499 return ((_x != 0) && ((_x & (~_x + 1)) == _x));
500 }
501
507 inline unsigned int roundUpPowerOfTwo(unsigned int _x)
508 {
509 if (_x == 0)
510 return 1;
511
512 if (isPowerOfTwo(_x))
513 return _x;
514
515 while (_x & (_x - 1))
516 _x = _x & (_x - 1);
517
518 _x = _x << 1;
519
520 return _x;
521 }
522
533 inline int roundUpMultiple(int _num, int _multiple)
534 {
535 if (_multiple == 0)
536 return _num;
537
538 int remainder = std::abs(_num) % _multiple;
539 if (remainder == 0)
540 return _num;
541
542 if (_num < 0)
543 return -(std::abs(_num) - remainder);
544 else
545 return _num + _multiple - remainder;
546 }
547
551 int GZ_MATH_VISIBLE parseInt(const std::string &_input);
552
557 double GZ_MATH_VISIBLE parseFloat(const std::string &_input);
558
565 const std::chrono::steady_clock::time_point &_time);
566
573 std::chrono::steady_clock::time_point GZ_MATH_VISIBLE
575 const uint64_t &_sec, const uint64_t &_nanosec);
576
583 std::chrono::steady_clock::duration GZ_MATH_VISIBLE secNsecToDuration(
584 const uint64_t &_sec, const uint64_t &_nanosec);
585
592 const std::chrono::steady_clock::duration &_dur);
593
594 // TODO(anyone): Replace this with std::chrono::days.
597
605 template<class...Durations, class DurationIn>
606 std::tuple<Durations...> breakDownDurations(DurationIn d) {
607 std::tuple<Durations...> retval;
608 using discard = int[];
609 (void)discard{0, (void((
610 (std::get<Durations>(retval) =
611 std::chrono::duration_cast<Durations>(d)),
612 (d -= std::chrono::duration_cast<DurationIn>(
613 std::get<Durations>(retval))))), 0)...};
614 return retval;
615 }
616
621 const std::chrono::steady_clock::time_point &_point);
622
627 const std::chrono::steady_clock::duration &_duration);
628
637 bool GZ_MATH_VISIBLE splitTimeBasedOnTimeRegex(
638 const std::string &_timeString,
639 uint64_t & numberDays, uint64_t & numberHours,
640 uint64_t & numberMinutes, uint64_t & numberSeconds,
641 uint64_t & numberMilliseconds);
642
647 inline bool isTimeString(const std::string &_timeString)
648 {
649 // These will be thrown away, just for making the function call
650 uint64_t d, h, m, s, ms;
651 return splitTimeBasedOnTimeRegex(_timeString, d, h, m, s, ms);
652 }
653
660 std::chrono::steady_clock::duration GZ_MATH_VISIBLE stringToDuration(
661 const std::string &_timeString);
662
669 std::chrono::steady_clock::time_point
670 GZ_MATH_VISIBLE stringToTimePoint(const std::string &_timeString);
671
672 // Degrade precision on Windows, which cannot handle 'long double'
673 // values properly. See the implementation of Unpair.
674 // 32 bit ARM processors also define 'long double' to be the same
675 // size as 'double', and must also be degraded
676#if defined _MSC_VER || defined __arm__
677 using PairInput = uint16_t;
678 using PairOutput = uint32_t;
679#else
680 using PairInput = uint32_t;
681 using PairOutput = uint64_t;
682#endif
683
693 PairOutput GZ_MATH_VISIBLE Pair(
694 const PairInput _a, const PairInput _b);
695
708 const PairOutput _key);
709 }
710 }
711}
712
713#endif