diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx index b15e2472..6752e19e 100644 --- a/simgear/math/SGMatrix.hxx +++ b/simgear/math/SGMatrix.hxx @@ -54,14 +54,8 @@ public: T m20, T m21, T m22, T m23, T m30, T m31, T m32, T m33) { - _data[0] = m00; _data[1] = m10; - _data[2] = m20; _data[3] = m30; - _data[4] = m01; _data[5] = m11; - _data[6] = m21; _data[7] = m31; - _data[8] = m02; _data[9] = m12; - _data[10] = m22; _data[11] = m32; - _data[12] = m03; _data[13] = m13; - _data[14] = m23; _data[15] = m33; + _data = simd4x4_t(m00,m01,m02,m03,m10,m11,m12,m13, + m20,m21,m22,m23,m30,m31,m32,m33); } /// Constructor, build up a SGMatrix from a translation @@ -82,13 +76,8 @@ public: template void set(const SGVec3& trans) { - simd4x4::zeros(_data); - _data[0] = 1; - _data[12] = T(trans(0)); - _data[5] = 1; - _data[13] = T(trans(1)); - _data[10] = 1; _data[14] = T(trans(2)); - _data[15] = 1; + simd4x4::unit(_data); + simd4x4::translate(_data, trans.simd3()); } /// Set from a scale/rotation and tranlation @@ -113,28 +102,15 @@ public: void set(const TransNegRef& tm) { const SGMatrix& m = tm.m; - _data[0] = m(0,0); - _data[1] = m(0,1); - _data[2] = m(0,2); + _data = simd4x4::transpose(m.simd4x4()); _data[3] = m(3,0); - - _data[4] = m(1,0); - _data[5] = m(1,1); - _data[6] = m(1,2); _data[7] = m(3,1); - - _data[8] = m(2,0); - _data[9] = m(2,1); - _data[10] = m(2,2); _data[11] = m(3,2); // Well, this one is ugly here, as that xform method on the current // object needs the above data to be already set ... SGVec3 t = xformVec(SGVec3(m(0,3), m(1,3), m(2,3))); - t = -t; - _data[12] = t(0); - _data[13] = t(1); - _data[14] = t(2); + _data.set(3, -t.simd3()); _data[15] = m(3,3); } @@ -193,24 +169,13 @@ public: template SGMatrix& preMultTranslate(const SGVec3& t) { - SGVec4 row3((*this)(3,0), (*this)(3,1), (*this)(3,2), (*this)(3,3)); - for (unsigned i = 0; i < 3; ++i) { - SGVec4 trow3 = T(t(i))*row3; - (*this)(i,0) += trow3(0); (*this)(i,1) += trow3(1); - (*this)(i,2) += trow3(2); (*this)(i,3) += trow3(3); - } + simd4x4::pre_translate(_data,t.simd3()); return *this; } template SGMatrix& postMultTranslate(const SGVec3& t) { - SGVec4 col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3)); - for (unsigned i = 0; i < SGMatrix::nCols-1; ++i) { - SGVec4 tmp((*this)(0,i), (*this)(1,i), (*this)(2,i), (*this)(3,i)); - col3 += T(t(i))*tmp; - } - (*this)(0,3) = col3(0); (*this)(1,3) = col3(1); - (*this)(2,3) = col3(2); (*this)(3,3) = col3(3); + simd4x4::post_translate(_data,t.simd3()); return *this; } @@ -235,21 +200,14 @@ public: SGVec3 xformPt(const SGVec3& pt) const { - SGVec3 tpt((*this)(0,3), (*this)(1,3), (*this)(2,3)); - for (unsigned i = 0; i < SGMatrix::nCols-1; ++i) { - SGVec3 coli((*this)(0,i), (*this)(1,i), (*this)(2,i)); - tpt += pt(i)*coli; - } + SGVec3 tpt; + tpt.simd3() = simd4x4::transform(_data,pt.simd3()); return tpt; } SGVec3 xformVec(const SGVec3& v) const { - SGVec3 tv((*this)(0,0), (*this)(1,0), (*this)(2,0)); - tv *= v(0); - for (unsigned i = 1; i < SGMatrix::nCols-1; ++i) { - SGVec3 coli((*this)(0,i), (*this)(1,i), (*this)(2,i)); - tv += v(i)*coli; - } + SGVec3 tv; + tv.simd3() = _data * v.simd3(); return tv; } diff --git a/simgear/math/simd.hxx b/simgear/math/simd.hxx index dd71a3c0..6ff9106c 100644 --- a/simgear/math/simd.hxx +++ b/simgear/math/simd.hxx @@ -238,6 +238,13 @@ public: } }; +template +inline simd4_t operator-(const simd4_t& v) { + simd4_t r = T(0); + r -= v; + return r; +} + template inline simd4_t operator+(simd4_t v1, const simd4_t& v2) { v1 += v2; @@ -733,19 +740,26 @@ public: inline simd4_t& operator*=(int i) { return operator*=(simd4_t(i)); } - // https://software.intel.com/en-us/forums/intel-c-compiler/topic/288768 inline simd4_t& operator*=(const simd4_t& v) { + return operator*=(v.v4()); + } + // https://software.intel.com/en-us/forums/intel-c-compiler/topic/288768 + inline simd4_t& operator*=(const __m128i& v) { #ifdef __SSE4_1__ - simd4 = _mm_mullo_epi32(simd4, v.v4()); + simd4 = _mm_mullo_epi32(simd4, v); #else - __m128i tmp1 = _mm_mul_epu32(simd4, v.v4()); + __m128i tmp1 = _mm_mul_epu32(simd4, v); __m128i tmp2 = _mm_mul_epu32(_mm_srli_si128(simd4,4), - _mm_srli_si128(v.v4(),4)); + _mm_srli_si128(v,4)); simd4 =_mm_unpacklo_epi32(_mm_shuffle_epi32(tmp1,_MM_SHUFFLE (0,0,2,0)), _mm_shuffle_epi32(tmp2, _MM_SHUFFLE (0,0,2,0))); #endif return *this; } + + inline simd4_t& operator/=(int s) { + return operator*=(1/s); + } }; namespace simd4 diff --git a/simgear/math/simd4x4.hxx b/simgear/math/simd4x4.hxx index 87a10de2..5a755480 100644 --- a/simgear/math/simd4x4.hxx +++ b/simgear/math/simd4x4.hxx @@ -88,6 +88,52 @@ inline simd4x4_t transpose(simd4x4_t mtx) { return m; } +template +inline void translate(simd4x4_t& m, const simd4_t& dist) { + for (int i=0; i<3; ++i) { + m.ptr()[3][i] -= dist[i]; + } +} + +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) { + simd4_t trow3 = T(dist[i])*row3; + for (int j=0; j<4; ++j) { + m.ptr()[j][i] += trow3[j]; + } + } +} + +template +inline void post_translate(simd4x4_t& m, const simd4_t& dist) +{ + simd4_t col3(m.ptr()[3]); + for (int i=0; i<3; ++i) { + simd4_t trow3(T(dist[i])); + trow3 *= m.ptr()[i]; + col3 += trow3; + } + for (int i=0; i<3; ++i) { + m.ptr()[3][i] = col3[i]; + } +} + + +template // point transform +inline simd4_t transform(const simd4x4_t& mtx, const simd4_t& pt) +{ + simd4_t tpt(mtx.ptr()[3][0],mtx.ptr()[3][1],mtx.ptr()[3][2]); + for (int i=0; i<3; ++i) { + simd4_t ptd(mtx.ptr()[i][0],mtx.ptr()[i][1],mtx.ptr()[i][2]); + ptd *= pt[i]; + tpt += ptd; + } + return tpt; +} + } /* namespace simd4x4 */ template @@ -102,6 +148,20 @@ private: public: simd4x4_t(void) {} + simd4x4_t(T m00, T m01, T m02, T m03, + T m10, T m11, T m12, T m13, + T m20, T m21, T m22, T m23, + T m30, T m31, T m32, T m33) + { + array[0] = m00; array[1] = m10; + array[2] = m20; array[3] = m30; + array[4] = m01; array[5] = m11; + array[6] = m21; array[7] = m31; + array[8] = m02; array[9] = m12; + array[10] = m22; array[11] = m32; + array[12] = m03; array[13] = m13; + array[14] = m23; array[15] = m33; + } simd4x4_t(const T m[N*N]) { std::memcpy(array, m, sizeof(T[N*N])); } @@ -137,6 +197,10 @@ public: return array; } + inline void set(int i, const simd4_t& v) { + 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; @@ -208,14 +272,14 @@ inline simd4x4_t operator-(simd4x4_t m) { } -template -inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) +template +inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) { - simd4_t mv; - simd4_t row(m); + simd4_t mv; + simd4_t row(m.ptr()[0]); mv = vi.ptr()[0] * row; - for (int j=1; j row(m[j*N]); + for (int j=1; j row(m.ptr()[j]); mv += vi.ptr()[j] * row; } return mv; @@ -247,6 +311,16 @@ private: public: simd4x4_t(void) {} + simd4x4_t(float m00, float m01, float m02, float m03, + float m10, float m11, float m12, float m13, + float m20, float m21, float m22, float m23, + float m30, float m31, float m32, float m33) + { + simd4x4[0] = _mm_set_ps(m30,m20,m10,m00); + simd4x4[1] = _mm_set_ps(m31,m21,m11,m01); + simd4x4[2] = _mm_set_ps(m32,m22,m12,m02); + simd4x4[3] = _mm_set_ps(m33,m23,m13,m03); + } simd4x4_t(const float m[4*4]) { for (int i=0; i<4; ++i) { simd4x4[i] = simd4_t((const float*)&m[4*i]).v4(); @@ -289,6 +363,10 @@ public: return array; } + inline void set(int i, const simd4_t& v) { + 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(); @@ -341,16 +419,17 @@ public: } }; -template<> -inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) +template +inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) { - simd4_t mv(m); + simd4_t mv(m.m4x4()[0]); mv *= vi.ptr()[0]; - for (int i=1; i<4; ++i) { - simd4_t row(m.m4x4()[i]); + for (int i=1; i row(m.m4x4()[i]); row *= vi.ptr()[i]; mv.v4() += row.v4(); } + for (int i=M; i<4; ++i) mv[i] = 0; return mv; } @@ -395,6 +474,45 @@ inline simd4x4_t transpose(simd4x4_t m) { return m; } +inline void translate(simd4x4_t& m, const simd4_t& dist) { + m.m4x4()[3] -= dist.v4(); +} + +template +inline void pre_translate(simd4x4_t& m, const simd4_t& dist) +{ + simd4x4_t mt = simd4x4::transpose(m); + __m128 row3 = mt.m4x4()[3]; + for (int i=0; i<3; ++i) { + __m128 t = _mm_set1_ps(float(dist[i])); + mt.m4x4()[i] = _mm_add_ps(mt.m4x4()[i], _mm_mul_ps(t, row3)); + } + m = simd4x4::transpose(mt); +} + +template +inline void post_translate(simd4x4_t& m, const simd4_t& dist) +{ + __m128 col3 = m.m4x4()[3]; + for (int i=0; i<3; ++i) { + __m128 t = _mm_set1_ps(float(dist[i])); + col3 = _mm_add_ps(col3, _mm_mul_ps(t, m.m4x4()[i])); + } + m.m4x4()[3] = col3; +} + +template<> +inline simd4_t transform(const simd4x4_t& m, const simd4_t& pt) { + simd4_t tpt; + tpt.v4() = m.m4x4()[3]; + for (int i=0; i<3; ++i) { + __m128 ptd = _mm_set1_ps(pt[i]); + tpt.v4() = _mm_add_ps(tpt.v4(), _mm_mul_ps(ptd, m.m4x4()[i])); + } + tpt[3] = 0.0; + return tpt; +} + } /* namespace simd4x */ # endif @@ -417,6 +535,20 @@ private: 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][0] = _mm_set_pd(m10,m00); + simd4x4[0][1] = _mm_set_pd(m30,m20); + simd4x4[1][0] = _mm_set_pd(m11,m01); + simd4x4[1][1] = _mm_set_pd(m31,m21); + simd4x4[2][0] = _mm_set_pd(m12,m02); + simd4x4[2][1] = _mm_set_pd(m32,m22); + 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; for (int i=0; i<4; ++i) { @@ -464,6 +596,11 @@ public: return array; } + inline void set(int i, const simd4_t& v) { + simd4x4[i][0] = v.v4()[0]; + 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) { @@ -533,18 +670,18 @@ public: }; - -template<> -inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) +template +inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) { - simd4_t mv(m); + simd4_t mv(m.m4x4()[0]); mv *= vi.ptr()[0]; - for (int i=1; i<4; i+=2) { - simd4_t row = m.m4x4()[i]; + for (int i=1; i row = m.m4x4()[i]; row *= vi.ptr()[i]; mv.v4()[0] += row.v4()[0]; mv.v4()[1] += row.v4()[1]; } + for (int i=M; i<4; ++i) mv[i] = 0; return mv; } @@ -609,6 +746,53 @@ inline simd4x4_t transpose(simd4x4_t m) { return mtx; } +inline void translate(simd4x4_t& m, const simd4_t& dist) { + m.m4x4()[3][0] -= dist.v4()[0]; + m.m4x4()[3][1] -= dist.v4()[1]; +} + +template +inline void pre_translate(simd4x4_t& m, const simd4_t& dist) +{ + simd4x4_t mt = simd4x4::transpose(m); + __m128d row3[2]; + row3[0] = mt.m4x4()[3][0]; + row3[1] = mt.m4x4()[3][1]; + for (int i=0; i<3; ++i) { + __m128d t = _mm_set1_pd(double(dist[i])); + mt.m4x4()[i][0] = _mm_add_pd(mt.m4x4()[i][0], _mm_mul_pd(t, row3[0])); + mt.m4x4()[i][1] = _mm_add_pd(mt.m4x4()[i][1], _mm_mul_pd(t, row3[1])); + } + m = simd4x4::transpose(mt); +} + +template +inline void post_translate(simd4x4_t& m, const simd4_t& dist) { + __m128d col3[2]; + col3[0] = m.m4x4()[3][0]; + col3[1] = m.m4x4()[3][1]; + for (int i=0; i<3; ++i) { + __m128d t = _mm_set1_pd(double(dist[i])); + col3[0] = _mm_add_pd(col3[0], _mm_mul_pd(t, m.m4x4()[i][0])); + col3[1] = _mm_add_pd(col3[1], _mm_mul_pd(t, m.m4x4()[i][1])); + } + m.m4x4()[3][0] = col3[0]; + m.m4x4()[3][1] = col3[1]; +} + +template<> +inline simd4_t transform(const simd4x4_t& m, const simd4_t& pt) { + simd4_t tpt; + tpt.v4()[0] = m.m4x4()[3][0]; + tpt.v4()[1] = m.m4x4()[3][1]; + for (int i=0; i<3; ++i) { + __m128d ptd = _mm_set1_pd(pt[i]); + 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; +} } /* namespace simd4x4 */ @@ -632,12 +816,21 @@ private: public: simd4x4_t(void) {} + simd4x4_t(int m00, int m01, int m02, int m03, + int m10, int m11, int m12, int m13, + int m20, int m21, int m22, int m23, + int m30, int m31, int m32, int m33) + { + simd4x4[0] = _mm_set_epi32(m30,m20,m10,m00); + simd4x4[1] = _mm_set_epi32(m31,m21,m11,m01); + simd4x4[2] = _mm_set_epi32(m32,m22,m12,m02); + simd4x4[3] = _mm_set_epi32(m33,m23,m13,m03); + } simd4x4_t(const int m[4*4]) { for (int i=0; i<4; ++i) { simd4x4[i] = simd4_t((const int*)&m[4*i]).v4(); } } - explicit simd4x4_t(const __mtx4i_t m) { for (int i=0; i<4; ++i) { simd4x4[i] = simd4_t(m[i]).v4(); @@ -674,6 +867,10 @@ public: return array; } + inline void set(int i, const simd4_t& v) { + 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(); @@ -726,16 +923,17 @@ public: } }; -template<> -inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) +template +inline simd4_t operator*(const simd4x4_t& m, const simd4_t& vi) { - simd4_t mv(m); + simd4_t mv(m.m4x4()[0]); mv *= vi.ptr()[0]; - for (int i=1; i<4; ++i) { + for (int i=1; i row(m.m4x4()[i]); row *= vi.ptr()[i]; mv.v4() += row.v4(); } + for (int i=M; i<4; ++i) mv[i] = 0; return mv; } @@ -776,6 +974,48 @@ inline simd4x4_t transpose(simd4x4_t m) { return m; } +inline void translate(simd4x4_t& m, const simd4_t& dist) { + m.m4x4()[3] = _mm_sub_epi32(m.m4x4()[3], dist.v4()); +} + +template +inline void pre_translate(simd4x4_t& m, const simd4_t& dist) +{ + simd4x4_t mt = simd4x4::transpose(m); + simd4_t row3(mt.ptr()[3]); + for (int i=0; i<3; ++i) { + simd4_t trow3 = int(dist[i]); + trow3 *= row3.v4(); + mt.m4x4()[i] = _mm_add_epi32(mt.m4x4()[i], trow3.v4()); + } + m = simd4x4::transpose(mt); +} + +template +inline void post_translate(simd4x4_t& m, const simd4_t& dist) +{ + __m128i col3 = m.m4x4()[3]; + for (int i=0; i<3; ++i) { + simd4_t trow3 = int(dist[i]); + trow3 *= m.m4x4()[i]; + col3 = _mm_add_epi32(col3, trow3.v4()); + } + m.m4x4()[3] = col3; +} + +template<> +inline simd4_t transform(const simd4x4_t& m, const simd4_t& pt) { + simd4_t tpt = m.m4x4()[3]; + for (int i=0; i<3; ++i) { + simd4_t ptd = m.m4x4()[i]; + ptd *= pt[i]; + tpt.v4() = _mm_add_epi32(tpt.v4(), ptd.v4()); + } + tpt[3] = 0.0; + return tpt; +} + + } /* namespace simd4x */ # endif