diff --git a/Engine.Core/Primitives/Matrix4x4.cs b/Engine.Core/Primitives/Matrix4x4.cs index 0288b41..9bce992 100644 --- a/Engine.Core/Primitives/Matrix4x4.cs +++ b/Engine.Core/Primitives/Matrix4x4.cs @@ -62,6 +62,11 @@ public readonly struct Matrix4x4( 0f, 0f, 0f, 1f ); + /// + /// Represents the inverted version of this . + /// + public Matrix4x4 Inverse => Invert(this); + public static Matrix4x4 operator *(Matrix4x4 a, Matrix4x4 b) => new( a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31 + a.M14 * b.M41, a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32 + a.M14 * b.M42, @@ -129,6 +134,74 @@ public readonly struct Matrix4x4( m.M13 * m.M21 * m.M32 * m.M44 - m.M11 * m.M23 * m.M32 * m.M44 - m.M12 * m.M21 * m.M33 * m.M44 + m.M11 * m.M22 * m.M33 * m.M44; + /// + /// Inverts the given . + /// + /// The . + /// The inverted of the given . + public static Matrix4x4 Invert(Matrix4x4 m) + { + float m1 = m.M11, m2 = m.M12, m3 = m.M13, m4 = m.M14; + float m5 = m.M21, m6 = m.M22, m7 = m.M23, m8 = m.M24; + float m9 = m.M31, m10 = m.M32, m11 = m.M33, m12 = m.M34; + float m13 = m.M41, m14 = m.M42, m15 = m.M43, m16 = m.M44; + + float num = m11 * m16 - m12 * m15; + float num2 = m10 * m16 - m12 * m14; + float num3 = m10 * m15 - m11 * m14; + float num4 = m9 * m16 - m12 * m13; + float num5 = m9 * m15 - m11 * m13; + float num6 = m9 * m14 - m10 * m13; + + float num7 = m6 * num - m7 * num2 + m8 * num3; + float num8 = -(m5 * num - m7 * num4 + m8 * num5); + float num9 = m5 * num2 - m6 * num4 + m8 * num6; + float num10 = -(m5 * num3 - m6 * num5 + m7 * num6); + + float invDet = 1f / (m1 * num7 + m2 * num8 + m3 * num9 + m4 * num10); + + float r11 = num7 * invDet; + float r21 = num8 * invDet; + float r31 = num9 * invDet; + float r41 = num10 * invDet; + + float r12 = (-(m2 * num - m3 * num2 + m4 * num3)) * invDet; + float r22 = (m1 * num - m3 * num4 + m4 * num5) * invDet; + float r32 = (-(m1 * num2 - m2 * num4 + m4 * num6)) * invDet; + float r42 = (m1 * num3 - m2 * num5 + m3 * num6) * invDet; + + float num12 = m7 * m16 - m8 * m15; + float num13 = m6 * m16 - m8 * m14; + float num14 = m6 * m15 - m7 * m14; + float num15 = m5 * m16 - m8 * m13; + float num16 = m5 * m15 - m7 * m13; + float num17 = m5 * m14 - m6 * m13; + + float r13 = (m2 * num12 - m3 * num13 + m4 * num14) * invDet; + float r23 = (-(m1 * num12 - m3 * num15 + m4 * num16)) * invDet; + float r33 = (m1 * num13 - m2 * num15 + m4 * num17) * invDet; + float r43 = (-(m1 * num14 - m2 * num16 + m3 * num17)) * invDet; + + float num18 = m7 * m12 - m8 * m11; + float num19 = m6 * m12 - m8 * m10; + float num20 = m6 * m11 - m7 * m10; + float num21 = m5 * m12 - m8 * m9; + float num22 = m5 * m11 - m7 * m9; + float num23 = m5 * m10 - m6 * m9; + + float r14 = (-(m2 * num18 - m3 * num19 + m4 * num20)) * invDet; + float r24 = (m1 * num18 - m3 * num21 + m4 * num22) * invDet; + float r34 = (-(m1 * num19 - m2 * num21 + m4 * num23)) * invDet; + float r44 = (m1 * num20 - m2 * num22 + m3 * num23) * invDet; + + return new( + r11, r12, r13, r14, + r21, r22, r23, r24, + r31, r32, r33, r34, + r41, r42, r43, r44 + ); + } + public static Matrix4x4 CreateTranslation(Vector3D position) => new( 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, @@ -136,6 +209,13 @@ public readonly struct Matrix4x4( position.X, position.Y, position.Z, 1 ); + public static Matrix4x4 CreateScale(float scale) => new( + scale, 0f, 0f, 0f, + 0f, scale, 0f, 0f, + 0f, 0f, scale, 0f, + 0f, 0f, 0f, 1f + ); + public static Matrix4x4 CreateScale(Vector3D scale) => new( scale.X, 0f, 0f, 0f, 0f, scale.Y, 0f, 0f, @@ -261,12 +341,18 @@ public static class Matrix4x4Extensions /// public static float Determinant(this Matrix4x4 matrix) => Matrix4x4.Determinant(matrix); + /// + public static Matrix4x4 Invert(this Matrix4x4 matrix) => Matrix4x4.Invert(matrix); + /// public static Matrix4x4 ApplyTranslation(this Matrix4x4 matrix, Vector3D translation) => matrix * Matrix4x4.CreateTranslation(translation); /// public static Matrix4x4 ApplyScale(this Matrix4x4 matrix, Vector3 scale) => matrix * Matrix4x4.CreateScale(scale); + /// + public static Matrix4x4 ApplyScale(this Matrix4x4 matrix, float scale) => matrix * Matrix4x4.CreateScale(scale); + /// public static Matrix4x4 ApplyRotationX(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationX(radians);