ardupilot/libraries/AP_Math/AP_Math.h

264 lines
6.8 KiB
C
Raw Normal View History

#pragma once
#include <cmath>
#include <limits>
#include <stdint.h>
#include <type_traits>
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include "definitions.h"
#include "edc.h"
#include "location.h"
#include "matrix3.h"
#include "polygon.h"
#include "quaternion.h"
#include "rotations.h"
#include "vector2.h"
#include "vector3.h"
#include "spline5.h"
// define AP_Param types AP_Vector3f and Ap_Matrix3f
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
/*
* Check whether two floats are equal
*/
template <typename Arithmetic1, typename Arithmetic2>
typename 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);
template <typename Arithmetic1, typename Arithmetic2>
typename std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type
is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
2015-04-28 03:06:13 -03:00
/*
* @brief: Check whether a float is zero
*/
template <typename T>
inline bool is_zero(const T fVal1) {
static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
"Template parameter not of type float");
2016-12-19 08:59:44 -04:00
return (fabsf(static_cast<float>(fVal1)) < FLT_EPSILON);
}
/*
* @brief: Check whether a float is greater than zero
*/
template <typename T>
inline bool is_positive(const T fVal1) {
static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
"Template parameter not of type float");
return (static_cast<float>(fVal1) >= FLT_EPSILON);
}
/*
* @brief: Check whether a float is less than zero
*/
template <typename T>
inline bool is_negative(const T fVal1) {
static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
"Template parameter not of type float");
return (static_cast<float>(fVal1) <= (-1.0 * FLT_EPSILON));
}
/*
* A variant of asin() that checks the input ranges and ensures a valid angle
* as output. If nan is given as input then zero is returned.
*/
template <typename T>
float safe_asin(const T v);
/*
* A variant of sqrt() that checks the input ranges and ensures a valid value
* as output. If a negative number is given then 0 is returned. The reasoning
* is that a negative number for sqrt() in our code is usually caused by small
* numerical rounding errors, so the real input should have been zero
*/
template <typename T>
float safe_sqrt(const T v);
// invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular
bool inverse3x3(float m[], float invOut[]);
// invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular
bool inverse4x4(float m[],float invOut[]);
// matrix multiplication of two NxN matrices
float *mat_mul(float *A, float *B, uint8_t n);
// matrix algebra
bool inverse(float x[], float y[], uint16_t dim);
/*
* Constrain an angle to be within the range: -180 to 180 degrees. The second
* parameter changes the units. Default: 1 == degrees, 10 == dezi,
* 100 == centi.
*/
template <typename T>
float wrap_180(const T angle, float unit_mod = 1);
2013-06-01 05:18:50 -03:00
/*
* Wrap an angle in centi-degrees. See wrap_180().
2013-06-01 05:18:50 -03:00
*/
template <typename T>
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f));
2013-06-01 05:18:50 -03:00
2015-11-24 17:11:34 -04:00
/*
* Constrain an euler angle to be within the range: 0 to 360 degrees. The
* second parameter changes the units. Default: 1 == degrees, 10 == dezi,
* 100 == centi.
2015-11-24 17:11:34 -04:00
*/
template <typename T>
float wrap_360(const T angle, float unit_mod = 1);
/*
* Wrap an angle in centi-degrees. See wrap_360().
*/
template <typename T>
auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f));
/*
wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
template <typename T>
float wrap_PI(const T radian);
/*
* wrap an angle in radians to 0..2PI
*/
template <typename T>
float wrap_2PI(const T radian);
2015-11-24 17:11:34 -04:00
/*
* Constrain a value to be within the range: low and high
*/
template <typename T>
T constrain_value(const T amt, const T low, const T high);
inline float constrain_float(const float amt, const float low, const float high)
{
return constrain_value(amt, low, high);
}
inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
{
return constrain_value(amt, low, high);
}
inline int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
{
return constrain_value(amt, low, high);
}
inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64_t high)
{
return constrain_value(amt, low, high);
}
// degrees -> radians
static inline constexpr float radians(float deg)
{
return deg * DEG_TO_RAD;
}
// radians -> degrees
static inline constexpr float degrees(float rad)
{
return rad * RAD_TO_DEG;
}
template<typename T>
float sq(const T val)
{
float v = static_cast<float>(val);
return v*v;
}
/*
* Variadic template for calculating the square norm of a vector of any
* dimension.
*/
template<typename T, typename... Params>
float sq(const T first, const Params... parameters)
{
return sq(first) + sq(parameters...);
}
/*
* Variadic template for calculating the norm (pythagoras) of a vector of any
* dimension.
*/
template<typename T, typename U, typename... Params>
float norm(const T first, const U second, const Params... parameters)
{
return sqrtf(sq(first, second, parameters...));
}
AP_Math: turn MIN/MAX macros into inline functions The problem with the current MIN/MAX macros is that they evaluate twice the arguments. For example, these cases provide unintended results: // a is incremented twice a = MIN(a++, b); // foo() with side-effects a = MIN(foo(), b); The alternative implementation here was provided by Daniel Frenzel using template function. It doesn't have type safety as std::min and std::max, but adding type safety would mean to check case by case what would be a reasonable type and add proper casts. Here the arguments for MIN and MAX can have different types and the return type is deduced from the expression in the function. Inspecting the current callers no place was found with the unintended results above, but some in which now we don't calculate twice the parameters will benefit from this new version. Examples: float velocity_max = MIN(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius)); float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch() accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f)); track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000);
2015-11-27 13:27:32 -04:00
template<typename A, typename B>
static inline auto MIN(const A &one, const B &two) -> decltype(one < two ? one : two)
{
AP_Math: turn MIN/MAX macros into inline functions The problem with the current MIN/MAX macros is that they evaluate twice the arguments. For example, these cases provide unintended results: // a is incremented twice a = MIN(a++, b); // foo() with side-effects a = MIN(foo(), b); The alternative implementation here was provided by Daniel Frenzel using template function. It doesn't have type safety as std::min and std::max, but adding type safety would mean to check case by case what would be a reasonable type and add proper casts. Here the arguments for MIN and MAX can have different types and the return type is deduced from the expression in the function. Inspecting the current callers no place was found with the unintended results above, but some in which now we don't calculate twice the parameters will benefit from this new version. Examples: float velocity_max = MIN(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius)); float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch() accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f)); track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000);
2015-11-27 13:27:32 -04:00
return one < two ? one : two;
}
template<typename A, typename B>
static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one : two)
{
AP_Math: turn MIN/MAX macros into inline functions The problem with the current MIN/MAX macros is that they evaluate twice the arguments. For example, these cases provide unintended results: // a is incremented twice a = MIN(a++, b); // foo() with side-effects a = MIN(foo(), b); The alternative implementation here was provided by Daniel Frenzel using template function. It doesn't have type safety as std::min and std::max, but adding type safety would mean to check case by case what would be a reasonable type and add proper casts. Here the arguments for MIN and MAX can have different types and the return type is deduced from the expression in the function. Inspecting the current callers no place was found with the unintended results above, but some in which now we don't calculate twice the parameters will benefit from this new version. Examples: float velocity_max = MIN(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius)); float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch() accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f)); track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000);
2015-11-27 13:27:32 -04:00
return one > two ? one : two;
}
inline uint32_t hz_to_nsec(uint32_t freq)
{
return AP_NSEC_PER_SEC / freq;
}
inline uint32_t nsec_to_hz(uint32_t nsec)
{
return AP_NSEC_PER_SEC / nsec;
}
inline uint32_t usec_to_nsec(uint32_t usec)
{
return usec * AP_NSEC_PER_USEC;
}
inline uint32_t nsec_to_usec(uint32_t nsec)
{
return nsec / AP_NSEC_PER_USEC;
}
inline uint32_t hz_to_usec(uint32_t freq)
{
return AP_USEC_PER_SEC / freq;
}
inline uint32_t usec_to_hz(uint32_t usec)
{
return AP_USEC_PER_SEC / usec;
}
/*
linear interpolation based on a variable in a range
*/
float linear_interpolate(float low_output, float high_output,
float var_value,
float var_low, float var_high);
2016-11-17 01:23:11 -04:00
/* simple 16 bit random number generator */
uint16_t get_random16(void);
2017-04-12 08:16:07 -03:00
// generate a random float between -1 and 1, for use in SITL
float rand_float(void);
// generate a random Vector3f of size 1
Vector3f rand_vec3f(void);
// confirm a value is a valid octal value
bool is_valid_octal(uint16_t octal);
2018-10-01 00:04:11 -03:00
// return true if two rotations are equal
bool rotation_equal(enum Rotation r1, enum Rotation r2);