struct matrix3x4_t
{
matrix3x4_t() {}
matrix3x4_t(
float m00, float m01, float m02, float m03,
float m10, float m11, float m12, float m13,
float m20, float m21, float m22, float m23)
{
m_flMatVal[0][0] = m00; m_flMatVal[0][1] = m01; m_flMatVal[0][2] = m02; m_flMatVal[0][3] = m03;
m_flMatVal[1][0] = m10; m_flMatVal[1][1] = m11; m_flMatVal[1][2] = m12; m_flMatVal[1][3] = m13;
m_flMatVal[2][0] = m20; m_flMatVal[2][1] = m21; m_flMatVal[2][2] = m22; m_flMatVal[2][3] = m23;
}
//------------------------------------------------ ------------------------------
// Creates a matrix where the X axis = forward
// the Y axis = left, and the Z axis = up
//------------------------------------------------ ------------------------------
void Init(const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector& vecOrigin)
{
m_flMatVal[0][0] = xAxis.x; m_flMatVal[0][1] = yAxis.x; m_flMatVal[0][2] = zAxis.x; m_flMatVal[0][3] = vecOrigin.x;
m_flMatVal[1][0] = xAxis.y; m_flMatVal[1][1] = yAxis.y; m_flMatVal[1][2] = zAxis.y; m_flMatVal[1][3] = vecOrigin.y;
m_flMatVal[2][0] = xAxis.z; m_flMatVal[2][1] = yAxis.z; m_flMatVal[2][2] = zAxis.z; m_flMatVal[2][3] = vecOrigin.z;
}
//------------------------------------------------ ------------------------------
// Creates a matrix where the X axis = forward
// the Y axis = left, and the Z axis = up
//------------------------------------------------ ------------------------------
matrix3x4_t(const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector& vecOrigin)
{
Init(xAxis, yAxis, zAxis, vecOrigin);
}
inline void SetOrigin(Vector const&p)
{
m_flMatVal[0][3] = px;
m_flMatVal[1][3] = py;
m_flMatVal[2][3] = pz;
}
inline Vector GetOrigin()
{
return Vector(m_flMatVal[0][3],
m_flMatVal[1][3],
m_flMatVal[2][3]);
}
inline void Invalidate(void)
{
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 4; j++)
{
m_flMatVal[i][j] = VEC_T_NAN;
}
}
}
inline bool IsValid()
{
if (this == nullptr)
return false;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 4; j++)
{
if (m_flMatVal[i][j] == VEC_T_NAN)
return false;
}
}
return true;
}
void Clear()
{
m_flMatVal[0][0] = 0.f; m_flMatVal[0][1] = 0.f; m_flMatVal[0][2] = 0.f; m_flMatVal[0][3] = 0.f;
m_flMatVal[1][0] = 0.f; m_flMatVal[1][1] = 0.f; m_flMatVal[1][2] = 0.f; m_flMatVal[1][3] = 0.f;
m_flMatVal[2][0] = 0.f; m_flMatVal[2][1] = 0.f; m_flMatVal[2][2] = 0.f; m_flMatVal[2][3] = 0.f;
}
void AngleMatrix(const Vector& angles)
{
float sr, sp, sy, cr, cp, cy;
DirectX::XMScalarSinCos(&sy, &cy, ToRadians(angles[1]));
DirectX::XMScalarSinCos(&sp, &cp, ToRadians(angles[0]));
DirectX::XMScalarSinCos(&sr, &cr, ToRadians(angles[2]));
m_flMatVal[0][0] = cp * cy;
m_flMatVal[1][0] = cp * sy;
m_flMatVal[2][0] = -sp;
float crcy = cr * cy;
float crsy = cr*sy;
float srcy = sr * cy;
float srsy = sr * sy;
m_flMatVal[0][1] = sp * srcy - crsy;
m_flMatVal[1][1] = sp * srsy + crcy;
m_flMatVal[2][1] = sr * cp;
m_flMatVal[0][2] = (sp * crcy + srsy);
m_flMatVal[1][2] = (sp * crsy - srcy);
m_flMatVal[2][2] = cr * cp;
m_flMatVal[0][3] = 0.0f;
m_flMatVal[1][3] = 0.0f;
m_flMatVal[2][3] = 0.0f;
}
void MatrixSetColumn(const Vector& in, int column)
{
m_flMatVal[0][column] = in.x;
m_flMatVal[1][column] = in.y;
m_flMatVal[2][column] = in.z;
}
void AngleMatrix(const Vector& angles, const Vector& position)
{
AngleMatrix(angles);
MatrixSetColumn(position, 3);
}
void QuaternionMatrix(const Quaternion& q)
{
m_flMatVal[0][0] = 1.0 - 2.0 * qy * qy - 2.0 * qz * qz;
m_flMatVal[1][0] = 2.0 * qx * qy + 2.0 * qw * qz;
m_flMatVal[2][0] = 2.0 * qx * qz - 2.0 * qw * qy;
m_flMatVal[0][1] = 2.0f * qx * qy - 2.0f * qw * qz;
m_flMatVal[1][1] = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz;
m_flMatVal[2][1] = 2.0f * qy * qz + 2.0f * qw * qx;
m_flMatVal[0][2] = 2.0f * qx * qz + 2.0f * qw * qy;
m_flMatVal[1][2] = 2.0f * qy * qz - 2.0f * qw * qx;
m_flMatVal[2][2] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy;
m_flMatVal[0][3] = 0.0f;
m_flMatVal[1][3] = 0.0f;
m_flMatVal[2][3] = 0.0f;
}
void QuaternionMatrix(const Quaternion& q, const Vector& pos)
{
QuaternionMatrix(q);
m_flMatVal[0][3] = pos.x;
m_flMatVal[1][3] = pos.y;
m_flMatVal[2][3] = pos.z;
}
matrix3x4_t matrix3x4_t::ConcatTransforms(matrix3x4_t in) const
{
matrix3x4_t out;
out[0][0] = m_flMatVal[0][0] * in[0][0] + m_flMatVal[0][1] * in[1][0] + m_flMatVal[0][2] * in[ 2][0];
out[0][1] = m_flMatVal[0][0] * in[0][1] + m_flMatVal[0][1] * in[1][1] + m_flMatVal[0][2] * in[ 2][1];
out[0][2] = m_flMatVal[0][0] * in[0][2] + m_flMatVal[0][1] * in[1][2] + m_flMatVal[0][2] * in[ 2][2];
out[0][3] = m_flMatVal[0][0] * in[0][3] + m_flMatVal[0][1] * in[1][3] + m_flMatVal[0][2] * in[ 2][3] + m_flMatVal[0][3];
out[1][0] = m_flMatVal[1][0] * in[0][0] + m_flMatVal[1][1] * in[1][0] + m_flMatVal[1][2] * in[ 2][0];
out[1][1] = m_flMatVal[1][0] * in[0][1] + m_flMatVal[1][1] * in[1][1] + m_flMatVal[1][2] * in[ 2][1];
out[1][2] = m_flMatVal[1][0] * in[0][2] + m_flMatVal[1][1] * in[1][2] + m_flMatVal[1][2] * in[ 2][2];
out[1][3] = m_flMatVal[1][0] * in[0][3] + m_flMatVal[1][1] * in[1][3] + m_flMatVal[1][2] * in[ 2][3] + m_flMatVal[1][3];
out[2][0] = m_flMatVal[2][0] * in[0][0] + m_flMatVal[2][1] * in[1][0] + m_flMatVal[2][2] * in[ 2][0];
out[2][1] = m_flMatVal[2][0] * in[0][1] + m_flMatVal[2][1] * in[1][1] + m_flMatVal[2][2] * in[ 2][1];
out[2][2] = m_flMatVal[2][0] * in[0][2] + m_flMatVal[2][1] * in[1][2] + m_flMatVal[2][2] * in[ 2][2];
out[2][3] = m_flMatVal[2][0] * in[0][3] + m_flMatVal[2][1] * in[1][3] + m_flMatVal[2][2] * in[ 2][3] + m_flMatVal[2][3];
return out;
}
float* operator[](int i) {
if ((i >= 0) && (i < 3) && m_flMatVal[i])
{
return m_flMatVal[i];
}
else return NULL;
}
const float* operator[](int i) const {
if ((i >= 0) && (i < 3) && m_flMatVal[i])
{
return m_flMatVal[i];
}
else return NULL;
}
vector matrix3x4_t::operator*(const Vector& vVec) const {
auto& m = m_flMatVal;
VectorvRet;
vRet.x = m[0][0] * vVec.x + m[0][1] * vVec.y + m[0][2] * vVec.z + m[0][3];
vRet.y = m[1][0] * vVec.x + m[1][1] * vVec.y + m[1][2] * vVec.z + m[1][3];
vRet.z = m[2][0] * vVec.x + m[2][1] * vVec.y + m[2][2] * vVec.z + m[2][3];
return vRet;
}
matrix3x4_t matrix3x4_t::operator+(const matrix3x4_t& other) const {
matrix3x4_t ret;
auto& m = m_flMatVal;
for (int i = 0; i < 12; i++) {
((float*)ret.m_flMatVal)[i] = ((float*)m)[i] + ((float*)other.m_flMatVal)[i];
}
return ret;
}
matrix3x4_t matrix3x4_t::operator-(const matrix3x4_t& other) const {
matrix3x4_t ret;
auto& m = m_flMatVal;
for (int i = 0; i < 12; i++) {
((float*)ret.m_flMatVal)[i] = ((float*)m)[i] - ((float*)other.m_flMatVal)[i];
}
return ret;
}
matrix3x4_t matrix3x4_t::operator*(const float& other) const {
matrix3x4_t ret;
auto& m = m_flMatVal;
for (int i = 0; i < 12; i++) {
((float*)ret.m_flMatVal)[i] = ((float*)m)[i] * other;
}
return ret;
}
matrix3x4_t operator*(const matrix3x4_t& vm) const {
return ConcatTransforms(vm);
}
float* Base() { return &m_flMatVal[0][0]; }
const float* Base() const { return &m_flMatVal[0][0]; }
float m_flMatVal[3][4];
};