diff --git a/simgear/math/SGVec2.hxx b/simgear/math/SGVec2.hxx index d1284d8e..e44344ec 100644 --- a/simgear/math/SGVec2.hxx +++ b/simgear/math/SGVec2.hxx @@ -344,14 +344,14 @@ template inline T dist(const SGVec2& v1, const SGVec2& v2) -{ return norm(v1 - v2); } +{ return simd4::magnitude(v1.simd2() - v2.simd2()); } /// The squared euclidean distance of the two vectors template inline T distSqr(SGVec2 v1, const SGVec2& v2) -{ v1 -= v2; return dot(v1, v1); } +{ return simd4::magnitude2(v1.simd2() - v2.simd2()); } // calculate the projection of u along the direction of d. template @@ -359,7 +359,7 @@ inline SGVec2 projection(const SGVec2& u, const SGVec2& d) { - T denom = dot(d, d); + T denom = simd4::magnitude2(d.simd2()); T ud = dot(u, d); if (SGLimits::min() < denom) return u; else return d * (dot(u, d) / denom); diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx index 9323063d..f57a1000 100644 --- a/simgear/math/SGVec3.hxx +++ b/simgear/math/SGVec3.hxx @@ -466,7 +466,7 @@ inline SGVec3 projection(const SGVec3& u, const SGVec3& d) { - T denom = simd4::magnitude2(d); + T denom = simd4::magnitude2(d.simd3()); T ud = dot(u, d); if (SGLimits::min() < denom) return u; else return d * (dot(u, d) / denom); diff --git a/simgear/math/SGVec4.hxx b/simgear/math/SGVec4.hxx index 94fc8f52..adf051c7 100644 --- a/simgear/math/SGVec4.hxx +++ b/simgear/math/SGVec4.hxx @@ -370,14 +370,14 @@ template inline T dist(const SGVec4& v1, const SGVec4& v2) -{ return norm(v1 - v2); } +{ return simd4::magnitude(v1.simd4() - v2.simd4()); } /// The squared euclidean distance of the two vectors template inline T distSqr(SGVec4 v1, const SGVec4& v2) -{ v1 -= v2; return dot(v1, v1); } +{ return simd4::magnitude2(v1.simd4() - v2.simd4()); } // calculate the projection of u along the direction of d. template @@ -385,7 +385,7 @@ inline SGVec4 projection(const SGVec4& u, const SGVec4& d) { - T denom = dot(d, d); + T denom = simd4::magnitude2(d.simd4()); T ud = dot(u, d); if (SGLimits::min() < denom) return u; else return d * (dot(u, d) / denom); diff --git a/simgear/math/simd.hxx b/simgear/math/simd.hxx index e5710930..004d673e 100644 --- a/simgear/math/simd.hxx +++ b/simgear/math/simd.hxx @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -132,7 +133,7 @@ public: template simd4_t(const simd4_t& v) { std::memcpy(_v4, v.ptr(), sizeof(T[M])); - for (int i=M; i<4; ++i) _v4[i] = 0; + for (int i=(M& operator=(T s) { - for (int i=0; i& operator=(const T v[N]) { - std::memcpy(vec, v, sizeof(T[N])); - for (int i=N; i<4; ++i) _v4[i] = 0; - return *this; - } - template - inline simd4_t& operator=(const simd4_t& v) { - std::memcpy(_v4, v.ptr(), sizeof(T[M])); - for (int i=M; i<4; ++i) _v4[i] = 0; - return *this; + inline T& operator[](unsigned n) { + assert(n& operator+=(T s) { @@ -189,9 +175,8 @@ public: } return *this; } - template - inline simd4_t& operator+=(const simd4_t& v) { - for (int i=0; i& operator+=(const simd4_t& v) { + for (int i=0; i - inline simd4_t& operator-=(const simd4_t& v) { + inline simd4_t& operator-=(const simd4_t& v) { for (int i=0; i - inline simd4_t& operator*=(const simd4_t& v) { + inline simd4_t& operator*=(const simd4_t& v) { for (int i=0; i - inline simd4_t& operator/=(const simd4_t& v) { + inline simd4_t& operator/=(const simd4_t& v) { for (int i=0; i operator-(const simd4_t& v) { return r; } -template -inline simd4_t operator+(simd4_t v1, const simd4_t& v2) { +template +inline simd4_t operator+(simd4_t v1, const simd4_t& v2) { v1 += v2; return v1; } -template -inline simd4_t operator-(simd4_t v1, const simd4_t& v2) { +template +inline simd4_t operator-(simd4_t v1, const simd4_t& v2) { v1 -= v2; return v1; } -template -inline simd4_t operator*(simd4_t v1, const simd4_t& v2) { +template +inline simd4_t operator*(simd4_t v1, const simd4_t& v2) { v1 *= v2; return v1; } -template -inline simd4_t operator/(simd4_t v1, const simd4_t& v2) { +template +inline simd4_t operator/(simd4_t v1, const simd4_t& v2) { v1 /= v2; return v1; } @@ -312,10 +294,21 @@ inline simd4_t operator*(simd4_t v, T f) { # define ALIGN32C __attribute__((aligned(32))) # endif -static const uint32_t m2a32[] = { 0xffffffff,0xffffffff,0,0 }; -static const uint32_t m3a32[] = { 0xffffffff,0xffffffff,0xffffffff,0 }; -static const uint64_t m2a64[] = { 0xffffffffffffffff,0xffffffffffffffff,0,0 }; -static const uint64_t m3a64[] = { 0xffffffffffffffff,0xffffffffffffffff,0xffffffffffffffff,0 }; +namespace simd4 +{ +static ALIGN16 const uint32_t m2a32[] ALIGN16C = { + 0xffffffff,0xffffffff,0,0 +}; +static ALIGN16 const uint32_t m3a32[] ALIGN16C = { + 0xffffffff,0xffffffff,0xffffffff,0 +}; +static ALIGN32 const uint64_t m2a64[] ALIGN32C = { + 0xffffffffffffffff,0xffffffffffffffff,0,0 +}; +static ALIGN32 const uint64_t m3a64[] ALIGN32C = { + 0xffffffffffffffff,0xffffffffffffffff,0xffffffffffffffff,0 +}; +}; /* namespace simd4 */ # endif # ifdef __SSE__ @@ -361,10 +354,9 @@ public: simd4 = _mm_set_ps(w,z,y,x); } simd4_t(const __vec4f_t v) {} - template - simd4_t(const simd4_t& v) { - simd4 = v.v4(); - } + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} simd4_t(const __m128& v) { simd4 = v; } @@ -383,52 +375,92 @@ public: return vec; } - inline operator const float*(void) const { - return vec; + inline const float& operator[](unsigned n) const { + assert(n& operator+=(float f) { - return operator+=(simd4_t(f)); + simd4 = _mm_add_ps(simd4, _mm_set1_ps(f)); + return *this; } - template - inline simd4_t& operator+=(const simd4_t& v) { + inline simd4_t& operator+=(const simd4_t& v) { simd4 = _mm_add_ps(simd4, v.v4()); return *this; } inline simd4_t& operator-=(float f) { - return operator-=(simd4_t(f)); + simd4 = _mm_sub_ps(simd4, _mm_set1_ps(f)); + return *this; } - template - inline simd4_t& operator-=(const simd4_t& v) { + inline simd4_t& operator-=(const simd4_t& v) { simd4 = _mm_sub_ps(simd4, v.v4()); return *this; } inline simd4_t& operator*=(float f) { - return operator*=(simd4_t(f)); + simd4 = _mm_mul_ps(simd4, _mm_set1_ps(f)); + return *this; } - template - inline simd4_t& operator*=(const simd4_t& v) { + inline simd4_t& operator*=(const simd4_t& v) { simd4 = _mm_mul_ps(simd4, v.v4()); return *this; } inline simd4_t& operator/=(float f) { - return operator/=(simd4_t(f)); + simd4 = _mm_div_ps(simd4, _mm_set1_ps(f)); + return *this; } - template - inline simd4_t& operator/=(const simd4_t& v) { + inline simd4_t& operator/=(const simd4_t& v) { simd4 = _mm_div_ps(simd4, v.v4()); return *this; } }; -static const __m128 fmask2 = _mm_load_ps((const float*)m2a32); -static const __m128 fmask3 = _mm_load_ps((const float*)m3a32); +static const __m128 fmask2 = _mm_load_ps((const float*)simd4::m2a32); +static const __m128 fmask3 = _mm_load_ps((const float*)simd4::m3a32); + +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_ps(v.v4(), fmask3); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_ps(v.v4(), fmask2); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_ps(v.v4(), fmask2); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} template<> inline simd4_t::simd4_t(const __vec4f_t v) { @@ -562,10 +594,9 @@ public: simd4 = _mm256_set_pd(w,z,y,x); } simd4_t(const __vec4d_t v) {} - template - simd4_t(const simd4_t& v) { - simd4 = v.v4(); - } + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} simd4_t(const __m256d& v) { simd4 = v; } @@ -584,52 +615,92 @@ public: return vec; } - inline operator const double*(void) const { - return vec; + inline const double& operator[](unsigned n) const { + assert(n& operator+=(double d) { - return operator+=(simd4_t(d)); + simd4 = _mm256_add_pd(simd4, _mm256_set1_pd(d)); + return *this; } - template - inline simd4_t& operator+=(const simd4_t& v) { + 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)); + simd4 = _mm256_sub_pd(simd4, _mm256_set1_pd(d)); + return *this; } - template - inline simd4_t& operator-=(const simd4_t& v) { + 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)); + simd4 = _mm256_mul_pd(simd4, _mm256_set1_pd(d)); + return *this; } - template - inline simd4_t& operator*=(const simd4_t& v) { + 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)); + simd4 = _mm256_div_pd(simd4, _mm256_set1_pd(d)); + return *this; } - template - inline simd4_t& operator/=(const simd4_t& v) { + inline simd4_t& operator/=(const simd4_t& v) { simd4 = _mm256_div_pd(simd4, v.v4()); return *this; } }; -static const __m256d dmask2 = _mm256_load_pd((const double*)m2a64); -static const __m256d dmask3 = _mm256_load_pd((const double*)m3a64); +static const __m256d dmask2 = _mm256_load_pd((const double*)simd4::m2a64); +static const __m256d dmask3 = _mm256_load_pd((const double*)simd4::m3a64); + +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_pd(v.v4(), dmask3); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_pd(v.v4(), dmask2); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_pd(v.v4(), dmask2); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} template<> inline simd4_t::simd4_t(const __vec4d_t v) { @@ -740,11 +811,9 @@ public: simd4[1] = _mm_set_pd(w,z); } simd4_t(const __vec4d_t v) {} - template - simd4_t(const simd4_t& v) { - simd4[0] = v.v4()[0]; - simd4[1] = v.v4()[1]; - } + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} simd4_t(const __m128d v[2]) { simd4[0] = v[0]; simd4[1] = v[1]; @@ -764,55 +833,110 @@ public: return vec; } - inline operator const double*(void) const { - return vec; + inline const double& operator[](unsigned n) const { + assert(n& operator+=(double d) { - return operator+=(simd4_t(d)); + __m128d d4 = _mm_set1_pd(d); + simd4[0] = _mm_add_pd(simd4[0], d4); + simd4[1] = _mm_add_pd(simd4[1], d4); + return *this; } - template - inline simd4_t& operator+=(const simd4_t& v) { + inline simd4_t& operator+=(const simd4_t& v) { simd4[0] = _mm_add_pd(simd4[0], v.v4()[0]); simd4[1] = _mm_add_pd(simd4[1], v.v4()[1]); return *this; } inline simd4_t& operator-=(double d) { - return operator-=(simd4_t(d)); + __m128d d4 = _mm_set1_pd(d); + simd4[0] = _mm_sub_pd(simd4[0], d4); + simd4[1] = _mm_sub_pd(simd4[1], d4); } - template - inline simd4_t& operator-=(const simd4_t& v) { + inline simd4_t& operator-=(const simd4_t& v) { simd4[0] = _mm_sub_pd(simd4[0], v.v4()[0]); simd4[1] = _mm_sub_pd(simd4[1], v.v4()[1]); return *this; } inline simd4_t& operator*=(double d) { - return operator*=(simd4_t(d)); + __m128d d4 = _mm_set1_pd(d); + simd4[0] = _mm_mul_pd(simd4[0], d4); + simd4[1] = _mm_mul_pd(simd4[1], d4); + return *this; } - template - inline simd4_t& operator*=(const simd4_t& v) { + inline simd4_t& operator*=(const simd4_t& v) { simd4[0] = _mm_mul_pd(simd4[0], v.v4()[0]); simd4[1] = _mm_mul_pd(simd4[1], v.v4()[1]); return *this; } inline simd4_t& operator/=(double d) { - return operator/=(simd4_t(d)); + __m128d d4 = _mm_set1_pd(d); + simd4[0] = _mm_div_pd(simd4[0], d4); + simd4[1] = _mm_div_pd(simd4[1], d4); } - template - inline simd4_t& operator/=(const simd4_t& v) { + inline simd4_t& operator/=(const simd4_t& v) { simd4[0] = _mm_div_pd(simd4[0], v.v4()[0]); simd4[1] = _mm_div_pd(simd4[1], v.v4()[1]); return *this; } }; -static const __m128d dmask3 = _mm_load_pd((const double*)(m3a64+2)); +static const __m128d dmask3 = _mm_load_pd((const double*)(simd4::m3a64+2)); + +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = v.v4()[1]; +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = v.v4()[1]; +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = v.v4()[1]; +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = _mm_and_pd(v.v4()[1], dmask3); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = v.v4()[1]; +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = _mm_setzero_pd(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = _mm_setzero_pd(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = _mm_setzero_pd(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4[0] = v.v4()[0]; + simd4[1] = _mm_setzero_pd(); +} template<> inline simd4_t::simd4_t(const __vec4d_t v) { @@ -945,10 +1069,9 @@ public: simd4 = _mm_set_epi32(w,z,y,x); } simd4_t(const __vec4i_t v) {} - template - simd4_t(const simd4_t& v) { - simd4 = v.v4(); - } + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} + simd4_t(const simd4_t& v) {} simd4_t(const __m128i& v) { simd4 = v; } @@ -969,37 +1092,38 @@ public: return vec; } - inline operator const int*(void) const { - return vec; + inline const int& operator[](unsigned n) const { + assert(n& operator+=(int i) { - return operator+=(simd4_t(i)); + simd4 = _mm_add_epi32(simd4, _mm_set1_epi32(i)); + return *this; } - template - inline simd4_t& operator+=(const simd4_t& v) { + inline simd4_t& operator+=(const simd4_t& v) { simd4 = _mm_add_epi32(simd4, v.v4()); return *this; } inline simd4_t& operator-=(int i) { - return operator-=(simd4_t(i)); + simd4 = _mm_sub_epi32(simd4, _mm_set1_epi32(i)); + return *this; } - template - inline simd4_t& operator-=(const simd4_t& v) { + inline simd4_t& operator-=(const simd4_t& v) { simd4 = _mm_sub_epi32(simd4, v.v4()); return *this; } inline simd4_t& operator*=(int i) { - return operator*=(simd4_t(i)); + return operator*=(_mm_set1_epi32(i)); } - template - inline simd4_t& operator*=(const simd4_t& v) { + inline simd4_t& operator*=(const simd4_t& v) { return operator*=(v.v4()); } // https://software.intel.com/en-us/forums/intel-c-compiler/topic/288768 @@ -1019,8 +1143,7 @@ public: inline simd4_t& operator/=(int s) { return operator/=(simd4_t(s)); } - template - inline simd4_t& operator/=(const simd4_t& v) { + inline simd4_t& operator/=(const simd4_t& v) { for (int i=0; i +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_si128(v.v4(), imask3); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_si128(v.v4(), imask2); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = _mm_and_si128(v.v4(), imask2); +} +template<> +inline simd4_t::simd4_t(const simd4_t& v) { + simd4 = v.v4(); +} template<> inline simd4_t::simd4_t(const __vec4i_t v) { diff --git a/simgear/math/simd4x4.hxx b/simgear/math/simd4x4.hxx index b88604fa..221e09c6 100644 --- a/simgear/math/simd4x4.hxx +++ b/simgear/math/simd4x4.hxx @@ -201,19 +201,6 @@ public: std::memcpy(mtx[i], v.v4(), sizeof(T[N])); } - inline simd4x4_t& operator=(const T m[N*N]) { - std::memcpy(array, m, sizeof(T[N*N])); - return *this; - } - inline simd4x4_t& operator=(const T m[N][N]) { - std::memcpy(array, m, sizeof(T[N*N])); - return *this; - } - inline simd4x4_t& operator=(const simd4x4_t& m) { - std::memcpy(array, m, sizeof(T[N*N])); - return *this; - } - inline simd4x4_t& operator+=(const simd4x4_t& m) { for (int i=0; i((const float*)&m[4*i]).v4(); + simd4x4[i] = _mm_loadu_ps((const float*)&m[4*i]); } } - explicit simd4x4_t(const __mtx4f_t m) { + simd4x4_t(const __mtx4f_t m) { for (int i=0; i<4; ++i) { - simd4x4[i] = simd4_t(m[i]).v4(); + simd4x4[i] = _mm_loadu_ps(m[i]); } } simd4x4_t(const simd4x4_t& m) { @@ -368,19 +355,6 @@ public: simd4x4[i] = v.v4(); } - inline simd4x4_t& operator=(const __mtx4f_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] = _mm_add_ps(simd4x4[i], m.m4x4()[i]); @@ -396,22 +370,21 @@ public: } inline simd4x4_t& operator*=(float f) { - simd4_t f4(f); + __m128 f4 = _mm_set1_ps(f); for (int i=0; i<4; ++i) { - simd4x4[i] = _mm_mul_ps(simd4x4[i], f4.v4()); + simd4x4[i] = _mm_mul_ps(simd4x4[i], f4); } return *this; } 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(m1.m4x4()[0], col); + row = _mm_mul_ps(simd4x4[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(m1.m4x4()[j], col)); + row = _mm_add_ps(row, _mm_mul_ps(simd4x4[j], col)); } simd4x4[i] = row; } @@ -505,7 +478,6 @@ inline simd4_t transform(const simd4x4_t& m, const simd __m128 ptd = _mm_set1_ps(pt[i]); tpt = _mm_add_ps(tpt, _mm_mul_ps(ptd, m.m4x4()[i])); } - tpt[3] = 0.0; return tpt; } @@ -515,7 +487,6 @@ inline simd4_t transform(const simd4x4_t& m, const simd # ifdef __AVX_unsupported__ -# include # include template<> @@ -542,15 +513,15 @@ public: 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]) { + simd4x4_t(const double m[4*4]) { for (int i=0; i<4; ++i) { - simd4x4[i] = simd4_t((const double*)&m[4*i]).v4(); + simd4x4[i] = _mm256_loadu_pd((const double*)&m[4*i]); } } - explicit simd4x4_t(const __mtx4d_t m) { + simd4x4_t(const __mtx4d_t m) { for (int i=0; i<4; ++i) { - simd4x4[i] = simd4_t(m[i]).v4(); + simd4x4[i] = _mm256_loadu_pd(m[i]); } } simd4x4_t(const simd4x4_t& m) { @@ -589,19 +560,6 @@ public: simd4x4[i] = v.v4(); } - 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]); @@ -617,28 +575,26 @@ public: } inline simd4x4_t& operator*=(double f) { - simd4_t f4(f); + __m256d f4 = _mm256_set1_pd(f); for (int i=0; i<4; ++i) { - simd4x4[i] = _mm256_mul_pd(simd4x4[i], f4.v4()); + simd4x4[i] = _mm256_mul_pd(simd4x4[i], f4); } return *this; } simd4x4_t& operator*=(const simd4x4_t& m2) { - simd4x4_t m1 = *this; __m256d row, col; for (int i=0; i<4; ++i ) { col = _mm256_set1_pd(m2.ptr()[i][0]); - row = _mm256_mul_pd(m1.m4x4()[0], col); + row = _mm256_mul_pd(simd4x4[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)); + row = _mm256_add_pd(row, _mm256_mul_pd(simd4x4[j], col)); } simd4x4[i] = row; } return *this; } - }; template @@ -654,7 +610,6 @@ inline simd4_t operator*(const simd4x4_t& m, const simd4_t inline simd4x4_t rotation_matrix(double angle, const simd4_t& axis) { @@ -719,9 +674,9 @@ inline void pre_translate(simd4x4_t& m, const simd4_t& dist) #if 0 // this is slower simd4x4_t mt = simd4x4::transpose(m); - __mm256d row3 = mt.m4x4()[3]; + __m256d row3 = mt.m4x4()[3]; for (int i=0; i<3; ++i) { - __mm256d _mm256_set1_pd(float(dist[i])); + __m256d _mm256_set1_pd(float(dist[i])); mt.m4x4()[i] = _mm256_add_pd(mt.m4x4()[i], _mm256_mul_pd(t, row3)); } m = simd4x4::transpose(mt); @@ -747,7 +702,6 @@ inline simd4_t transform(const simd4x4_t& m, const s tpt = _mm256_add_pd(tpt, _mm256_mul_pd(ptd, m.m4x4()[i])); } res = tpt; - res[3] = 0.0; return res; } @@ -784,19 +738,17 @@ public: simd4x4[3][0] = _mm_set_pd(m13,m03); simd4x4[3][1] = _mm_set_pd(m33,m23); } - explicit simd4x4_t(const double m[4*4]) { - const double *p = m; + simd4x4_t(const double m[4*4]) { 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]); } } - explicit simd4x4_t(const __mtx4d_t m) { + 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]); } } simd4x4_t(const simd4x4_t& m) { @@ -837,31 +789,6 @@ public: simd4x4[i][1] = v.v4()[1]; } - inline simd4x4_t& operator=(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]; - } - return *this; - } - - inline simd4x4_t& operator=(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]; - } - return *this; - } - inline simd4x4_t& operator=(const simd4x4_t& m) { - for (int i=0; i<4; ++i) { - simd4x4[i][0] = m.m4x4()[i][0]; - simd4x4[i][1] = m.m4x4()[i][1]; - } - return *this; - } - inline simd4x4_t& operator+=(const simd4x4_t& m) { for (int i=0; i<4; ++i) { simd4x4[i][0] = _mm_add_pd(simd4x4[i][0], m.m4x4()[i][0]); @@ -879,26 +806,27 @@ public: } inline simd4x4_t& operator*=(double f) { - simd4_t f4(f); + __m128d f4 = _mm_set1_pd(f); for (int i=0; i<4; ++i) { - simd4x4[i][0] = _mm_mul_pd(simd4x4[i][0], f4.v4()[0]); - simd4x4[i][1] = _mm_mul_pd(simd4x4[i][1], f4.v4()[0]); + simd4x4[i][0] = _mm_mul_pd(simd4x4[i][0], f4); + simd4x4[i][1] = _mm_mul_pd(simd4x4[i][1], f4); } return *this; } simd4x4_t& operator*=(const simd4x4_t& m2) { - simd4x4_t m1 = *this; - simd4_t row, col; + __m128d row[2], col; for (int i=0; i<4; ++i ) { - simd4_t col = m1.m4x4()[0]; - row = col * m2.ptr()[i][0]; + 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); for (int j=1; j<4; ++j) { - col = m1.m4x4()[j]; - row += col * m2.ptr()[i][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)); } - simd4x4[i][0] = row.v4()[0]; - simd4x4[i][1] = row.v4()[1]; + simd4x4[i][0] = row[0]; + simd4x4[i][1] = row[1]; } return *this; } @@ -1038,7 +966,6 @@ inline simd4_t transform(const simd4x4_t& m, const s tpt.v4()[0] = _mm_add_pd(tpt.v4()[0], _mm_mul_pd(ptd, m.m4x4()[i][0])); tpt.v4()[1] = _mm_add_pd(tpt.v4()[1], _mm_mul_pd(ptd, m.m4x4()[i][1])); } - tpt[3] = 0.0; return tpt; } @@ -1076,12 +1003,12 @@ public: } simd4x4_t(const int m[4*4]) { for (int i=0; i<4; ++i) { - simd4x4[i] = simd4_t((const int*)&m[4*i]).v4(); + simd4x4[i] = _mm_loadu_si128((const __m128i*)&m[4*i]); } } - explicit simd4x4_t(const __mtx4i_t m) { + simd4x4_t(const __mtx4i_t m) { for (int i=0; i<4; ++i) { - simd4x4[i] = simd4_t(m[i]).v4(); + simd4x4[i] = _mm_loadu_si128((const __m128i*)&m[i]); } } simd4x4_t(const simd4x4_t& m) { @@ -1120,53 +1047,38 @@ public: simd4x4[i] = v.v4(); } - inline simd4x4_t& operator=(const __mtx4i_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] += m.m4x4()[i]; + simd4x4[i] = _mm_add_epi32(simd4x4[i], m.m4x4()[i]); } return *this; } inline simd4x4_t& operator-=(const simd4x4_t& m) { for (int i=0; i<4; ++i) { - simd4x4[i] -= m.m4x4()[i]; + simd4x4[i] = _mm_sub_epi32(simd4x4[i], m.m4x4()[i]); } return *this; } - inline simd4x4_t& operator*=(int f) { - simd4_t f4(f); + inline simd4x4_t& operator*=(int v) { + __m128i v4 = _mm_set1_epi32(v); for (int i=0; i<4; ++i) { - simd4x4[i] *= f4.v4(); + simd4x4[i] = _mm_mul_epi32(simd4x4[i], v4); } return *this; } simd4x4_t& operator*=(const simd4x4_t& m2) { - simd4x4_t m1 = *this; - simd4_t row, col; - - for (int i=0; i<4; ++i) { - simd4_t col(m2.ptr()[i][0]); - row.v4() = m1.m4x4()[0] * col.v4(); + __m128i row, col; + for (int i=0; i<4; ++i ) { + col = _mm_set1_epi32(m2.ptr()[i][0]); + row = _mm_mul_epi32(simd4x4[0], col); for (int j=1; j<4; ++j) { - simd4_t col(m2.ptr()[i][j]); - row.v4() += m1.m4x4()[j] * col.v4(); + col = _mm_set1_epi32(m2.ptr()[i][j]); + row = _mm_add_epi32(row, _mm_mul_epi32(simd4x4[j], col)); } - simd4x4[i] = row.v4(); + simd4x4[i] = row; } return *this; } @@ -1260,7 +1172,6 @@ inline simd4_t transform(const simd4x4_t& m, const simd4_t