feat: added inverse & float scale methods to 4x4 matrices

Engine.Core/Primitives/Matrix4x4.cs

@@ -62,6 +62,11 @@ public readonly struct Matrix4x4(
         0f, 0f, 0f, 1f
     );
 
+    /// <summary>
+    /// Represents the inverted version of this <see cref="Matrix4x4"/>.
+    /// </summary>
+    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;
 
+    /// <summary>
+    /// Inverts the given <see cref="Matrix4x4"/>.
+    /// </summary>
+    /// <param name="m">The <see cref="Matrix4x4"/>.</param>
+    /// <returns>The inverted <see cref="Matrix4x4"/> of the given <see cref="Matrix4x4"/>.</returns>
+    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
     /// <inheritdoc cref="Matrix4x4.Determinant(Matrix4x4)" />
     public static float Determinant(this Matrix4x4 matrix) => Matrix4x4.Determinant(matrix);
 
+    /// <inheritdoc cref="Matrix4x4.Invert(Matrix4x4)" />
+    public static Matrix4x4 Invert(this Matrix4x4 matrix) => Matrix4x4.Invert(matrix);
+
     /// <inheritdoc cref="Matrix4x4.CreateTranslation(Vector3D)" />
     public static Matrix4x4 ApplyTranslation(this Matrix4x4 matrix, Vector3D translation) => matrix * Matrix4x4.CreateTranslation(translation);
 
     /// <inheritdoc cref="Matrix4x4.CreateScale(Vector3D)" />
     public static Matrix4x4 ApplyScale(this Matrix4x4 matrix, Vector3 scale) => matrix * Matrix4x4.CreateScale(scale);
 
+    /// <inheritdoc cref="Matrix4x4.CreateScale(float)" />
+    public static Matrix4x4 ApplyScale(this Matrix4x4 matrix, float scale) => matrix * Matrix4x4.CreateScale(scale);
+
     /// <inheritdoc cref="Matrix4x4.CreateRotationZ(float)" />
     public static Matrix4x4 ApplyRotationX(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationX(radians);