242 lines
9.8 KiB
C++
242 lines
9.8 KiB
C++
#include "CMatrix4f.h"
|
|
#include "CVector3f.h"
|
|
#include "CVector4f.h"
|
|
#include "CTransform4f.h"
|
|
|
|
CMatrix4f::CMatrix4f()
|
|
{
|
|
}
|
|
|
|
CMatrix4f::CMatrix4f(float v)
|
|
{
|
|
*this = skZero;
|
|
m[0][0] = v;
|
|
m[1][1] = v;
|
|
m[2][2] = v;
|
|
m[3][3] = v;
|
|
}
|
|
|
|
CMatrix4f::CMatrix4f(float m00, float m01, float m02, float m03,
|
|
float m10, float m11, float m12, float m13,
|
|
float m20, float m21, float m22, float m23,
|
|
float m30, float m31, float m32, float m33)
|
|
{
|
|
m[0][0] = m00;
|
|
m[0][1] = m01;
|
|
m[0][2] = m02;
|
|
m[0][3] = m03;
|
|
m[1][0] = m10;
|
|
m[1][1] = m11;
|
|
m[1][2] = m12;
|
|
m[1][3] = m13;
|
|
m[2][0] = m20;
|
|
m[2][1] = m21;
|
|
m[2][2] = m22;
|
|
m[2][3] = m23;
|
|
m[3][0] = m30;
|
|
m[3][1] = m31;
|
|
m[3][2] = m32;
|
|
m[3][3] = m33;
|
|
}
|
|
|
|
// ************ MATH ************
|
|
CMatrix4f CMatrix4f::Transpose() const
|
|
{
|
|
return CMatrix4f(m[0][0], m[1][0], m[2][0], m[3][0],
|
|
m[0][1], m[1][1], m[2][1], m[3][1],
|
|
m[0][2], m[1][2], m[2][2], m[3][2],
|
|
m[0][3], m[1][3], m[2][3], m[3][3]);
|
|
}
|
|
|
|
CMatrix4f CMatrix4f::Inverse() const
|
|
{
|
|
// Copied from Ogre.
|
|
// todo after developing a better understanding of the math - rewrite
|
|
float m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
|
|
float m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
|
|
float m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
|
|
float m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];
|
|
|
|
float v0 = m20 * m31 - m21 * m30;
|
|
float v1 = m20 * m32 - m22 * m30;
|
|
float v2 = m20 * m33 - m23 * m30;
|
|
float v3 = m21 * m32 - m22 * m31;
|
|
float v4 = m21 * m33 - m23 * m31;
|
|
float v5 = m22 * m33 - m23 * m32;
|
|
|
|
float t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
|
|
float t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
|
|
float t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
|
|
float t30 = - (v3 * m10 - v1 * m11 + v0 * m12);
|
|
|
|
float invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);
|
|
|
|
float d00 = t00 * invDet;
|
|
float d10 = t10 * invDet;
|
|
float d20 = t20 * invDet;
|
|
float d30 = t30 * invDet;
|
|
|
|
float d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
|
|
float d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
|
|
float d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
|
|
float d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
|
|
|
|
v0 = m10 * m31 - m11 * m30;
|
|
v1 = m10 * m32 - m12 * m30;
|
|
v2 = m10 * m33 - m13 * m30;
|
|
v3 = m11 * m32 - m12 * m31;
|
|
v4 = m11 * m33 - m13 * m31;
|
|
v5 = m12 * m33 - m13 * m32;
|
|
|
|
float d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
|
|
float d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
|
|
float d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
|
|
float d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
|
|
|
|
v0 = m21 * m10 - m20 * m11;
|
|
v1 = m22 * m10 - m20 * m12;
|
|
v2 = m23 * m10 - m20 * m13;
|
|
v3 = m22 * m11 - m21 * m12;
|
|
v4 = m23 * m11 - m21 * m13;
|
|
v5 = m23 * m12 - m22 * m13;
|
|
|
|
float d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
|
|
float d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
|
|
float d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
|
|
float d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
|
|
|
|
return CMatrix4f(
|
|
d00, d01, d02, d03,
|
|
d10, d11, d12, d13,
|
|
d20, d21, d22, d23,
|
|
d30, d31, d32, d33);
|
|
}
|
|
|
|
float CMatrix4f::Determinant() const
|
|
{
|
|
float AA = m[1][1] * ((m[2][2] * m[3][3]) - (m[2][3] * m[3][2]));
|
|
float AB = m[1][2] * ((m[2][1] * m[3][3]) - (m[2][3] * m[3][1]));
|
|
float AC = m[1][3] * ((m[2][1] * m[3][2]) - (m[2][2] * m[3][1]));
|
|
float A = m[0][0] * (AA - AB + AC);
|
|
|
|
float BA = m[1][0] * ((m[2][2] * m[3][3]) - (m[2][3] * m[3][2]));
|
|
float BB = m[1][2] * ((m[2][0] * m[3][3]) - (m[2][3] * m[3][0]));
|
|
float BC = m[1][3] * ((m[2][0] * m[3][2]) - (m[2][2] * m[3][0]));
|
|
float B = m[0][1] * (BA - BB + BC);
|
|
|
|
float CA = m[1][0] * ((m[2][1] * m[3][3]) - (m[2][3] * m[3][1]));
|
|
float CB = m[1][1] * ((m[2][0] * m[3][3]) - (m[2][3] * m[3][0]));
|
|
float CC = m[1][3] * ((m[2][0] * m[3][1]) - (m[2][1] * m[3][0]));
|
|
float C = m[0][2] * (CA - CB + CC);
|
|
|
|
float DA = m[1][0] * ((m[2][1] * m[3][2]) - (m[2][2] * m[3][1]));
|
|
float DB = m[1][1] * ((m[2][0] * m[3][2]) - (m[2][2] * m[3][0]));
|
|
float DC = m[1][2] * ((m[2][0] * m[3][1]) - (m[2][1] * m[3][0]));
|
|
float D = m[0][3] * (DA - DB + DC);
|
|
|
|
return (A - B + C - D);
|
|
}
|
|
|
|
glm::mat4 CMatrix4f::ToGlmMat4() const
|
|
{
|
|
glm::mat4 out = glm::mat4(1);
|
|
memcpy(&out[0][0], &m[0][0], sizeof(glm::mat4));
|
|
return out;
|
|
}
|
|
|
|
// ************ STATIC ************
|
|
CMatrix4f CMatrix4f::FromGlmMat4(glm::mat4 src)
|
|
{
|
|
CMatrix4f out;
|
|
memcpy(&out[0][0], &src[0][0], sizeof(CMatrix4f));
|
|
return out;
|
|
}
|
|
|
|
// ************ OPERATORS ************
|
|
inline float* CMatrix4f::operator[](long index)
|
|
{
|
|
return m[index];
|
|
}
|
|
|
|
inline const float* CMatrix4f::operator[](long index) const
|
|
{
|
|
return m[index];
|
|
}
|
|
|
|
CVector3f CMatrix4f::operator*(const CVector3f& vec) const
|
|
{
|
|
// For vec3 multiplication, the vector w component is considered to be 1.0
|
|
CVector3f out;
|
|
float w = (m[3][0] * vec.x) + (m[3][1] * vec.y) + (m[3][2] * vec.z) + m[3][3];
|
|
out.x = ((m[0][0] * vec.x) + (m[0][1] * vec.y) + (m[0][2] * vec.z) + m[0][3]) / w;
|
|
out.y = ((m[1][0] * vec.x) + (m[1][1] * vec.y) + (m[1][2] * vec.z) + m[1][3]) / w;
|
|
out.z = ((m[2][0] * vec.x) + (m[2][1] * vec.y) + (m[2][2] * vec.z) + m[1][3]) / w;
|
|
return out;
|
|
}
|
|
|
|
CVector4f CMatrix4f::operator*(const CVector4f& vec) const
|
|
{
|
|
CVector4f out;
|
|
out.x = (m[0][0] * vec.x) + (m[0][1] * vec.y) + (m[0][2] * vec.z) + (m[0][3] * vec.w);
|
|
out.y = (m[1][0] * vec.x) + (m[1][1] * vec.y) + (m[1][2] * vec.z) + (m[1][3] * vec.w);
|
|
out.z = (m[2][0] * vec.x) + (m[2][1] * vec.y) + (m[2][2] * vec.z) + (m[2][3] * vec.w);
|
|
out.w = (m[3][0] * vec.x) + (m[3][1] * vec.y) + (m[3][2] * vec.z) + (m[3][3] * vec.w);
|
|
return out;
|
|
}
|
|
|
|
CMatrix4f CMatrix4f::operator*(const CTransform4f& mtx) const
|
|
{
|
|
// CTransform4f is a 3x4 matrix with an implicit fourth row of {0, 0, 0, 1}
|
|
CMatrix4f out;
|
|
out[0][0] = (m[0][0] * mtx[0][0]) + (m[0][1] * mtx[1][0]) + (m[0][2] * mtx[2][0]);
|
|
out[0][1] = (m[0][0] * mtx[0][1]) + (m[0][1] * mtx[1][1]) + (m[0][2] * mtx[2][1]);
|
|
out[0][2] = (m[0][0] * mtx[0][2]) + (m[0][1] * mtx[1][2]) + (m[0][2] * mtx[2][2]);
|
|
out[0][3] = (m[0][0] * mtx[0][3]) + (m[0][1] * mtx[1][3]) + (m[0][2] * mtx[2][3]) + m[0][3];
|
|
out[1][0] = (m[1][0] * mtx[0][0]) + (m[1][1] * mtx[1][0]) + (m[1][2] * mtx[2][0]);
|
|
out[1][1] = (m[1][0] * mtx[0][1]) + (m[1][1] * mtx[1][1]) + (m[1][2] * mtx[2][1]);
|
|
out[1][2] = (m[1][0] * mtx[0][2]) + (m[1][1] * mtx[1][2]) + (m[1][2] * mtx[2][2]);
|
|
out[1][3] = (m[1][0] * mtx[0][3]) + (m[1][1] * mtx[1][3]) + (m[1][2] * mtx[2][3]) + m[1][3];
|
|
out[2][0] = (m[2][0] * mtx[0][0]) + (m[2][1] * mtx[1][0]) + (m[2][2] * mtx[2][0]);
|
|
out[2][1] = (m[2][0] * mtx[0][1]) + (m[2][1] * mtx[1][1]) + (m[2][2] * mtx[2][1]);
|
|
out[2][2] = (m[2][0] * mtx[0][2]) + (m[2][1] * mtx[1][2]) + (m[2][2] * mtx[2][2]);
|
|
out[2][3] = (m[2][0] * mtx[0][3]) + (m[2][1] * mtx[1][3]) + (m[2][2] * mtx[2][3]) + m[2][3];
|
|
out[3][0] = (m[3][0] * mtx[0][0]) + (m[3][1] * mtx[1][0]) + (m[3][2] * mtx[2][0]);
|
|
out[3][1] = (m[3][0] * mtx[0][1]) + (m[3][1] * mtx[1][1]) + (m[3][2] * mtx[2][1]);
|
|
out[3][2] = (m[3][0] * mtx[0][2]) + (m[3][1] * mtx[1][2]) + (m[3][2] * mtx[2][2]);
|
|
out[3][3] = (m[3][0] * mtx[0][3]) + (m[3][1] * mtx[1][3]) + (m[3][2] * mtx[2][3]) + m[3][3];
|
|
return out;
|
|
}
|
|
|
|
CMatrix4f CMatrix4f::operator*(const CMatrix4f& mtx) const
|
|
{
|
|
CMatrix4f out;
|
|
out[0][0] = (m[0][0] * mtx[0][0]) + (m[0][1] * mtx[1][0]) + (m[0][2] * mtx[2][0]) + (m[0][3] * mtx[3][0]);
|
|
out[0][1] = (m[0][0] * mtx[0][1]) + (m[0][1] * mtx[1][1]) + (m[0][2] * mtx[2][1]) + (m[0][3] * mtx[3][1]);
|
|
out[0][2] = (m[0][0] * mtx[0][2]) + (m[0][1] * mtx[1][2]) + (m[0][2] * mtx[2][2]) + (m[0][3] * mtx[3][2]);
|
|
out[0][3] = (m[0][0] * mtx[0][3]) + (m[0][1] * mtx[1][3]) + (m[0][2] * mtx[2][3]) + (m[0][3] * mtx[3][3]);
|
|
out[1][0] = (m[1][0] * mtx[0][0]) + (m[1][1] * mtx[1][0]) + (m[1][2] * mtx[2][0]) + (m[1][3] * mtx[3][0]);
|
|
out[1][1] = (m[1][0] * mtx[0][1]) + (m[1][1] * mtx[1][1]) + (m[1][2] * mtx[2][1]) + (m[1][3] * mtx[3][1]);
|
|
out[1][2] = (m[1][0] * mtx[0][2]) + (m[1][1] * mtx[1][2]) + (m[1][2] * mtx[2][2]) + (m[1][3] * mtx[3][2]);
|
|
out[1][3] = (m[1][0] * mtx[0][3]) + (m[1][1] * mtx[1][3]) + (m[1][2] * mtx[2][3]) + (m[1][3] * mtx[3][3]);
|
|
out[2][0] = (m[2][0] * mtx[0][0]) + (m[2][1] * mtx[1][0]) + (m[2][2] * mtx[2][0]) + (m[2][3] * mtx[3][0]);
|
|
out[2][1] = (m[2][0] * mtx[0][1]) + (m[2][1] * mtx[1][1]) + (m[2][2] * mtx[2][1]) + (m[2][3] * mtx[3][1]);
|
|
out[2][2] = (m[2][0] * mtx[0][2]) + (m[2][1] * mtx[1][2]) + (m[2][2] * mtx[2][2]) + (m[2][3] * mtx[3][2]);
|
|
out[2][3] = (m[2][0] * mtx[0][3]) + (m[2][1] * mtx[1][3]) + (m[2][2] * mtx[2][3]) + (m[2][3] * mtx[3][3]);
|
|
out[3][0] = (m[3][0] * mtx[0][0]) + (m[3][1] * mtx[1][0]) + (m[3][2] * mtx[2][0]) + (m[3][3] * mtx[3][0]);
|
|
out[3][1] = (m[3][0] * mtx[0][1]) + (m[3][1] * mtx[1][1]) + (m[3][2] * mtx[2][1]) + (m[3][3] * mtx[3][1]);
|
|
out[3][2] = (m[3][0] * mtx[0][2]) + (m[3][1] * mtx[1][2]) + (m[3][2] * mtx[2][2]) + (m[3][3] * mtx[3][2]);
|
|
out[3][3] = (m[3][0] * mtx[0][3]) + (m[3][1] * mtx[1][3]) + (m[3][2] * mtx[2][3]) + (m[3][3] * mtx[3][3]);
|
|
return out;
|
|
}
|
|
|
|
// ************ CONSTANT ************
|
|
const CMatrix4f CMatrix4f::skZero(0.0f, 0.0f, 0.0f, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 0.0f);
|
|
|
|
const CMatrix4f CMatrix4f::skIdentity(1.0f, 0.0f, 0.0f, 0.0f,
|
|
0.0f, 1.0f, 0.0f, 0.0f,
|
|
0.0f, 0.0f, 1.0f, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|