diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 1b17bbf304..b12aaee2f2 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -7,7 +7,7 @@ * compatibility with old code. Expands to the same as comparing the values * directly */ -template +template typename std::enable_if::type>::value ,bool>::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2) { @@ -19,7 +19,7 @@ is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2) * is_equal(): double/float implementation - takes into account * std::numeric_limits::epsilon() to return if 2 values are equal. */ -template +template typename std::enable_if::type>::value, bool>::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2) { @@ -36,7 +36,7 @@ template bool is_equal(const short v_1, const short v_2); template bool is_equal(const float v_1, const float v_2); template bool is_equal(const double v_1, const double v_2); -template +template float safe_asin(const T v) { const float f = static_cast(v); @@ -57,7 +57,7 @@ template float safe_asin(const short v); template float safe_asin(const float v); template float safe_asin(const double v); -template +template float safe_sqrt(const T v) { float ret = sqrtf(static_cast(v)); @@ -89,7 +89,7 @@ float linear_interpolate(float low_output, float high_output, return low_output + p * (high_output - low_output); } -template +template float wrap_180(const T angle, float unit_mod) { auto res = wrap_360(angle, unit_mod); @@ -104,7 +104,7 @@ template float wrap_180(const short angle, float unit_mod); template float wrap_180(const float angle, float unit_mod); template float wrap_180(const double angle, float unit_mod); -template +template auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f)) { return wrap_180(angle, 100.f); @@ -115,7 +115,7 @@ template auto wrap_180_cd(const int angle) -> decltype(wrap_180(angle, 100. template auto wrap_180_cd(const short angle) -> decltype(wrap_180(angle, 100.f)); template auto wrap_180_cd(const double angle) -> decltype(wrap_360(angle, 100.f)); -template +template float wrap_360(const T angle, float unit_mod) { const float ang_360 = 360.f * unit_mod; @@ -131,7 +131,7 @@ template float wrap_360(const short angle, float unit_mod); template float wrap_360(const float angle, float unit_mod); template float wrap_360(const double angle, float unit_mod); -template +template auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f)) { return wrap_360(angle, 100.f); @@ -142,7 +142,7 @@ template auto wrap_360_cd(const int angle) -> decltype(wrap_360(angle, 100. template auto wrap_360_cd(const short angle) -> decltype(wrap_360(angle, 100.f)); template auto wrap_360_cd(const double angle) -> decltype(wrap_360(angle, 100.f)); -template +template float wrap_PI(const T radian) { auto res = wrap_2PI(radian); @@ -157,7 +157,7 @@ template float wrap_PI(const short radian); template float wrap_PI(const float radian); template float wrap_PI(const double radian); -template +template float wrap_2PI(const T radian) { float res = fmodf(static_cast(radian), M_2PI); @@ -172,7 +172,7 @@ template float wrap_2PI(const short radian); template float wrap_2PI(const float radian); template float wrap_2PI(const double radian); -template +template T constrain_value(const T amt, const T low, const T high) { // the check for NaN as a float prevents propagation of floating point diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index e7b1e1c392..adfb7a6df0 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -24,18 +24,18 @@ AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F); /* * Check whether two floats are equal */ -template +template typename std::enable_if::type>::value ,bool>::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2); -template +template typename std::enable_if::type>::value, bool>::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2); /* * @brief: Check whether a float is zero */ -template +template inline bool is_zero(const T fVal1) { static_assert(std::is_floating_point::value || std::is_base_of::value, "Template parameter not of type float"); @@ -45,7 +45,7 @@ inline bool is_zero(const T fVal1) { /* * @brief: Check whether a float is greater than zero */ -template +template inline bool is_positive(const T fVal1) { static_assert(std::is_floating_point::value || std::is_base_of::value, "Template parameter not of type float"); @@ -56,7 +56,7 @@ inline bool is_positive(const T fVal1) { /* * @brief: Check whether a float is less than zero */ -template +template inline bool is_negative(const T fVal1) { static_assert(std::is_floating_point::value || std::is_base_of::value, "Template parameter not of type float"); @@ -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 +template 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 +template 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); * parameter changes the units. Default: 1 == degrees, 10 == dezi, * 100 == centi. */ -template +template float wrap_180(const T angle, float unit_mod = 1); /* * Wrap an angle in centi-degrees. See wrap_180(). */ -template +template 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 +template float wrap_360(const T angle, float unit_mod = 1); /* * Wrap an angle in centi-degrees. See wrap_360(). */ -template +template 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 +template float wrap_PI(const T radian); /* * wrap an angle in radians to 0..2PI */ -template +template float wrap_2PI(const T radian); /* * Constrain a value to be within the range: low and high */ -template +template 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) return rad * RAD_TO_DEG; } -template +template float sq(const T val) { return powf(static_cast(val), 2); @@ -175,7 +175,7 @@ float sq(const T val) * Variadic template for calculating the square norm of a vector of any * dimension. */ -template +template 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) * Variadic template for calculating the norm (pythagoras) of a vector of any * dimension. */ -template +template float norm(const T first, const U second, const Params... parameters) { return sqrtf(sq(first, second, parameters...));