APM:Libraries
AP_Math.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <limits>
5 #include <stdint.h>
6 #include <type_traits>
7 
8 #include <AP_Common/AP_Common.h>
9 #include <AP_Param/AP_Param.h>
10 
11 #include "definitions.h"
12 #include "edc.h"
13 #include "location.h"
14 #include "matrix3.h"
15 #include "polygon.h"
16 #include "quaternion.h"
17 #include "rotations.h"
18 #include "vector2.h"
19 #include "vector3.h"
20 #include "spline5.h"
21 
22 // define AP_Param types AP_Vector3f and Ap_Matrix3f
24 
25 /*
26  * Check whether two floats are equal
27  */
28 template <typename Arithmetic1, typename Arithmetic2>
29 typename std::enable_if<std::is_integral<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value ,bool>::type
30 is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
31 
32 template <typename Arithmetic1, typename Arithmetic2>
33 typename std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type
34 is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
35 
36 /*
37  * @brief: Check whether a float is zero
38  */
39 template <typename T>
40 inline bool is_zero(const T fVal1) {
42  "Template parameter not of type float");
43  return (fabsf(static_cast<float>(fVal1)) < FLT_EPSILON);
44 }
45 
46 /*
47  * @brief: Check whether a float is greater than zero
48  */
49 template <typename T>
50 inline bool is_positive(const T fVal1) {
52  "Template parameter not of type float");
53  return (static_cast<float>(fVal1) >= FLT_EPSILON);
54 }
55 
56 
57 /*
58  * @brief: Check whether a float is less than zero
59  */
60 template <typename T>
61 inline bool is_negative(const T fVal1) {
63  "Template parameter not of type float");
64  return (static_cast<float>(fVal1) <= (-1.0 * FLT_EPSILON));
65 }
66 
67 
68 /*
69  * A variant of asin() that checks the input ranges and ensures a valid angle
70  * as output. If nan is given as input then zero is returned.
71  */
72 template <typename T>
73 float safe_asin(const T v);
74 
75 /*
76  * A variant of sqrt() that checks the input ranges and ensures a valid value
77  * as output. If a negative number is given then 0 is returned. The reasoning
78  * is that a negative number for sqrt() in our code is usually caused by small
79  * numerical rounding errors, so the real input should have been zero
80  */
81 template <typename T>
82 float safe_sqrt(const T v);
83 
84 // invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular
85 bool inverse3x3(float m[], float invOut[]);
86 
87 // invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular
88 bool inverse4x4(float m[],float invOut[]);
89 
90 // matrix multiplication of two NxN matrices
91 float *mat_mul(float *A, float *B, uint8_t n);
92 
93 // matrix algebra
94 bool inverse(float x[], float y[], uint16_t dim);
95 
96 /*
97  * Constrain an angle to be within the range: -180 to 180 degrees. The second
98  * parameter changes the units. Default: 1 == degrees, 10 == dezi,
99  * 100 == centi.
100  */
101 template <typename T>
102 float wrap_180(const T angle, float unit_mod = 1);
103 
104 /*
105  * Wrap an angle in centi-degrees. See wrap_180().
106  */
107 template <typename T>
108 auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f));
109 
110 /*
111  * Constrain an euler angle to be within the range: 0 to 360 degrees. The
112  * second parameter changes the units. Default: 1 == degrees, 10 == dezi,
113  * 100 == centi.
114  */
115 template <typename T>
116 float wrap_360(const T angle, float unit_mod = 1);
117 
118 /*
119  * Wrap an angle in centi-degrees. See wrap_360().
120  */
121 template <typename T>
122 auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f));
123 
124 /*
125  wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
126  */
127 template <typename T>
128 float wrap_PI(const T radian);
129 
130 /*
131  * wrap an angle in radians to 0..2PI
132  */
133 template <typename T>
134 float wrap_2PI(const T radian);
135 
136 /*
137  * Constrain a value to be within the range: low and high
138  */
139 template <typename T>
140 T constrain_value(const T amt, const T low, const T high);
141 
142 inline float constrain_float(const float amt, const float low, const float high)
143 {
144  return constrain_value(amt, low, high);
145 }
146 
147 inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
148 {
149  return constrain_value(amt, low, high);
150 }
151 
152 inline int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
153 {
154  return constrain_value(amt, low, high);
155 }
156 
157 // degrees -> radians
158 static inline constexpr float radians(float deg)
159 {
160  return deg * DEG_TO_RAD;
161 }
162 
163 // radians -> degrees
164 static inline constexpr float degrees(float rad)
165 {
166  return rad * RAD_TO_DEG;
167 }
168 
169 template<typename T>
170 float sq(const T val)
171 {
172  return powf(static_cast<float>(val), 2);
173 }
174 
175 /*
176  * Variadic template for calculating the square norm of a vector of any
177  * dimension.
178  */
179 template<typename T, typename... Params>
180 float sq(const T first, const Params... parameters)
181 {
182  return sq(first) + sq(parameters...);
183 }
184 
185 /*
186  * Variadic template for calculating the norm (pythagoras) of a vector of any
187  * dimension.
188  */
189 template<typename T, typename U, typename... Params>
190 float norm(const T first, const U second, const Params... parameters)
191 {
192  return sqrtf(sq(first, second, parameters...));
193 }
194 
195 template<typename A, typename B>
196 static inline auto MIN(const A &one, const B &two) -> decltype(one < two ? one : two)
197 {
198  return one < two ? one : two;
199 }
200 
201 template<typename A, typename B>
202 static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one : two)
203 {
204  return one > two ? one : two;
205 }
206 
207 inline uint32_t hz_to_nsec(uint32_t freq)
208 {
209  return AP_NSEC_PER_SEC / freq;
210 }
211 
212 inline uint32_t nsec_to_hz(uint32_t nsec)
213 {
214  return AP_NSEC_PER_SEC / nsec;
215 }
216 
217 inline uint32_t usec_to_nsec(uint32_t usec)
218 {
219  return usec * AP_NSEC_PER_USEC;
220 }
221 
222 inline uint32_t nsec_to_usec(uint32_t nsec)
223 {
224  return nsec / AP_NSEC_PER_USEC;
225 }
226 
227 inline uint32_t hz_to_usec(uint32_t freq)
228 {
229  return AP_USEC_PER_SEC / freq;
230 }
231 
232 inline uint32_t usec_to_hz(uint32_t usec)
233 {
234  return AP_USEC_PER_SEC / usec;
235 }
236 
237 /*
238  linear interpolation based on a variable in a range
239  */
240 float linear_interpolate(float low_output, float high_output,
241  float var_value,
242  float var_low, float var_high);
243 
244 /* simple 16 bit random number generator */
245 uint16_t get_random16(void);
246 
247 // generate a random float between -1 and 1, for use in SITL
248 float rand_float(void);
249 
250 // generate a random Vector3f of size 1
251 Vector3f rand_vec3f(void);
252 
253 // confirm a value is a valid octal value
254 bool is_valid_octal(uint16_t octal);
255 
float norm(const T first, const U second, const Params... parameters)
Definition: AP_Math.h:190
float linear_interpolate(float low_output, float high_output, float var_value, float var_low, float var_high)
Definition: AP_Math.cpp:81
#define AP_NSEC_PER_USEC
Definition: definitions.h:91
Vector3f rand_vec3f(void)
Definition: AP_Math.cpp:229
bool inverse3x3(float m[], float invOut[])
Definition: matrix_alg.cpp:243
int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
Definition: AP_Math.h:147
float safe_asin(const T v)
Definition: AP_Math.cpp:43
uint32_t nsec_to_usec(uint32_t nsec)
Definition: AP_Math.h:222
float wrap_PI(const T radian)
Definition: AP_Math.cpp:152
static auto MIN(const A &one, const B &two) -> decltype(one< two ? one :two)
Definition: AP_Math.h:196
float wrap_180(const T angle, float unit_mod=1)
Definition: AP_Math.cpp:96
uint32_t hz_to_nsec(uint32_t freq)
Definition: AP_Math.h:207
std::enable_if< std::is_integral< typename std::common_type< Arithmetic1, Arithmetic2 >::type >::value,bool >::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
Definition: AP_Math.cpp:12
bool inverse4x4(float m[], float invOut[])
Definition: matrix_alg.cpp:282
bool is_positive(const T fVal1)
Definition: AP_Math.h:50
static constexpr float degrees(float rad)
Definition: AP_Math.h:164
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
Definition: AP_Math.cpp:111
float rand_float(void)
Definition: AP_Math.cpp:224
#define DEG_TO_RAD
Definition: definitions.h:30
static auto MAX(const A &one, const B &two) -> decltype(one > two ? one :two)
Definition: AP_Math.h:202
auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f))
Definition: AP_Math.cpp:140
A system for managing and storing variables that are of general interest to the system.
#define x(i)
uint32_t nsec_to_hz(uint32_t nsec)
Definition: AP_Math.h:212
bool is_valid_octal(uint16_t octal)
Definition: AP_Math.cpp:241
#define constexpr
Definition: AP_HAL_Macros.h:16
T constrain_value(const T amt, const T low, const T high)
Definition: AP_Math.cpp:182
#define AP_NSEC_PER_SEC
Definition: definitions.h:90
bool is_zero(const T fVal1)
Definition: AP_Math.h:40
#define f(i)
uint32_t usec_to_nsec(uint32_t usec)
Definition: AP_Math.h:217
int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
Definition: AP_Math.h:152
float * mat_mul(float *A, float *B, uint8_t n)
Definition: matrix_alg.cpp:42
#define RAD_TO_DEG
Definition: definitions.h:31
float v
Definition: Printf.cpp:15
float wrap_360(const T angle, float unit_mod=1)
Definition: AP_Math.cpp:123
uint16_t get_random16(void)
Definition: AP_Math.cpp:212
Common definitions and utility routines for the ArduPilot libraries.
float safe_sqrt(const T v)
Definition: AP_Math.cpp:64
float constrain_float(const float amt, const float low, const float high)
Definition: AP_Math.h:142
#define AP_USEC_PER_SEC
Definition: definitions.h:92
static constexpr float radians(float deg)
Definition: AP_Math.h:158
float wrap_2PI(const T radian)
Definition: AP_Math.cpp:167
bool inverse(float x[], float y[], uint16_t dim)
Definition: matrix_alg.cpp:433
bool is_negative(const T fVal1)
Definition: AP_Math.h:61
float sq(const T val)
Definition: AP_Math.h:170
float value
uint32_t hz_to_usec(uint32_t freq)
Definition: AP_Math.h:227
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F)
uint32_t usec_to_hz(uint32_t usec)
Definition: AP_Math.h:232