diff --git a/Engine.Core/Primitives/Matrix4x4.cs b/Engine.Core/Primitives/Matrix4x4.cs
new file mode 100644
index 0000000..9ff3501
--- /dev/null
+++ b/Engine.Core/Primitives/Matrix4x4.cs
@@ -0,0 +1,289 @@
+using System;
+using System.Numerics;
+
+namespace Engine.Core;
+
+// TODO Comments
+
+///
+/// Represents a 4D left handed space matrix.
+///
+///
+/// Initializes a new instance of the struct with the specified values.
+///
+[System.Diagnostics.DebuggerDisplay("{ToString(),nq}")]
+public readonly struct Matrix4x4(
+ float m11, float m12, float m13, float m14,
+ float m21, float m22, float m23, float m24,
+ float m31, float m32, float m33, float m34,
+ float m41, float m42, float m43, float m44
+) : IEquatable
+{
+ public readonly float M11 = m11, M12 = m12, M13 = m13, M14 = m14;
+ public readonly float M21 = m21, M22 = m22, M23 = m23, M24 = m24;
+ public readonly float M31 = m31, M32 = m32, M33 = m33, M34 = m34;
+ public readonly float M41 = m41, M42 = m42, M43 = m43, M44 = m44;
+
+ ///
+ /// Extracts the position (translation) from the .
+ ///
+ public readonly Vector3D Position => new(M41, M42, M43);
+
+ ///
+ /// Extracts the scale from the .
+ ///
+ public readonly Vector3D Scale
+ {
+ get
+ {
+ float scaleX = new Vector3D(M11, M12, M13).Length();
+ float scaleY = new Vector3D(M21, M22, M23).Length();
+ float scaleZ = new Vector3D(M31, M32, M33).Length();
+
+ if (Determinant(this) < 0)
+ scaleX *= -1;
+
+ return new(scaleX, scaleY, scaleZ);
+ }
+ }
+
+ ///
+ /// Extracts the rotation from the .
+ ///
+ public readonly Quaternion Rotation => Quaternion.FromRotationMatrix4x4(this).Normalized;
+
+ ///
+ /// Represents the identity .
+ ///
+ public static Matrix4x4 Identity => new(
+ 1f, 0f, 0f, 0f,
+ 0f, 1f, 0f, 0f,
+ 0f, 0f, 1f, 0f,
+ 0f, 0f, 0f, 1f
+ );
+
+ 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,
+ a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33 + a.M14 * b.M43,
+ a.M11 * b.M14 + a.M12 * b.M24 + a.M13 * b.M34 + a.M14 * b.M44,
+
+ a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31 + a.M24 * b.M41,
+ a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32 + a.M24 * b.M42,
+ a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33 + a.M24 * b.M43,
+ a.M21 * b.M14 + a.M22 * b.M24 + a.M23 * b.M34 + a.M24 * b.M44,
+
+ a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31 + a.M34 * b.M41,
+ a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32 + a.M34 * b.M42,
+ a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33 + a.M34 * b.M43,
+ a.M31 * b.M14 + a.M32 * b.M24 + a.M33 * b.M34 + a.M34 * b.M44,
+
+ a.M41 * b.M11 + a.M42 * b.M21 + a.M43 * b.M31 + a.M44 * b.M41,
+ a.M41 * b.M12 + a.M42 * b.M22 + a.M43 * b.M32 + a.M44 * b.M42,
+ a.M41 * b.M13 + a.M42 * b.M23 + a.M43 * b.M33 + a.M44 * b.M43,
+ a.M41 * b.M14 + a.M42 * b.M24 + a.M43 * b.M34 + a.M44 * b.M44
+ );
+
+ public static bool operator ==(Matrix4x4 left, Matrix4x4 right) =>
+ left.M11 == right.M11 && left.M12 == right.M12 && left.M13 == right.M13 && left.M14 == right.M14 &&
+ left.M21 == right.M21 && left.M22 == right.M22 && left.M23 == right.M23 && left.M24 == right.M24 &&
+ left.M31 == right.M31 && left.M32 == right.M32 && left.M33 == right.M33 && left.M34 == right.M34 &&
+ left.M41 == right.M41 && left.M42 == right.M42 && left.M43 == right.M43 && left.M44 == right.M44;
+
+ public static bool operator !=(Matrix4x4 left, Matrix4x4 right) =>
+ left.M11 != right.M11 || left.M12 != right.M12 || left.M13 != right.M13 || left.M14 != right.M14 ||
+ left.M21 != right.M21 || left.M22 != right.M22 || left.M23 != right.M23 || left.M24 != right.M24 ||
+ left.M31 != right.M31 || left.M32 != right.M32 || left.M33 != right.M33 || left.M34 != right.M34 ||
+ left.M41 != right.M41 || left.M42 != right.M42 || left.M43 != right.M43 || left.M44 != right.M44;
+
+ public static implicit operator System.Numerics.Matrix4x4(Matrix4x4 m) => new(
+ m.M11, m.M12, m.M13, m.M14,
+ m.M21, m.M22, m.M23, m.M24,
+ m.M31, m.M32, m.M33, m.M34,
+ m.M41, m.M42, m.M43, m.M44
+ );
+
+ public static implicit operator Matrix4x4(System.Numerics.Matrix4x4 m) => new(
+ m.M11, m.M12, m.M13, m.M14,
+ m.M21, m.M22, m.M23, m.M24,
+ m.M31, m.M32, m.M33, m.M34,
+ m.M41, m.M42, m.M43, m.M44
+ );
+
+ ///
+ /// Calculates the determinant of the .
+ ///
+ /// The .
+ /// The determinant of the .
+ public static float Determinant(Matrix4x4 m) => // https://www.euclideanspace.com/maths/algebra/matrix/functions/determinant/fourD/index.htm
+ m.M14 * m.M23 * m.M32 * m.M41 - m.M13 * m.M24 * m.M32 * m.M41 -
+ m.M14 * m.M22 * m.M33 * m.M41 + m.M12 * m.M24 * m.M33 * m.M41 +
+ m.M13 * m.M22 * m.M34 * m.M41 - m.M12 * m.M23 * m.M34 * m.M41 -
+ m.M14 * m.M23 * m.M31 * m.M42 + m.M13 * m.M24 * m.M31 * m.M42 +
+ m.M14 * m.M21 * m.M33 * m.M42 - m.M11 * m.M24 * m.M33 * m.M42 -
+ m.M13 * m.M21 * m.M34 * m.M42 + m.M11 * m.M23 * m.M34 * m.M42 +
+ m.M14 * m.M22 * m.M31 * m.M43 - m.M12 * m.M24 * m.M31 * m.M43 -
+ m.M14 * m.M21 * m.M32 * m.M43 + m.M11 * m.M24 * m.M32 * m.M43 +
+ m.M12 * m.M21 * m.M34 * m.M43 - m.M11 * m.M22 * m.M34 * m.M43 -
+ m.M13 * m.M22 * m.M31 * m.M44 + m.M12 * m.M23 * m.M31 * m.M44 +
+ 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;
+
+ public static Matrix4x4 CreateTranslation(Vector3D position) => new(
+ 1f, 0f, 0f, 0f,
+ 0f, 1f, 0f, 0f,
+ 0f, 0f, 1f, 0f,
+ position.X, position.Y, position.Z, 1
+ );
+
+ public static Matrix4x4 CreateScale(Vector3D scale) => new(
+ scale.X, 0f, 0f, 0f,
+ 0f, scale.Y, 0f, 0f,
+ 0f, 0f, scale.Z, 0f,
+ 0f, 0f, 0f, 1f
+ );
+
+ public static Matrix4x4 CreateRotationX(float radians)
+ {
+ float c = Math.Cos(radians);
+ float s = Math.Sin(radians);
+
+ return new Matrix4x4(
+ 1f, 0f, 0f, 0f,
+ 0f, c, s, 0f,
+ 0f, -s, c, 0f,
+ 0f, 0f, 0f, 1f
+ );
+ }
+
+ public static Matrix4x4 CreateRotationY(float radians)
+ {
+ float c = Math.Cos(radians);
+ float s = Math.Sin(radians);
+
+ return new Matrix4x4(
+ c, 0f, -s, 0f,
+ 0f, 1f, 0f, 0f,
+ s, 0f, c, 0f,
+ 0f, 0f, 0f, 1f
+ );
+ }
+
+ public static Matrix4x4 CreateRotationZ(float radians)
+ {
+ float c = Math.Cos(radians);
+ float s = Math.Sin(radians);
+
+ return new Matrix4x4(
+ c, s, 0f, 0f,
+ -s, c, 0f, 0f,
+ 0f, 0f, 1f, 0f,
+ 0f, 0f, 0f, 1f
+ );
+ }
+
+ // TODO Find a better calculation for this
+ public static Matrix4x4 CreateRotation(Quaternion quaternion)
+ {
+ Vector3D angles = quaternion.ToAngles();
+ return Identity * CreateRotationX(angles.X) * CreateRotationY(angles.Y) * CreateRotationZ(angles.Z);
+ }
+
+ public static Matrix4x4 CreateLookMatrix(Vector3D forward, Vector3D up)
+ {
+ Vector3D z = forward.Normalized;
+ Vector3D x = up.Cross(z).Normalized;
+ Vector3D y = z.Cross(x);
+
+ return new Matrix4x4(
+ x.X, y.X, z.X, 0f,
+ x.Y, y.Y, z.Y, 0f,
+ x.Z, y.Z, z.Z, 0f,
+ 0f, 0f, 0f, 1f
+ );
+ }
+
+ public static Matrix4x4 CreateLookMatrix(Vector3D position, Vector3D target, Vector3D up)
+ {
+ Vector3D z = position.FromTo(target).Normalized;
+ Vector3D x = up.Cross(z).Normalized;
+ Vector3D y = z.Cross(x);
+
+ return new Matrix4x4(
+ x.X, y.X, z.X, 0f,
+ x.Y, y.Y, z.Y, 0f,
+ x.Z, y.Z, z.Z, 0f,
+ -x.Dot(position), -y.Dot(position), -z.Dot(position), 1f
+ );
+ }
+
+ public static Matrix4x4 CreatePerspectiveFieldOfView(float fieldOfViewInRadians, float aspectRatio, float nearPlane, float farPlane)
+ {
+ float yScale = 1f / Math.Tan(fieldOfViewInRadians / 2f);
+ float xScale = yScale / aspectRatio;
+
+ return new Matrix4x4(
+ xScale, 0f, 0f, 0f,
+ 0f, yScale, 0f, 0f,
+ 0f, 0f, farPlane / (farPlane - nearPlane), 1f,
+ 0f, 0f, -nearPlane * farPlane / (farPlane - nearPlane), 0f
+ );
+ }
+
+ public static Matrix4x4 ToRightHanded(Matrix4x4 m) => new(
+ m.M11, m.M12, m.M13, m.M14,
+ m.M31, m.M32, m.M33, m.M34,
+ m.M21, m.M22, m.M23, m.M24,
+ m.M41, m.M42, m.M43, m.M44
+ );
+
+ public override bool Equals(object? obj) => obj is Matrix4x4 matrix && this == matrix;
+ public bool Equals(Matrix4x4 other) => this == other;
+
+ public override int GetHashCode() => HashCode.Combine(
+ HashCode.Combine(M11, M12, M13, M14, M21, M22, M23, M24),
+ HashCode.Combine(M31, M32, M33, M34, M41, M42, M43, M44)
+ );
+
+ public override string ToString() => $"Matrix4x4({M11}, {M12}, {M13}, {M14},{M21}, {M22}, {M23}, {M24},{M31}, {M32}, {M33}, {M34},{M41}, {M42}, {M43}, {M44})";
+}
+
+///
+/// Provides extension methods for type.
+///
+public static class Matrix4x4Extensions
+{
+ ///
+ public static float Determinant(this Matrix4x4 matrix) => Matrix4x4.Determinant(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 ApplyRotationX(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationX(radians);
+
+ ///
+ public static Matrix4x4 ApplyRotationY(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationY(radians);
+
+ ///
+ public static Matrix4x4 ApplyRotationZ(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationZ(radians);
+
+ ///
+ public static Matrix4x4 ApplyRotation(this Matrix4x4 matrix, Quaternion quaternion) => matrix * Matrix4x4.CreateRotation(quaternion);
+
+ ///
+ public static Matrix4x4 ApplyLookRotationTo(this Matrix4x4 matrix, Vector3D forward, Vector3 up) => matrix * Matrix4x4.CreateLookMatrix(forward, up);
+
+ ///
+ public static Matrix4x4 CreateLookMatrixTo(this Vector3D from, Vector3D to, Vector3 up) => Matrix4x4.CreateLookMatrix(from, to, up);
+
+ ///
+ public static Matrix4x4 ApplyPerspectiveFieldOfView(this Matrix4x4 matrix, float fieldOfViewInRadians, float aspectRatio, float nearPlane, float farPlane)
+ => matrix * Matrix4x4.CreatePerspectiveFieldOfView(fieldOfViewInRadians, aspectRatio, nearPlane, farPlane);
+
+ /// from given .
+ /// Calculates the from given .
///
/// The rotation .
- /// The rotation calculated by the given .
- public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(Quaternion quaternion)
+ /// The rotation calculated by the given .
+ public static Matrix4x4 ToRotationMatrix4x4(Quaternion quaternion)
{
float m00 = 1 - 2 * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
float m01 = 2 * (quaternion.X * quaternion.Y - quaternion.W * quaternion.Z);
@@ -332,11 +332,11 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab
}
///
- /// Calculates the from given .
+ /// Calculates the from given .
///
- /// The rotation .
- /// The rotation calculated by the given .
- public static Quaternion FromRotationMatrix4x4(System.Numerics.Matrix4x4 martix)
+ /// The rotation .
+ /// The rotation calculated by the given .
+ public static Quaternion FromRotationMatrix4x4(Matrix4x4 martix)
{
float trace = martix.M11 + martix.M22 + martix.M33;
float w, x, y, z;
@@ -459,7 +459,7 @@ public static class QuaternionExtensions
public static float Dot(this Quaternion left, Quaternion right) => Quaternion.Dot(left, right);
///
- public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(this Quaternion quaternion) => Quaternion.ToRotationMatrix4x4(quaternion);
+ public static Matrix4x4 ToRotationMatrix4x4(this Quaternion quaternion) => Quaternion.ToRotationMatrix4x4(quaternion);
///
public static Quaternion CreateRotation(this Vector3D axis, float angle) => Quaternion.FromAxisAngle(axis, angle);