Added determinant SSE draft

glm/core/intrinsic_matrix.inl

@@ -12,6 +12,20 @@ static const __m128 pi = _mm_set_ps1(3.141592653589793238462643383279f);
 static const __m128 _m128_rad_ps = _mm_set_ps1(3.141592653589793238462643383279f / 180.f);
 static const __m128 _m128_deg_ps = _mm_set_ps1(180.f / 3.141592653589793238462643383279f);
 
+template <typename matType>
+inline matType _mm_comp_mul_ps
+(
+	__m128 const in1[4],
+	__m128 const in2[4],
+	__m128 out[4]
+)
+{
+	out[0] = _mm_mul_ps(in1[0], in2[0]);
+	out[1] = _mm_mul_ps(in1[1], in2[1]);
+	out[2] = _mm_mul_ps(in1[2], in2[2]);
+	out[3] = _mm_mul_ps(in1[3], in2[3]);
+}
+
 inline void _mm_add_ps(__m128 in1[4], __m128 in2[4], __m128 out[4])
 {
 	{
@@ -167,6 +181,297 @@ inline void _mm_transpose_ps(__m128 const in[4], __m128 out[4])
     out[3] = _mm_shuffle_ps(tmp2, tmp3, 0xDD);
 }
 
+inline __m128 _mm_det_ps(__m128 const in[4])
+{
+	__m128 Fac0;
+	{
+		//	valType SubFactor00 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
+		//	valType SubFactor00 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
+		//	valType SubFactor06 = m[1][2] * m[3][3] - m[3][2] * m[1][3];
+		//	valType SubFactor13 = m[1][2] * m[2][3] - m[2][2] * m[1][3];
+
+		__m128 Swp0a = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(3, 3, 3, 3));
+		__m128 Swp0b = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(2, 2, 2, 2));
+
+		__m128 Swp00 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(2, 2, 2, 2));
+		__m128 Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp03 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(3, 3, 3, 3));
+
+		__m128 Mul00 = _mm_mul_ps(Swp00, Swp01);
+		__m128 Mul01 = _mm_mul_ps(Swp02, Swp03);
+		Fac0 = _mm_sub_ps(Mul00, Mul01);
+
+		bool stop = true;
+	}
+
+	__m128 Fac1;
+	{
+		//	valType SubFactor01 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
+		//	valType SubFactor01 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
+		//	valType SubFactor07 = m[1][1] * m[3][3] - m[3][1] * m[1][3];
+		//	valType SubFactor14 = m[1][1] * m[2][3] - m[2][1] * m[1][3];
+
+		__m128 Swp0a = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(3, 3, 3, 3));
+		__m128 Swp0b = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(1, 1, 1, 1));
+
+		__m128 Swp00 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(1, 1, 1, 1));
+		__m128 Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp03 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(3, 3, 3, 3));
+
+		__m128 Mul00 = _mm_mul_ps(Swp00, Swp01);
+		__m128 Mul01 = _mm_mul_ps(Swp02, Swp03);
+		Fac1 = _mm_sub_ps(Mul00, Mul01);
+
+		bool stop = true;
+	}
+
+
+	__m128 Fac2;
+	{
+		//	valType SubFactor02 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
+		//	valType SubFactor02 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
+		//	valType SubFactor08 = m[1][1] * m[3][2] - m[3][1] * m[1][2];
+		//	valType SubFactor15 = m[1][1] * m[2][2] - m[2][1] * m[1][2];
+
+		__m128 Swp0a = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(2, 2, 2, 2));
+		__m128 Swp0b = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(1, 1, 1, 1));
+
+		__m128 Swp00 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(1, 1, 1, 1));
+		__m128 Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp03 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(2, 2, 2, 2));
+
+		__m128 Mul00 = _mm_mul_ps(Swp00, Swp01);
+		__m128 Mul01 = _mm_mul_ps(Swp02, Swp03);
+		Fac2 = _mm_sub_ps(Mul00, Mul01);
+
+		bool stop = true;
+	}
+
+	__m128 Fac3;
+	{
+		//	valType SubFactor03 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
+		//	valType SubFactor03 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
+		//	valType SubFactor09 = m[1][0] * m[3][3] - m[3][0] * m[1][3];
+		//	valType SubFactor16 = m[1][0] * m[2][3] - m[2][0] * m[1][3];
+
+		__m128 Swp0a = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(3, 3, 3, 3));
+		__m128 Swp0b = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(0, 0, 0, 0));
+
+		__m128 Swp00 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(0, 0, 0, 0));
+		__m128 Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp03 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(3, 3, 3, 3));
+
+		__m128 Mul00 = _mm_mul_ps(Swp00, Swp01);
+		__m128 Mul01 = _mm_mul_ps(Swp02, Swp03);
+		Fac3 = _mm_sub_ps(Mul00, Mul01);
+
+		bool stop = true;
+	}
+
+	__m128 Fac4;
+	{
+		//	valType SubFactor04 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
+		//	valType SubFactor04 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
+		//	valType SubFactor10 = m[1][0] * m[3][2] - m[3][0] * m[1][2];
+		//	valType SubFactor17 = m[1][0] * m[2][2] - m[2][0] * m[1][2];
+
+		__m128 Swp0a = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(2, 2, 2, 2));
+		__m128 Swp0b = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(0, 0, 0, 0));
+
+		__m128 Swp00 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(0, 0, 0, 0));
+		__m128 Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp03 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(2, 2, 2, 2));
+
+		__m128 Mul00 = _mm_mul_ps(Swp00, Swp01);
+		__m128 Mul01 = _mm_mul_ps(Swp02, Swp03);
+		Fac4 = _mm_sub_ps(Mul00, Mul01);
+
+		bool stop = true;
+	}
+
+	__m128 Fac5;
+	{
+		//	valType SubFactor05 = m[2][0] * m[3][1] - m[3][0] * m[2][1];
+		//	valType SubFactor05 = m[2][0] * m[3][1] - m[3][0] * m[2][1];
+		//	valType SubFactor12 = m[1][0] * m[3][1] - m[3][0] * m[1][1];
+		//	valType SubFactor18 = m[1][0] * m[2][1] - m[2][0] * m[1][1];
+
+		__m128 Swp0a = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(1, 1, 1, 1));
+		__m128 Swp0b = _mm_shuffle_ps(in[3], in[2], _MM_SHUFFLE(0, 0, 0, 0));
+
+		__m128 Swp00 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(0, 0, 0, 0));
+		__m128 Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));
+		__m128 Swp03 = _mm_shuffle_ps(in[2], in[1], _MM_SHUFFLE(1, 1, 1, 1));
+
+		__m128 Mul00 = _mm_mul_ps(Swp00, Swp01);
+		__m128 Mul01 = _mm_mul_ps(Swp02, Swp03);
+		Fac5 = _mm_sub_ps(Mul00, Mul01);
+
+		bool stop = true;
+	}
+
+	__m128 SignA = _mm_set_ps( 1.0f,-1.0f, 1.0f,-1.0f);
+	__m128 SignB = _mm_set_ps(-1.0f, 1.0f,-1.0f, 1.0f);
+
+	// m[1][0]
+	// m[0][0]
+	// m[0][0]
+	// m[0][0]
+	__m128 Temp0 = _mm_shuffle_ps(in[1], in[0], _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 Vec0 = _mm_shuffle_ps(Temp0, Temp0, _MM_SHUFFLE(2, 2, 2, 0));
+
+	// m[1][1]
+	// m[0][1]
+	// m[0][1]
+	// m[0][1]
+	__m128 Temp1 = _mm_shuffle_ps(in[1], in[0], _MM_SHUFFLE(1, 1, 1, 1));
+	__m128 Vec1 = _mm_shuffle_ps(Temp1, Temp1, _MM_SHUFFLE(2, 2, 2, 0));
+
+	// m[1][2]
+	// m[0][2]
+	// m[0][2]
+	// m[0][2]
+	__m128 Temp2 = _mm_shuffle_ps(in[1], in[0], _MM_SHUFFLE(2, 2, 2, 2));
+	__m128 Vec2 = _mm_shuffle_ps(Temp2, Temp2, _MM_SHUFFLE(2, 2, 2, 0));
+
+	// m[1][3]
+	// m[0][3]
+	// m[0][3]
+	// m[0][3]
+	__m128 Temp3 = _mm_shuffle_ps(in[1], in[0], _MM_SHUFFLE(3, 3, 3, 3));
+	__m128 Vec3 = _mm_shuffle_ps(Temp3, Temp3, _MM_SHUFFLE(2, 2, 2, 0));
+
+	// col0
+	// + (Vec1[0] * Fac0[0] - Vec2[0] * Fac1[0] + Vec3[0] * Fac2[0]),
+	// - (Vec1[1] * Fac0[1] - Vec2[1] * Fac1[1] + Vec3[1] * Fac2[1]),
+	// + (Vec1[2] * Fac0[2] - Vec2[2] * Fac1[2] + Vec3[2] * Fac2[2]),
+	// - (Vec1[3] * Fac0[3] - Vec2[3] * Fac1[3] + Vec3[3] * Fac2[3]),
+	__m128 Mul00 = _mm_mul_ps(Vec1, Fac0);
+	__m128 Mul01 = _mm_mul_ps(Vec2, Fac1);
+	__m128 Mul02 = _mm_mul_ps(Vec3, Fac2);
+	__m128 Sub00 = _mm_sub_ps(Mul00, Mul01);
+	__m128 Add00 = _mm_add_ps(Sub00, Mul02);
+	__m128 Inv0 = _mm_mul_ps(SignB, Add00);
+
+	// col1
+	// - (Vec0[0] * Fac0[0] - Vec2[0] * Fac3[0] + Vec3[0] * Fac4[0]),
+	// + (Vec0[0] * Fac0[1] - Vec2[1] * Fac3[1] + Vec3[1] * Fac4[1]),
+	// - (Vec0[0] * Fac0[2] - Vec2[2] * Fac3[2] + Vec3[2] * Fac4[2]),
+	// + (Vec0[0] * Fac0[3] - Vec2[3] * Fac3[3] + Vec3[3] * Fac4[3]),
+	__m128 Mul03 = _mm_mul_ps(Vec0, Fac0);
+	__m128 Mul04 = _mm_mul_ps(Vec2, Fac3);
+	__m128 Mul05 = _mm_mul_ps(Vec3, Fac4);
+	__m128 Sub01 = _mm_sub_ps(Mul03, Mul04);
+	__m128 Add01 = _mm_add_ps(Sub01, Mul05);
+	__m128 Inv1 = _mm_mul_ps(SignA, Add01);
+
+	// col2
+	// + (Vec0[0] * Fac1[0] - Vec1[0] * Fac3[0] + Vec3[0] * Fac5[0]),
+	// - (Vec0[0] * Fac1[1] - Vec1[1] * Fac3[1] + Vec3[1] * Fac5[1]),
+	// + (Vec0[0] * Fac1[2] - Vec1[2] * Fac3[2] + Vec3[2] * Fac5[2]),
+	// - (Vec0[0] * Fac1[3] - Vec1[3] * Fac3[3] + Vec3[3] * Fac5[3]),
+	__m128 Mul06 = _mm_mul_ps(Vec0, Fac1);
+	__m128 Mul07 = _mm_mul_ps(Vec1, Fac3);
+	__m128 Mul08 = _mm_mul_ps(Vec3, Fac5);
+	__m128 Sub02 = _mm_sub_ps(Mul06, Mul07);
+	__m128 Add02 = _mm_add_ps(Sub02, Mul08);
+	__m128 Inv2 = _mm_mul_ps(SignB, Add02);
+
+	// col3
+	// - (Vec1[0] * Fac2[0] - Vec1[0] * Fac4[0] + Vec2[0] * Fac5[0]),
+	// + (Vec1[0] * Fac2[1] - Vec1[1] * Fac4[1] + Vec2[1] * Fac5[1]),
+	// - (Vec1[0] * Fac2[2] - Vec1[2] * Fac4[2] + Vec2[2] * Fac5[2]),
+	// + (Vec1[0] * Fac2[3] - Vec1[3] * Fac4[3] + Vec2[3] * Fac5[3]));
+	__m128 Mul09 = _mm_mul_ps(Vec0, Fac2);
+	__m128 Mul10 = _mm_mul_ps(Vec1, Fac4);
+	__m128 Mul11 = _mm_mul_ps(Vec2, Fac5);
+	__m128 Sub03 = _mm_sub_ps(Mul09, Mul10);
+	__m128 Add03 = _mm_add_ps(Sub03, Mul11);
+	__m128 Inv3 = _mm_mul_ps(SignA, Add03);
+
+	__m128 Row0 = _mm_shuffle_ps(Inv0, Inv1, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 Row1 = _mm_shuffle_ps(Inv2, Inv3, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 Row2 = _mm_shuffle_ps(Row0, Row1, _MM_SHUFFLE(2, 0, 2, 0));
+
+	//	valType Determinant = m[0][0] * Inverse[0][0]
+	//						+ m[0][1] * Inverse[1][0]
+	//						+ m[0][2] * Inverse[2][0]
+	//						+ m[0][3] * Inverse[3][0];
+	__m128 Det0 = _mm_dot_ps(in[0], Row2);
+	return Det0;
+}
+
+template <typename T>
+inline typename detail::tmat4x4<T>::value_type _mm_det2_ps
+(
+	__m128 const & m[4]
+)
+{
+	GLM_STATIC_ASSERT(detail::type<T>::is_float, "'determinant' only accept floating-point inputs");
+
+	//T SubFactor00 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
+	//T SubFactor01 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
+	//T SubFactor02 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
+	//T SubFactor03 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
+	//T SubFactor04 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
+	//T SubFactor05 = m[2][0] * m[3][1] - m[3][0] * m[2][1];
+
+	// First 2 columns
+	__m128 Swp2A = _mm_shuffle_ps(m[2], m[2], _MM_SHUFFLE(0, 1, 1, 2));
+	__m128 Swp3A = _mm_shuffle_ps(m[3], m[3], _MM_SHUFFLE(3, 2, 3, 3));
+	__m128 MulA = __mm_mul_ps(Swp2A, Swp3A);
+
+	// Second 2 columns
+	__m128 Swp2B = _mm_shuffle_ps(m[2], m[2], _MM_SHUFFLE(0, 1, 1, 2));
+	__m128 Swp3B = _mm_shuffle_ps(m[3], m[3], _MM_SHUFFLE(3, 2, 3, 3));
+	__m128 MulB = __mm_mul_ps(Swp2A, Swp3A);
+
+	// Columns subtraction
+	__m128 SubAB = __mm_sub_ps(MulA, MulB);
+
+	// Last 2 rows
+	__m128 Swp2C = _mm_shuffle_ps(m[2], m[2], _MM_SHUFFLE(1, 2, 0, 0));
+	__m128 Swp3C = _mm_shuffle_ps(m[3], m[3], _MM_SHUFFLE(0, 0, 1, 2));
+	__m128 MulC = __mm_mul_ps(Swp2C, Swp3C);
+	__m128 SwpD = __mm_hl_ps(MulC);
+
+	//detail::tvec4<T> DetCof(
+	//	+ (m[1][1] * SubFactor00 - m[1][2] * SubFactor01 + m[1][3] * SubFactor02),
+	//	- (m[1][0] * SubFactor00 - m[1][2] * SubFactor03 + m[1][3] * SubFactor04),
+	//	+ (m[1][0] * SubFactor01 - m[1][1] * SubFactor03 + m[1][3] * SubFactor05),
+	//	- (m[1][0] * SubFactor02 - m[1][1] * SubFactor04 + m[1][2] * SubFactor05));
+
+	__128 SubFacA = _mm_shuffle_ps(SubAB, SubAB, _MM_SHUFFLE(2, 1, 0, 0));
+	__128 SwpFacA = _mm_shuffle_ps(m[1], m[1], _MM_SHUFFLE(0, 0, 0, 1));
+	__128 MulFacA = __mm_mul_ps(SwpFacA, SubFacA);
+
+	__128 SubFacB = ;
+	__128 SwpFacB = _mm_shuffle_ps(m[1], m[1], _MM_SHUFFLE(1, 1, 2, 2));
+	__128 MulFacB = __mm_mul_ps(SwpFacB, SubFacB);
+
+	__128 SubRes = __mm_sub_ps(MulFacA, MulFacA);
+
+	__128 SubFacC = ;
+	__128 SwpFacC = _mm_shuffle_ps(m[1], m[1], _MM_SHUFFLE(1, 1, 2, 2));
+	__128 MulFacC = __mm_mul_ps(SwpFacC, SubFacC);
+
+	__m128 AddRes = __mm_add_ps(SubRes, MulFacC);
+	__m128 DetCof = __mm_mul_ps(AddRes, _mm_set_ps( 1.0f,-1.0f, 1.0f,-1.0f));
+
+	//return m[0][0] * DetCof[0]
+	//	 + m[0][1] * DetCof[1]
+	//	 + m[0][2] * DetCof[2]
+	//	 + m[0][3] * DetCof[3];
+
+	return _mm_dot_ps(m[0], Signed);
+}
+
 inline void _mm_inverse_ps(__m128 const in[4], __m128 out[4])
 {
 	__m128 Fac0;
@@ -702,3 +1007,11 @@ void _mm_rotate_ps(__m128 const in[4], float Angle, float const v[3], __m128 out
 	//return Result;
 	_mm_mul_ps(in, Result, out);
 }
+
+void _mm_outer_ps(__m128 const & c, __m128 const & r, __m128 out[4])
+{
+	out[0] = _mm_mul_ps(c, _mm_shuffle_ps(r, r, _MM_SHUFFLE(0, 0, 0, 0));
+	out[1] = _mm_mul_ps(c, _mm_shuffle_ps(r, r, _MM_SHUFFLE(1, 1, 1, 1));
+	out[2] = _mm_mul_ps(c, _mm_shuffle_ps(r, r, _MM_SHUFFLE(2, 2, 2, 2));
+	out[3] = _mm_mul_ps(c, _mm_shuffle_ps(r, r, _MM_SHUFFLE(3, 3, 3, 3));
+}