You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
172 lines
4.7 KiB
172 lines
4.7 KiB
#pragma once
|
|
|
|
class Matrix4x4
|
|
{
|
|
public:
|
|
union
|
|
{
|
|
double m[16];
|
|
double _m[4][4];
|
|
};
|
|
|
|
Matrix4x4() { LoadIdentity(); }
|
|
|
|
void LoadNull()
|
|
{
|
|
for (int i = 0; i < 16; i++)
|
|
m[i] = 0;
|
|
}
|
|
|
|
void LoadIdentity()
|
|
{
|
|
m[0] = m[5] = m[10] = m[15] = 1;
|
|
m[1] = m[2] = m[3] = m[4] =
|
|
m[6] = m[7] = m[8] = m[9] =
|
|
m[11] = m[12] = m[13] = m[14] = 0;
|
|
}
|
|
|
|
void SetAxis(int axis, const Vec3& value)
|
|
{
|
|
m[0 + axis] = value.x;
|
|
m[4 + axis] = value.y;
|
|
m[8 + axis] = value.z;
|
|
}
|
|
|
|
void SetTranslation(const Vec3& value)
|
|
{
|
|
m[12] = value.x;
|
|
m[13] = value.y;
|
|
m[14] = value.z;
|
|
}
|
|
|
|
Vec3 TransformPoint(const Vec3& point)
|
|
{
|
|
return Vec3(
|
|
(double)point.x * m[0] +
|
|
(double)point.y * m[4] +
|
|
(double)point.z * m[8] +
|
|
m[12],
|
|
|
|
(double)point.x * m[1] +
|
|
(double)point.y * m[5] +
|
|
(double)point.z * m[9] +
|
|
m[13],
|
|
|
|
(double)point.x * m[2] +
|
|
(double)point.y * m[6] +
|
|
(double)point.z * m[10] +
|
|
m[14]);
|
|
}
|
|
|
|
Vec3 TransformDirection(const Vec3& dir)
|
|
{
|
|
return Vec3(
|
|
(double)dir.x * m[0] +
|
|
(double)dir.y * m[4] +
|
|
(double)dir.z * m[8],
|
|
|
|
(double)dir.x * m[1] +
|
|
(double)dir.y * m[5] +
|
|
(double)dir.z * m[9],
|
|
|
|
(double)dir.x * m[2] +
|
|
(double)dir.y * m[6] +
|
|
(double)dir.z * m[10]);
|
|
}
|
|
|
|
bool Invert()
|
|
{
|
|
double tmp[12];
|
|
double src[16];
|
|
double dst[16];
|
|
|
|
// Transpose matrix
|
|
for (int i = 0; i < 4; i++) {
|
|
src[i + 0] = m[i * 4 + 0];
|
|
src[i + 4] = m[i * 4 + 1];
|
|
src[i + 8] = m[i * 4 + 2];
|
|
src[i + 12] = m[i * 4 + 3];
|
|
}
|
|
|
|
// Calculate pairs for first 8 elements (cofactors)
|
|
tmp[0] = src[10] * src[15];
|
|
tmp[1] = src[11] * src[14];
|
|
tmp[2] = src[9] * src[15];
|
|
tmp[3] = src[11] * src[13];
|
|
tmp[4] = src[9] * src[14];
|
|
tmp[5] = src[10] * src[13];
|
|
tmp[6] = src[8] * src[15];
|
|
tmp[7] = src[11] * src[12];
|
|
tmp[8] = src[8] * src[14];
|
|
tmp[9] = src[10] * src[12];
|
|
tmp[10] = src[8] * src[13];
|
|
tmp[11] = src[9] * src[12];
|
|
|
|
// Calculate first 8 elements (cofactors)
|
|
dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
|
|
dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
|
|
dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
|
|
dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
|
|
dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
|
|
dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
|
|
dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
|
|
dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
|
|
dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
|
|
dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
|
|
dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
|
|
dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
|
|
dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
|
|
dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
|
|
dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
|
|
dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];
|
|
|
|
// Calculate pairs for second 8 elements (cofactors)
|
|
tmp[0] = src[2] * src[7];
|
|
tmp[1] = src[3] * src[6];
|
|
tmp[2] = src[1] * src[7];
|
|
tmp[3] = src[3] * src[5];
|
|
tmp[4] = src[1] * src[6];
|
|
tmp[5] = src[2] * src[5];
|
|
tmp[6] = src[0] * src[7];
|
|
tmp[7] = src[3] * src[4];
|
|
tmp[8] = src[0] * src[6];
|
|
tmp[9] = src[2] * src[4];
|
|
tmp[10] = src[0] * src[5];
|
|
tmp[11] = src[1] * src[4];
|
|
|
|
// Calculate second 8 elements (cofactors)
|
|
dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
|
|
dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
|
|
dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
|
|
dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
|
|
dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
|
|
dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15];
|
|
dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
|
|
dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
|
|
dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
|
|
dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
|
|
dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
|
|
dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
|
|
dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
|
|
dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
|
|
dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
|
|
dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];
|
|
|
|
// Calculate determinant
|
|
double det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];
|
|
|
|
if (fabs(det) < FLT_EPSILON)
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
// Calculate matrix inverse
|
|
det = 1.0 / det;
|
|
for (int i = 0; i < 16; i++)
|
|
m[i] = dst[i] * det;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
};
|