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

Implement one float equality check and use it everywhere

Browse Source
This commit is contained in:
Matthias Grob 2019-09-16 16:07:50 +02:00 committed by Julian Kent
parent 1e80807e8e
commit 5844b0e46e
2 changed files with 19 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
matrix/Matrix.hpp
View File

@ -295,12 +295,9 @@ public:
{
const Matrix<Type, M, N> &self = *this;
// TODO: set this based on Type
static constexpr float eps = 1e-4f;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (fabs(self(i, j) - other(i, j)) > eps) {
if (!isEqualF(self(i, j), other(i, j))) {
return false;
}
}
@ -560,43 +557,7 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
template<typename Type, size_t M, size_t N>
bool isEqual(const Matrix<Type, M, N> &x,
const Matrix<Type, M, N> &y, const Type eps=1e-4f) {
bool equal = true;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (fabs(x(i, j) - y(i, j)) > eps) {
equal = false;
break;
}
}
if (equal == false) break;
}
if (!equal) {
printf("not equal\nx:\n");
x.print();
printf("y:\n");
y.print();
}
return equal;
}
template<typename Type>
bool isEqualF(Type x,
Type y, Type eps=1e-4f) {
bool equal = true;
if (fabs(x - y) > eps) {
equal = false;
}
if (!equal) {
printf("not equal\nx:\n%g\ny:\n%g\n", double(x), double(y));
}
return equal;
return x == y;
}
#if defined(SUPPORT_STDIOSTREAM)

17
matrix/helper_functions.hpp
View File

@ -20,6 +20,23 @@ bool is_finite(Type x) {
#endif
}
/**
* Compare if two floating point numbers are equal
*
* Note: Smaller or EQUAL than is important to correctly
* handle the comparison to infinite or nan.
*
* @param x right side of equality check
* @param y left side of equality check
* @param eps numerical tolerance of the check
* @return true if the two values are considered equal, false otherwise
*/
template<typename Type>
bool isEqualF(const Type x, const Type y, const Type eps = 1e-4f)
{
return matrix::fabs(x - y) <= eps;
}
/**
* Wrap value to stay in range [low, high)
*