diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp index dbcb307..3063051 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -66,8 +66,8 @@ class SquareMatrix : public Matrix return res; } - // get matrix upper right triangle - Vector urt() const + // get matrix upper right triangle in a row-major vector format + Vector upper_right_triangle() const { Vector res; const SquareMatrix &self = *this; @@ -149,7 +149,7 @@ bool inv(const SquareMatrix & A, SquareMatrix & inv) for (size_t n = 0; n < M; n++) { // if diagonal is zero, swap with row below - if (fabs(static_cast(U(n, n))) < __FLT_EPSILON__) { + if (fabs(static_cast(U(n, n))) < FLT_EPSILON) { //printf("trying pivot for row %d\n",n); for (size_t i = n + 1; i < M; i++) { @@ -174,7 +174,7 @@ bool inv(const SquareMatrix & A, SquareMatrix & inv) #endif // failsafe, return zero matrix - if (fabs(static_cast(U(n, n))) < __FLT_EPSILON__) { + if (fabs(static_cast(U(n, n))) < FLT_EPSILON) { return false; } diff --git a/matrix/stdlib_imports.hpp b/matrix/stdlib_imports.hpp index 1f6b88b..6e98a0d 100644 --- a/matrix/stdlib_imports.hpp +++ b/matrix/stdlib_imports.hpp @@ -15,6 +15,10 @@ namespace matrix { +#if !defined(FLT_EPSILON) +#define FLT_EPSILON __FLT_EPSILON__ +#endif + #if defined(__PX4_NUTTX) /* * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually. diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2453f70..0dc8480 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -18,7 +18,7 @@ set(tests hatvee copyto least_squares - urt + upperRightTriangle ) add_custom_target(test_build) diff --git a/test/helper.cpp b/test/helper.cpp index dda3c90..a21c90a 100644 --- a/test/helper.cpp +++ b/test/helper.cpp @@ -5,15 +5,15 @@ using namespace matrix; int main() { - TEST(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < __FLT_EPSILON__); - TEST(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < __FLT_EPSILON__); - TEST(fabs(wrap_pi(3.0) - (3.0)) < __FLT_EPSILON__); + TEST(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < FLT_EPSILON); + TEST(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < FLT_EPSILON); + TEST(fabs(wrap_pi(3.0) - (3.0)) < FLT_EPSILON); TEST(!is_finite(wrap_pi(1000.0f))); TEST(!is_finite(wrap_pi(-1000.0f))); wrap_pi(NAN); - TEST(fabs(wrap_2pi(-4.0) - (-4.0 + 2*M_PI)) < __FLT_EPSILON__); - TEST(fabs(wrap_2pi(3.0) - (3.0)) < __FLT_EPSILON__); + TEST(fabs(wrap_2pi(-4.0) - (-4.0 + 2*M_PI)) < FLT_EPSILON); + TEST(fabs(wrap_2pi(3.0) - (3.0)) < FLT_EPSILON); TEST(!is_finite(wrap_2pi(1000.0f))); TEST(!is_finite(wrap_2pi(-1000.0f))); wrap_2pi(NAN); diff --git a/test/matrixAssignment.cpp b/test/matrixAssignment.cpp index 81e3412..994af85 100644 --- a/test/matrixAssignment.cpp +++ b/test/matrixAssignment.cpp @@ -22,7 +22,7 @@ int main() Matrix3f m2(data); for(int i=0; i<9; i++) { - TEST(fabs(data[i] - m2.data()[i]) < __FLT_EPSILON__); + TEST(fabs(data[i] - m2.data()[i]) < FLT_EPSILON); } Matrix3f m_nan; @@ -38,7 +38,7 @@ int main() }; m2 = Matrix3f(data2d); for(int i=0; i<9; i++) { - TEST(fabs(data[i] - m2.data()[i]) < __FLT_EPSILON__); + TEST(fabs(data[i] - m2.data()[i]) < FLT_EPSILON); } float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; @@ -100,16 +100,16 @@ int main() m4.swapCols(2, 2); TEST(isEqual(m4, Matrix3f(data))); - TEST(fabs(m4.min() - 1) < __FLT_EPSILON__); - TEST(fabs((-m4).min() + 9) < __FLT_EPSILON__); + TEST(fabs(m4.min() - 1) < FLT_EPSILON); + TEST(fabs((-m4).min() + 9) < FLT_EPSILON); Scalar s = 1; const Vector & s_vect = s; - TEST(fabs(s - 1) < __FLT_EPSILON__); - TEST(fabs(s_vect(0) - 1.0f) < __FLT_EPSILON__); + TEST(fabs(s - 1) < FLT_EPSILON); + TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON); Matrix m5 = s; - TEST(fabs(m5(0,0) - s) < __FLT_EPSILON__); + TEST(fabs(m5(0,0) - s) < FLT_EPSILON); Matrix m6; m6.setRow(0, Vector2f(1, 2)); diff --git a/test/setIdentity.cpp b/test/setIdentity.cpp index fd96791..289e286 100644 --- a/test/setIdentity.cpp +++ b/test/setIdentity.cpp @@ -11,10 +11,10 @@ int main() for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { if (i == j) { - TEST(fabs(A(i, j) - 1) < __FLT_EPSILON__); + TEST(fabs(A(i, j) - 1) < FLT_EPSILON); } else { - TEST(fabs(A(i, j) - 0) < __FLT_EPSILON__); + TEST(fabs(A(i, j) - 0) < FLT_EPSILON); } } } @@ -25,10 +25,10 @@ int main() for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { if (i == j) { - TEST(fabs(B(i, j) - 1) < __FLT_EPSILON__); + TEST(fabs(B(i, j) - 1) < FLT_EPSILON); } else { - TEST(fabs(B(i, j) - 0) < __FLT_EPSILON__); + TEST(fabs(B(i, j) - 0) < FLT_EPSILON); } } } diff --git a/test/squareMatrix.cpp b/test/squareMatrix.cpp index 1fc4bed..861139e 100644 --- a/test/squareMatrix.cpp +++ b/test/squareMatrix.cpp @@ -14,7 +14,7 @@ int main() Vector3 diag_check(1, 5, 10); TEST(isEqual(A.diag(), diag_check)); - TEST(A.trace() - 16 < __FLT_EPSILON__); + TEST(A.trace() - 16 < FLT_EPSILON); float data_check[9] = { 1.01158503f, 0.02190432f, 0.03238144f, diff --git a/test/test_data.py b/test/test_data.py index 85fa79c..61ef1d0 100644 --- a/test/test_data.py +++ b/test/test_data.py @@ -75,9 +75,9 @@ def quat_to_dcm(q): print('euler', phi, theta, psi) q = euler_to_quat(phi, theta, psi) -assert(abs(norm(q) - 1) < __FLT_EPSILON__) -assert(abs(norm(q) - 1) < __FLT_EPSILON__) -assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < __FLT_EPSILON__) +assert(abs(norm(q) - 1) < FLT_EPSILON) +assert(abs(norm(q) - 1) < FLT_EPSILON) +assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON) print('\nq:') pprint(q) @@ -85,8 +85,8 @@ def quat_to_dcm(q): assert(norm(dcm[:,0]) == 1) assert(norm(dcm[:,1]) == 1) assert(norm(dcm[:,2]) == 1) -assert(abs(dcm[:,0].dot(dcm[:,1])) < __FLT_EPSILON__) -assert(abs(dcm[:,0].dot(dcm[:,2])) < __FLT_EPSILON__) +assert(abs(dcm[:,0].dot(dcm[:,1])) < FLT_EPSILON) +assert(abs(dcm[:,0].dot(dcm[:,2])) < FLT_EPSILON) print('\ndcm:') pprint(dcm) diff --git a/test/test_macros.hpp b/test/test_macros.hpp index f21c5a6..d6ca2d2 100644 --- a/test/test_macros.hpp +++ b/test/test_macros.hpp @@ -10,7 +10,6 @@ #include #include // cmath has to be introduced BEFORE we poison the C library identifiers -#include "float.h" #define TEST(X) if(!(X)) { fprintf(stderr, "test failed on %s:%d\n", __FILE__, __LINE__); return -1;} diff --git a/test/urt.cpp b/test/upperRightTriangle.cpp similarity index 83% rename from test/urt.cpp rename to test/upperRightTriangle.cpp index 7b54d05..9807365 100644 --- a/test/urt.cpp +++ b/test/upperRightTriangle.cpp @@ -14,7 +14,7 @@ int main() SquareMatrix A(data); for(int i=0; i<6; i++) { - TEST(fabs(urt[i] - A.urt().data()[i]) < __FLT_EPSILON__); + TEST(fabs(urt[i] - A.upper_right_triangle().data()[i]) < FLT_EPSILON); } return 0; diff --git a/test/vector2.cpp b/test/vector2.cpp index d5bf782..1ea88fc 100644 --- a/test/vector2.cpp +++ b/test/vector2.cpp @@ -10,29 +10,29 @@ int main() { Vector2f a(1, 0); Vector2f b(0, 1); - TEST(fabs(a % b - 1.0f) < __FLT_EPSILON__); + TEST(fabs(a % b - 1.0f) < FLT_EPSILON); Vector2f c; - TEST(fabs(c(0) - 0) < __FLT_EPSILON__); - TEST(fabs(c(1) - 0) < __FLT_EPSILON__); + TEST(fabs(c(0) - 0) < FLT_EPSILON); + TEST(fabs(c(1) - 0) < FLT_EPSILON); Matrix d(a); - TEST(fabs(d(0,0) - 1) < __FLT_EPSILON__); - TEST(fabs(d(1,0) - 0) < __FLT_EPSILON__); + TEST(fabs(d(0,0) - 1) < FLT_EPSILON); + TEST(fabs(d(1,0) - 0) < FLT_EPSILON); Vector2f e(d); - TEST(fabs(e(0) - 1) < __FLT_EPSILON__); - TEST(fabs(e(1) - 0) < __FLT_EPSILON__); + TEST(fabs(e(0) - 1) < FLT_EPSILON); + TEST(fabs(e(1) - 0) < FLT_EPSILON); float data[] = {4,5}; Vector2f f(data); - TEST(fabs(f(0) - 4) < __FLT_EPSILON__); - TEST(fabs(f(1) - 5) < __FLT_EPSILON__); + TEST(fabs(f(0) - 4) < FLT_EPSILON); + TEST(fabs(f(1) - 5) < FLT_EPSILON); Vector3f g(1.23f, 423.4f, 3221.f); Vector2f h(g); - TEST(fabs(h(0) - 1.23f) < __FLT_EPSILON__); - TEST(fabs(h(1) - 423.4f) < __FLT_EPSILON__); + TEST(fabs(h(0) - 1.23f) < FLT_EPSILON); + TEST(fabs(h(1) - 423.4f) < FLT_EPSILON); return 0; }