FGFS-4.1/simgear
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

We need to copy the matrix before altering it's contents: Fix a SGMathTest failure

Browse Source
This commit is contained in:
Erik Hofman
2017-02-03 15:07:35 +01:00
parent 3417ca7e49
commit 7f65e7f905
4 changed files with 92 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
simgear/math/SGMathTest.cxx
View File

@@ -28,6 +28,9 @@
#include "SGRect.hxx"
#include "sg_random.h"
int lineno = 0;
template<typename T>
bool
Vec3Test(void)
@@ -38,45 +41,45 @@ Vec3Test(void)
v1 = SGVec3<T>(1, 2, 3);
v2 = SGVec3<T>(3, 2, 1);
if (equivalent(v1, v2))
return false;
{ lineno = __LINE__; return false; }
// Check the unary minus operator
v3 = SGVec3<T>(-1, -2, -3);
if (!equivalent(-v1, v3))
return false;
{ lineno = __LINE__; return false; }
// Check the unary plus operator
v3 = SGVec3<T>(1, 2, 3);
if (!equivalent(+v1, v3))
return false;
{ lineno = __LINE__; return false; }
// Check the addition operator
v3 = SGVec3<T>(4, 4, 4);
if (!equivalent(v1 + v2, v3))
return false;
{ lineno = __LINE__; return false; }
// Check the subtraction operator
v3 = SGVec3<T>(-2, 0, 2);
if (!equivalent(v1 - v2, v3))
return false;
{ lineno = __LINE__; return false; }
// Check the scaler multiplication operator
v3 = SGVec3<T>(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<T>::epsilon())
return false;
{ lineno = __LINE__; return false; }
// Check the cross product
v3 = SGVec3<T>(-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<T>::epsilon())
return false;
{ lineno = __LINE__; return false; }
return true;
}
@@ -89,11 +92,11 @@ isSameRotation(const SGQuat<T>& q1, const SGQuat<T>& q2)
const SGVec3<T> e2(0, 1, 0);
const SGVec3<T> 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<T>(1, 2, 3);
v2 = SGVec3<T>(1, -2, -3);
if (!equivalent(q1.transform(v1), v2))
return false;
{ lineno = __LINE__; return false; }
// Check a rotation around the x axis
q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
v2 = SGVec3<T>(1, 3, -2);
if (!equivalent(q1.transform(v1), v2))
return false;
{ lineno = __LINE__; return false; }
// Check a rotation around the y axis
q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
v2 = SGVec3<T>(-1, 2, -3);
if (!equivalent(q1.transform(v1), v2))
return false;
{ lineno = __LINE__; return false; }
// Check a rotation around the y axis
q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
v2 = SGVec3<T>(-3, 2, 1);
if (!equivalent(q1.transform(v1), v2))
return false;
{ lineno = __LINE__; return false; }
// Check a rotation around the z axis
q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
v2 = SGVec3<T>(-1, -2, 3);
if (!equivalent(q1.transform(v1), v2))
return false;
{ lineno = __LINE__; return false; }
// Check a rotation around the z axis
q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
v2 = SGVec3<T>(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<T>::pi();
@@ -162,7 +165,7 @@ QuatTest(void)
v2 = q3.transform(q2.transform(q1.transform(v1)));
q4 = SGQuat<T>::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<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
@@ -172,15 +175,15 @@ QuatTest(void)
q1.getAngleAxis(angleAxis);
q4 = SGQuat<T>::fromAngleAxis(angleAxis);
if (!isSameRotation(q1, q4))
return false;
{ lineno = __LINE__; return false; }
q2.getAngleAxis(angleAxis);
q4 = SGQuat<T>::fromAngleAxis(angleAxis);
if (!isSameRotation(q2, q4))
return false;
{ lineno = __LINE__; return false; }
q3.getAngleAxis(angleAxis);
q4 = SGQuat<T>::fromAngleAxis(angleAxis);
if (!isSameRotation(q3, q4))
return false;
{ lineno = __LINE__; return false; }
/// Test angle axis forward and back transform
q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
@@ -189,15 +192,15 @@ QuatTest(void)
SGVec3<T> positiveAngleAxis = q1.getPositiveRealImag();
q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
if (!isSameRotation(q1, q4))
return false;
{ lineno = __LINE__; return false; }
positiveAngleAxis = q2.getPositiveRealImag();
q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
if (!isSameRotation(q2, q4))
return false;
{ lineno = __LINE__; return false; }
positiveAngleAxis = q3.getPositiveRealImag();
q4 = SGQuat<T>::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<T> av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
if (!equivalent(av, av2))
return false;
{ lineno = __LINE__; return false; }
// Test with the equivalent orientation
o1 = -o1;
av2 = SGQuat<T>::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<T>::unit()))
return false;
{ lineno = __LINE__; return false; }
if (!equivalent(m1*m0, SGMatrix<T>::unit()))
return false;
{ lineno = __LINE__; return false; }
if (!equivalent(m0*m2, SGMatrix<T>::unit()))
return false;
{ lineno = __LINE__; return false; }
if (!equivalent(m2*m0, SGMatrix<T>::unit()))
return false;
{ lineno = __LINE__; return false; }
if (!equivalent(m0*m3, SGMatrix<T>::unit()))
return false;
{ lineno = __LINE__; return false; }
if (!equivalent(m3*m0, SGMatrix<T>::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<double>::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<float>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
if (!Vec3Test<double>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
// Do quaternion tests
if (!QuatTest<float>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
if (!QuatTest<double>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
if (!QuatDerivativeTest<float>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
if (!QuatDerivativeTest<double>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
// Do matrix tests
if (!MatrixTest<float>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
if (!MatrixTest<double>())
return EXIT_FAILURE;
{ fprintf(stderr, "Error at line: %i called from line: %i\n", lineno, __LINE__); return EXIT_FAILURE; }
// Do rect tests
doRectTest<int>();
@@ -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;

9
simgear/math/simd.hxx
View File

@@ -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 <simgear/simgear_config.h>
#endif
#include <cstdint>
#include <cstring>
#include <cassert>
@@ -305,6 +309,7 @@ inline simd4_t<T,N> operator*(simd4_t<T,N> v, T f) {
return v;
}
#ifdef ENABLE_SIMD
# ifdef __SSE__
namespace simd4
@@ -1294,5 +1299,7 @@ inline simd4_t<int,N> max(simd4_t<int,N> v1, const simd4_t<int,N>& v2) {
# endif
#endif /* ENABLE_SIMD */
#endif /* __SIMD_H__ */

45
simgear/math/simd4x4.hxx
View File

@@ -20,6 +20,10 @@
#include <cstring>
#ifdef HAVE_CONFIG_H
# include <simgear/simgear_config.h>
#endif
#if defined(__GNUC__) && defined(__ARM_NEON__)
# include <simgear/math/simd4x4_neon.hxx>
#endif
@@ -285,6 +289,8 @@ inline simd4x4_t<T,N> operator*(const simd4x4_t<T,N>& m1, const simd4x4_t<T,N>&
}
#ifdef ENABLE_SIMD
# ifdef __SSE__
template<>
class alignas(16) simd4x4_t<float,4>
@@ -380,13 +386,14 @@ public:
}
simd4x4_t<float,4>& operator*=(const simd4x4_t<float,4>& m2) {
simd4x4_t<float,4> 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<double,4>& operator*=(const simd4x4_t<double,4>& m2) {
simd4x4_t<double,4> 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<double,4> 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<double,4>(m[i]).v4()[0];
simd4x4[i][1] = simd4_t<double,4>(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<double,4>& operator*=(const simd4x4_t<double,4>& m2) {
__m128d row[2], col;
simd4x4_t<double,4> m1 = *this;
simd4_t<double,4> 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<double,4> 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<int,3> transform<int>(const simd4x4_t<int,4>& m, const simd4_t<in
} /* namespace simd4x */
# endif
#endif /* ENABLE_SIMD */
#endif /* __SIMD4X4_H__ */

1
simgear/simgear_config_cmake.h.in
View File

@@ -20,3 +20,4 @@
#cmakedefine SYSTEM_EXPAT
#cmakedefine ENABLE_SOUND
#cmakedefine ENABLE_SIMD