MathUtils.h
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package ArmarX::Core
17 * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
18 * @date 2015
20 * GNU General Public License
21 */
22
23 #pragma once
24
25 #include <Eigen/Core>
26 #include <vector>
27 #include <math.h>
28
29 namespace armarx::math
30 {
31  class MathUtils
32  {
33  public:
34  static int Sign(double value)
35  {
36  return value >= 0 ? 1 : -1;
37  }
38
39  static int LimitMinMax(int min, int max, int value)
40  {
41  return value < min ? min : (value > max ? max : value);
42  }
43  static float LimitMinMax(float min, float max, float value)
44  {
45  return value < min ? min : (value > max ? max : value);
46  }
47  static double LimitMinMax(double min, double max, double value)
48  {
49  return value < min ? min : (value > max ? max : value);
50  }
51  static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
52  {
53  return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)), LimitMinMax(min(1), max(1), value(1)), LimitMinMax(min(2), max(2), value(2)));
54  }
55
56  static double LimitTo(double value, double absThreshold)
57  {
58  return LimitMinMax(-absThreshold, absThreshold, value);
59  //int sign = (value >= 0) ? 1 : -1;
60  //return sign * std::min<double>(fabs(value), absThreshold);
61  }
62
63  static Eigen::Vector3f LimitTo(const Eigen::Vector3f& val, float maxNorm)
64  {
65  float norm = val.norm();
66  if (norm > maxNorm)
67  {
68  return val / norm * maxNorm;
69  }
70  return val;
71  }
72
73  static bool CheckMinMax(int min, int max, int value)
74  {
75  return value >= min && value <= max;
76  }
77  static bool CheckMinMax(float min, float max, float value)
78  {
79  return value >= min && value <= max;
80  }
81  static bool CheckMinMax(double min, double max, double value)
82  {
83  return value >= min && value <= max;
84  }
85  static bool CheckMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
86  {
87  return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) && CheckMinMax(min(2), max(2), value(2));
88  }
89
90  static std::vector<float> VectorSubtract(const std::vector<float>& v1, const std::vector<float>& v2)
91  {
92  std::vector<float> result;
93
94  for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
95  {
96  result.push_back(v1.at(i) - v2.at(i));
97  }
98
99  return result;
100  }
101  static std::vector<float> VectorAbsDiff(const std::vector<float>& v1, const std::vector<float>& v2)
102  {
103  std::vector<float> result;
104
105  for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
106  {
107  result.push_back(std::fabs(v1.at(i) - v2.at(i)));
108  }
109
110  return result;
111  }
112
113  static float VectorMax(const std::vector<float>& vec)
114  {
115  float max = vec.at(0);
116
117  for (size_t i = 1; i < vec.size(); i++)
118  {
119  max = std::max(max, vec.at(i));
120  }
121
122  return max;
123  }
124
125  static float fmod(float value, float boundLow, float boundHigh)
126  {
127  value = std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
128  if (value < boundLow)
129  {
130  value += boundHigh - boundLow;
131  }
132  return value;
133  }
134
135  static float angleMod2PI(float value)
136  {
137  return fmod(value, 0, 2 * M_PI);
138  }
139
140  static float angleModPI(float value)
141  {
142  return angleMod2PI(value + M_PI) - M_PI;
143  }
144
145  static float angleModX(float value, float center)
146  {
147  return angleMod2PI(value + M_PI - center) - M_PI + center;
148  }
149
150  static float Lerp(float a, float b, float f)
151  {
152  return a * (1 - f) + b * f;
153  }
154
155  static float LerpClamp(float a, float b, float f)
156  {
157  return Lerp(a, b, LimitMinMax(0.f, 1.f, f));
158  }
159
160  static float ILerp(float a, float b, float f)
161  {
162  return (f - a) / (b - a);
163  }
164
165  static float ILerpClamp(float a, float b, float f)
166  {
167  return LimitMinMax(0.f, 1.f, ILerp(a, b, f));
168  }
169
170  static float AngleLerp(float a, float b, float f)
171  {
172  b = fmod(b, a - M_PI, a + M_PI);
173  return Lerp(a, b, f);
174  }
175
176  static float AngleDelta(float angle1, float angle2)
177  {
178  return angleModPI(angle2 - angle1);
179  }
180
181  };
182 }
armarx::math::MathUtils::LimitTo
static Eigen::Vector3f LimitTo(const Eigen::Vector3f &val, float maxNorm)
Definition: MathUtils.h:63
armarx::math::MathUtils::ILerp
static float ILerp(float a, float b, float f)
Definition: MathUtils.h:160
armarx::math::MathUtils::VectorMax
static float VectorMax(const std::vector< float > &vec)
Definition: MathUtils.h:113
armarx::math::MathUtils::AngleLerp
static float AngleLerp(float a, float b, float f)
Definition: MathUtils.h:170
armarx::math::MathUtils::AngleDelta
static float AngleDelta(float angle1, float angle2)
Definition: MathUtils.h:176
armarx::math::MathUtils::VectorSubtract
static std::vector< float > VectorSubtract(const std::vector< float > &v1, const std::vector< float > &v2)
Definition: MathUtils.h:90
armarx::math::MathUtils::LimitMinMax
static double LimitMinMax(double min, double max, double value)
Definition: MathUtils.h:47
armarx::math::MathUtils::fmod
static float fmod(float value, float boundLow, float boundHigh)
Definition: MathUtils.h:125
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::math::MathUtils::angleModX
static float angleModX(float value, float center)
Definition: MathUtils.h:145
armarx::math::MathUtils::CheckMinMax
static bool CheckMinMax(const Eigen::Vector3f &min, const Eigen::Vector3f &max, const Eigen::Vector3f &value)
Definition: MathUtils.h:85
armarx::math::MathUtils::angleMod2PI
static float angleMod2PI(float value)
Definition: MathUtils.h:135
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::math::MathUtils::LimitMinMax
static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f &min, const Eigen::Vector3f &max, const Eigen::Vector3f &value)
Definition: MathUtils.h:51
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::math::MathUtils::CheckMinMax
static bool CheckMinMax(double min, double max, double value)
Definition: MathUtils.h:81
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::math::MathUtils::angleModPI
static float angleModPI(float value)
Definition: MathUtils.h:140
armarx::math::MathUtils
Definition: MathUtils.h:31
armarx::math::MathUtils::LimitMinMax
static float LimitMinMax(float min, float max, float value)
Definition: MathUtils.h:43
armarx::math::MathUtils::LimitTo
static double LimitTo(double value, double absThreshold)
Definition: MathUtils.h:56
armarx::math::MathUtils::LimitMinMax
static int LimitMinMax(int min, int max, int value)
Definition: MathUtils.h:39
armarx::math::MathUtils::LerpClamp
static float LerpClamp(float a, float b, float f)
Definition: MathUtils.h:155
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:294
armarx::math::MathUtils::Sign
static int Sign(double value)
Definition: MathUtils.h:34
armarx::math::MathUtils::VectorAbsDiff
static std::vector< float > VectorAbsDiff(const std::vector< float > &v1, const std::vector< float > &v2)
Definition: MathUtils.h:101
armarx::math
Definition: LinearizeAngularTrajectory.cpp:27
armarx::math::MathUtils::ILerpClamp
static float ILerpClamp(float a, float b, float f)
Definition: MathUtils.h:165
armarx::math::MathUtils::CheckMinMax
static bool CheckMinMax(int min, int max, int value)
Definition: MathUtils.h:73
armarx::math::MathUtils::CheckMinMax
static bool CheckMinMax(float min, float max, float value)
Definition: MathUtils.h:77
armarx::math::MathUtils::Lerp
static float Lerp(float a, float b, float f)
Definition: MathUtils.h:150
norm
double norm(const Point &a)
Definition: point.hpp:94