From 4f8d2059f8e66078874b68476bbab37f1aff081f Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Tue, 24 Jan 2017 09:57:43 -0800
Subject: [PATCH] AP_Math: use right epsilon for is_equal()

We are calling fabsf(), which returns a float. We should use the epsilon
from float type, not from the argument type passed to fabsf().

On the other hand when the double version is instantiated we do want to
use the std::numeric_limits<double>::epsilon() value.

This adds a branch to the function, but it's removed when the function
is intantiated by the compiler since the type is known at compile-time.

Fixes this warning when building for PX4:
../../libraries/AP_Math/AP_Math.cpp: In instantiation of 'typename std::enable_if<std::is_floating_point<typename std::common_type<_Tp, _Up>::type>::value, bool>::type is_equal(Arithmetic1, Arithmetic2) [with Arithmetic1 = double; Arithmetic2 = double; typename std::enable_if<std::is_floating_point<typename std::common_type<_Tp, _Up>::type>::value, bool>::type = bool]':
../../libraries/AP_Math/AP_Math.cpp:23:66:   required from here
../../libraries/AP_Math/AP_Math.cpp:17:29: warning: implicit conversion from 'float' to 'double' to match other operand of binary expression [-Wdouble-promotion]
     return fabsf(v_1 - v_2) < std::numeric_limits<decltype(v_1 - v_2)>::epsilon();
                             ^
---
 libraries/AP_Math/AP_Math.cpp | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index c0289c6a80..fdd675d44f 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -1,6 +1,12 @@
 #include "AP_Math.h"
+
 #include <float.h>
 
+/*
+ * is_equal(): Integer implementation, provided for convenience and
+ * compatibility with old code. Expands to the same as comparing the values
+ * directly
+ */
 template <class Arithmetic1, class 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)
@@ -9,11 +15,20 @@ is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
     return static_cast<common_type>(v_1) == static_cast<common_type>(v_2);
 }
 
+/*
+ * is_equal(): double/float implementation - takes into account
+ * std::numeric_limits<T>::epsilon() to return if 2 values are equal.
+ */
 template <class Arithmetic1, class 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)
 {
-    return fabsf(v_1 - v_2) < std::numeric_limits<decltype(v_1 - v_2)>::epsilon();
+    typedef typename std::common_type<Arithmetic1, Arithmetic2>::type common_type;
+    typedef typename std::remove_cv<common_type>::type common_type_nonconst;
+    if (std::is_same<double, common_type_nonconst>::value) {
+        return fabs(v_1 - v_2) < std::numeric_limits<double>::epsilon();
+    }
+    return fabsf(v_1 - v_2) < std::numeric_limits<float>::epsilon();
 }
 
 template bool is_equal<int>(const int v_1, const int v_2);