1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
|
#include "UnityPrefix.h"
#include "Matrix4x4.h"
#include <algorithm>
void CopyMatrixREF ( const float* __restrict lhs, float* __restrict res)
{
::memcpy( res, lhs, sizeof(Matrix4x4f) );
}
void TransposeMatrix4x4REF (const Matrix4x4f* __restrict lhs, Matrix4x4f* __restrict res)
{
CopyMatrix(lhs->m_Data, res->m_Data);
std::swap( res->Get(0,1), res->Get(1,0) );
std::swap( res->Get(0,2), res->Get(2,0) );
std::swap( res->Get(0,3), res->Get(3,0) );
std::swap( res->Get(1,2), res->Get(2,1) );
std::swap( res->Get(1,3), res->Get(3,1) );
std::swap( res->Get(2,3), res->Get(3,2) );
}
void MultiplyMatrices4x4REF (const Matrix4x4f* __restrict lhs, const Matrix4x4f* __restrict rhs, Matrix4x4f* __restrict res)
{
Assert (lhs != rhs && lhs != res && rhs != res);
for (int i=0;i<4;i++)
{
res->m_Data[i] = lhs->m_Data[i] * rhs->m_Data[0] + lhs->m_Data[i+4] * rhs->m_Data[1] + lhs->m_Data[i+8] * rhs->m_Data[2] + lhs->m_Data[i+12] * rhs->m_Data[3];
res->m_Data[i+4] = lhs->m_Data[i] * rhs->m_Data[4] + lhs->m_Data[i+4] * rhs->m_Data[5] + lhs->m_Data[i+8] * rhs->m_Data[6] + lhs->m_Data[i+12] * rhs->m_Data[7];
res->m_Data[i+8] = lhs->m_Data[i] * rhs->m_Data[8] + lhs->m_Data[i+4] * rhs->m_Data[9] + lhs->m_Data[i+8] * rhs->m_Data[10] + lhs->m_Data[i+12] * rhs->m_Data[11];
res->m_Data[i+12] = lhs->m_Data[i] * rhs->m_Data[12] + lhs->m_Data[i+4] * rhs->m_Data[13] + lhs->m_Data[i+8] * rhs->m_Data[14] + lhs->m_Data[i+12] * rhs->m_Data[15];
}
}
void MultiplyMatrixArray4x4REF (const Matrix4x4f* __restrict a, const Matrix4x4f* __restrict b, Matrix4x4f* __restrict res, size_t count)
{
Assert(a);
Assert(b);
Assert(res);
for (size_t i = 0; i < count; ++i)
{
MultiplyMatrices4x4(a+i, b+i, res+i);
}
}
void MultiplyMatrixArrayWithBase4x4REF (const Matrix4x4f* __restrict base,
const Matrix4x4f* __restrict a, const Matrix4x4f* __restrict b, Matrix4x4f* __restrict res, size_t count)
{
Assert(base);
Assert(a);
Assert(b);
Assert(res);
Matrix4x4f tmp;
for (size_t i = 0; i < count; ++i)
{
MultiplyMatrices4x4(base, a+i, &tmp);
MultiplyMatrices4x4(&tmp, b+i, res+i);
}
}
|
