math_util.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
31 
32 #include <cmath>
33 
34 namespace swri_math_util
35 {
36  double Round(double value)
37  {
38  return (value > 0.0) ? std::floor(value + 0.5) : std::ceil(value - 0.5);
39  }
40 
41  double ToNearest(double value, double multiple)
42  {
43  if (multiple == 0)
44  {
45  return 0;
46  }
47 
48  return Round(value / multiple) * multiple;
49  }
50 
51  double UpToNearest(double value, double multiple)
52  {
53  if (multiple == 0)
54  {
55  return 0;
56  }
57 
58  return std::ceil(value / multiple) * multiple;
59  }
60 
61  bool IsNear(double v1, double v2, double epsilon)
62  {
63  return std::fabs(v1 - v2) <= epsilon;
64  }
65 
66  double unWrapAngle(double static_angle,
67  double variable_angle,
68  double threshold)
69  {
70  if (std::abs(static_angle - variable_angle) > threshold)
71  {
72  if (variable_angle < static_angle)
73  {
74  variable_angle += swri_math_util::_2pi;
75  }
76  else
77  {
78  variable_angle -= swri_math_util::_2pi;
79  }
80  }
81  return variable_angle;
82  }
83 }
double UpToNearest(double value, double multiple)
Definition: math_util.cpp:51
double unWrapAngle(double static_angle, double variable_angle, double threshold=_pi)
Definition: math_util.cpp:66
double ToNearest(double value, double multiple)
Definition: math_util.cpp:41
bool IsNear(double v1, double v2, double epsilon)
Definition: math_util.cpp:61
double Round(double value)
Definition: math_util.cpp:36
static const long double _2pi
Definition: constants.h:37


swri_math_util
Author(s): Marc Alban
autogenerated on Sat Jan 21 2023 03:13:11