mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-06 23:18:52 +08:00
helper: consider matrices with the same NANs and INFINITYs equal
to simplify bulk checks when these values are expected
This commit is contained in:
committed by
Julian Kent
parent
bbaa93880b
commit
c34e8dc98f
@@ -23,8 +23,8 @@ bool is_finite(Type x) {
|
|||||||
/**
|
/**
|
||||||
* Compare if two floating point numbers are equal
|
* Compare if two floating point numbers are equal
|
||||||
*
|
*
|
||||||
* Note: Smaller or EQUAL than is important to correctly
|
* NAN is considered equal to NAN and -NAN
|
||||||
* handle the comparison to infinite or nan.
|
* INFINITY is considered equal INFINITY but not -INFINITY
|
||||||
*
|
*
|
||||||
* @param x right side of equality check
|
* @param x right side of equality check
|
||||||
* @param y left 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)
|
return (matrix::fabs(x - y) <= eps)
|
||||||
|| (isnan(x) && isnan(y))
|
|| (isnan(x) && isnan(y))
|
||||||
|| (isinf(x) && isinf(y));
|
|| (isinf(x) && isinf(y) && isnan(x - y));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -48,7 +48,11 @@ int main()
|
|||||||
TEST(!isEqualF(1.f, INFINITY));
|
TEST(!isEqualF(1.f, INFINITY));
|
||||||
TEST(isEqualF(NAN, NAN));
|
TEST(isEqualF(NAN, NAN));
|
||||||
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));
|
||||||
|
TEST(isEqualF(-INFINITY, -INFINITY));
|
||||||
|
|
||||||
Vector3f a(1, 2, 3);
|
Vector3f a(1, 2, 3);
|
||||||
Vector3f b(4, 5, 6);
|
Vector3f b(4, 5, 6);
|
||||||
|
|||||||
+2
-9
@@ -28,16 +28,9 @@ int main()
|
|||||||
TEST(isEqual(B, B_check));
|
TEST(isEqual(B, B_check));
|
||||||
Matrix3f C = B_check.edivide(C_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
|
// off diagonal are NANs because division by 0
|
||||||
for (size_t i = 0; i < 3; i++) {
|
TEST(C == Matrix3f(off_diagonal_nan));
|
||||||
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 non-square matrix
|
// Test non-square matrix
|
||||||
float data_43[12] = {1,3,2,
|
float data_43[12] = {1,3,2,
|
||||||
|
|||||||
Reference in New Issue
Block a user