Floating point comparison routines

libuavcan/include/uavcan/build_config.hpp

@@ -15,9 +15,9 @@
  * This definition contains the integer year number after which the standard was named:
  *  - 2003 for C++03
  *  - 2011 for C++11
- * 
+ *
  * This config automatically sets according to the actual C++ standard used by the compiler.
- * 
+ *
  * In C++03 mode the library has almost zero dependency on the C++ standard library, which allows
  * to use it on platforms with a very limited C++ support. On the other hand, C++11 mode requires
  * many parts of the standard library (e.g. <functional>), thus the user might want to force older
@@ -125,7 +125,6 @@
 
 namespace uavcan
 {
-
 /**
  * Memory pool block size.
  *
@@ -173,4 +172,16 @@ struct UAVCAN_EXPORT IsDynamicallyAllocatable
     }
 };
 
+/**
+ * Float comparison precision.
+ * For details refer to:
+ *  http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ *  https://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h
+ */
+#ifdef UAVCAN_FLOAT_COMPARISON_EPSILON_MULT
+static const unsigned FloatComparisonEpsilonMult = UAVCAN_FLOAT_COMPARISON_EPSILON_MULT;
+#else
+static const unsigned FloatComparisonEpsilonMult = 10;
+#endif
+
 }

libuavcan/include/uavcan/marshal/type_util.hpp

@@ -4,8 +4,9 @@
 
 #pragma once
 
