/// <summary>
/// Create and assign matrix values with rows.
/// </summary>
public Tensor4(Euler4 x, Euler4 y, Euler4 z, Euler4 t, Euler4 u, Euler4 v)
{
ex = x;
ey = y;
ez = z;
et = t;
eu = u;
ev = v;
}
/// <summary>
/// Create a diagonal tensor matrix.
/// </summary>
public Tensor4(Euler4 scale)
{
ex = new Euler4(scale.x, 0, 0, 0, 0, 0);
ey = new Euler4(0, scale.y, 0, 0, 0, 0);
ez = new Euler4(0, 0, scale.z, 0, 0, 0);
et = new Euler4(0, 0, 0, scale.t, 0, 0);
eu = new Euler4(0, 0, 0, 0, scale.u, 0);
ev = new Euler4(0, 0, 0, 0, 0, scale.v);
}
/// <summary>
/// Create a diagonally uniform tensor matrix.
/// </summary>
public Tensor4(float scale)
{
ex = new Euler4(scale, 0, 0, 0, 0, 0);
ey = new Euler4(0, scale, 0, 0, 0, 0);
ez = new Euler4(0, 0, scale, 0, 0, 0);
et = new Euler4(0, 0, 0, scale, 0, 0);
eu = new Euler4(0, 0, 0, 0, scale, 0);
ev = new Euler4(0, 0, 0, 0, 0, scale);
}
/// <summary>
/// Create inertia from rotation matrix.
/// </summary>
public static Tensor4 Cross(Matrix4 t)
{
return(new Tensor4(
Euler4.Cross(t.ey, t.ez),
Euler4.Cross(t.ez, t.ex),
Euler4.Cross(t.ex, t.ey),
Euler4.Cross(t.ex, t.ew),
Euler4.Cross(t.ey, t.ew),
Euler4.Cross(t.ez, t.ew)
));
}
/// <summary>
/// Transform euler rotation by the tensor.
/// </summary>
static public Euler4 operator *(Tensor4 lhs, Euler4 rhs)
{
return(new Euler4()
{
x = Euler4.Dot(lhs.ex, rhs),
y = Euler4.Dot(lhs.ey, rhs),
z = Euler4.Dot(lhs.ez, rhs),
t = Euler4.Dot(lhs.et, rhs),
u = Euler4.Dot(lhs.eu, rhs),
v = Euler4.Dot(lhs.ev, rhs),
});
}
/// <summary>
/// Get Nth-row of the matrix
/// </summary>
public Euler4 this[int index]
{
get
{
switch (index)
{
case 0: return(ex);
case 1: return(ey);
case 2: return(ez);
case 3: return(et);
case 4: return(eu);
case 5: return(ev);
default: throw new IndexOutOfRangeException();
}
}
set
{
switch (index)
{
case 0: ex = value; break;
case 1: ey = value; break;
case 2: ez = value; break;
case 3: et = value; break;
case 4: eu = value; break;
case 5: ev = value; break;
default: throw new IndexOutOfRangeException();
}
}
}
/// <summary>
/// Convert matrix into euler degree rotation
/// </summary>
/// <remarks>
/// The method is not valid in 90 deg singularity (WIP).
/// The method won't check for orthogonality.
/// </remarks>
public Euler4 ToEuler()
{
// Singularity check.
//var d = Diagonal; var t = d.x * d.y * d.z * d.w; var t2 = Vector4.Dot(d, Vector4.one); t2 = t2 * t2;
//if (t > 0.99F) return ToEulerInSingularity(); // Because Utility.Atan2AvoidPi doesn't care about 180 deg singularities...
// if (t2 - 1 < 0.001F) return ToEulerInHalfSingularity(); // 90 deg is bad. Some cos() parameters got zero, and could results in chaos because of losing sign information
// Back to main topic..
// Based on Euler() sequence, the raw formula is...
// matrix{{ach, dh, -bch, -j}, {k(bg-adf)-acjl, cfk-djl, k(bdf+ag)+bcjl, -hl}, {-ln(bg-adf)+m(adg+bf)-acjkn, -cgm-cfln-djkn, -ln(bdf+ag)+m(af-bdg)+bcjkn, -hkn}, {lm(bg-adf)+n(adg+bf)+acjkm, cflm+djkm-cgn, lm(bdf+ag)+n(af-bdg)-bcjkm, hkm}}
// matrix{{ach, -dh, bch, -j}, {k(adf+bg)-acjl, cfk+djl, k(bdf-ag)-bcjl, -hl},
// { -ln(adf+bg)+m(adg-bf)-acjkn, cgm+djkn-cfln, -ln(bdf-ag)+m(bdg+af)-bcjkn, -hkn},
// { lm(adf+bg)+n(adg-bf)+acjkm, cflm-djkm+cgn, lm(bdf-ag)+n(bdg+af)+bcjkm, hkm}}
Euler4 result = new Euler4();
result.y = AngleTidy(Math.Atan2(this[0, 2], this[0, 0])); // ach, bch
result.v = AngleTidy(Math.Atan2(-this[2, 3], this[3, 3])); // hkm, -hkn
// (So lucky) We got the first and last matrix sequence. Now make the matrix simpler
// Simplify into : matrix{{ch, dh, 0, -j}, {-cjl-dfk, cfk-djl, gk, -hl}, {dg, -cg, f, 0}, {cjk-dfl, cfl+djk, gl, hk}}
// matrix{{ch, -dh, 0, -j}, {dfk-cjl, cfk+djl, -gk, -hl}, {dg, cg, f, 0}, {cjk+dfl, cfl-djk, -gl, hk}}
var m2 = Euler(5, -result.v) * this * Euler(1, -result.y);
result.z = AngleTidy(Math.Atan2(-m2[0, 1], m2[0, 0])); // ch, -dh
result.u = AngleTidy(Math.Atan2(-m2[1, 3], m2[3, 3])); // hk, -hl
// Idk if it's safe to get x and t from that matrix, but working > efficient
// Simplify into : matrix{{h, 0, 0, -j}, {0, f, g, 0}, {0, -g, f, 0}, {j, 0, 0, h}}
var m3 = Euler(4, -result.u) * m2 * Euler(2, -result.z);
result.x = AngleTidy(Math.Atan2(m3[2, 1], m3[1, 1])); // f, g
result.t = AngleTidy(Math.Atan2(-m3[0, 3], m3[0, 0])); // h, -j
return(result);
}
/// <summary>
/// Rotate this transform (in euler) in given space orientation.
/// </summary>
public void Rotate(Euler4 value, Space4 space)
{
Rotate(Matrix4.Euler(value), space);
}
/// <summary>
/// Convert degree euler to orthogonal matrix rotation.
/// </summary>
public static Matrix4 Euler(Euler4 rot)
{
return(Euler(rot.x, rot.y, rot.z, rot.t, rot.u, rot.v));
}
