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

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

Browse Source
This commit is contained in:
Matthias Grob 2019-09-17 17:53:54 +02:00 committed by Julian Kent
parent 33a629105c
commit bbaa93880b
2 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
matrix/helper_functions.hpp
View File

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

7
test/helper.cpp
View File

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