helper: consider NAN equal to NAN such that vectors can be compared e…

…xactly
PX4 · Sep 18, 2019 · bbaa938 · bbaa938
1 parent 33a6291
commit bbaa938
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 4 deletions.
diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
@@ -34,7 +34,9 @@ bool is_finite(Type x) {
 template<typename Type>
 bool isEqualF(const Type x, const Type y, const Type eps = 1e-4f)
 {
-    return matrix::fabs(x - y) <= eps;
+    return (matrix::fabs(x - y) <= eps)
+           || (isnan(x) && isnan(y))
+           || (isinf(x) && isinf(y));
 }
 
 /**

diff --git a/test/helper.cpp b/test/helper.cpp
@@ -46,8 +46,9 @@ int main()
     TEST(!isEqualF(1.f, NAN));
     TEST(!isEqualF(INFINITY, 1.f));
     TEST(!isEqualF(1.f, INFINITY));
-    TEST(!isEqualF(NAN, NAN));
-    TEST(!isEqualF(INFINITY, INFINITY));
+    TEST(isEqualF(NAN, NAN));
+    TEST(isEqualF(NAN, -NAN));
+    TEST(isEqualF(INFINITY, INFINITY));
 
     Vector3f a(1, 2, 3);
     Vector3f b(4, 5, 6);
@@ -58,7 +59,7 @@ int main()
     Vector3f d(1, 2, NAN);
     TEST(!isEqual(c, d));
     TEST(isEqual(c, c));
-    TEST(!isEqual(d, d));
+    TEST(isEqual(d, d));
 
     return 0;
 }