mirror of https://github.com/axmolengine/axmol.git
153 lines
4.7 KiB
Plaintext
153 lines
4.7 KiB
Plaintext
|
NS_CC_MATH_BEGIN
|
||
|
|
|
||
|
|
inline void MathUtil::addMatrix(const __m128 m[4], float scalar, __m128 dst[4])
|
||
|
|
{
|
||
|
|
__m128 s = _mm_set1_ps(scalar);
|
||
|
|
dst[0] = _mm_add_ps(m[0], s);
|
||
|
|
dst[1] = _mm_add_ps(m[1], s);
|
||
|
|
dst[2] = _mm_add_ps(m[2], s);
|
||
|
|
dst[3] = _mm_add_ps(m[3], s);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void MathUtil::addMatrix(const __m128 m1[4], const __m128 m2[4], __m128 dst[4])
|
||
|
|
{
|
||
|
|
dst[0] = _mm_add_ps(m1[0], m2[0]);
|
||
|
|
dst[1] = _mm_add_ps(m1[1], m2[1]);
|
||
|
|
dst[2] = _mm_add_ps(m1[2], m2[2]);
|
||
|
|
dst[3] = _mm_add_ps(m1[3], m2[3]);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void MathUtil::subtractMatrix(const __m128 m1[4], const __m128 m2[4], __m128 dst[4])
|
||
|
|
{
|
||
|
|
dst[0] = _mm_sub_ps(m1[0], m2[0]);
|
||
|
|
dst[1] = _mm_sub_ps(m1[1], m2[1]);
|
||
|
|
dst[2] = _mm_sub_ps(m1[2], m2[2]);
|
||
|
|
dst[3] = _mm_sub_ps(m1[3], m2[3]);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void MathUtil::multiplyMatrix(const __m128 m[4], float scalar, __m128 dst[4])
|
||
|
|
{
|
||
|
|
__m128 s = _mm_set1_ps(scalar);
|
||
|
|
dst[0] = _mm_mul_ps(m[0], s);
|
||
|
|
dst[1] = _mm_mul_ps(m[1], s);
|
||
|
|
dst[2] = _mm_mul_ps(m[2], s);
|
||
|
|
dst[3] = _mm_mul_ps(m[3], s);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void MathUtil::multiplyMatrix(const __m128 m1[4], const __m128 m2[4], __m128 dst[4])
|
||
|
|
{
|
||
|
|
__m128 dst0, dst1, dst2, dst3;
|
||
|
|
{
|
||
|
|
__m128 e0 = _mm_shuffle_ps(m2[0], m2[0], _MM_SHUFFLE(0, 0, 0, 0));
|
||
|
|
__m128 e1 = _mm_shuffle_ps(m2[0], m2[0], _MM_SHUFFLE(1, 1, 1, 1));
|
||
|
|
__m128 e2 = _mm_shuffle_ps(m2[0], m2[0], _MM_SHUFFLE(2, 2, 2, 2));
|
||
|
|
__m128 e3 = _mm_shuffle_ps(m2[0], m2[0], _MM_SHUFFLE(3, 3, 3, 3));
|
||
|
|
|
||
|
|
__m128 v0 = _mm_mul_ps(m1[0], e0);
|
||
|
|
__m128 v1 = _mm_mul_ps(m1[1], e1);
|
||
|
|
__m128 v2 = _mm_mul_ps(m1[2], e2);
|
||
|
|
__m128 v3 = _mm_mul_ps(m1[3], e3);
|
||
|
|
|
||
|
|
__m128 a0 = _mm_add_ps(v0, v1);
|
||
|
|
__m128 a1 = _mm_add_ps(v2, v3);
|
||
|
|
__m128 a2 = _mm_add_ps(a0, a1);
|
||
|
|
|
||
|
|
dst0 = a2;
|
||
|
|
}
|
||
|
|
|
||
|
|
{
|
||
|
|
__m128 e0 = _mm_shuffle_ps(m2[1], m2[1], _MM_SHUFFLE(0, 0, 0, 0));
|
||
|
|
__m128 e1 = _mm_shuffle_ps(m2[1], m2[1], _MM_SHUFFLE(1, 1, 1, 1));
|
||
|
|
__m128 e2 = _mm_shuffle_ps(m2[1], m2[1], _MM_SHUFFLE(2, 2, 2, 2));
|
||
|
|
__m128 e3 = _mm_shuffle_ps(m2[1], m2[1], _MM_SHUFFLE(3, 3, 3, 3));
|
||
|
|
|
||
|
|
__m128 v0 = _mm_mul_ps(m1[0], e0);
|
||
|
|
__m128 v1 = _mm_mul_ps(m1[1], e1);
|
||
|
|
__m128 v2 = _mm_mul_ps(m1[2], e2);
|
||
|
|
__m128 v3 = _mm_mul_ps(m1[3], e3);
|
||
|
|
|
||
|
|
__m128 a0 = _mm_add_ps(v0, v1);
|
||
|
|
__m128 a1 = _mm_add_ps(v2, v3);
|
||
|
|
__m128 a2 = _mm_add_ps(a0, a1);
|
||
|
|
|
||
|
|
dst1 = a2;
|
||
|
|
}
|
||
|
|
|
||
|
|
{
|
||
|
|
__m128 e0 = _mm_shuffle_ps(m2[2], m2[2], _MM_SHUFFLE(0, 0, 0, 0));
|
||
|
|
__m128 e1 = _mm_shuffle_ps(m2[2], m2[2], _MM_SHUFFLE(1, 1, 1, 1));
|
||
|
|
__m128 e2 = _mm_shuffle_ps(m2[2], m2[2], _MM_SHUFFLE(2, 2, 2, 2));
|
||
|
|
__m128 e3 = _mm_shuffle_ps(m2[2], m2[2], _MM_SHUFFLE(3, 3, 3, 3));
|
||
|
|
|
||
|
|
__m128 v0 = _mm_mul_ps(m1[0], e0);
|
||
|
|
__m128 v1 = _mm_mul_ps(m1[1], e1);
|
||
|
|
__m128 v2 = _mm_mul_ps(m1[2], e2);
|
||
|
|
__m128 v3 = _mm_mul_ps(m1[3], e3);
|
||
|
|
|
||
|
|
__m128 a0 = _mm_add_ps(v0, v1);
|
||
|
|
__m128 a1 = _mm_add_ps(v2, v3);
|
||
|
|
__m128 a2 = _mm_add_ps(a0, a1);
|
||
|
|
|
||
|
|
dst2 = a2;
|
||
|
|
}
|
||
|
|
|
||
|
|
{
|
||
|
|
__m128 e0 = _mm_shuffle_ps(m2[3], m2[3], _MM_SHUFFLE(0, 0, 0, 0));
|
||
|
|
__m128 e1 = _mm_shuffle_ps(m2[3], m2[3], _MM_SHUFFLE(1, 1, 1, 1));
|
||
|
|
__m128 e2 = _mm_shuffle_ps(m2[3], m2[3], _MM_SHUFFLE(2, 2, 2, 2));
|
||
|
|
__m128 e3 = _mm_shuffle_ps(m2[3], m2[3], _MM_SHUFFLE(3, 3, 3, 3));
|
||
|
|
|
||
|
|
__m128 v0 = _mm_mul_ps(m1[0], e0);
|
||
|
|
__m128 v1 = _mm_mul_ps(m1[1], e1);
|
||
|
|
__m128 v2 = _mm_mul_ps(m1[2], e2);
|
||
|
|
__m128 v3 = _mm_mul_ps(m1[3], e3);
|
||
|
|
|
||
|
|
__m128 a0 = _mm_add_ps(v0, v1);
|
||
|
|
__m128 a1 = _mm_add_ps(v2, v3);
|
||
|
|
__m128 a2 = _mm_add_ps(a0, a1);
|
||
|
|
|
||
|
|
dst3 = a2;
|
||
|
|
}
|
||
|
|
dst[0] = dst0;
|
||
|
|
dst[1] = dst1;
|
||
|
|
dst[2] = dst2;
|
||
|
|
dst[3] = dst3;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void MathUtil::negateMatrix(const __m128 m[4], __m128 dst[4])
|
||
|
|
{
|
||
|
|
__m128 z = _mm_setzero_ps();
|
||
|
|
dst[0] = _mm_sub_ps(z, m[0]);
|
||
|
|
dst[1] = _mm_sub_ps(z, m[1]);
|
||
|
|
dst[2] = _mm_sub_ps(z, m[2]);
|
||
|
|
dst[3] = _mm_sub_ps(z, m[3]);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void MathUtil::transposeMatrix(const __m128 m[4], __m128 dst[4])
|
||
|
|
{
|
||
|
|
__m128 tmp0 = _mm_shuffle_ps(m[0], m[1], 0x44);
|
||
|
|
__m128 tmp2 = _mm_shuffle_ps(m[0], m[1], 0xEE);
|
||
|
|
__m128 tmp1 = _mm_shuffle_ps(m[2], m[3], 0x44);
|
||
|
|
__m128 tmp3 = _mm_shuffle_ps(m[2], m[3], 0xEE);
|
||
|
|
|
||
|
|
dst[0] = _mm_shuffle_ps(tmp0, tmp1, 0x88);
|
||
|
|
dst[1] = _mm_shuffle_ps(tmp0, tmp1, 0xDD);
|
||
|
|
dst[2] = _mm_shuffle_ps(tmp2, tmp3, 0x88);
|
||
|
|
dst[3] = _mm_shuffle_ps(tmp2, tmp3, 0xDD);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void MathUtil::transformVec4(const __m128 m[4], const __m128& v, __m128& dst)
|
||
|
|
{
|
||
|
|
__m128 col1 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(0, 0, 0, 0));
|
||
|
|
__m128 col2 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(1, 1, 1, 1));
|
||
|
|
__m128 col3 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(2, 2, 2, 2));
|
||
|
|
__m128 col4 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(3, 3, 3, 3));
|
||
|
|
|
||
|
|
dst = _mm_add_ps(
|
||
|
|
_mm_add_ps(_mm_mul_ps(m[0], col1), _mm_mul_ps(m[1], col2)),
|
||
|
|
_mm_add_ps(_mm_mul_ps(m[2], col3), _mm_mul_ps(m[3], col4))
|
||
|
|
);
|
||
|
|
}
|
||
|
|
|
||
|
|
NS_CC_MATH_END
|