278 lines
11 KiB
C++
278 lines
11 KiB
C++
/* -----------------------------------------------------------------------------
|
|
GSFramework
|
|
Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
|
|
------------------------------------------------------------------------------*/
|
|
|
|
|
|
#include "math/matrix4.h"
|
|
#include "math/matrix3.h"
|
|
#include "math/quaternion.h"
|
|
#include "metafile/nml.h"
|
|
|
|
using namespace GS;
|
|
|
|
Matrix4 Matrix4::static_identity(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix4 Matrix4::FromMatrix3(const Matrix3 &m)
|
|
{
|
|
return Matrix4(
|
|
m.m[0][0], m.m[1][0], m.m[2][0], 0,
|
|
m.m[0][1], m.m[1][1], m.m[2][1], 0,
|
|
m.m[0][2], m.m[1][2], m.m[2][2], 0,
|
|
0, 0, 0, 1
|
|
);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
const Matrix4 &Matrix4WithInverse::Get() const
|
|
{ return matrix; }
|
|
const Matrix4 &Matrix4WithInverse::GetInverse() const
|
|
{ return imatrix; }
|
|
void Matrix4WithInverse::Commit()
|
|
{ imatrix = matrix.InversedFast(); }
|
|
void Matrix4WithInverse::Set(const Matrix4 &m)
|
|
{
|
|
matrix = m;
|
|
Commit();
|
|
}
|
|
Vector4 Matrix4WithInverse::GetRow(uint n, bool w_1) const
|
|
{ return matrix.GetRow(n, w_1); }
|
|
Vector4 Matrix4WithInverse::GetColumn(uint n, bool w_1) const
|
|
{ return matrix.GetColumn(n, w_1); }
|
|
void Matrix4WithInverse::SetRow(uint n, const Vector4 &row, bool w_1)
|
|
{
|
|
matrix.SetRow(n, row, w_1);
|
|
Commit();
|
|
}
|
|
void Matrix4WithInverse::SetColumn(uint n, const Vector4 &col, bool w_1)
|
|
{
|
|
matrix.SetColumn(n, col, w_1);
|
|
Commit();
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
NML::Tag *Matrix4WithInverse::AsMetaTag(const char *id) const
|
|
{ return matrix.AsMetaTag(id); }
|
|
bool Matrix4WithInverse::FromMetaTag(NML::Tag &tag)
|
|
{
|
|
if (!matrix.FromMetaTag(tag))
|
|
return false;
|
|
Commit();
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix4 Matrix4::TransformationMatrix(const Vector4 &p, const Matrix3 &r, const Vector4 &s, const Vector4 *o)
|
|
{
|
|
Matrix4 m =
|
|
Matrix4::TranslationMatrix(p) *
|
|
Matrix4::FromMatrix3(r) *
|
|
Matrix4::ScaleMatrix(s);
|
|
return o ? m * Matrix4::TranslationMatrix(*o) : m;
|
|
}
|
|
Matrix4 Matrix4::TransformationMatrix(const Vector4 &p, const Vector4 &r, const Vector4 &s, const Vector4 *o)
|
|
{
|
|
Matrix4 m =
|
|
Matrix4::TranslationMatrix(p) *
|
|
Matrix4::FromMatrix3(Matrix3::FromEuler(r.x, r.y, r.z)) *
|
|
Matrix4::ScaleMatrix(s);
|
|
return o ? m * Matrix4::TranslationMatrix(*o) : m;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix4 Matrix4::LerpAsOrthonormalBase(const Matrix4 &a, const Matrix4 &b, float k, bool fast)
|
|
{
|
|
if (fast)
|
|
{
|
|
Matrix4 o;
|
|
for (int m = 0; m < 4; ++m)
|
|
for (int n = 0; n < 4; ++n)
|
|
o.m[m][n] = (b.m[m][n] - a.m[m][n]) * k + a.m[m][n];
|
|
return o;
|
|
}
|
|
|
|
Matrix3 a_matrix3, b_matrix3;
|
|
Vector4 a_position, b_position, a_scale, b_scale;
|
|
|
|
a.Decompose(&a_position, &a_scale, &a_matrix3);
|
|
b.Decompose(&b_position, &b_scale, &b_matrix3);
|
|
|
|
Quaternion a_orientation(Quaternion::FromMatrix3(a_matrix3));
|
|
Quaternion b_orientation(Quaternion::FromMatrix3(b_matrix3));
|
|
|
|
return Matrix4::TranslationMatrix((b_position - a_position) * k + a_position) *
|
|
Matrix4::FromMatrix3(Quaternion::Slerp(k, a_orientation, b_orientation).AsMatrix3()) *
|
|
Matrix4::ScaleMatrix((b_scale - a_scale) * k + a_scale);
|
|
}
|
|
void Matrix4::Decompose(Vector4 *position, Vector4 *scale, Vector4 *rotation, Math::rOrder order) const
|
|
{
|
|
Matrix3 m3;
|
|
Decompose(position, scale, &m3);
|
|
if (rotation)
|
|
*rotation = m3.AsEuler(order);
|
|
}
|
|
void Matrix4::Decompose(Vector4 *position, Vector4 *scale, Matrix3 *rotation) const
|
|
{
|
|
// Extract position.
|
|
if (position)
|
|
*position = GetRow(3);
|
|
|
|
// Extract scale.
|
|
Vector4 scl;
|
|
scl.Set(GetRow(0).Len(), GetRow(1).Len(), GetRow(2).Len());
|
|
|
|
// Handle negative scale (permute X to preserve left-handedness).
|
|
Vector4 left = GetRow(1).Cross(GetRow(2));
|
|
if (left.Dot(GetRow(0)) < 0)
|
|
scl.x = -scl.x;
|
|
if (scale)
|
|
*scale = scl;
|
|
|
|
// Rotation 3x3 (renormalized).
|
|
if (rotation)
|
|
{
|
|
if (scl.x)
|
|
{
|
|
scl.x = 1 / scl.x;
|
|
rotation->SetRow(0, Vector4(m[0][0] * scl.x, m[1][0] * scl.x, m[2][0] * scl.x));
|
|
}
|
|
else rotation->SetRow(0, Vector4(1, 0, 0));
|
|
|
|
if (scl.y)
|
|
{
|
|
scl.y = 1 / scl.y;
|
|
rotation->SetRow(1, Vector4(m[0][1] * scl.y, m[1][1] * scl.y, m[2][1] * scl.y));
|
|
}
|
|
else rotation->SetRow(1, Vector4(0, 1, 0));
|
|
|
|
if (scl.z)
|
|
{
|
|
scl.z = 1 / scl.z;
|
|
rotation->SetRow(2, Vector4(m[0][2] * scl.z, m[1][2] * scl.z, m[2][2] * scl.z));
|
|
}
|
|
else rotation->SetRow(2, Vector4(0, 0, 1));
|
|
}
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix4 Matrix4::InversedFast() const
|
|
{
|
|
// Extract inverse scale.
|
|
Vector4 scl(1.f / GetRow(0).Len(), 1.f / GetRow(1).Len(), 1.f / GetRow(2).Len());
|
|
|
|
// Inverse rotation 3x3 (renormalized).
|
|
Matrix3 irt (
|
|
m[0][0] * scl.x, m[0][1] * scl.y, m[0][2] * scl.z,
|
|
m[1][0] * scl.x, m[1][1] * scl.y, m[1][2] * scl.z,
|
|
m[2][0] * scl.x, m[2][1] * scl.y, m[2][2] * scl.z
|
|
);
|
|
|
|
// Recompose as inverse matrix.
|
|
return Matrix4::ScaleMatrix(scl) * (Matrix4::FromMatrix3(irt) * Matrix4::TranslationMatrix(GetRow(3).Reversed()));
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix4 Matrix4::AsOrthonormalBase() const
|
|
{
|
|
Matrix3 rcp (
|
|
m[0][0], m[1][0], m[2][0],
|
|
m[0][1], m[1][1], m[2][1],
|
|
m[0][2], m[1][2], m[2][2]
|
|
);
|
|
rcp = rcp.AsOrthonormalBase();
|
|
|
|
Matrix4 otb(*this);
|
|
for (int i = 0; i < 3; ++i)
|
|
for (int j = 0; j < 3; ++j)
|
|
otb.m[i][j] = rcp.m[i][j];
|
|
return otb;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix4 Matrix4::TranslationMatrix(const Vector4 &t)
|
|
{ return Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1); }
|
|
Matrix4 Matrix4::ScaleMatrix(const Vector4 &s)
|
|
{ return Matrix4(s.x, 0, 0, 0, 0, s.y, 0, 0, 0, 0, s.z, 0, 0, 0, 0, 1); }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
NML::Tag *Matrix4::AsMetaTag(const char *id) const
|
|
{
|
|
NML::Tag *root = new NML::Tag(id ? id : "Mtx4");
|
|
root->AddChild(GetRow(0, false).AsMetaTag("R0", true));
|
|
root->AddChild(GetRow(1, false).AsMetaTag("R1", true));
|
|
root->AddChild(GetRow(2, false).AsMetaTag("R2", true));
|
|
root->AddChild(GetRow(3, false).AsMetaTag("R3", true));
|
|
return root;
|
|
}
|
|
bool Matrix4::FromMetaTag(NML::Tag &tag)
|
|
{
|
|
NML::Tag *t;
|
|
Vector4 R;
|
|
|
|
List <NML::Tag *> ::Iterator i(tag.GetTags().GetRoot());
|
|
|
|
t = i.ObjectPtr();
|
|
if (!t) return false;
|
|
R.FromMetaTag(*t); SetRow(0, R, false);
|
|
++i;
|
|
|
|
t = i.ObjectPtr();
|
|
if (!t) return false;
|
|
R.FromMetaTag(*t); SetRow(1, R, false);
|
|
++i;
|
|
|
|
t = i.ObjectPtr();
|
|
if (!t) return false;
|
|
R.FromMetaTag(*t); SetRow(2, R, false);
|
|
++i;
|
|
|
|
t = i.ObjectPtr();
|
|
if (!t) return false;
|
|
R.FromMetaTag(*t); SetRow(3, R, false);
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
bool Matrix4::Inverse(Matrix4 &out) const
|
|
{
|
|
float inv[16], det;
|
|
|
|
inv[0] = m[1][1] * m[2][2] * m[3][3] - m[1][1] * m[2][3] * m[3][2] - m[2][1] * m[1][2] * m[3][3] + m[2][1] * m[1][3] * m[3][2] + m[3][1] * m[1][2] * m[2][3] - m[3][1] * m[1][3] * m[2][2];
|
|
inv[4] = -m[1][0] * m[2][2] * m[3][3] + m[1][0] * m[2][3] * m[3][2] + m[2][0] * m[1][2] * m[3][3] - m[2][0] * m[1][3] * m[3][2] - m[3][0] * m[1][2] * m[2][3] + m[3][0] * m[1][3] * m[2][2];
|
|
inv[8] = m[1][0] * m[2][1] * m[3][3] - m[1][0] * m[2][3] * m[3][1] - m[2][0] * m[1][1] * m[3][3] + m[2][0] * m[1][3] * m[3][1] + m[3][0] * m[1][1] * m[2][3] - m[3][0] * m[1][3] * m[2][1];
|
|
inv[12] = -m[1][0] * m[2][1] * m[3][2] + m[1][0] * m[2][2] * m[3][1] + m[2][0] * m[1][1] * m[3][2] - m[2][0] * m[1][2] * m[3][1] - m[3][0] * m[1][1] * m[2][2] + m[3][0] * m[1][2] * m[2][1];
|
|
inv[1] = -m[0][1] * m[2][2] * m[3][3] + m[0][1] * m[2][3] * m[3][2] + m[2][1] * m[0][2] * m[3][3] - m[2][1] * m[0][3] * m[3][2] - m[3][1] * m[0][2] * m[2][3] + m[3][1] * m[0][3] * m[2][2];
|
|
inv[5] = m[0][0] * m[2][2] * m[3][3] - m[0][0] * m[2][3] * m[3][2] - m[2][0] * m[0][2] * m[3][3] + m[2][0] * m[0][3] * m[3][2] + m[3][0] * m[0][2] * m[2][3] - m[3][0] * m[0][3] * m[2][2];
|
|
inv[9] = -m[0][0] * m[2][1] * m[3][3] + m[0][0] * m[2][3] * m[3][1] + m[2][0] * m[0][1] * m[3][3] - m[2][0] * m[0][3] * m[3][1] - m[3][0] * m[0][1] * m[2][3] + m[3][0] * m[0][3] * m[2][1];
|
|
inv[13] = m[0][0] * m[2][1] * m[3][2] - m[0][0] * m[2][2] * m[3][1] - m[2][0] * m[0][1] * m[3][2] + m[2][0] * m[0][2] * m[3][1] + m[3][0] * m[0][1] * m[2][2] - m[3][0] * m[0][2] * m[2][1];
|
|
inv[2] = m[0][1] * m[1][2] * m[3][3] - m[0][1] * m[1][3] * m[3][2] - m[1][1] * m[0][2] * m[3][3] + m[1][1] * m[0][3] * m[3][2] + m[3][1] * m[0][2] * m[1][3] - m[3][1] * m[0][3] * m[1][2];
|
|
inv[6] = -m[0][0] * m[1][2] * m[3][3] + m[0][0] * m[1][3] * m[3][2] + m[1][0] * m[0][2] * m[3][3] - m[1][0] * m[0][3] * m[3][2] - m[3][0] * m[0][2] * m[1][3] + m[3][0] * m[0][3] * m[1][2];
|
|
inv[10] = m[0][0] * m[1][1] * m[3][3] - m[0][0] * m[1][3] * m[3][1] - m[1][0] * m[0][1] * m[3][3] + m[1][0] * m[0][3] * m[3][1] + m[3][0] * m[0][1] * m[1][3] - m[3][0] * m[0][3] * m[1][1];
|
|
inv[14] = -m[0][0] * m[1][1] * m[3][2] + m[0][0] * m[1][2] * m[3][1] + m[1][0] * m[0][1] * m[3][2] - m[1][0] * m[0][2] * m[3][1] - m[3][0] * m[0][1] * m[1][2] + m[3][0] * m[0][2] * m[1][1];
|
|
inv[3] = -m[0][1] * m[1][2] * m[2][3] + m[0][1] * m[1][3] * m[2][2] + m[1][1] * m[0][2] * m[2][3] - m[1][1] * m[0][3] * m[2][2] - m[2][1] * m[0][2] * m[1][3] + m[2][1] * m[0][3] * m[1][2];
|
|
inv[7] = m[0][0] * m[1][2] * m[2][3] - m[0][0] * m[1][3] * m[2][2] - m[1][0] * m[0][2] * m[2][3] + m[1][0] * m[0][3] * m[2][2] + m[2][0] * m[0][2] * m[1][3] - m[2][0] * m[0][3] * m[1][2];
|
|
inv[11] = -m[0][0] * m[1][1] * m[2][3] + m[0][0] * m[1][3] * m[2][1] + m[1][0] * m[0][1] * m[2][3] - m[1][0] * m[0][3] * m[2][1] - m[2][0] * m[0][1] * m[1][3] + m[2][0] * m[0][3] * m[1][1];
|
|
inv[15] = m[0][0] * m[1][1] * m[2][2] - m[0][0] * m[1][2] * m[2][1] - m[1][0] * m[0][1] * m[2][2] + m[1][0] * m[0][2] * m[2][1] + m[2][0] * m[0][1] * m[1][2] - m[2][0] * m[0][2] * m[1][1];
|
|
det = m[0][0] * inv[0] + m[0][1] * inv[4] + m[0][2] * inv[8] + m[0][3] * inv[12];
|
|
|
|
if (det == 0)
|
|
return false;
|
|
|
|
det = 1.f / det;
|
|
|
|
for (int i = 0; i < 16; i++)
|
|
((float *)out.m)[i] = inv[i] * det;
|
|
|
|
return true;
|
|
}
|