namespace System.Numerics;
/// A 4×4 matrix of `f32`s, as .NET's: what moves, turns, scales and
/// projects `Vector3`s (row vectors: `Vector3.Transform(v, m)`; `a * b` is
/// `a`, then `b`), its translation in `M41`…`M43`.
public struct Matrix4x4(f32 m11, f32 m12, f32 m13, f32 m14, f32 m21, f32 m22, f32 m23, f32 m24, f32 m31, f32 m32, f32 m33, f32 m34, f32 m41, f32 m42, f32 m43, f32 m44) : IEquatable<Matrix4x4> {
public f32 M11 = m11;
public f32 M12 = m12;
public f32 M13 = m13;
public f32 M14 = m14;
public f32 M21 = m21;
public f32 M22 = m22;
public f32 M23 = m23;
public f32 M24 = m24;
public f32 M31 = m31;
public f32 M32 = m32;
public f32 M33 = m33;
public f32 M34 = m34;
public f32 M41 = m41;
public f32 M42 = m42;
public f32 M43 = m43;
public f32 M44 = m44;
/// What changes nothing.
public static readonly Matrix4x4 Identity = new Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
/// Where it moves the origin.
public readonly Vector3 Translation => new Vector3(M41, M42, M43);
public static Matrix4x4 CreateTranslation(Vector3 position) =>
new Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, position.X, position.Y, position.Z, 1);
public static Matrix4x4 CreateScale(Vector3 scales) =>
new Matrix4x4(scales.X, 0, 0, 0, 0, scales.Y, 0, 0, 0, 0, scales.Z, 0, 0, 0, 0, 1);
public static Matrix4x4 CreateScale(f32 scale) => CreateScale(new Vector3(scale, scale, scale));
/// Turning by `radians` about X, Y, Z.
public static Matrix4x4 CreateRotationX(f32 radians) {
let (c, s) = ((f32)Math.Cos(radians), (f32)Math.Sin(radians));
return new Matrix4x4(1, 0, 0, 0, 0, c, s, 0, 0, -s, c, 0, 0, 0, 0, 1);
}
public static Matrix4x4 CreateRotationY(f32 radians) {
let (c, s) = ((f32)Math.Cos(radians), (f32)Math.Sin(radians));
return new Matrix4x4(c, 0, -s, 0, 0, 1, 0, 0, s, 0, c, 0, 0, 0, 0, 1);
}
public static Matrix4x4 CreateRotationZ(f32 radians) {
let (c, s) = ((f32)Math.Cos(radians), (f32)Math.Sin(radians));
return new Matrix4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
}
/// Turning as `q` does.
public static Matrix4x4 CreateFromQuaternion(Quaternion q) {
let (xx, yy, zz) = (q.X * q.X, q.Y * q.Y, q.Z * q.Z);
let (xy, wz, xz) = (q.X * q.Y, q.Z * q.W, q.Z * q.X);
let (wy, yz, wx) = (q.Y * q.W, q.Y * q.Z, q.X * q.W);
return new Matrix4x4(
1 - 2 * (yy + zz), 2 * (xy + wz), 2 * (xz - wy), 0,
2 * (xy - wz), 1 - 2 * (zz + xx), 2 * (yz + wx), 0,
2 * (xz + wy), 2 * (yz - wx), 1 - 2 * (yy + xx), 0,
0, 0, 0, 1);
}
/// A camera at `cameraPosition` looking at `cameraTarget`, `cameraUpVector` up (right-handed).
public static Matrix4x4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) {
let z = Vector3.Normalize(cameraPosition - cameraTarget);
let x = Vector3.Normalize(Vector3.Cross(cameraUpVector, z));
let y = Vector3.Cross(z, x);
return new Matrix4x4(
x.X, y.X, z.X, 0,
x.Y, y.Y, z.Y, 0,
x.Z, y.Z, z.Z, 0,
-Vector3.Dot(x, cameraPosition), -Vector3.Dot(y, cameraPosition), -Vector3.Dot(z, cameraPosition), 1);
}
/// A perspective of `fieldOfView` radians up and down, `aspectRatio` wide, between the planes (right-handed).
public static Matrix4x4 CreatePerspectiveFieldOfView(f32 fieldOfView, f32 aspectRatio, f32 nearPlaneDistance, f32 farPlaneDistance) {
let y = 1 / (f32)Math.Tan(fieldOfView / 2);
let x = y / aspectRatio;
let range = farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
return new Matrix4x4(x, 0, 0, 0, 0, y, 0, 0, 0, 0, range, -1, 0, 0, range * nearPlaneDistance, 0);
}
public static Matrix4x4 Transpose(Matrix4x4 m) => new Matrix4x4(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 readonly f32 GetDeterminant() {
let (a, b, c, d) = (M11, M12, M13, M14);
let (e, f, g, h) = (M21, M22, M23, M24);
let (i, j, k, l) = (M31, M32, M33, M34);
let (m, n, o, p) = (M41, M42, M43, M44);
let (kp, jp, jo) = (k * p - l * o, j * p - l * n, j * o - k * n);
let (ip, io, iN) = (i * p - l * m, i * o - k * m, i * n - j * m);
return a * (f * kp - g * jp + h * jo) - b * (e * kp - g * ip + h * io)
+ c * (e * jp - f * ip + h * iN) - d * (e * jo - f * io + g * iN);
}
/// What undoes `m`; `null` when nothing does (its determinant is zero).
public static Matrix4x4? Invert(Matrix4x4 m) {
let det = m.GetDeterminant();
if (det == 0) {
return null;
}
let (a, b, c, d) = (m.M11, m.M12, m.M13, m.M14);
let (e, f, g, h) = (m.M21, m.M22, m.M23, m.M24);
let (i, j, k, l) = (m.M31, m.M32, m.M33, m.M34);
let (mm, n, o, p) = (m.M41, m.M42, m.M43, m.M44);
let (kp, jp, jo) = (k * p - l * o, j * p - l * n, j * o - k * n);
let (ip, io, iN) = (i * p - l * mm, i * o - k * mm, i * n - j * mm);
let (gp, fp, fo) = (g * p - h * o, f * p - h * n, f * o - g * n);
let (ep, eo, en) = (e * p - h * mm, e * o - g * mm, e * n - f * mm);
let (gl, fl, fk) = (g * l - h * k, f * l - h * j, f * k - g * j);
let (el, ek, ej) = (e * l - h * i, e * k - g * i, e * j - f * i);
let inv = 1 / det;
return new Matrix4x4(
(f * kp - g * jp + h * jo) * inv, -(b * kp - c * jp + d * jo) * inv, (b * gp - c * fp + d * fo) * inv, -(b * gl - c * fl + d * fk) * inv,
-(e * kp - g * ip + h * io) * inv, (a * kp - c * ip + d * io) * inv, -(a * gp - c * ep + d * eo) * inv, (a * gl - c * el + d * ek) * inv,
(e * jp - f * ip + h * iN) * inv, -(a * jp - b * ip + d * iN) * inv, (a * fp - b * ep + d * en) * inv, -(a * fl - b * el + d * ej) * inv,
-(e * jo - f * io + g * iN) * inv, (a * jo - b * io + c * iN) * inv, -(a * fo - b * eo + c * en) * inv, (a * fk - b * ek + c * ej) * inv);
}
public readonly bool Equals(Matrix4x4 other) => this == other;
/// `a`, then `b`.
public static Matrix4x4 operator *(Matrix4x4 a, Matrix4x4 b) => new Matrix4x4(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 Matrix4x4 operator *(Matrix4x4 m, f32 s) => new Matrix4x4(m.M11 * s, m.M12 * s, m.M13 * s, m.M14 * s, m.M21 * s, m.M22 * s, m.M23 * s, m.M24 * s, m.M31 * s, m.M32 * s, m.M33 * s, m.M34 * s, m.M41 * s, m.M42 * s, m.M43 * s, m.M44 * s);
public static Matrix4x4 operator +(Matrix4x4 a, Matrix4x4 b) => new Matrix4x4(a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13, a.M14 + b.M14, a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23, a.M24 + b.M24, a.M31 + b.M31, a.M32 + b.M32, a.M33 + b.M33, a.M34 + b.M34, a.M41 + b.M41, a.M42 + b.M42, a.M43 + b.M43, a.M44 + b.M44);
public static Matrix4x4 operator -(Matrix4x4 a, Matrix4x4 b) => new Matrix4x4(a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13, a.M14 - b.M14, a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23, a.M24 - b.M24, a.M31 - b.M31, a.M32 - b.M32, a.M33 - b.M33, a.M34 - b.M34, a.M41 - b.M41, a.M42 - b.M42, a.M43 - b.M43, a.M44 - b.M44);
public static bool operator ==(Matrix4x4 a, Matrix4x4 b) => a.M11 == b.M11 && a.M12 == b.M12 && a.M13 == b.M13 && a.M14 == b.M14 && a.M21 == b.M21 && a.M22 == b.M22 && a.M23 == b.M23 && a.M24 == b.M24 && a.M31 == b.M31 && a.M32 == b.M32 && a.M33 == b.M33 && a.M34 == b.M34 && a.M41 == b.M41 && a.M42 == b.M42 && a.M43 == b.M43 && a.M44 == b.M44;
public static bool operator !=(Matrix4x4 a, Matrix4x4 b) => !(a == b);
}