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
+}
+
 }