public static NitroxMatrix4x4 TRS(NitroxVector3 localPos, NitroxQuaternion localRotation, NitroxVector3 localScale)
{
NitroxMatrix4x4 scaleMatrix = SetScale(localScale);
NitroxMatrix4x4 rotationMatrix = SetRotation(localRotation);
NitroxMatrix4x4 translationMatrix = SetTranslation(localPos);
NitroxMatrix4x4 result = translationMatrix * rotationMatrix * scaleMatrix;
return(result);
}
public static NitroxVector3 ExtractTranslation(ref NitroxMatrix4x4 matrix)
{
NitroxVector3 position;
position.X = matrix[0, 3];
position.Y = matrix[1, 3];
position.Z = matrix[2, 3];
return(position);
}
public static void DecomposeMatrix(ref NitroxMatrix4x4 matrix, out NitroxVector3 localPosition, out NitroxQuaternion localRotation, out NitroxVector3 localScale)
{
NitroxMatrix4x4 before = new NitroxMatrix4x4(matrix);
localScale = ExtractScale(ref matrix);
localRotation = ExtractRotation(ref matrix);
localPosition = ExtractTranslation(ref matrix);
matrix = before;
}
public NitroxMatrix4x4(NitroxMatrix4x4 matrix)
{
M = new float[4, 4];
for (int x = 0; x < 4; x++)
{
for (int y = 0; y < 4; y++)
{
M[x, y] = matrix[x, y];
}
}
}
public static NitroxQuaternion ExtractRotation(ref NitroxMatrix4x4 matrix)
{
NitroxQuaternion q = new NitroxQuaternion(0f, 0f, 0f, 1f);
float trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2];
if (trace > 0)
{
float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
q.W = 0.25f / s;
q.X = (matrix[2, 1] - matrix[1, 2]) * s;
q.Y = (matrix[0, 2] - matrix[2, 0]) * s;
q.Z = (matrix[1, 0] - matrix[0, 1]) * s;
}
else
{
if (matrix[0, 0] > matrix[1, 1] && matrix[0, 0] > matrix[2, 2])
{
float s = 2.0f * (float)Math.Sqrt(1.0f + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]);
q.W = (matrix[2, 1] - matrix[1, 2]) / s;
q.X = 0.25f * s;
q.Y = (matrix[0, 1] + matrix[1, 0]) / s;
q.Z = (matrix[0, 2] + matrix[2, 0]) / s;
}
else if (matrix[1, 1] > matrix[2, 2])
{
float s = 2.0f * (float)Math.Sqrt(1.0f + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]);
q.W = (matrix[0, 2] - matrix[2, 0]) / s;
q.X = (matrix[0, 1] + matrix[1, 0]) / s;
q.Y = 0.25f * s;
q.Z = (matrix[1, 2] + matrix[2, 1]) / s;
}
else
{
float s = 2.0f * (float)Math.Sqrt(1.0f + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]);
q.W = (matrix[1, 0] - matrix[0, 1]) / s;
q.W = (matrix[0, 2] + matrix[2, 0]) / s;
q.Y = (matrix[1, 2] + matrix[2, 1]) / s;
q.Z = 0.25f * s;
}
}
NitroxQuaternion.Normalize(q);
return(q);
}
public static NitroxVector3 ExtractScale(ref NitroxMatrix4x4 matrix)
{
NitroxVector3 scale;
scale.X = NitroxVector3.Length(new NitroxVector3(matrix[0, 0], matrix[0, 1], matrix[0, 2]));
scale.Y = NitroxVector3.Length(new NitroxVector3(matrix[1, 0], matrix[1, 1], matrix[1, 2]));
scale.Z = NitroxVector3.Length(new NitroxVector3(matrix[2, 0], matrix[2, 1], matrix[2, 2]));
matrix[0, 0] /= scale.X;
matrix[0, 1] /= scale.X;
matrix[0, 2] /= scale.X;
matrix[1, 0] /= scale.Y;
matrix[1, 1] /= scale.Y;
matrix[1, 2] /= scale.Y;
matrix[2, 0] /= scale.Z;
matrix[2, 1] /= scale.Z;
matrix[2, 2] /= scale.Z;
return(scale);
}
public static NitroxMatrix4x4 Invert(NitroxMatrix4x4 matrix)
{
NitroxMatrix4x4 result;
result.M = new float[4, 4];
float a = matrix[0, 0], b = matrix[0, 1], c = matrix[0, 2], d = matrix[0, 3];
float e = matrix[1, 0], f = matrix[1, 1], g = matrix[1, 2], h = matrix[1, 3];
float i = matrix[2, 0], j = matrix[2, 1], k = matrix[2, 2], l = matrix[2, 3];
float m = matrix[3, 0], n = matrix[3, 1], o = matrix[3, 2], p = matrix[3, 3];
float kp_lo = k * p - l * o;
float jp_ln = j * p - l * n;
float jo_kn = j * o - k * n;
float ip_lm = i * p - l * m;
float io_km = i * o - k * m;
float in_jm = i * n - j * m;
float a11 = +(f * kp_lo - g * jp_ln + h * jo_kn);
float a12 = -(e * kp_lo - g * ip_lm + h * io_km);
float a13 = +(e * jp_ln - f * ip_lm + h * in_jm);
float a14 = -(e * jo_kn - f * io_km + g * in_jm);
float det = a * a11 + b * a12 + c * a13 + d * a14;
float invDet = 1.0f / det;
result[0, 0] = a11 * invDet;
result[1, 0] = a12 * invDet;
result[2, 0] = a13 * invDet;
result[3, 0] = a14 * invDet;
result[0, 1] = -(b * kp_lo - c * jp_ln + d * jo_kn) * invDet;
result[1, 1] = +(a * kp_lo - c * ip_lm + d * io_km) * invDet;
result[2, 1] = -(a * jp_ln - b * ip_lm + d * in_jm) * invDet;
result[3, 1] = +(a * jo_kn - b * io_km + c * in_jm) * invDet;
float gp_ho = g * p - h * o;
float fp_hn = f * p - h * n;
float fo_gn = f * o - g * n;
float ep_hm = e * p - h * m;
float eo_gm = e * o - g * m;
float en_fm = e * n - f * m;
result[0, 2] = +(b * gp_ho - c * fp_hn + d * fo_gn) * invDet;
result[1, 2] = -(a * gp_ho - c * ep_hm + d * eo_gm) * invDet;
result[2, 2] = +(a * fp_hn - b * ep_hm + d * en_fm) * invDet;
result[3, 2] = -(a * fo_gn - b * eo_gm + c * en_fm) * invDet;
float gl_hk = g * l - h * k;
float fl_hj = f * l - h * j;
float fk_gj = f * k - g * j;
float el_hi = e * l - h * i;
float ek_gi = e * k - g * i;
float ej_fi = e * j - f * i;
result[0, 3] = -(b * gl_hk - c * fl_hj + d * fk_gj) * invDet;
result[1, 3] = +(a * gl_hk - c * el_hi + d * ek_gi) * invDet;
result[2, 3] = -(a * fl_hj - b * el_hi + d * ej_fi) * invDet;
result[3, 3] = +(a * fk_gj - b * ek_gi + c * ej_fi) * invDet;
return(result);
}
