fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

helper: consider matrices with the same NANs and INFINITYs equal

Browse Source 
to simplify bulk checks when these values are expected
This commit is contained in:
Matthias Grob 2019-09-17 21:02:21 +02:00 committed by Julian Kent
parent bbaa93880b
commit c34e8dc98f
3 changed files with 9 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
matrix/helper_functions.hpp
View File

@ -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));
}
/**

4
test/helper.cpp
View File

@ -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);

11
test/matrixMult.cpp
View File

@ -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,