AP_Math: specialize float and double functions to use fabsf() and simple comparison otherwise

libraries/AP_Math/AP_Math.h

@@ -53,7 +53,7 @@ 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");
-    return (fabsf(static_cast<float>(fVal1)) < FLT_EPSILON);
+    return is_zero(static_cast<float>(fVal1));
 }
 
 /* 
@@ -77,14 +77,6 @@ inline bool is_negative(const T fVal1) {
     return (static_cast<float>(fVal1) <= (-1.0 * FLT_EPSILON));
 }
 
-
-/*
- * @brief: Check whether a double is zero
- */
-inline bool is_zero(const double fVal1) {
-    return (fabsf(fVal1) < static_cast<double>(FLT_EPSILON));
-}
-
 /*
  * @brief: Check whether a double is greater than zero
  */

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<float>(x)) < FLT_EPSILON;
+#endif
+}

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<n;j++){
             if (std::is_same<T, double>::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;
                 }
             }

libraries/AP_Math/quaternion.cpp

@@ -655,10 +655,7 @@ void QuaternionT<T>::normalize(void)
 // Checks if each element of the quaternion is zero
 template <typename T>
 bool QuaternionT<T>::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<T>::zero(void)
 template <typename T>
 bool QuaternionT<T>::is_unit_length(void) const
 {
-    if (fabsf(length_squared() - 1) < 1E-3) {
+    if (fabsF(length_squared() - 1) < 1E-3) {
         return true;
     }
 

libraries/AP_Math/vector2.cpp

@@ -176,7 +176,7 @@ bool Vector2<T>::segment_intersection(const Vector2<T>& seg1_start, const Vector
     const Vector2<T> 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 {

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<float>::is_zero(void) const {
+    return ::is_zero(x) && ::is_zero(y);
+}
+
+template<> inline bool Vector2<double>::is_zero(void) const {
+    return ::is_zero(x) && ::is_zero(y);
+}
+
 typedef Vector2<int16_t>        Vector2i;
 typedef Vector2<uint16_t>       Vector2ui;
 typedef Vector2<int32_t>        Vector2l;

libraries/AP_Math/vector3.cpp

@@ -587,7 +587,7 @@ void Vector3<T>::segment_to_segment_closest_point(const Vector3<T>& 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<T>::segment_plane_intersect(const Vector3<T>& 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;

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<T> perpendicular(const Vector3<T> &p1, const Vector3<T> &v1)
     {
         const T d = p1 * v1;
-        if (fabsf(d) < FLT_EPSILON) {
+        if (::is_zero(d)) {
             return p1;
         }
         const Vector3<T> parallel = (v1 * d) / v1.length_squared();
@@ -315,6 +313,15 @@ public:
     static bool segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point);
 };
 
+// check if all elements are zero
+template<> inline bool Vector3<float>::is_zero(void) const {
+    return ::is_zero(x) && ::is_zero(y) && ::is_zero(z);
+}
+
+template<> inline bool Vector3<double>::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)