118 lines
3.8 KiB
C
118 lines
3.8 KiB
C
#include "StgSharpGraphic.h"
|
|
#include <stdio.h>
|
|
#include <pmmintrin.h>
|
|
|
|
|
|
SSGC_API matrix4map* add_mat4(matrix4map* mat1, matrix4map* mat2)
|
|
{
|
|
matrix4map* ret = _aligned_malloc(sizeof(matrix4), 16);
|
|
|
|
if (ret != 0)
|
|
{
|
|
ret->colum[0] = _mm_add_ps(mat1->colum[0], mat2->colum[0]);
|
|
ret->colum[1] = _mm_add_ps(mat1->colum[1], mat2->colum[1]);
|
|
ret->colum[2] = _mm_add_ps(mat1->colum[2], mat2->colum[2]);
|
|
ret->colum[3] = _mm_add_ps(mat1->colum[3], mat2->colum[3]);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
SSGC_API matrix4map* sub_mat4(matrix4map* mat1, matrix4map* mat2)
|
|
{
|
|
matrix4map* ret = _aligned_malloc(sizeof(matrix4), 16);
|
|
|
|
if (ret != 0)
|
|
{
|
|
ret->colum[0] = _mm_sub_ps(mat1->colum[0], mat2->colum[0]);
|
|
ret->colum[1] = _mm_sub_ps(mat1->colum[1], mat2->colum[1]);
|
|
ret->colum[2] = _mm_sub_ps(mat1->colum[2], mat2->colum[2]);
|
|
ret->colum[3] = _mm_sub_ps(mat1->colum[3], mat2->colum[3]);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
SSGC_API __inline matrix4map* __cdecl mul_const(matrix4map* mat, float value)
|
|
{
|
|
matrix4map* ret = _aligned_malloc(sizeof(matrix4),16);
|
|
|
|
if (NULL != ret)
|
|
{
|
|
float cstarray[4] = { value, value, value, value };
|
|
|
|
__m128 cst128 = _mm_loadu_ps(&cstarray);
|
|
|
|
ret->colum[0] = _mm_mul_ps(mat->colum[0], cst128);
|
|
ret->colum[1] = _mm_mul_ps(mat->colum[1], cst128);
|
|
ret->colum[2] = _mm_mul_ps(mat->colum[2], cst128);
|
|
ret->colum[3] = _mm_mul_ps(mat->colum[3], cst128);
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
//left is the transposed matrix of real left
|
|
SSGC_API __inline matrix4* __cdecl mul_4_4(matrix4map* left, matrix4map* right)
|
|
{
|
|
matrix4map* retPtr = _aligned_malloc(sizeof(matrix4map), 16);
|
|
if (NULL != retPtr)
|
|
{
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
for (size_t t = 0; t < 4; t++)
|
|
{
|
|
retPtr->m[i][t] = mulps_sum_internal(
|
|
ALIGN( left->colum[i]),ALIGN(right->colum[t]));
|
|
}
|
|
}
|
|
}
|
|
return retPtr;
|
|
}
|
|
|
|
SSGC_API __inline void __cdecl transpose4to2(matrix4map* source, matrix4map* target)
|
|
{
|
|
__m128 t0 = _mm_shuffle_ps(ALIGN(source->colum[0]), ALIGN(source->colum[1]), _MM_SHUFFLE(1, 0, 1, 0));
|
|
__m128 t2 = _mm_shuffle_ps(ALIGN(source->colum[2]), ALIGN(source->colum[3]), _MM_SHUFFLE(1, 0, 1, 0));
|
|
|
|
target->colum[0] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(2, 0, 2, 0));
|
|
target->colum[1] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(3, 1, 3, 1));
|
|
}
|
|
|
|
SSGC_API __inline void __cdecl transpose4to4(matrix4map* source, matrix4map* target)
|
|
{
|
|
__m128 t0 = _mm_shuffle_ps(ALIGN(source->colum[0]), ALIGN(source->colum[1]), _MM_SHUFFLE(1, 0, 1, 0));
|
|
__m128 t1 = _mm_shuffle_ps(ALIGN(source->colum[0]), ALIGN(source->colum[1]), _MM_SHUFFLE(3, 2, 3, 2));
|
|
__m128 t2 = _mm_shuffle_ps(ALIGN(source->colum[2]), ALIGN(source->colum[3]), _MM_SHUFFLE(1, 0, 1, 0));
|
|
__m128 t3 = _mm_shuffle_ps(ALIGN(source->colum[2]), ALIGN(source->colum[3]), _MM_SHUFFLE(3, 2, 3, 2));
|
|
|
|
target->colum[0] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(2, 0, 2, 0));
|
|
target->colum[1] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(3, 1, 3, 1));
|
|
target->colum[2] = _mm_shuffle_ps(t1, t3, _MM_SHUFFLE(2, 0, 2, 0));
|
|
target->colum[3] = _mm_shuffle_ps(t1, t3, _MM_SHUFFLE(3, 1, 3, 1));
|
|
|
|
}
|
|
|
|
SSGC_API __inline float __cdecl det_mat4(matrix4* matPtr)
|
|
{
|
|
__m128 cogeneration = _mm_set_ps(
|
|
det_mat3_internal(matPtr->matrix.colum[1], matPtr->matrix.colum[2], matPtr->matrix.colum[3]),
|
|
det_mat3_internal(matPtr->matrix.colum[0], matPtr->matrix.colum[2], matPtr->matrix.colum[3]),
|
|
det_mat3_internal(matPtr->matrix.colum[0], matPtr->matrix.colum[1], matPtr->matrix.colum[3]),
|
|
det_mat3_internal(matPtr->matrix.colum[0], matPtr->matrix.colum[1], matPtr->matrix.colum[2])
|
|
);
|
|
|
|
printf("%f\n\n", det_mat3_internal(matPtr->matrix.colum[1], matPtr->matrix.colum[2], matPtr->matrix.colum[3]));
|
|
|
|
__m128 retvec = _mm_mul_ps((matPtr->transpose).colum[3],cogeneration);
|
|
retvec = _mm_hsub_ps(retvec,retvec);
|
|
retvec = _mm_hadd_ps(retvec,retvec);
|
|
return -retvec.m128_f32[0];
|
|
|
|
}
|
|
|
|
SSGC_API __inline void __cdecl deinit_mat4(matrix4map* matPtr)
|
|
{
|
|
_aligned_free(matPtr);
|
|
} |