Gazebo Math

API Reference

8.1.0
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
41#ifdef M_PI
42#define GZ_PI M_PI
43#define GZ_PI_2 M_PI_2
44#define GZ_PI_4 M_PI_4
45#define GZ_SQRT2 M_SQRT2
46#else
47#define GZ_PI 3.14159265358979323846
48#define GZ_PI_2 1.57079632679489661923
49#define GZ_PI_4 0.78539816339744830962
50#define GZ_SQRT2 1.41421356237309504880
51#endif
52
56#if defined __FLT_EVAL_METHOD__ && __FLT_EVAL_METHOD__ == 2
57#define GZ_FP_VOLATILE volatile
58#else
59#define GZ_FP_VOLATILE
60#endif
61
64#define GZ_SPHERE_VOLUME(_radius) (4.0*GZ_PI*std::pow(_radius, 3)/3.0)
65
69#define GZ_CONE_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2) / 3.0)
70
74#define GZ_CYLINDER_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2))
75
80#define GZ_BOX_VOLUME(_x, _y, _z) (_x *_y * _z)
81
84#define GZ_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())
85
86namespace gz::math
87{
88 // Inline bracket to help doxygen filtering.
89 inline namespace GZ_MATH_VERSION_NAMESPACE {
90 //
92 static const size_t GZ_ZERO_SIZE_T = 0u;
93
95 static const size_t GZ_ONE_SIZE_T = 1u;
96
98 static const size_t GZ_TWO_SIZE_T = 2u;
99
101 static const size_t GZ_THREE_SIZE_T = 3u;
102
104 static const size_t GZ_FOUR_SIZE_T = 4u;
105
107 static const size_t GZ_FIVE_SIZE_T = 5u;
108
110 static const size_t GZ_SIX_SIZE_T = 6u;
111
113 static const size_t GZ_SEVEN_SIZE_T = 7u;
114
116 static const size_t GZ_EIGHT_SIZE_T = 8u;
117
119 static const size_t GZ_NINE_SIZE_T = 9u;
120
123
126
129
132
135
138
141
144
147
150
153
156
160
163
166
169
173
176
179
182
186
189
192
195
199
202
205
208
212
215
218
221
225
228
231
239 template<typename T>
240 inline T clamp(T _v, T _min, T _max)
241 {
242 return std::max(std::min(_v, _max), _min);
243 }
244
248 inline bool isnan(float _v)
249 {
250 return (std::isnan)(_v);
251 }
252
256 inline bool isnan(double _v)
257 {
258 return (std::isnan)(_v);
259 }
260
264 inline float fixnan(float _v)
265 {
266 return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
267 }
268
272 inline double fixnan(double _v)
273 {
274 return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
275 }
276
280 inline bool isEven(const int _v)
281 {
282 return !(_v % 2);
283 }
284
288 inline bool isEven(const unsigned int _v)
289 {
290 return !(_v % 2);
291 }
292
296 inline bool isOdd(const int _v)
297 {
298 return (_v % 2) != 0;
299 }
300
304 inline bool isOdd(const unsigned int _v)
305 {
306 return (_v % 2) != 0;
307 }
308
315 template<typename T>
316 inline int sgn(T _value)
317 {
318 return (T(0) < _value) - (_value < T(0));
319 }
320
327 template<typename T>
328 inline int signum(T _value)
329 {
330 return sgn(_value);
331 }
332
336 template<typename T>
337 inline T mean(const std::vector<T> &_values)
338 {
339 T sum = 0;
340 for (unsigned int i = 0; i < _values.size(); ++i)
341 sum += _values[i];
342 return sum / static_cast<T>(_values.size());
343 }
344
348 template<typename T>
350 {
351 T avg = mean<T>(_values);
352
353 T sum = 0;
354 for (unsigned int i = 0; i < _values.size(); ++i)
355 sum += (_values[i] - avg) * (_values[i] - avg);
356 return sum / static_cast<T>(_values.size());
357 }
358
362 template<typename T>
363 inline T max(const std::vector<T> &_values)
364 {
366 }
367
371 template<typename T>
372 inline T min(const std::vector<T> &_values)
373 {
375 }
376
382 template<typename T>
383 inline bool equal(const T &_a, const T &_b,
384 const T &_epsilon = T(1e-6))
385 {
386 GZ_FP_VOLATILE T diff = std::abs(_a - _b);
387 return diff <= _epsilon;
388 }
389
395 template<typename T>
396 inline bool lessOrNearEqual(const T &_a, const T &_b,
397 const T &_epsilon = 1e-6)
398 {
399 return _a < _b + _epsilon;
400 }
401
407 template<typename T>
408 inline bool greaterOrNearEqual(const T &_a, const T &_b,
409 const T &_epsilon = 1e-6)
410 {
411 return _a > _b - _epsilon;
412 }
413
418 template<typename T>
419 inline T precision(const T &_a, const unsigned int &_precision)
420 {
421 auto p = std::pow(10, _precision);
422 return static_cast<T>(std::round(_a * p) / p);
423 }
424
430 template<typename T>
431 inline void sort2(T &_a, T &_b)
432 {
433 if (_b < _a)
434 std::swap(_a, _b);
435 }
436
444 template<typename T>
445 inline void sort3(T &_a, T &_b, T &_c)
446 {
447 // _a <= _b
448 sort2(_a, _b);
449 // _a <= _c, _b <= _c
450 sort2(_b, _c);
451 // _a <= _b <= _c
452 sort2(_a, _b);
453 }
454
458 template<typename T>
460 {
462 {
463 _out << 0;
464 }
465 else
466 {
467 _out << _number;
468 }
469 }
470
474 template<>
476 {
477 _out << _number;
478 }
479
483 inline bool isPowerOfTwo(unsigned int _x)
484 {
485 return ((_x != 0) && ((_x & (~_x + 1)) == _x));
486 }
487
493 inline unsigned int roundUpPowerOfTwo(unsigned int _x)
494 {
495 if (_x == 0)
496 return 1;
497
498 if (isPowerOfTwo(_x))
499 return _x;
500
501 while (_x & (_x - 1))
502 _x = _x & (_x - 1);
503
504 _x = _x << 1;
505
506 return _x;
507 }
508
519 inline int roundUpMultiple(int _num, int _multiple)
520 {
521 if (_multiple == 0)
522 return _num;
523
524 int remainder = std::abs(_num) % _multiple;
525 if (remainder == 0)
526 return _num;
527
528 if (_num < 0)
529 return -(std::abs(_num) - remainder);
530 else
531 return _num + _multiple - remainder;
532 }
533
538
544
551 const std::chrono::steady_clock::time_point &_time);
552
559 std::chrono::steady_clock::time_point GZ_MATH_VISIBLE
561 const uint64_t &_sec, const uint64_t &_nanosec);
562
569 std::chrono::steady_clock::duration GZ_MATH_VISIBLE secNsecToDuration(
570 const uint64_t &_sec, const uint64_t &_nanosec);
571
578 const std::chrono::steady_clock::duration &_dur);
579
580 // TODO(anyone): Replace this with std::chrono::days.
583
591 template<class...Durations, class DurationIn>
594 using discard = int[];
595 (void)discard{0, (void((
596 (std::get<Durations>(retval) =
597 std::chrono::duration_cast<Durations>(d)),
598 (d -= std::chrono::duration_cast<DurationIn>(
599 std::get<Durations>(retval))))), 0)...};
600 return retval;
601 }
602
607 const std::chrono::steady_clock::time_point &_point);
608
613 const std::chrono::steady_clock::duration &_duration);
614
625 uint64_t & numberDays, uint64_t & numberHours,
626 uint64_t & numberMinutes, uint64_t & numberSeconds,
627 uint64_t & numberMilliseconds);
628
634 {
635 // These will be thrown away, just for making the function call
636 uint64_t d, h, m, s, ms;
637 return splitTimeBasedOnTimeRegex(_timeString, d, h, m, s, ms);
638 }
639
646 std::chrono::steady_clock::duration GZ_MATH_VISIBLE stringToDuration(
647 const std::string &_timeString);
648
655 std::chrono::steady_clock::time_point
657
658 // Degrade precision on Windows, which cannot handle 'long double'
659 // values properly. See the implementation of Unpair.
660 // 32 bit ARM processors also define 'long double' to be the same
661 // size as 'double', and must also be degraded
662#if defined _MSC_VER || defined __arm__
663 using PairInput = uint16_t;
664 using PairOutput = uint32_t;
665#else
666 using PairInput = uint32_t;
667 using PairOutput = uint64_t;
668#endif
669
680 const PairInput _a, const PairInput _b);
681
694 const PairOutput _key);
695 } // namespace GZ_MATH_VERSION_NAMESPACE
696} // namespace gz::math
697#endif // GZ_MATH_FUNCTIONS_HH_