Files
Syntriax.Engine/Engine.Core/Primitives/Matrix4x4.cs

440 lines
18 KiB
C#

using System;
namespace Engine.Core;
// TODO Comments
/// <summary>
/// Represents a 4D left handed space matrix in a Column Major convention.
/// </summary>
/// <remarks>
/// Initializes a new instance of the <see cref="Matrix4x4"/> struct with the specified values.
/// </remarks>
[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<Matrix4x4>
{
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;
/// <summary>
/// Extracts the position (translation) from the <see cref="Matrix4x4"/>.
/// </summary>
public readonly Vector3D Position => new(M41, M42, M43);
/// <summary>
/// Extracts the scale from the <see cref="Matrix4x4"/>.
/// </summary>
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);
}
}
/// <summary>
/// Extracts the rotation from the <see cref="Matrix4x4"/>.
/// </summary>
public readonly Quaternion Rotation => Quaternion.FromRotationMatrix4x4(this).Normalized;
/// <summary>
/// Represents the identity <see cref="Matrix4x4"/>.
/// </summary>
public static Matrix4x4 Identity => new(
1f, 0f, 0f, 0f,
0f, 1f, 0f, 0f,
0f, 0f, 1f, 0f,
0f, 0f, 0f, 1f
);
/// <summary>
/// Represents the inverted version of this <see cref="Matrix4x4"/>.
/// </summary>
public Matrix4x4 Inverse => Invert(this);
/// <summary>
/// Represents the transposed version of this <see cref="Matrix4x4"/>.
/// </summary>
public Matrix4x4 Transposed => Transpose(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,
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 Vector4D operator *(Matrix4x4 m, Vector4D v) => new(
m.M11 * v.X + m.M12 * v.Y + m.M13 * v.Z + m.M14 * v.W,
m.M21 * v.X + m.M22 * v.Y + m.M23 * v.Z + m.M24 * v.W,
m.M31 * v.X + m.M32 * v.Y + m.M33 * v.Z + m.M34 * v.W,
m.M41 * v.X + m.M42 * v.Y + m.M43 * v.Z + m.M44 * v.W
);
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
);
/// <summary>
/// Calculates the determinant of the <see cref="Matrix4x4"/>.
/// </summary>
/// <param name="m">The <see cref="Matrix4x4"/>.</param>
/// <returns>The determinant of the <see cref="Matrix4x4"/>.</returns>
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;
/// <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
);
}
/// <summary>
/// Transposes the given <see cref="Matrix4x4"/>.
/// </summary>
/// <param name="m">The <see cref="Matrix4x4"/>.</param>
/// <returns>The transposed <see cref="Matrix4x4"/> of the given <see cref="Matrix4x4"/>.</returns>
public static Matrix4x4 Transpose(Matrix4x4 m) => new(
m.M11, m.M21, m.M31, m.M41,
m.M12, m.M22, m.M32, m.M42,
m.M13, m.M23, m.M33, m.M43,
m.M14, m.M24, m.M34, m.M44
);
public static Matrix4x4 CreateTranslation(Vector3D position) => new(
1f, 0f, 0f, position.X,
0f, 1f, 0f, position.Y,
0f, 0f, 1f, position.Z,
0f, 0f, 0f, 1f
);
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,
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(
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(
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(
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(
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 f = (target - position).Normalized;
Vector3D s = f.Cross(up).Normalized;
Vector3D u = s.Cross(f);
return new(
s.X, u.X, -f.X, 0f,
s.Y, u.Y, -f.Y, 0f,
s.Z, u.Z, -f.Z, 0f,
-s.Dot(position), -u.Dot(position), f.Dot(position), 1f
);
}
public static Matrix4x4 CreateOrthographicView(float width, float height, float nearPlane = -1f, float farPlane = 1f)
{
float invDepth = 1f / (farPlane - nearPlane);
return new(
2f / width, 0f, 0f, 0f,
0f, -2f / height, 0f, 0f,
0f, 0f, 1f * invDepth, 0f,
-1f, 1f, -nearPlane * invDepth, 1f
);
}
public static Matrix4x4 CreateOrthographicViewCentered(float width, float height, float nearPlane = -1f, float farPlane = 1f)
{
float invDepth = 1f / (farPlane - nearPlane);
return new(
2f / width, 0f, 0f, 0f,
0f, 2f / height, 0f, 0f,
0f, 0f, 1f * invDepth, 0f,
0f, 0f, -nearPlane * invDepth, 1f
);
}
public static Matrix4x4 CreatePerspectiveFieldOfView(float fovRadians, float aspectRatio, float nearPlane, float farPlane)
{
float yScale = 1f / Math.Tan(fovRadians / 2f);
float xScale = yScale / aspectRatio;
float zRange = farPlane - nearPlane;
return new(
xScale, 0f, 0f, 0f,
0f, yScale, 0f, 0f,
0f, 0f, -(farPlane + nearPlane) / zRange, -1f,
0f, 0f, -(2f * nearPlane * farPlane) / zRange, 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 hashCode = new();
hashCode.Add(M11); hashCode.Add(M12); hashCode.Add(M13); hashCode.Add(M14);
hashCode.Add(M21); hashCode.Add(M22); hashCode.Add(M23); hashCode.Add(M24);
hashCode.Add(M31); hashCode.Add(M32); hashCode.Add(M33); hashCode.Add(M34);
hashCode.Add(M41); hashCode.Add(M42); hashCode.Add(M43); hashCode.Add(M44);
return hashCode.ToHashCode();
}
public override string ToString() => $"Matrix4x4({M11}, {M12}, {M13}, {M14},{M21}, {M22}, {M23}, {M24},{M31}, {M32}, {M33}, {M34},{M41}, {M42}, {M43}, {M44})";
}
/// <summary>
/// Provides extension methods for <see cref="Matrix4x4"/> type.
/// </summary>
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.Transpose(Matrix4x4)" />
public static Matrix4x4 Transpose(this Matrix4x4 matrix) => Matrix4x4.Transpose(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, Vector3D 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);
/// <inheritdoc cref="Matrix4x4.CreateRotationY(float)" />
public static Matrix4x4 ApplyRotationY(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationY(radians);
/// <inheritdoc cref="Matrix4x4.CreateRotationZ(float)" />
public static Matrix4x4 ApplyRotationZ(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationZ(radians);
/// <inheritdoc cref="Matrix4x4.CreateRotation(Quater)" />
public static Matrix4x4 ApplyRotation(this Matrix4x4 matrix, Quaternion quaternion) => matrix * Matrix4x4.CreateRotation(quaternion);
/// <inheritdoc cref="Matrix4x4.CreateLookMatrix( Vector3D, Vector3D)" />
public static Matrix4x4 ApplyLookRotationTo(this Matrix4x4 matrix, Vector3D forward, Vector3D up) => matrix * Matrix4x4.CreateLookMatrix(forward, up);
/// <inheritdoc cref="Matrix4x4.CreateLookMatrix(Vector3D, Vector3D, Vector3D)" />
public static Matrix4x4 CreateLookMatrixTo(this Vector3D from, Vector3D to, Vector3D up) => Matrix4x4.CreateLookMatrix(from, to, up);
/// <inheritdoc cref="Matrix4x4.CreatePerspectiveFieldOfView(float, float, float, float)" />
public static Matrix4x4 ApplyPerspectiveFieldOfView(this Matrix4x4 matrix, float fieldOfViewInRadians, float aspectRatio, float nearPlane, float farPlane)
=> matrix * Matrix4x4.CreatePerspectiveFieldOfView(fieldOfViewInRadians, aspectRatio, nearPlane, farPlane);
/// <inheritdoc cref="Matrix4x4.CreateOrthographicView(float, float, float, float)" />
public static Matrix4x4 ApplyOrthographicView(this Matrix4x4 matrix, float width, float height, float nearPlane = -1f, float farPlane = 1f)
=> matrix * Matrix4x4.CreateOrthographicView(width, height, nearPlane, farPlane);
/// <inheritdoc cref="Matrix4x4.CreateOrthographicViewCentered(float, float, float, float)" />
public static Matrix4x4 ApplyOrthographicViewCentered(this Matrix4x4 matrix, float width, float height, float nearPlane = -1f, float farPlane = 1f)
=> matrix * Matrix4x4.CreateOrthographicViewCentered(width, height, nearPlane, farPlane);
/// <inheritdoc cref="Matrix4x4.ToRightHanded(Matrix4x4) />
public static Matrix4x4 ToRightHanded(this Matrix4x4 matrix) => Matrix4x4.ToRightHanded(matrix);
}