From 2c2a57f3686e0631a9197e59c4c0f48ecba278cb Mon Sep 17 00:00:00 2001 From: Erik Hofman Date: Wed, 21 Dec 2016 09:04:46 +0100 Subject: [PATCH] Addd support for AVX, 4 double precision operations in one go --- simgear/math/SGMatrix.hxx | 2 +- simgear/math/SGVec2.hxx | 3 +- simgear/math/SGVec3.hxx | 3 +- simgear/math/SGVec4.hxx | 2 +- simgear/math/simd.hxx | 170 ++++++++++++++++++++++++- simgear/math/simd4x4.hxx | 258 +++++++++++++++++++++++++++++++++++++- 6 files changed, 427 insertions(+), 11 deletions(-) diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx index 6752e19e..3792185d 100644 --- a/simgear/math/SGMatrix.hxx +++ b/simgear/math/SGMatrix.hxx @@ -18,7 +18,7 @@ #ifndef SGMatrix_H #define SGMatrix_H -#include "simd4x4.hxx" +#include /// Expression templates for poor programmers ... :) template diff --git a/simgear/math/SGVec2.hxx b/simgear/math/SGVec2.hxx index 88847719..ec46475e 100644 --- a/simgear/math/SGVec2.hxx +++ b/simgear/math/SGVec2.hxx @@ -23,8 +23,7 @@ #include #include #include - -#include "simd.hxx" +#include /// 2D Vector Class template diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx index 841fe8df..5c936937 100644 --- a/simgear/math/SGVec3.hxx +++ b/simgear/math/SGVec3.hxx @@ -22,8 +22,7 @@ #include #include - -#include "simd.hxx" +#include /// 3D Vector Class template diff --git a/simgear/math/SGVec4.hxx b/simgear/math/SGVec4.hxx index a52a90c5..bc7f2468 100644 --- a/simgear/math/SGVec4.hxx +++ b/simgear/math/SGVec4.hxx @@ -20,7 +20,7 @@ #include -#include "simd.hxx" +#include /// 4D Vector Class template diff --git a/simgear/math/simd.hxx b/simgear/math/simd.hxx index 3cf2b208..3dfe5f65 100644 --- a/simgear/math/simd.hxx +++ b/simgear/math/simd.hxx @@ -21,8 +21,8 @@ #include #include -#include "SGLimits.hxx" -#include "SGMisc.hxx" +#include +#include template class simd4_t; @@ -286,10 +286,14 @@ inline simd4_t operator*(simd4_t v, T f) { # include # if defined(_MSC_VER) # define ALIGN16 __declspec(align(16)) +# define ALIGN32 __declspec(align(32)) # define ALIGN16C +# define ALIGN32C # elif defined(__GNUC__) # define ALIGN16 +# define ALIGN32 # define ALIGN16C __attribute__((aligned(16))) +# define ALIGN32C __attribute__((aligned(32))) # endif # endif @@ -461,7 +465,167 @@ inline simd4_tabs(simd4_t v) { # endif -# ifdef __SSE2__ +# ifdef __AVX__ +# include +# include + +template +class simd4_t +{ +private: + typedef double __vec4d_t[N]; + + union ALIGN32 { + __m256d simd4; + __vec4d_t vec; + double _v4[4]; + } ALIGN32C; + +public: + simd4_t(void) {} + simd4_t(double d) { + simd4 = _mm256_set1_pd(d); + for (int i=N; i<4; ++i) _v4[i] = 0.0; + } + simd4_t(double x, double y) : simd4_t(x,y,0,0) {} + simd4_t(double x, double y, double z) : simd4_t(x,y,z,0) {} + simd4_t(double x, double y, double z, double w) { + simd4 = _mm256_set_pd(w,z,y,x); + } + explicit simd4_t(const __vec4d_t v) { + simd4 = _mm256_loadu_pd(v); + for (int i=N; i<4; ++i) _v4[i] = 0.0; + } + template + simd4_t(const simd4_t& v) { + simd4 = v.v4(); + for (int i=M; i<4; ++i) _v4[i] = 0.0; + } + simd4_t(const __m256d v) { + simd4 = v; + } + + inline const __m256d (&v4(void) const) { + return simd4; + } + inline __m256d (&v4(void)) { + return simd4; + } + + inline const double (&ptr(void) const)[N] { + return vec; + } + inline double (&ptr(void))[N] { + return vec; + } + + inline operator const double*(void) const { + return vec; + } + inline operator double*(void) { + return vec; + } + + inline simd4_t& operator=(double d) { + simd4 = _mm256_set1_pd(d); + for (int i=N; i<4; ++i) _v4[i] = 0.0; + return *this; + } + inline simd4_t& operator=(const __vec4d_t v) { + simd4 = _mm256_loadu_pd(v); + for (int i=N; i<4; ++i) _v4[i] = 0.0; + return *this; + } + template + inline simd4_t& operator=(const simd4_t& v) { + simd4 = v.v4(); + for (int i=M; i<4; ++i) _v4[i] = 0.0; + return *this; + } + inline simd4_t& operator=(const __m256d v) { + simd4 = v; + return *this; + } + + inline simd4_t& operator+=(double d) { + return operator+=(simd4_t(d)); + } + inline simd4_t& operator+=(const simd4_t& v) { + simd4 = _mm256_add_pd(simd4, v.v4()); + return *this; + } + + inline simd4_t& operator-=(double d) { + return operator-=(simd4_t(d)); + } + inline simd4_t& operator-=(const simd4_t& v) { + simd4 = _mm256_sub_pd(simd4, v.v4()); + return *this; + } + + inline simd4_t& operator*=(double d) { + return operator*=(simd4_t(d)); + } + inline simd4_t& operator*=(const simd4_t& v) { + simd4 = _mm256_mul_pd(simd4, v.v4()); + return *this; + } + + inline simd4_t& operator/=(double d) { + return operator/=(simd4_t(d)); + } + inline simd4_t& operator/=(const simd4_t& v) { + simd4 = _mm256_div_pd(simd4, v.v4()); + return *this; + } +}; + +namespace simd4 +{ +// http://berenger.eu/blog/sseavxsimd-horizontal-sum-sum-simd-vector-intrinsic/ +inline float hsum_pd_avx(__m256d v) { + const __m128d valupper = _mm256_extractf128_pd(v, 1); + const __m128d vallower = _mm256_castpd256_pd128(v); + _mm256_zeroupper(); + const __m128d valval = _mm_add_pd(valupper, vallower); + const __m128d sums = _mm_add_pd(_mm_permute_pd(valval,1), valval); + return _mm_cvtsd_f64(sums); +} + +template<> +inline double magnitude2(simd4_t v) { + v *= v; + return hsum_pd_avx(v.v4()); +} + +template<> +inline double dot(simd4_t v1, const simd4_t& v2) { + v1 *= v2; + return hsum_pd_avx(v1.v4()); +} + +template +inline simd4_t min(simd4_t v1, const simd4_t& v2) { + v1.v4() = _mm256_min_pd(v1.v4(), v2.v4()); + return v1; +} + +template +inline simd4_t max(simd4_t v1, const simd4_t& v2) { + v1.v4() = _mm256_max_pd(v1.v4(), v2.v4()); + return v1; +} + +template +inline simd4_tabs(simd4_t v) { + static const __m256d sign_mask = _mm256_set1_pd(-0.); // -0. = 1 << 63 + v.v4() = _mm256_andnot_pd(sign_mask, v.v4()); + return v; +} + +} /* namespace simd4 */ + +# elif defined __SSE2__ # include template diff --git a/simgear/math/simd4x4.hxx b/simgear/math/simd4x4.hxx index a0c189a7..9daf0cb4 100644 --- a/simgear/math/simd4x4.hxx +++ b/simgear/math/simd4x4.hxx @@ -20,7 +20,7 @@ #include -#include "simd.hxx" +#include template class simd4x4_t; @@ -514,7 +514,259 @@ inline simd4_t transform(const simd4x4_t& m, const simd # endif -# ifdef __SSE2__ +# ifdef __AVX__ +# include +# include + +template<> +class simd4x4_t +{ +private: + typedef double __mtx4d_t[4][4]; + + union ALIGN32 { + __m256d simd4x4[4]; + __mtx4d_t mtx; + double array[4*4]; + } ALIGN32C; + +public: + simd4x4_t(void) {} + simd4x4_t(double m00, double m01, double m02, double m03, + double m10, double m11, double m12, double m13, + double m20, double m21, double m22, double m23, + double m30, double m31, double m32, double m33) + { + simd4x4[0] = _mm256_set_pd(m30,m20,m10,m00); + simd4x4[1] = _mm256_set_pd(m31,m21,m11,m01); + simd4x4[2] = _mm256_set_pd(m32,m22,m12,m02); + simd4x4[3] = _mm256_set_pd(m33,m23,m13,m03); + } + explicit simd4x4_t(const double m[4*4]) { + for (int i=0; i<4; ++i) { + simd4x4[i] = simd4_t((const double*)&m[4*i]).v4(); + } + } + + explicit simd4x4_t(const __mtx4d_t m) { + for (int i=0; i<4; ++i) { + simd4x4[i] = simd4_t(m[i]).v4(); + } + } + simd4x4_t(const simd4x4_t& m) { + for (int i=0; i<4; ++i) { + simd4x4[i] = m.m4x4()[i]; + } + } + ~simd4x4_t(void) {} + + inline __m256d (&m4x4(void))[4] { + return simd4x4; + } + + inline const __m256d (&m4x4(void) const)[4] { + return simd4x4; + } + + inline const double (&ptr(void) const)[4][4] { + return mtx; + } + + inline double (&ptr(void))[4][4] { + return mtx; + } + + inline operator const double*(void) const { + return array; + } + + inline operator double*(void) { + return array; + } + + inline void set(int i, const simd4_t& v) { + simd4x4[i] = v.v4(); + } + + inline simd4x4_t& operator=(const double m[4*4]) { + for (int i=0; i<4; ++i) { + simd4x4[i] = simd4_t((const double*)&m[4*i]).v4(); + } + return *this; + } + + inline simd4x4_t& operator=(const __mtx4d_t m) { + for (int i=0; i<4; ++i) { + simd4x4[i] = simd4_t(m[i]).v4(); + } + return *this; + } + inline simd4x4_t& operator=(const simd4x4_t& m) { + for (int i=0; i<4; ++i) { + simd4x4[i] = m.m4x4()[i]; + } + return *this; + } + + inline simd4x4_t& operator+=(const simd4x4_t& m) { + for (int i=0; i<4; ++i) { + simd4x4[i] = _mm256_add_pd(simd4x4[i], m.m4x4()[i]); + } + return *this; + } + + inline simd4x4_t& operator-=(const simd4x4_t& m) { + for (int i=0; i<4; ++i) { + simd4x4[i] = _mm256_sub_pd(simd4x4[i], m.m4x4()[i]); + } + return *this; + } + + inline simd4x4_t& operator*=(double f) { + simd4_t f4(f); + for (int i=0; i<4; ++i) { + simd4x4[i] = _mm256_mul_pd(simd4x4[i], f4.v4()); + } + return *this; + } + + simd4x4_t& operator*=(const simd4x4_t& m2) { + simd4x4_t m1 = *this; + for (int i=0; i<4; ++i ) { + __m256d col = _mm256_set1_pd(m2.ptr()[i][0]); + __m256d 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(m1.m4x4()[j], col)); + } + simd4x4[i] = row; + } + return *this; + } + +}; + +template +inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) +{ + __m256d mv; + + mv = _mm256_mul_pd(m.m4x4()[0], _mm256_set1_pd(vi.ptr()[0])); + for (int i=1; i +inline simd4x4_t rotation_matrix(double angle, const simd4_t& axis) +{ + double s = std::sin(angle), c = std::cos(angle), t = 1.0-c; + simd4_t axt, at = axis*t, as = axis*s; + simd4x4_t m; + + simd4x4::unit(m); + axt = axis.ptr()[0]*at; + m.m4x4()[0] = axt.v4(); + + axt = axis.ptr()[1]*at; + m.ptr()[0][0] += c; + m.ptr()[0][1] += as.ptr()[2]; + m.ptr()[0][2] -= as.ptr()[1]; + + m.m4x4()[1] = axt.v4(); + + axt = axis.ptr()[2]*at; + m.ptr()[1][0] -= as.ptr()[2]; + m.ptr()[1][1] += c; + m.ptr()[1][2] += as.ptr()[0]; + + m.m4x4()[2] = axt.v4(); + + m.ptr()[2][0] += as.ptr()[1]; + m.ptr()[2][1] -= as.ptr()[0]; + m.ptr()[2][2] += c; + + return m; +} + +template<> +inline simd4x4_t transpose(simd4x4_t m) { + simd4x4_t mtx; +#if 1 + for (int i=0; i<4; ++i) { + for(int j=0; j<4; ++j) { + mtx.ptr()[j][i] = m.ptr()[i][j]; + } + } +#else + __m256d T0 = _mm256_unpacklo_pd(m.m4x4()[0], m.m4x4()[1]); + __m256d T1 = _mm256_unpacklo_pd(m.m4x4()[2], m.m4x4()[3]); + __m256d T2 = _mm256_unpackhi_pd(m.m4x4()[0], m.m4x4()[1]); + __m256d T3 = _mm256_unpackhi_pd(m.m4x4()[2], m.m4x4()[3]); + + mtx.m4x4()[0] = _mm256_unpacklo_pd(T0, T1); + mtx.m4x4()[1] = _mm256_unpackhi_pd(T0, T1); + mtx.m4x4()[2] = _mm256_unpacklo_pd(T2, T3); + mtx.m4x4()[3] = _mm256_unpackhi_pd(T2, T3); +#endif + return mtx; +} + +inline void translate(simd4x4_t& m, const simd4_t& dist) { + m.m4x4()[3] = _mm256_sub_pd(m.m4x4()[3], dist.v4()); +} + +template +inline void pre_translate(simd4x4_t& m, const simd4_t& dist) +{ + simd4_t row3(m.ptr()[0][3],m.ptr()[1][3],m.ptr()[2][3],m.ptr()[3][3]); + for (int i=0; i<3; ++i) { + for (int j=0; j<4; ++j) { + m.ptr()[j][i] += row3[j]*double(dist[i]); + } + } +#if 0 + simd4x4_t mt = simd4x4::transpose(m); + __mm256d row3 = mt.m4x4()[3]; + for (int i=0; i<3; ++i) { + __mm256d _mm256_set1_pd(float(dist[i])); + mt.m4x4()[i] = _mm256_add_pd(mt.m4x4()[i], _mm256_mul_pd(t, row3)); + } + m = simd4x4::transpose(mt); +#endif +} + +template +inline void post_translate(simd4x4_t& m, const simd4_t& dist) { + __m256d col3 = m.m4x4()[3]; + for (int i=0; i<3; ++i) { + __m256d t = _mm256_set1_pd(double(dist[i])); + col3 = _mm256_add_pd(col3, _mm256_mul_pd(t, m.m4x4()[i])); + } + m.m4x4()[3] = col3; +} + +template<> +inline simd4_t transform(const simd4x4_t& m, const simd4_t& pt) { + simd4_t res; + __m256d tpt = m.m4x4()[3]; + for (int i=0; i<3; ++i) { + __m256d ptd = _mm256_set1_pd(pt[i]); + tpt = _mm256_add_pd(tpt, _mm256_mul_pd(ptd, m.m4x4()[i])); + } + res = tpt; + res[3] = 0.0; + return res; +} + +} /* namespace simd4x4 */ + +# elif defined __SSE2__ # include template<> @@ -752,12 +1004,14 @@ inline void translate(simd4x4_t& m, const simd4_t& dist) { template inline void pre_translate(simd4x4_t& m, const simd4_t& dist) { +#if 1 simd4_t row3(m.ptr()[0][3],m.ptr()[1][3],m.ptr()[2][3],m.ptr()[3][3]); for (int i=0; i<3; ++i) { for (int j=0; j<4; ++j) { m.ptr()[j][i] += row3[j]*double(dist[i]); } } +#endif #if 0 // twice as slow simd4x4_t mt = simd4x4::transpose(m);