helper: consider matrices with the same NANs and INFINITYs equal

to simplify bulk checks when these values are expected
PX4 · Sep 18, 2019 · c34e8dc · c34e8dc
1 parent bbaa938
commit c34e8dc
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 12 deletions.
diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
@@ -23,8 +23,8 @@ bool is_finite(Type x) {
 /**
  * Compare if two floating point numbers are equal
  *
- * Note: Smaller or EQUAL than is important to correctly
- * handle the comparison to infinite or nan.
+ * NAN is considered equal to NAN and -NAN
+ * INFINITY is considered equal INFINITY but not -INFINITY
  *
  * @param x right side of equality check
  * @param y left side of equality check
@@ -36,7 +36,7 @@ bool isEqualF(const Type x, const Type y, const Type eps = 1e-4f)
 {
     return (matrix::fabs(x - y) <= eps)
            || (isnan(x) && isnan(y))
-           || (isinf(x) && isinf(y));
+           || (isinf(x) && isinf(y) && isnan(x - y));
 }
 
 /**

diff --git a/test/helper.cpp b/test/helper.cpp
@@ -48,7 +48,11 @@ int main()
     TEST(!isEqualF(1.f, INFINITY));
     TEST(isEqualF(NAN, NAN));
     TEST(isEqualF(NAN, -NAN));
+    TEST(isEqualF(-NAN, NAN));
     TEST(isEqualF(INFINITY, INFINITY));
+    TEST(!isEqualF(INFINITY, -INFINITY));
+    TEST(!isEqualF(-INFINITY, INFINITY));
+    TEST(isEqualF(-INFINITY, -INFINITY));
 
     Vector3f a(1, 2, 3);
     Vector3f b(4, 5, 6);

diff --git a/test/matrixMult.cpp b/test/matrixMult.cpp
@@ -28,16 +28,9 @@ int main()
     TEST(isEqual(B, B_check));
     Matrix3f C = B_check.edivide(C_check);
 
+    float off_diagonal_nan[9] = {2, NAN, NAN, NAN, 2, NAN, NAN, NAN, 2};
     // off diagonal are NANs because division by 0
-    for (size_t i = 0; i < 3; i++) {
-        for (size_t j = 0; j < 3; j++) {
-            if (i == j) {
-                TEST(isEqualF(C(i,j), 2.f));
-            } else {
-                TEST(isnan(C(i,j)));
-            }
-        }
-    }
+    TEST(C == Matrix3f(off_diagonal_nan));
 
     // Test non-square matrix
     float data_43[12] = {1,3,2,