mirror of
https://github.com/Cesarus85/ringfall-mjolnir-vr.git
synced 2026-10-26 21:00:47 +01:00
74 lines
3.4 KiB
C++
74 lines
3.4 KiB
C++
#include <xmmintrin.h>
|
|
#include "math_internal.h"
|
|
#pragma code_seg("D3D")
|
|
namespace D3D
|
|
{
|
|
void WINAPI ScaleVector3(
|
|
D3DVECTOR *out,
|
|
const D3DVECTOR *v,
|
|
float scale)
|
|
{
|
|
out->x = scale * v->x;
|
|
out->y = scale * v->y;
|
|
out->z = scale * v->z;
|
|
return;
|
|
}
|
|
void WINAPI AddVectors3(
|
|
D3DVECTOR *out,
|
|
const D3DVECTOR *a,
|
|
const D3DVECTOR *b)
|
|
{
|
|
out->x = a->x + b->x;
|
|
out->y = a->y + b->y;
|
|
out->z = a->z + b->z;
|
|
return;
|
|
}
|
|
float WINAPI SquareMagnitude3(
|
|
const D3DVECTOR *v)
|
|
{
|
|
return v->x * v->x + v->y * v->y + v->z * v->z;
|
|
}
|
|
void WINAPI XformBy4x3(
|
|
D3DVECTOR *out,
|
|
const D3DVECTOR *v,
|
|
float w,
|
|
const D3DMATRIX *m)
|
|
{
|
|
// The original uses aligned matrix row loads and unaligned vector scalars.
|
|
__m128 result = _mm_mul_ps(_mm_set1_ps(v->x), _mm_load_ps(m->m[0]));
|
|
result = _mm_add_ps(result, _mm_mul_ps(_mm_set1_ps(v->y), _mm_load_ps(m->m[1])));
|
|
result = _mm_add_ps(result, _mm_mul_ps(_mm_set1_ps(v->z), _mm_load_ps(m->m[2])));
|
|
result = _mm_add_ps(result, _mm_mul_ps(_mm_set1_ps(w), _mm_load_ps(m->m[3])));
|
|
_mm_storel_pi((__m64 *)out, result);
|
|
_mm_store_ss(&out->z, _mm_shuffle_ps(result, result, 2));
|
|
return;
|
|
}
|
|
void WINAPI MatrixProduct4x4(
|
|
D3DMATRIX *out,
|
|
const D3DMATRIX *a,
|
|
const D3DMATRIX *b)
|
|
{
|
|
__m128 row0 = _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[0]), _mm_load_ps(a->m[0]), 0x00), _mm_load_ps(b->m[0]));
|
|
row0 = _mm_add_ps(row0, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[0]), _mm_load_ps(a->m[0]), 0x55), _mm_load_ps(b->m[1])));
|
|
row0 = _mm_add_ps(row0, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[0]), _mm_load_ps(a->m[0]), 0xaa), _mm_load_ps(b->m[2])));
|
|
row0 = _mm_add_ps(row0, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[0]), _mm_load_ps(a->m[0]), 0xff), _mm_load_ps(b->m[3])));
|
|
__m128 row1 = _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[1]), _mm_load_ps(a->m[1]), 0x00), _mm_load_ps(b->m[0]));
|
|
row1 = _mm_add_ps(row1, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[1]), _mm_load_ps(a->m[1]), 0x55), _mm_load_ps(b->m[1])));
|
|
row1 = _mm_add_ps(row1, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[1]), _mm_load_ps(a->m[1]), 0xaa), _mm_load_ps(b->m[2])));
|
|
row1 = _mm_add_ps(row1, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[1]), _mm_load_ps(a->m[1]), 0xff), _mm_load_ps(b->m[3])));
|
|
__m128 row2 = _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[2]), _mm_load_ps(a->m[2]), 0x00), _mm_load_ps(b->m[0]));
|
|
row2 = _mm_add_ps(row2, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[2]), _mm_load_ps(a->m[2]), 0x55), _mm_load_ps(b->m[1])));
|
|
row2 = _mm_add_ps(row2, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[2]), _mm_load_ps(a->m[2]), 0xaa), _mm_load_ps(b->m[2])));
|
|
row2 = _mm_add_ps(row2, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[2]), _mm_load_ps(a->m[2]), 0xff), _mm_load_ps(b->m[3])));
|
|
__m128 row3 = _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[3]), _mm_load_ps(a->m[3]), 0x00), _mm_load_ps(b->m[0]));
|
|
row3 = _mm_add_ps(row3, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[3]), _mm_load_ps(a->m[3]), 0x55), _mm_load_ps(b->m[1])));
|
|
row3 = _mm_add_ps(row3, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[3]), _mm_load_ps(a->m[3]), 0xaa), _mm_load_ps(b->m[2])));
|
|
row3 = _mm_add_ps(row3, _mm_mul_ps(_mm_shuffle_ps(_mm_load_ps(a->m[3]), _mm_load_ps(a->m[3]), 0xff), _mm_load_ps(b->m[3])));
|
|
_mm_store_ps(out->m[0], row0);
|
|
_mm_store_ps(out->m[1], row1);
|
|
_mm_store_ps(out->m[2], row2);
|
|
_mm_store_ps(out->m[3], row3);
|
|
return;
|
|
}
|
|
}
|