From 87a369727a43d2d907c32b993619e7cf42662f25 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 25 Jan 2022 21:15:41 +0100 Subject: [PATCH] AP_Math: specialize float and double functions to use fabsf() and simple comparison otherwise --- libraries/AP_Math/AP_Math.h | 10 +--------- libraries/AP_Math/ftype.h | 20 ++++++++++++++++++++ libraries/AP_Math/matrix_alg.cpp | 4 ++-- libraries/AP_Math/quaternion.cpp | 7 ++----- libraries/AP_Math/vector2.cpp | 2 +- libraries/AP_Math/vector2.h | 17 +++++++++++++---- libraries/AP_Math/vector3.cpp | 4 ++-- libraries/AP_Math/vector3.h | 15 +++++++++++---- 8 files changed, 52 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 7d458c658a..5b71dea1f9 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -53,7 +53,7 @@ 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"); - return (fabsf(static_cast(fVal1)) < FLT_EPSILON); + return is_zero(static_cast(fVal1)); } /* @@ -77,14 +77,6 @@ inline bool is_negative(const T fVal1) { return (static_cast(fVal1) <= (-1.0 * FLT_EPSILON)); } - -/* - * @brief: Check whether a double is zero - */ -inline bool is_zero(const double fVal1) { - return (fabsf(fVal1) < static_cast(FLT_EPSILON)); -} - /* * @brief: Check whether a double is greater than zero */ diff --git a/libraries/AP_Math/ftype.h b/libraries/AP_Math/ftype.h index b68170cd13..04cf0b5027 100644 --- a/libraries/AP_Math/ftype.h +++ b/libraries/AP_Math/ftype.h @@ -27,6 +27,7 @@ typedef double ftype; #define ceilF(x) ceil(x) #define fminF(x,y) fmin(x,y) #define fmodF(x,y) fmod(x,y) +#define fabsF(x) fabs(x) #define toftype todouble #else typedef float ftype; @@ -45,6 +46,7 @@ typedef float ftype; #define ceilF(x) ceilf(x) #define fminF(x,y) fminf(x,y) #define fmodF(x,y) fmodf(x,y) +#define fabsF(x) fabsf(x) #define toftype tofloat #endif @@ -53,3 +55,21 @@ typedef float ftype; #else #define ZERO_FARRAY(a) memset(a, 0, sizeof(a)) #endif + +/* + * @brief: Check whether a float is zero + */ +inline bool is_zero(const float x) { + return fabsf(x) < FLT_EPSILON; +} + +/* + * @brief: Check whether a double is zero + */ +inline bool is_zero(const double x) { +#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS + return fabs(x) < FLT_EPSILON; +#else + return fabsf(static_cast(x)) < FLT_EPSILON; +#endif +} diff --git a/libraries/AP_Math/matrix_alg.cpp b/libraries/AP_Math/matrix_alg.cpp index c2962a5b0f..cda345b21b 100644 --- a/libraries/AP_Math/matrix_alg.cpp +++ b/libraries/AP_Math/matrix_alg.cpp @@ -78,11 +78,11 @@ static void mat_pivot(const T* A, T* pivot, uint16_t n) uint16_t max_j = i; for(uint16_t j=i;j::value) { - if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) { + if(fabsF(A[j*n + i]) > fabsF(A[max_j*n + i])) { max_j = j; } } else { - if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) { + if(fabsF(A[j*n + i]) > fabsF(A[max_j*n + i])) { max_j = j; } } diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index ee24d3a1bb..93918818b5 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -655,10 +655,7 @@ void QuaternionT::normalize(void) // Checks if each element of the quaternion is zero template bool QuaternionT::is_zero(void) const { - return (fabsf(q1) < FLT_EPSILON) && - (fabsf(q2) < FLT_EPSILON) && - (fabsf(q3) < FLT_EPSILON) && - (fabsf(q4) < FLT_EPSILON); + return ::is_zero(q1) && ::is_zero(q2) && ::is_zero(q3) && ::is_zero(q4); } // zeros the quaternion to [0, 0, 0, 0], an invalid quaternion @@ -675,7 +672,7 @@ void QuaternionT::zero(void) template bool QuaternionT::is_unit_length(void) const { - if (fabsf(length_squared() - 1) < 1E-3) { + if (fabsF(length_squared() - 1) < 1E-3) { return true; } diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp index 35d4826597..a04fe80f8d 100644 --- a/libraries/AP_Math/vector2.cpp +++ b/libraries/AP_Math/vector2.cpp @@ -176,7 +176,7 @@ bool Vector2::segment_intersection(const Vector2& seg1_start, const Vector const Vector2 ss2_ss1 = seg2_start - seg1_start; const T r1xr2 = r1 % r2; const T q_pxr = ss2_ss1 % r1; - if (fabsf(r1xr2) < FLT_EPSILON) { + if (::is_zero(r1xr2)) { // either collinear or parallel and non-intersecting return false; } else { diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index 466dddbe96..35a61d5687 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -108,7 +108,7 @@ struct Vector2 // check if all elements are zero bool is_zero(void) const WARN_IF_UNUSED { - return (fabsf(x) < FLT_EPSILON) && (fabsf(y) < FLT_EPSILON); + return x == 0 && y == 0; } // allow a vector2 to be used as an array, 0 indexed @@ -242,14 +242,14 @@ struct Vector2 const T expected_run = seg_end.x-seg_start.x; const T intersection_run = point.x-seg_start.x; // check slopes are identical: - if (fabsf(expected_run) < FLT_EPSILON) { - if (fabsf(intersection_run) > FLT_EPSILON) { + if (::is_zero(expected_run)) { + if (fabsF(intersection_run) > FLT_EPSILON) { return false; } } else { const T expected_slope = (seg_end.y-seg_start.y)/expected_run; const T intersection_slope = (point.y-seg_start.y)/intersection_run; - if (fabsf(expected_slope - intersection_slope) > FLT_EPSILON) { + if (fabsF(expected_slope - intersection_slope) > FLT_EPSILON) { return false; } } @@ -276,6 +276,15 @@ struct Vector2 } }; +// check if all elements are zero +template<> inline bool Vector2::is_zero(void) const { + return ::is_zero(x) && ::is_zero(y); +} + +template<> inline bool Vector2::is_zero(void) const { + return ::is_zero(x) && ::is_zero(y); +} + typedef Vector2 Vector2i; typedef Vector2 Vector2ui; typedef Vector2 Vector2l; diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 00a2248b7b..95864cca7f 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -587,7 +587,7 @@ void Vector3::segment_to_segment_closest_point(const Vector3& seg1_start, } } // finally do the division to get tc - tc = (fabsf(tN) < FLT_EPSILON ? 0.0 : tN / tD); + tc = (::is_zero(tN) ? 0.0 : tN / tD); // closest point on seg2 closest_point = seg2_start + line2*tc; @@ -603,7 +603,7 @@ bool Vector3::segment_plane_intersect(const Vector3& seg_start, const Vect T D = plane_normal * u; T N = -(plane_normal * w); - if (fabsf(D) < FLT_EPSILON) { + if (::is_zero(D)) { if (::is_zero(N)) { // segment lies in this plane return true; diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index e471a0a83c..3862a23ef4 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -187,9 +187,7 @@ public: // check if all elements are zero bool is_zero(void) const WARN_IF_UNUSED { - return (fabsf(x) < FLT_EPSILON) && - (fabsf(y) < FLT_EPSILON) && - (fabsf(z) < FLT_EPSILON); + return x == 0 && y == 0 && z == 0; } @@ -290,7 +288,7 @@ public: static Vector3 perpendicular(const Vector3 &p1, const Vector3 &v1) { const T d = p1 * v1; - if (fabsf(d) < FLT_EPSILON) { + if (::is_zero(d)) { return p1; } const Vector3 parallel = (v1 * d) / v1.length_squared(); @@ -315,6 +313,15 @@ public: static bool segment_plane_intersect(const Vector3& seg_start, const Vector3& seg_end, const Vector3& plane_normal, const Vector3& plane_point); }; +// check if all elements are zero +template<> inline bool Vector3::is_zero(void) const { + return ::is_zero(x) && ::is_zero(y) && ::is_zero(z); +} + +template<> inline bool Vector3::is_zero(void) const { + return ::is_zero(x) && ::is_zero(y) && ::is_zero(z); +} + // The creation of temporary vector objects as return types creates a significant overhead in certain hot // code paths. This allows callers to select the inline versions where profiling shows a significant benefit #if defined(AP_INLINE_VECTOR_OPS) && !defined(HAL_DEBUG_BUILD)