|
|
|
@ -24,18 +24,18 @@ AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
@@ -24,18 +24,18 @@ AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
|
|
|
|
|
/*
|
|
|
|
|
* Check whether two floats are equal |
|
|
|
|
*/ |
|
|
|
|
template <class Arithmetic1, class Arithmetic2> |
|
|
|
|
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 <class Arithmetic1, class Arithmetic2> |
|
|
|
|
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); |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* @brief: Check whether a float is zero |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
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"); |
|
|
|
@ -45,7 +45,7 @@ inline bool is_zero(const T fVal1) {
@@ -45,7 +45,7 @@ inline bool is_zero(const T fVal1) {
|
|
|
|
|
/*
|
|
|
|
|
* @brief: Check whether a float is greater than zero |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
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"); |
|
|
|
@ -56,7 +56,7 @@ inline bool is_positive(const T fVal1) {
@@ -56,7 +56,7 @@ inline bool is_positive(const T fVal1) {
|
|
|
|
|
/*
|
|
|
|
|
* @brief: Check whether a float is less than zero |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
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"); |
|
|
|
@ -68,7 +68,7 @@ inline bool is_negative(const T fVal1) {
@@ -68,7 +68,7 @@ inline bool is_negative(const T fVal1) {
|
|
|
|
|
* 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 <class T> |
|
|
|
|
template <typename T> |
|
|
|
|
float safe_asin(const T v); |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
@ -77,7 +77,7 @@ float safe_asin(const T v);
@@ -77,7 +77,7 @@ float safe_asin(const T v);
|
|
|
|
|
* 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 <class T> |
|
|
|
|
template <typename T> |
|
|
|
|
float safe_sqrt(const T v); |
|
|
|
|
|
|
|
|
|
// invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular
|
|
|
|
@ -97,13 +97,13 @@ bool inverse(float x[], float y[], uint16_t dim);
@@ -97,13 +97,13 @@ bool inverse(float x[], float y[], uint16_t dim);
|
|
|
|
|
* parameter changes the units. Default: 1 == degrees, 10 == dezi, |
|
|
|
|
* 100 == centi. |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
template <typename T> |
|
|
|
|
float wrap_180(const T angle, float unit_mod = 1); |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Wrap an angle in centi-degrees. See wrap_180(). |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
template <typename T> |
|
|
|
|
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f)); |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
@ -111,31 +111,31 @@ auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f));
@@ -111,31 +111,31 @@ auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f));
|
|
|
|
|
* second parameter changes the units. Default: 1 == degrees, 10 == dezi, |
|
|
|
|
* 100 == centi. |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
template <typename T> |
|
|
|
|
float wrap_360(const T angle, float unit_mod = 1); |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Wrap an angle in centi-degrees. See wrap_360(). |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
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 <class T> |
|
|
|
|
template <typename T> |
|
|
|
|
float wrap_PI(const T radian); |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* wrap an angle in radians to 0..2PI |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
template <typename T> |
|
|
|
|
float wrap_2PI(const T radian); |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Constrain a value to be within the range: low and high |
|
|
|
|
*/ |
|
|
|
|
template <class T> |
|
|
|
|
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) |
|
|
|
@ -165,7 +165,7 @@ static inline float degrees(float rad)
@@ -165,7 +165,7 @@ static inline float degrees(float rad)
|
|
|
|
|
return rad * RAD_TO_DEG; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
template<class T> |
|
|
|
|
template<typename T> |
|
|
|
|
float sq(const T val) |
|
|
|
|
{ |
|
|
|
|
return powf(static_cast<float>(val), 2); |
|
|
|
@ -175,7 +175,7 @@ float sq(const T val)
@@ -175,7 +175,7 @@ float sq(const T val)
|
|
|
|
|
* Variadic template for calculating the square norm of a vector of any |
|
|
|
|
* dimension. |
|
|
|
|
*/ |
|
|
|
|
template<class T, class... Params> |
|
|
|
|
template<typename T, typename... Params> |
|
|
|
|
float sq(const T first, const Params... parameters) |
|
|
|
|
{ |
|
|
|
|
return sq(first) + sq(parameters...); |
|
|
|
@ -185,7 +185,7 @@ float sq(const T first, const Params... parameters)
@@ -185,7 +185,7 @@ float sq(const T first, const Params... parameters)
|
|
|
|
|
* Variadic template for calculating the norm (pythagoras) of a vector of any |
|
|
|
|
* dimension. |
|
|
|
|
*/ |
|
|
|
|
template<class T, class U, class... Params> |
|
|
|
|
template<typename T, typename U, typename... Params> |
|
|
|
|
float norm(const T first, const U second, const Params... parameters) |
|
|
|
|
{ |
|
|
|
|
return sqrtf(sq(first, second, parameters...)); |
|
|
|
|