290 lines
12 KiB
C#
290 lines
12 KiB
C#
using System;
|
|
using System.Numerics;
|
|
|
|
namespace Engine.Core;
|
|
|
|
// TODO Comments
|
|
|
|
/// <summary>
|
|
/// Represents a 4D left handed space matrix.
|
|
/// </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
|
|
);
|
|
|
|
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
|
|
);
|
|
|
|
/// <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;
|
|
|
|
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})";
|
|
}
|
|
|
|
/// <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.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.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, Vector3 up) => matrix * Matrix4x4.CreateLookMatrix(forward, up);
|
|
|
|
/// <inheritdoc cref="Matrix4x4.CreateLookMatrix(Vector3D, Vector3D, Vector3D)" />
|
|
public static Matrix4x4 CreateLookMatrixTo(this Vector3D from, Vector3D to, Vector3 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.ToRightHanded(Matrix4x4) />
|
|
public static Matrix4x4 ToRightHanded(this Matrix4x4 matrix) => Matrix4x4.ToRightHanded(matrix);
|
|
}
|