diff --git a/simgear/math/SGVec2.hxx b/simgear/math/SGVec2.hxx index 3939899a..f29744ee 100644 --- a/simgear/math/SGVec2.hxx +++ b/simgear/math/SGVec2.hxx @@ -46,7 +46,7 @@ public: } /// Constructor. Initialize by the given values SGVec2(T x, T y) - { data()[0] = x; data()[1] = y; } + { _data = simd4_t(x, y); } /// Constructor. Initialize by the content of a plain array, /// make sure it has at least 2 elements explicit SGVec2(const T* d) @@ -177,35 +177,35 @@ mult(SGVec2 v1, const SGVec2& v2) template inline SGVec2 -min(const SGVec2& v1, const SGVec2& v2) -{return SGVec2(SGMisc::min(v1(0), v2(0)), SGMisc::min(v1(1), v2(1)));} +min(SGVec2 v1, const SGVec2& v2) +{ v1.simd2() = simd4::min(v1.simd2(), v2.simd2()); return v1; } template inline SGVec2 -min(const SGVec2& v, S s) -{ return SGVec2(SGMisc::min(s, v(0)), SGMisc::min(s, v(1))); } +min(SGVec2 v, S s) +{ v.simd2() = simd4::min(v.simd2(), simd4_t(s)); return v; } template inline SGVec2 -min(S s, const SGVec2& v) -{ return SGVec2(SGMisc::min(s, v(0)), SGMisc::min(s, v(1))); } +min(S s, SGVec2 v) +{ v.sim2() = simd4::min(v.simd2(), simd4_t(s)); return v; } /// component wise max template inline SGVec2 max(const SGVec2& v1, const SGVec2& v2) -{return SGVec2(SGMisc::max(v1(0), v2(0)), SGMisc::max(v1(1), v2(1)));} +{ v1 = simd4::max(v1.simd2(), v2.simd2()); return v1; } template inline SGVec2 max(const SGVec2& v, S s) -{ return SGVec2(SGMisc::max(s, v(0)), SGMisc::max(s, v(1))); } +{ v = simd4::max(v.simd2(), simd4_t(s)); return v; } template inline SGVec2 max(S s, const SGVec2& v) -{ return SGVec2(SGMisc::max(s, v(0)), SGMisc::max(s, v(1))); } +{ v = simd4::max(v.simd2(), simd4_t(s)); return v; } /// Add two vectors taking care of (integer) overflows. The values are limited /// to the respective minimum and maximum values. diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx index 1b0786f7..92951a45 100644 --- a/simgear/math/SGVec3.hxx +++ b/simgear/math/SGVec3.hxx @@ -56,7 +56,7 @@ public: /// Constructor. Initialize by the given values SGVec3(T x, T y, T z) - { data()[0] = x; data()[1] = y; data()[2] = z; } + { _data = simd4_t(x, y, z); } /// Constructor. Initialize by the content of a plain array, /// make sure it has at least 3 elements explicit SGVec3(const T* d) @@ -65,7 +65,7 @@ public: explicit SGVec3(const SGVec3& d) { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; } explicit SGVec3(const SGVec2& v2, const T& v3 = 0) - { data()[0] = v2[0]; data()[1] = v2[1]; data()[2] = v3; } + { _data = v2.simd2(); data()[2] = v3; } /// Access by index, the index is unchecked const T& operator()(unsigned i) const @@ -246,59 +246,35 @@ mult(SGVec3 v1, const SGVec3& v2) template inline SGVec3 -min(const SGVec3& v1, const SGVec3& v2) -{ - return SGVec3(SGMisc::min(v1(0), v2(0)), - SGMisc::min(v1(1), v2(1)), - SGMisc::min(v1(2), v2(2))); -} +min(SGVec3 v1, const SGVec3& v2) +{ v1.simd3() = simd4::min(v1.simd3(), v2.simd3()); return v1; } template inline SGVec3 -min(const SGVec3& v, S s) -{ - return SGVec3(SGMisc::min(s, v(0)), - SGMisc::min(s, v(1)), - SGMisc::min(s, v(2))); -} +min(SGVec3 v, S s) +{ v.simd3() = simd4::min(v.simd3(), simd4_t(s)); return v; } template inline SGVec3 -min(S s, const SGVec3& v) -{ - return SGVec3(SGMisc::min(s, v(0)), - SGMisc::min(s, v(1)), - SGMisc::min(s, v(2))); -} +min(S s, SGVec3 v) +{ v.simd3() = simd4::min(v.simd3(), simd4_t(s)); return v; } /// component wise max template inline SGVec3 -max(const SGVec3& v1, const SGVec3& v2) -{ - return SGVec3(SGMisc::max(v1(0), v2(0)), - SGMisc::max(v1(1), v2(1)), - SGMisc::max(v1(2), v2(2))); -} +max(SGVec3 v1, const SGVec3& v2) +{ v1.simd3() = simd4::max(v1.simd3(), v2.simd3()); return v1; } template inline SGVec3 -max(const SGVec3& v, S s) -{ - return SGVec3(SGMisc::max(s, v(0)), - SGMisc::max(s, v(1)), - SGMisc::max(s, v(2))); -} +max(SGVec3 v, S s) +{ v.simd3() = simd4::max(v.simd3(), simd4_t(s)); return v; } template inline SGVec3 -max(S s, const SGVec3& v) -{ - return SGVec3(SGMisc::max(s, v(0)), - SGMisc::max(s, v(1)), - SGMisc::max(s, v(2))); -} +max(S s, SGVec3 v) +{ v.simd3() = simd4::max(v.simd3(), simd4_t(s)); return v; } /// Add two vectors taking care of (integer) overflows. The values are limited /// to the respective minimum and maximum values. diff --git a/simgear/math/SGVec4.hxx b/simgear/math/SGVec4.hxx index 2c05763a..50d75592 100644 --- a/simgear/math/SGVec4.hxx +++ b/simgear/math/SGVec4.hxx @@ -42,7 +42,7 @@ public: } /// Constructor. Initialize by the given values SGVec4(T x, T y, T z, T w) - { data()[0] = x; data()[1] = y; data()[2] = z; data()[3] = w; } + { _data = simd4_t(z, y, z, w); } /// Constructor. Initialize by the content of a plain array, /// make sure it has at least 3 elements explicit SGVec4(const T* d) @@ -51,7 +51,7 @@ public: explicit SGVec4(const SGVec4& d) { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; } explicit SGVec4(const SGVec3& v3, const T& v4 = 0) - { data()[0] = v3[0]; data()[1] = v3[1]; data()[2] = v3[2]; data()[3] = v4; } + { _data = v3.simd3(); data()[3] = v4; } /// Access by index, the index is unchecked const T& operator()(unsigned i) const @@ -192,65 +192,35 @@ mult(SGVec4 v1, const SGVec4& v2) template inline SGVec4 -min(const SGVec4& v1, const SGVec4& v2) -{ - return SGVec4(SGMisc::min(v1(0), v2(0)), - SGMisc::min(v1(1), v2(1)), - SGMisc::min(v1(2), v2(2)), - SGMisc::min(v1(3), v2(3))); -} +min(SGVec4 v1, const SGVec4& v2) +{ v1.simd4() = simd4::min(v1.simd4(), v2.simd4()); return v1; } template inline SGVec4 -min(const SGVec4& v, S s) -{ - return SGVec4(SGMisc::min(s, v(0)), - SGMisc::min(s, v(1)), - SGMisc::min(s, v(2)), - SGMisc::min(s, v(3))); -} +min(SGVec4 v, S s) +{ v.simd4() = simd4::min(v.simd4(), simd4_t(s)); return v; } template inline SGVec4 -min(S s, const SGVec4& v) -{ - return SGVec4(SGMisc::min(s, v(0)), - SGMisc::min(s, v(1)), - SGMisc::min(s, v(2)), - SGMisc::min(s, v(3))); -} +min(S s, SGVec4 v) +{ v.simd4() = simd4::min(v.simd4(), simd4_t(s)); return v; } /// component wise max template inline SGVec4 -max(const SGVec4& v1, const SGVec4& v2) -{ - return SGVec4(SGMisc::max(v1(0), v2(0)), - SGMisc::max(v1(1), v2(1)), - SGMisc::max(v1(2), v2(2)), - SGMisc::max(v1(3), v2(3))); -} +max(SGVec4 v1, const SGVec4& v2) +{ v1.simd4() = simd4::max(v1.simd4(), v2.simd4()); return v1; } template inline SGVec4 -max(const SGVec4& v, S s) -{ - return SGVec4(SGMisc::max(s, v(0)), - SGMisc::max(s, v(1)), - SGMisc::max(s, v(2)), - SGMisc::max(s, v(3))); -} +max(SGVec4 v, S s) +{ v.simd4() = simd4::max(v.simd4(), simd4_t(s)); return v; } template inline SGVec4 -max(S s, const SGVec4& v) -{ - return SGVec4(SGMisc::max(s, v(0)), - SGMisc::max(s, v(1)), - SGMisc::max(s, v(2)), - SGMisc::max(s, v(3))); -} +max(S s, SGVec4 v) +{ v.simd4() = simd4::max(v.simd4(), simd4_t(s)); return v; } /// Add two vectors taking care of (integer) overflows. The values are limited /// to the respective minimum and maximum values. diff --git a/simgear/math/simd.hxx b/simgear/math/simd.hxx index 690ba48f..10ff8717 100644 --- a/simgear/math/simd.hxx +++ b/simgear/math/simd.hxx @@ -21,12 +21,30 @@ #include #include +#include +#include template class simd4_t; namespace simd4 { +template +inline simd4_t min(simd4_t v1, const simd4_t& v2) { + for (int i=0; i::min(v1[i], v2[i]); + } + return v1; +} + +template +inline simd4_t max(simd4_t v1, const simd4_t& v2) { + for (int i=0; i::max(v1[i], v2[i]); + } + return v1; +} + template inline simd4_t abs(simd4_t v) { for (int i=0; i operator*(simd4_t v, T f) { # ifdef __MMX__ # include +# if defined(_MSC_VER) +# define ALIGN16 __declspec(align(16)) +# define ALIGN16C +# elif defined(__GNUC__) +# define ALIGN16 +# define ALIGN16C __attribute__((aligned(16))) +# endif # endif # ifdef __SSE__ # include +# ifdef __SSE3__ +# include +# endif template class simd4_t @@ -315,11 +352,11 @@ class simd4_t private: typedef float __vec4f_t[N]; - union { + union ALIGN16 { __m128 simd4; __vec4f_t vec; float _v4[4]; - }; + } ALIGN16C; public: simd4_t(void) {} @@ -327,6 +364,15 @@ public: simd4 = _mm_set1_ps(f); for (int i=N; i<4; i++) _v4[i] = 0.0f; } + simd4_t(float x, float y) { + simd4 = _mm_setr_ps(x,y,0,0); + } + simd4_t(float x, float y, float z) { + simd4 = _mm_setr_ps(x,y,z,0); + } + simd4_t(float x, float y, float z, float w) { + simd4 = _mm_setr_ps(x,y,z,w); + } explicit simd4_t(const __vec4f_t v) { simd4 = _mm_loadu_ps(v); for (int i=N; i<4; i++) _v4[i] = 0.0f; @@ -424,6 +470,47 @@ public: namespace simd4 { +// http://stackoverflow.com/questions/6996764/fastest-way-to-do-horizontal-float-vector-sum-on-x86 +# ifdef __SSE3__ + inline float hsum_ps_sse(__m128 v) { + __m128 shuf = _mm_movehdup_ps(v); + __m128 sums = _mm_add_ps(v, shuf); + shuf = _mm_movehl_ps(shuf, sums); + sums = _mm_add_ss(sums, shuf); + return _mm_cvtss_f32(sums); + } +# else + inline float hsum_ps_sse(__m128 v) { + __m128 shuf = _mm_shuffle_ps(v, v, _MM_SHUFFLE(2, 3, 0, 1)); + __m128 sums = _mm_add_ps(v, shuf); + shuf = _mm_movehl_ps(shuf, sums); + sums = _mm_add_ss(sums, shuf); + return _mm_cvtss_f32(sums); + } +# endif + +template<> +inline float magnitude2(simd4_t v) { + return hsum_ps_sse(v.v4()*v.v4()); +} + +template<> +inline float dot(simd4_t v1, const simd4_t& v2) { + return hsum_ps_sse(v1.v4()*v2.v4()); +} + +template +inline simd4_t min(simd4_t v1, const simd4_t& v2) { + v1.v4() = _mm_min_ps(v1.v4(), v2.v4()); + return v1; +} + +template +inline simd4_t max(simd4_t v1, const simd4_t& v2) { + v1.v4() = _mm_max_ps(v1.v4(), v2.v4()); + return v1; +} + template inline simd4_tabs(simd4_t v) { static const __m128 sign_mask = _mm_set1_ps(-0.f); // -0.f = 1 << 31 @@ -443,11 +530,11 @@ class simd4_t private: typedef double __vec4d_t[N]; - union { + union ALIGN16 { __m128d simd4[2]; __vec4d_t vec; double _v4[4]; - }; + } ALIGN16C; public: simd4_t(void) {} @@ -455,6 +542,15 @@ public: simd4[0] = simd4[1] = _mm_set1_pd(d); for (int i=N; i<4; i++) _v4[i] = 0.0; } + simd4_t(double x, double y) { + simd4[0] = _mm_setr_pd(x,y); simd4[1] = _mm_setzero_pd(); + } + simd4_t(double x, double y, double z) { + simd4[0] = _mm_setr_pd(x,y); simd4[1] = _mm_setr_pd(z,0); + } + simd4_t(double x, double y, double z, double w) { + simd4[0] = _mm_setr_pd(x,y); simd4[1] = _mm_setr_pd(z,w); + } explicit simd4_t(const __vec4d_t v) { simd4[0] = _mm_loadu_pd(v); simd4[1] = _mm_loadu_pd(v+2); @@ -570,6 +666,38 @@ public: namespace simd4 { +// http://stackoverflow.com/questions/6996764/fastest-way-to-do-horizontal-float-vector-sum-on-x86 +inline double hsum_pd_sse(__m128d vd) { + __m128 undef = _mm_undefined_ps(); + __m128 shuftmp= _mm_movehl_ps(undef, _mm_castpd_ps(vd)); + __m128d shuf = _mm_castps_pd(shuftmp); + return _mm_cvtsd_f64(_mm_add_sd(vd, shuf)); +} + +template<> +inline double magnitude2(simd4_t v) { + return hsum_pd_sse(v.v4()[0]*v.v4()[0]) + hsum_pd_sse(v.v4()[1]*v.v4()[1]); +} + +template<> +inline double dot(simd4_t v1, const simd4_t& v2) { + return hsum_pd_sse(v1.v4()[0]*v2.v4()[0])+hsum_pd_sse(v1.v4()[1]*v2.v4()[1]); +} + +template +inline simd4_t min(simd4_t v1, const simd4_t& v2) { + v1.v4()[0] = _mm_min_pd(v1.v4()[0], v2.v4()[0]); + v1.v4()[1] = _mm_min_pd(v1.v4()[1], v2.v4()[1]); + return v1; +} + +template +inline simd4_t max(simd4_t v1, const simd4_t& v2) { + v1.v4()[0] = _mm_max_pd(v1.v4()[0], v2.v4()[0]); + v1.v4()[1] = _mm_max_pd(v1.v4()[1], v2.v4()[1]); + return v1; +} + template inline simd4_tabs(simd4_t v) { static const __m128d sign_mask = _mm_set1_pd(-0.); // -0. = 1 << 63 @@ -590,11 +718,11 @@ class simd4_t private: typedef int __vec4i_t[N]; - union { + union ALIGN16 { __m128i simd4; __vec4i_t vec; int _v4[4]; - }; + } ALIGN16C; public: simd4_t(void) {} @@ -602,6 +730,15 @@ public: simd4 = _mm_set1_epi32(i); for (int i=N; i<4; i++) _v4[i] = 0; } + simd4_t(float x, float y) { + simd4 = _mm_setr_epi32(x,y,0,0); + } + simd4_t(float x, float y, float z) { + simd4 = _mm_setr_epi32(x,y,z,0); + } + simd4_t(float x, float y, float z, float w) { + simd4 = _mm_setr_epi32(x,y,z,w); + } explicit simd4_t(const __vec4i_t v) { simd4 = _mm_loadu_si128((__m128i*)v); for (int i=N; i<4; i++) _v4[i] = 0; diff --git a/simgear/math/simd4x4.hxx b/simgear/math/simd4x4.hxx index 5d22a864..b3b9ca95 100644 --- a/simgear/math/simd4x4.hxx +++ b/simgear/math/simd4x4.hxx @@ -262,11 +262,11 @@ class simd4x4_t private: typedef float __mtx4f_t[4][4]; - union { + union ALIGN16 { __m128 simd4x4[4]; __mtx4f_t mtx; float array[4*4]; - }; + } ALIGN16C; public: simd4x4_t(void) {} @@ -400,11 +400,11 @@ class simd4x4_t private: typedef double __mtx4d_t[4][4]; - union { + union ALIGN16 { __m128d simd4x4[4][2]; __mtx4d_t mtx; double array[4*4]; - }; + } ALIGN16C; public: simd4x4_t(void) {} @@ -615,11 +615,11 @@ class simd4x4_t private: typedef int __mtx4i_t[4][4]; - union { + union ALIGN16 { __m128i simd4x4[4]; __mtx4i_t mtx; int array[4*4]; - }; + } ALIGN16C; public: simd4x4_t(void) {}