-#include <uavcan/util/templates.hpp>
 #include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/comparison.hpp>
 
 namespace uavcan
 {

libuavcan/include/uavcan/util/comparison.hpp

@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/util/templates.hpp>
+#include <uavcan/build_config.hpp>
+
+namespace uavcan
+{
+/**
+ * This function performs fuzzy comparison of two floating point numbers.
+ * Type of T can be either float, double or long double.
+ * For details refer to http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ */
+template <typename T>
+UAVCAN_EXPORT
+inline bool areFloatsClose(T a, T b, const T absolute_epsilon, const T relative_epsilon)
+{
+    // NAN
+    if (isNaN(a) || isNaN(b))
+    {
+        return false;
+    }
+
+    // Infinity
+    if (isInfinity(a) || isInfinity(b))
+    {
+        return a == b;
+    }
+
+    // Close numbers near zero
+    const T diff = std::fabs(a - b);
+    if (diff <= absolute_epsilon)
+    {
+        return true;
+    }
+
+    // General case
+    a = std::fabs(a);
+    b = std::fabs(b);
+    const T largest = (b > a) ? b : a;
+    return (diff <= largest * relative_epsilon);
+}
+
+/**
+ * Generic comparison function.
+ * This function properly handles floating point comparison, including mixed floating point type comparison,
+ * e.g. float with long double.
+ */
+template <typename L, typename R>
+UAVCAN_EXPORT
+inline bool areClose(const L& left, const R& right)
+{
+    return left == right;
+}
+
+/*
+ * Float comparison specializations
+ */
+template <>
+UAVCAN_EXPORT
+inline bool areClose<float, float>(const float& left, const float& right)
+{
+    return areFloatsClose(left, right, NumericTraits<float>::epsilon(),
+                          NumericTraits<float>::epsilon() * FloatComparisonEpsilonMult);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<double, double>(const double& left, const double& right)
+{
+    return areFloatsClose(left, right, NumericTraits<double>::epsilon(),
+                          NumericTraits<double>::epsilon() * FloatComparisonEpsilonMult);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<long double, long double>(const long double& left, const long double& right)
+{
+    return areFloatsClose(left, right, NumericTraits<long double>::epsilon(),
+                          NumericTraits<long double>::epsilon() * FloatComparisonEpsilonMult);
+}
+
+/*
+ * Mixed floating type comparison - coercing larger type to smaller type
+ */
+template <>
+UAVCAN_EXPORT
+inline bool areClose<float, double>(const float& left, const double& right)
+{
+    return areClose(left, static_cast<float>(right));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<double, float>(const double& left, const float& right)
+{
+    return areClose(static_cast<float>(left), right);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<float, long double>(const float& left, const long double& right)
+{
+    return areClose(left, static_cast<float>(right));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<long double, float>(const long double& left, const float& right)
+{
+    return areClose(static_cast<float>(left), right);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<double, long double>(const double& left, const long double& right)
+{
+    return areClose(left, static_cast<double>(right));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<long double, double>(const long double& left, const double& right)
+{
+    return areClose(static_cast<double>(left), right);
+}
+
+}

libuavcan/include/uavcan/util/templates.hpp

@@ -422,4 +422,50 @@ struct UAVCAN_EXPORT NumericTraits<long double>
 };
 #endif
 
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# undef isnan
+# undef isinf
+# undef signbit
+#endif
+
+/**
+ * Replacement for std::isnan()
+ */
+template <typename T>
+inline bool isNaN(T arg)
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+    return std::isnan(arg);
+#else
+    // cppcheck-suppress duplicateExpression
+    return arg != arg;
+#endif
+}
+
+/**
+ * Replacement for std::isinf()
+ */
+template <typename T>
+inline bool isInfinity(T arg)
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+    return std::isinf(arg);
+#else
+    return arg == NumericTraits<T>::infinity() || arg == -NumericTraits<T>::infinity();
+#endif
+}
+
+/**
+ * Replacement for std::signbit()
+ */
+template <typename T>
+inline bool getSignBit(T arg)
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+    return std::signbit(arg);
+#else
+    return arg < T(0) || (arg == T(0) && T(1) / arg < T(0));
+#endif
+}
+
 }

libuavcan/src/marshal/uc_float_spec.cpp

@@ -14,59 +14,24 @@
 # include <limits>
 #endif
 
-#undef signbit
-#undef isnan
-#undef isinf
-
 namespace uavcan
 {
 /*
  * IEEE754Converter
  * Float16 conversion algorithm: http://half.sourceforge.net/ (MIT License)
  */
-template <typename T>
-static inline bool signbit(T arg)
-{
-#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
-    return std::signbit(arg);
-#else
-    return arg < T(0) || (arg == T(0) && T(1) / arg < T(0));
-#endif
-}
-
-template <typename T>
-static inline bool isnan(T arg)
-{
-#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
-    return std::isnan(arg);
-#else
-    // cppcheck-suppress duplicateExpression
-    return arg != arg;
-#endif
-}
-
-template <typename T>
-static inline bool isinf(T arg)
-{
-#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
-    return std::isinf(arg);
-#else
-    return arg == NumericTraits<T>::infinity() || arg == -NumericTraits<T>::infinity();
-#endif
-}
-
 uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value)
 {
-    uint16_t hbits = signbit(value) << 15;
+    uint16_t hbits = static_cast<uint16_t>(getSignBit(value)) << 15;
     if (value == 0.0f)
     {
         return hbits;
     }
-    if (isnan(value))
+    if (isNaN(value))
     {
         return hbits | 0x7FFFU;
     }
-    if (isinf(value))
+    if (isInfinity(value))
     {
         return hbits | 0x7C00U;
     }

libuavcan/test/util/comparison.cpp

@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/util/comparison.hpp>
+
+TEST(Comparison, Basic)
+{
+    // Basic same type floats
+    ASSERT_TRUE(uavcan::areClose(0.1, 0.1));
+    ASSERT_TRUE(uavcan::areClose(0.1F, 0.1F));
+    ASSERT_TRUE(uavcan::areClose(0.1L, 0.1L));
+
+    // Basic mixed type floats
+    ASSERT_TRUE(uavcan::areClose(0.1F, 0.1));
+    ASSERT_TRUE(uavcan::areClose(0.1,  0.1F));
+    ASSERT_TRUE(uavcan::areClose(0.1F, 0.1L));
+    ASSERT_TRUE(uavcan::areClose(0.1L, 0.1F));
+    ASSERT_TRUE(uavcan::areClose(0.1,  0.1L));
+    ASSERT_TRUE(uavcan::areClose(0.1L, 0.1));
+
+    // Basic floats
+    ASSERT_TRUE(uavcan::areClose(0x07, '\x07'));
+    ASSERT_TRUE(uavcan::areClose(123456789LL, 123456789));
+    ASSERT_TRUE(uavcan::areClose("123", std::string("123")));
+
+    // Non-equality
+    ASSERT_FALSE(uavcan::areClose(-0.1, 0.1));
+    ASSERT_FALSE(uavcan::areClose(-0.1F, 0.0L));
+    ASSERT_FALSE(uavcan::areClose("123", std::string("12")));
+    ASSERT_FALSE(uavcan::areClose(0x07L, '\0'));
+}
+
+TEST(Comparison, FloatSpecialCase)
+{
+    ASSERT_FALSE(uavcan::areClose(0.1, std::numeric_limits<double>::infinity()));
+
+    ASSERT_TRUE(uavcan::areClose(std::numeric_limits<float>::infinity(),
+                                 std::numeric_limits<long double>::infinity()));
+
+    ASSERT_FALSE(uavcan::areClose(std::numeric_limits<float>::infinity(), -std::numeric_limits<float>::infinity()));
+
+    ASSERT_FALSE(uavcan::areClose(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN()));
+}
+
+TEST(Comparison, FloatULP)
+{
+    ASSERT_FALSE(uavcan::areClose(0.100000000000000001L, 0.1L));
+    ASSERT_TRUE( uavcan::areClose(0.100000000000000001,  0.1L));
+    ASSERT_TRUE( uavcan::areClose(0.100000000000000001F, 0.1L));
+
+    // Near zero
+    ASSERT_TRUE( uavcan::areClose(0.0F,  std::numeric_limits<float>::epsilon()));
+    ASSERT_TRUE( uavcan::areClose(0.0F, -std::numeric_limits<float>::epsilon()));
+    ASSERT_FALSE(uavcan::areClose(0.0F,  std::numeric_limits<float>::epsilon() * 2));
+}
+
+TEST(Comparison, BruteforceValidation)
+{
+    const std::streamsize default_precision = std::cout.precision();
+    std::cout.precision(20);
+
+    float x = -uavcan::NumericTraits<float>::max();
+
+    while (x < uavcan::NumericTraits<float>::max())
+    {
+        const float y1 = x + std::abs(x) * (uavcan::NumericTraits<float>::epsilon() * 2);                // Still equal
+        const float y2 = x + uavcan::max(std::abs(x), 1.F) * (uavcan::NumericTraits<float>::epsilon() * 20); // Nope
+
+        if (!uavcan::areClose(y1, x))
+        {
+            std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl;
+            ASSERT_TRUE(false);
+        }
+        if (uavcan::areClose(y2, x))
+        {
+            std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl;
+            ASSERT_TRUE(false);
+        }
+
+        x = y2;
+    }
+
+    std::cout.precision(default_precision);
+}