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); }