320 lines
9.8 KiB
C++
320 lines
9.8 KiB
C++
/* -----------------------------------------------------------------------------
|
|
GSFramework
|
|
Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
|
|
----------------------------------------------------------------------------- */
|
|
|
|
|
|
#include <cmath>
|
|
#include "math/matrix3.h"
|
|
#include "math/matrix4.h"
|
|
#include "metafile/nml.h"
|
|
|
|
using namespace GS;
|
|
using namespace GS::Math;
|
|
|
|
Matrix3 Matrix3::static_identity;
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool Matrix3::Inverse(Matrix3 &i) const
|
|
{
|
|
// Covariants.
|
|
i.m[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
|
|
i.m[0][1] = m[0][2] * m[2][1] - m[0][1] * m[2][2];
|
|
i.m[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
|
|
i.m[1][0] = m[1][2] * m[2][0] - m[1][0] * m[2][2];
|
|
i.m[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
|
|
i.m[1][2] = m[0][2] * m[1][0] - m[0][0] * m[1][2];
|
|
i.m[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
|
|
i.m[2][1] = m[0][1] * m[2][0] - m[0][0] * m[2][1];
|
|
i.m[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
|
|
|
|
float k = m[0][0] * i.m[0][0] + m[0][1] * i.m[1][0] + m[0][2] * i.m[2][0];
|
|
if (!k)
|
|
return false;
|
|
|
|
k = 1.f / k;
|
|
i.m[0][0] *= k; i.m[0][1] *= k; i.m[0][2] *= k;
|
|
i.m[1][0] *= k; i.m[1][1] *= k; i.m[1][2] *= k;
|
|
i.m[2][0] *= k; i.m[2][1] *= k; i.m[2][2] *= k;
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix3 Matrix3::VectorMatrix(const Vector4 &v)
|
|
{ return Matrix3(v.x, 0, 0, v.y, 0, 0, v.z, 0, 0); }
|
|
Matrix3 Matrix3::CrossProductMatrix(const Vector4 &v)
|
|
{ return Matrix3(0, -v.z, v.y, v.z, 0, -v.x, -v.y, v.x, 0); }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix3 Matrix3::Normalized() const
|
|
{
|
|
Vector4 x(GetRow(0)), y(GetRow(1)), z(GetRow(2));
|
|
|
|
Matrix3 m;
|
|
m.SetRow(0, x.Normalized());
|
|
m.SetRow(1, y.Normalized());
|
|
m.SetRow(2, z.Normalized());
|
|
return m;
|
|
}
|
|
Matrix3 Matrix3::AsOrthonormalBase() const
|
|
{
|
|
Vector4 x(GetRow(0)), y(GetRow(1));
|
|
|
|
Matrix3 m;
|
|
x = x.Normalized();
|
|
m.SetRow(0, x);
|
|
Vector4 z(x.Cross(y).Normalized());
|
|
m.SetRow(2, z);
|
|
y = z.Cross(x).Normalized();
|
|
m.SetRow(1, y);
|
|
return m;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Vector4 Matrix3::AsEuler(rOrder rorder) const
|
|
{
|
|
Vector4 euler(0, 0, 0);
|
|
|
|
switch (rorder)
|
|
{
|
|
case rOrder_ZYX:
|
|
euler.y = ASin(-m[2][0]);
|
|
euler.z = atan2(m[1][0], m[0][0]);
|
|
euler.x = atan2(m[2][1], m[2][2]);
|
|
break;
|
|
|
|
case rOrder_XZY:
|
|
euler.z = ASin(-m[0][1]);
|
|
euler.x = atan2(m[2][1], m[1][1]);
|
|
euler.y = atan2(m[0][2], m[0][0]);
|
|
break;
|
|
|
|
case rOrder_XYZ:
|
|
euler.y = ASin(m[0][2]);
|
|
euler.x = atan2(-m[1][2], m[2][2]);
|
|
euler.z = atan2(-m[0][1], m[0][0]);
|
|
break;
|
|
|
|
case rOrder_YZX:
|
|
euler.z = ASin(m[1][0]);
|
|
euler.x = atan2(-m[1][2], m[1][1]);
|
|
euler.y = atan2(-m[2][0], m[0][0]);
|
|
break;
|
|
|
|
default:
|
|
case rOrder_YXZ: // Engine default.
|
|
euler.x = ASin(-m[1][2]);
|
|
euler.y = atan2(m[0][2], m[2][2]);
|
|
euler.z = atan2(m[1][0], m[1][1]);
|
|
break;
|
|
|
|
case rOrder_ZXY: // MAX default.
|
|
euler.x = ASin(m[2][1]);
|
|
euler.y = atan2(-m[2][0], m[2][2]);
|
|
euler.z = atan2(-m[0][1], m[1][1]);
|
|
break;
|
|
|
|
case rOrder_XY:
|
|
euler.y = ACos(m[0][0]);
|
|
euler.x = ACos(m[1][1]);
|
|
euler.z = 0;
|
|
break;
|
|
}
|
|
return euler;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix3 Matrix3::FromEuler(const Vector4 &euler, rOrder rorder)
|
|
{ return Matrix3::FromEuler(euler.x, euler.y, euler.z, rorder); }
|
|
Matrix3 Matrix3::FromEuler(float x, float y, float z, rOrder rorder)
|
|
{
|
|
float cx = Cos(x), cy = Cos(y), cz = Cos(z),
|
|
sx = Sin(x), sy = Sin(y), sz = Sin(z);
|
|
|
|
switch (rorder)
|
|
{
|
|
case rOrder_XZY:
|
|
return Matrix3 ( cy * cz, sx * sy + cx * cy * sz, -cx * sy + cy * sx * sz,
|
|
-sz, cx * cz, cz * sx,
|
|
cz * sy, -cy * sx + cx * sy * sz, cx * cy + sx * sy * sz );
|
|
|
|
case rOrder_ZYX:
|
|
return Matrix3 ( cy * cz, cy * sz, -sy,
|
|
cz * sx * sy - cx * sz, cx * cz + sx * sy * sz, cy * sx,
|
|
cx *cz * sy + sx * sz, -cz * sx + cx * sy * sz, cx * cy );
|
|
|
|
case rOrder_XYZ:
|
|
return Matrix3 ( cy * cz, cz * sx * sy + cx * sz, -cx * cz * sy + sx * sz,
|
|
-cy * sz, cx * cz - sx * sy * sz, cz * sx + cx * sy * sz,
|
|
sy, -cy * sx, cx * cy );
|
|
|
|
case rOrder_ZXY:
|
|
return Matrix3 ( cy * cz - sx * sy * sz, cz * sx * sy + cy * sz, -cx * sy,
|
|
-cx * sz, cx * cz, sx,
|
|
cz * sy + cy * sx * sz, -cy * cz * sx + sy * sz, cx * cy );
|
|
|
|
case rOrder_YZX:
|
|
return Matrix3 ( cy * cz, sz, -cz * sy,
|
|
sx * sy - cx * cy * sz, cx * cz, cy * sx + cx * sy * sz,
|
|
cx * sy + cy * sx * sz, -cz * sx, cx * cy - sx * sy * sz );
|
|
|
|
case rOrder_YXZ:
|
|
return Matrix3 ( cy * cz + sx * sy * sz, cx * sz, -cz * sy + cy * sx * sz,
|
|
cz * sx * sy - cy * sz, cx * cz, cy * cz * sx + sy * sz,
|
|
cx * sy, -sx, cx * cy );
|
|
|
|
case rOrder_XY:
|
|
return Matrix3 ( cy, sx * sy, -cx * sy,
|
|
0, cx, sx,
|
|
sy, -cy * sx, cx * cy );
|
|
}
|
|
return Matrix3::IdentityMatrix();
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix3 Matrix3::TranslationMatrix(const Vector4 &t)
|
|
{ return Matrix3(1, 0, 0, 0, 1, 0, t.x, t.y, 1); }
|
|
Matrix3 Matrix3::TranslationMatrix(const Vector2 &t)
|
|
{ return Matrix3(1, 0, 0, 0, 1, 0, t.x, t.y, 1); }
|
|
Matrix3 Matrix3::ScaleMatrix(const Vector4 &s)
|
|
{ return Matrix3(s.x, 0, 0, 0, s.y, 0, 0, 0, s.z); }
|
|
Matrix3 Matrix3::ScaleMatrix(const Vector2 &s)
|
|
{ return Matrix3(s.x, 0, 0, 0, s.y, 0, 0, 0, 1); }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix3 Matrix3::RotationMatrixXAxis(float a)
|
|
{ return Matrix3(1, 0, 0, 0, Cos(a), Sin(a), 0, -Sin(a), Cos(a)); }
|
|
Matrix3 Matrix3::RotationMatrixYAxis(float a)
|
|
{ return Matrix3(Cos(a), 0, -Sin(a), 0, 1, 0, Sin(a), 0, Cos(a)); }
|
|
Matrix3 Matrix3::RotationMatrixZAxis(float a)
|
|
{ return Matrix3(Cos(a), Sin(a), 0, -Sin(a), Cos(a), 0, 0, 0, 1); }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void Matrix3::SetRow(uint n, const Vector4 &row)
|
|
{ m[0][n] = row.x; m[1][n] = row.y; m[2][n] = row.z; }
|
|
void Matrix3::SetColumn(uint n, const Vector4 &col)
|
|
{ m[n][0] = col.x; m[n][1] = col.y; m[n][2] = col.z; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
Matrix3 Matrix3::FromOrthonormalBasis(const Vector4 &w, const Vector4 *v)
|
|
{
|
|
Matrix3 mtx;
|
|
|
|
float l = w.Len();
|
|
if (!l)
|
|
return Matrix3::IdentityMatrix();
|
|
|
|
Vector4 wn = w / l, u;
|
|
|
|
if (!v)
|
|
{
|
|
if (!EqualZero(wn.x) || !EqualZero(wn.z))
|
|
{
|
|
u.Set(wn.z, 0, -wn.x); // Cross with up = {0,1,0}.
|
|
u = u.Normalized();
|
|
}
|
|
else
|
|
u.Set(-1, 0, 0);
|
|
|
|
Vector4 c(wn.Cross(u));
|
|
mtx.SetRow(1, c);
|
|
}
|
|
else
|
|
{
|
|
Vector4 vn(v->Normalized());
|
|
mtx.SetRow(1, vn);
|
|
u = vn.Cross(wn);
|
|
}
|
|
|
|
mtx.SetRow(0, u);
|
|
mtx.SetRow(2, wn);
|
|
return mtx;
|
|
}
|
|
Matrix3 Matrix3::FromMatrix4(const Matrix4 &mtx)
|
|
{
|
|
return Matrix3(
|
|
mtx.m[0][0], mtx.m[1][0], mtx.m[2][0],
|
|
mtx.m[0][1], mtx.m[1][1], mtx.m[2][1],
|
|
mtx.m[0][2], mtx.m[1][2], mtx.m[2][2]
|
|
);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Matrix3::Apply(Vector4 *o, const Vector4 *v, uint n) const
|
|
//-----------------------------------------------------------------------------
|
|
{
|
|
for (uint c = 0; c < n; c++)
|
|
{
|
|
float x = v->x, y = v->y, z = v->z;
|
|
o->x = x * m[0][0] + y * m[0][1] + z * m[0][2];
|
|
o->y = x * m[1][0] + y * m[1][1] + z * m[1][2];
|
|
o->z = x * m[2][0] + y * m[2][1] + z * m[2][2];
|
|
o->w = 1;
|
|
o++; v++;
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void Matrix3::Set
|
|
(
|
|
float m00, float m10, float m20,
|
|
float m01, float m11, float m21,
|
|
float m02, float m12, float m22
|
|
)
|
|
{
|
|
m[0][0] = m00; m[1][0] = m10; m[2][0] = m20;
|
|
m[0][1] = m01; m[1][1] = m11; m[2][1] = m21;
|
|
m[0][2] = m02; m[1][2] = m12; m[2][2] = m22;
|
|
}
|
|
void Matrix3::Set(const Vector4 &u, const Vector4 &v, const Vector4 &w)
|
|
{
|
|
m[0][0] = u.x; m[1][0] = u.y; m[2][0] = u.z;
|
|
m[0][1] = v.x; m[1][1] = v.y; m[2][1] = v.z;
|
|
m[0][2] = w.x; m[1][2] = w.y; m[2][2] = w.z;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
NML::Tag *Matrix3::AsMetaTag(const char *id) const
|
|
{
|
|
NML::Tag *root = new NML::Tag(id ? id : "Mtx3");
|
|
root->AddChild(GetRow(0).AsMetaTag("R0"));
|
|
root->AddChild(GetRow(1).AsMetaTag("R1"));
|
|
root->AddChild(GetRow(2).AsMetaTag("R2"));
|
|
return root;
|
|
}
|
|
bool Matrix3::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);
|
|
++i;
|
|
|
|
t = i.ObjectPtr();
|
|
if (!t) return false;
|
|
R.FromMetaTag(*t); SetRow(1, R);
|
|
++i;
|
|
|
|
t = i.ObjectPtr();
|
|
if (!t) return false;
|
|
R.FromMetaTag(*t); SetRow(2, R);
|
|
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|