From 7f65e7f9052000ba0f6bee4ee3ceaf23efa49e75 Mon Sep 17 00:00:00 2001 From: Erik Hofman Date: Fri, 3 Feb 2017 15:07:35 +0100 Subject: [PATCH] We need to copy the matrix before altering it's contents: Fix a SGMathTest failure --- simgear/math/SGMathTest.cxx | 103 +++++++++++++++--------------- simgear/math/simd.hxx | 9 ++- simgear/math/simd4x4.hxx | 45 ++++++++----- simgear/simgear_config_cmake.h.in | 1 + 4 files changed, 92 insertions(+), 66 deletions(-) diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx index 247d9e72..c36b6e3e 100644 --- a/simgear/math/SGMathTest.cxx +++ b/simgear/math/SGMathTest.cxx @@ -28,6 +28,9 @@ #include "SGRect.hxx" #include "sg_random.h" +int lineno = 0; + + template bool Vec3Test(void) @@ -38,45 +41,45 @@ Vec3Test(void) v1 = SGVec3(1, 2, 3); v2 = SGVec3(3, 2, 1); if (equivalent(v1, v2)) - return false; + { lineno = __LINE__; return false; } // Check the unary minus operator v3 = SGVec3(-1, -2, -3); if (!equivalent(-v1, v3)) - return false; + { lineno = __LINE__; return false; } // Check the unary plus operator v3 = SGVec3(1, 2, 3); if (!equivalent(+v1, v3)) - return false; + { lineno = __LINE__; return false; } // Check the addition operator v3 = SGVec3(4, 4, 4); if (!equivalent(v1 + v2, v3)) - return false; + { lineno = __LINE__; return false; } // Check the subtraction operator v3 = SGVec3(-2, 0, 2); if (!equivalent(v1 - v2, v3)) - return false; + { lineno = __LINE__; return false; } // Check the scaler multiplication operator v3 = SGVec3(2, 4, 6); if (!equivalent(2*v1, v3)) - return false; + { lineno = __LINE__; return false; } // Check the dot product if (fabs(dot(v1, v2) - 10) > 10*SGLimits::epsilon()) - return false; + { lineno = __LINE__; return false; } // Check the cross product v3 = SGVec3(-4, 8, -4); if (!equivalent(cross(v1, v2), v3)) - return false; + { lineno = __LINE__; return false; } // Check the euclidean length if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits::epsilon()) - return false; + { lineno = __LINE__; return false; } return true; } @@ -89,11 +92,11 @@ isSameRotation(const SGQuat& q1, const SGQuat& q2) const SGVec3 e2(0, 1, 0); const SGVec3 e3(0, 0, 1); if (!equivalent(q1.transform(e1), q2.transform(e1))) - return false; + { lineno = __LINE__; return false; } if (!equivalent(q1.transform(e2), q2.transform(e2))) - return false; + { lineno = __LINE__; return false; } if (!equivalent(q1.transform(e3), q2.transform(e3))) - return false; + { lineno = __LINE__; return false; } return true; } @@ -111,37 +114,37 @@ QuatTest(void) v1 = SGVec3(1, 2, 3); v2 = SGVec3(1, -2, -3); if (!equivalent(q1.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } // Check a rotation around the x axis q1 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e1); v2 = SGVec3(1, 3, -2); if (!equivalent(q1.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } // Check a rotation around the y axis q1 = SGQuat::fromAngleAxis(SGMisc::pi(), e2); v2 = SGVec3(-1, 2, -3); if (!equivalent(q1.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } // Check a rotation around the y axis q1 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e2); v2 = SGVec3(-3, 2, 1); if (!equivalent(q1.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } // Check a rotation around the z axis q1 = SGQuat::fromAngleAxis(SGMisc::pi(), e3); v2 = SGVec3(-1, -2, 3); if (!equivalent(q1.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } // Check a rotation around the z axis q1 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e3); v2 = SGVec3(2, -1, 3); if (!equivalent(q1.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } // Now check some successive transforms // We can reuse the prevously tested stuff @@ -150,7 +153,7 @@ QuatTest(void) q3 = q1*q2; v2 = q2.transform(q1.transform(v1)); if (!equivalent(q3.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } /// Test from Euler angles float x = 0.2*SGMisc::pi(); @@ -162,7 +165,7 @@ QuatTest(void) v2 = q3.transform(q2.transform(q1.transform(v1))); q4 = SGQuat::fromEulerRad(z, y, x); if (!equivalent(q4.transform(v1), v2)) - return false; + { lineno = __LINE__; return false; } /// Test angle axis forward and back transform q1 = SGQuat::fromAngleAxis(0.2*SGMisc::pi(), e1); @@ -172,15 +175,15 @@ QuatTest(void) q1.getAngleAxis(angleAxis); q4 = SGQuat::fromAngleAxis(angleAxis); if (!isSameRotation(q1, q4)) - return false; + { lineno = __LINE__; return false; } q2.getAngleAxis(angleAxis); q4 = SGQuat::fromAngleAxis(angleAxis); if (!isSameRotation(q2, q4)) - return false; + { lineno = __LINE__; return false; } q3.getAngleAxis(angleAxis); q4 = SGQuat::fromAngleAxis(angleAxis); if (!isSameRotation(q3, q4)) - return false; + { lineno = __LINE__; return false; } /// Test angle axis forward and back transform q1 = SGQuat::fromAngleAxis(0.2*SGMisc::pi(), e1); @@ -189,15 +192,15 @@ QuatTest(void) SGVec3 positiveAngleAxis = q1.getPositiveRealImag(); q4 = SGQuat::fromPositiveRealImag(positiveAngleAxis); if (!isSameRotation(q1, q4)) - return false; + { lineno = __LINE__; return false; } positiveAngleAxis = q2.getPositiveRealImag(); q4 = SGQuat::fromPositiveRealImag(positiveAngleAxis); if (!isSameRotation(q2, q4)) - return false; + { lineno = __LINE__; return false; } positiveAngleAxis = q3.getPositiveRealImag(); q4 = SGQuat::fromPositiveRealImag(positiveAngleAxis); if (!isSameRotation(q3, q4)) - return false; + { lineno = __LINE__; return false; } return true; } @@ -219,13 +222,13 @@ QuatDerivativeTest(void) // Check if we can restore the angular velocity SGVec3 av2 = SGQuat::forwardDifferenceVelocity(o0, o1, dt); if (!equivalent(av, av2)) - return false; + { lineno = __LINE__; return false; } // Test with the equivalent orientation o1 = -o1; av2 = SGQuat::forwardDifferenceVelocity(o0, o1, dt); if (!equivalent(av, av2)) - return false; + { lineno = __LINE__; return false; } } return true; } @@ -250,23 +253,23 @@ MatrixTest(void) invert(m2, m0); m3 = transNeg(m0); if (!equivalent(m1, m2)) - return false; + { lineno = __LINE__; return false; } if (!equivalent(m2, m3)) - return false; + { lineno = __LINE__; return false; } // Check matrix multiplication and inversion if (!equivalent(m0*m1, SGMatrix::unit())) - return false; + { lineno = __LINE__; return false; } if (!equivalent(m1*m0, SGMatrix::unit())) - return false; + { lineno = __LINE__; return false; } if (!equivalent(m0*m2, SGMatrix::unit())) - return false; + { lineno = __LINE__; return false; } if (!equivalent(m2*m0, SGMatrix::unit())) - return false; + { lineno = __LINE__; return false; } if (!equivalent(m0*m3, SGMatrix::unit())) - return false; + { lineno = __LINE__; return false; } if (!equivalent(m3*m0, SGMatrix::unit())) - return false; + { lineno = __LINE__; return false; } return true; } @@ -320,13 +323,13 @@ GeodesyTest(void) if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) || epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) || epsM < fabs(geod0.getElevationM() - geod1.getElevationM())) - return false; + { lineno = __LINE__; return false; } // Test the conversion routines to radial coordinates geoc0 = SGGeoc::fromCart(cart0); cart1 = SGVec3::fromGeoc(geoc0); if (!equivalent(cart0, cart1)) - return false; + { lineno = __LINE__; return false; } // test course / advance routines // uses examples from Williams aviation formulary @@ -336,12 +339,12 @@ GeodesyTest(void) double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM; std::cout << "distance is " << distNm << std::endl; if (0.5 < fabs(distNm - 2144)) // 2144 nm - return false; + { lineno = __LINE__; return false; } double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES; std::cout << "course is " << crsDeg << std::endl; if (0.5 < fabs(crsDeg - 66)) // 66 degrees - return false; + { lineno = __LINE__; return false; } SGGeoc adv; SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv); @@ -349,7 +352,7 @@ GeodesyTest(void) if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) || 0.01 < fabs(adv.getLatitudeRad() - 0.604180)) - return false; + { lineno = __LINE__; return false; } return true; } @@ -361,25 +364,25 @@ main(void) // Do vector tests if (!Vec3Test()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } if (!Vec3Test()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } // Do quaternion tests if (!QuatTest()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } if (!QuatTest()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } if (!QuatDerivativeTest()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } if (!QuatDerivativeTest()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } // Do matrix tests if (!MatrixTest()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } if (!MatrixTest()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } // Do rect tests doRectTest(); @@ -387,7 +390,7 @@ main(void) // Check geodetic/geocentric/cartesian conversions if (!GeodesyTest()) - return EXIT_FAILURE; + { fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; } std::cout << "Successfully passed all tests!" << std::endl; return EXIT_SUCCESS; diff --git a/simgear/math/simd.hxx b/simgear/math/simd.hxx index da80aafa..8a4a9aa8 100644 --- a/simgear/math/simd.hxx +++ b/simgear/math/simd.hxx @@ -1,4 +1,4 @@ -// Copyright (C) 2016 Erik Hofman - erik@ehofman.com +// Copyright (C) 2016-2017 Erik Hofman - erik@ehofman.com // // This library is free software; you can redistribute it and/or // modify it under the terms of the GNU Library General Public @@ -18,6 +18,10 @@ #ifndef __SIMD_H__ #define __SIMD_H__ 1 +#ifdef HAVE_CONFIG_H +# include +#endif + #include #include #include @@ -305,6 +309,7 @@ inline simd4_t operator*(simd4_t v, T f) { return v; } +#ifdef ENABLE_SIMD # ifdef __SSE__ namespace simd4 @@ -1294,5 +1299,7 @@ inline simd4_t max(simd4_t v1, const simd4_t& v2) { # endif +#endif /* ENABLE_SIMD */ + #endif /* __SIMD_H__ */ diff --git a/simgear/math/simd4x4.hxx b/simgear/math/simd4x4.hxx index 3c5ceb43..69203511 100644 --- a/simgear/math/simd4x4.hxx +++ b/simgear/math/simd4x4.hxx @@ -20,6 +20,10 @@ #include +#ifdef HAVE_CONFIG_H +# include +#endif + #if defined(__GNUC__) && defined(__ARM_NEON__) # include #endif @@ -285,6 +289,8 @@ inline simd4x4_t operator*(const simd4x4_t& m1, const simd4x4_t& } +#ifdef ENABLE_SIMD + # ifdef __SSE__ template<> class alignas(16) simd4x4_t @@ -380,13 +386,14 @@ public: } simd4x4_t& operator*=(const simd4x4_t& m2) { + simd4x4_t m1 = *this; __m128 row, col; - for (int i=0; i<4; ++i ) { + for (int i=0; i<4; ++i) { col = _mm_set1_ps(m2.ptr()[i][0]); - row = _mm_mul_ps(simd4x4[0], col); + row = _mm_mul_ps(m1.m4x4()[0], col); for (int j=1; j<4; ++j) { col = _mm_set1_ps(m2.ptr()[i][j]); - row = _mm_add_ps(row, _mm_mul_ps(simd4x4[j], col)); + row = _mm_add_ps(row, _mm_mul_ps(m1.m4x4()[j], col)); } simd4x4[i] = row; } @@ -583,13 +590,14 @@ public: } simd4x4_t& operator*=(const simd4x4_t& m2) { + simd4x4_t m1 = *this; __m256d row, col; - for (int i=0; i<4; ++i ) { + for (int i=0; i<4; ++i) { col = _mm256_set1_pd(m2.ptr()[i][0]); - row = _mm256_mul_pd(simd4x4[0], col); + row = _mm256_mul_pd(m1.m4x4()[0], col); for (int j=1; j<4; ++j) { col = _mm256_set1_pd(m2.ptr()[i][j]); - row = _mm256_add_pd(row, _mm256_mul_pd(simd4x4[j], col)); + row = _mm256_add_pd(row, _mm256_mul_pd(m1.m4x4()[j], col)); } simd4x4[i] = row; } @@ -738,7 +746,11 @@ public: simd4x4[3][1] = _mm_set_pd(m33,m23); } simd4x4_t(const double m[4*4]) { + const double *p = m; for (int i=0; i<4; ++i) { + simd4_t vec4(p); + simd4x4[i][0] = vec4.v4()[0]; p += 4; + simd4x4[i][1] = vec4.v4()[1]; simd4x4[i][0] = _mm_loadu_pd((const double*)&m[4*i]); simd4x4[i][1] = _mm_loadu_pd((const double*)&m[4*i+2]); } @@ -746,6 +758,8 @@ public: simd4x4_t(const __mtx4d_t m) { for (int i=0; i<4; ++i) { + simd4x4[i][0] = simd4_t(m[i]).v4()[0]; + simd4x4[i][1] = simd4_t(m[i]).v4()[1]; simd4x4[i][0] = _mm_loadu_pd(m[i]); simd4x4[i][1] = _mm_loadu_pd(m[i+2]); } @@ -814,18 +828,17 @@ public: } simd4x4_t& operator*=(const simd4x4_t& m2) { - __m128d row[2], col; + simd4x4_t m1 = *this; + simd4_t row, col; for (int i=0; i<4; ++i ) { - col = _mm_set1_pd(m2.ptr()[i][0]); - row[0] = _mm_mul_pd(simd4x4[0][0], col); - row[1] = _mm_mul_pd(simd4x4[0][1], col); + simd4_t col = m1.m4x4()[0]; + row = col * m2.ptr()[i][0]; for (int j=1; j<4; ++j) { - col = _mm_set1_pd(m2.ptr()[i][j]); - row[0] = _mm_add_pd(row[0], _mm_mul_pd(simd4x4[j][0], col)); - row[1] = _mm_add_pd(row[1], _mm_mul_pd(simd4x4[j][1], col)); + col = m1.m4x4()[j]; + row += col * m2.ptr()[i][j]; } - simd4x4[i][0] = row[0]; - simd4x4[i][1] = row[1]; + simd4x4[i][0] = row.v4()[0]; + simd4x4[i][1] = row.v4()[1]; } return *this; } @@ -1178,5 +1191,7 @@ inline simd4_t transform(const simd4x4_t& m, const simd4_t