Files
CavesOfSwift/Sources/Maths/Matrix4x4.swift

544 lines
14 KiB
Swift

#if USE_SIMD
import simd
#endif
#if USE_SIMD
public typealias Matrix4x4 = simd_float4x4
public extension Matrix4x4
{
typealias T = Float
}
#else
public struct Matrix4x4<T: FloatingPoint>: Equatable
{
public typealias Scalar = T
public var m00: T, m01: T, m02: T, m03: T
public var m10: T, m11: T, m12: T, m13: T
public var m20: T, m21: T, m22: T, m23: T
public var m30: T, m31: T, m32: T, m33: T
public init()
{
self.m00 = 1; self.m01 = 0; self.m02 = 0; self.m03 = 0
self.m10 = 0; self.m11 = 1; self.m12 = 0; self.m13 = 0
self.m20 = 0; self.m21 = 0; self.m22 = 1; self.m23 = 0
self.m30 = 0; self.m31 = 0; self.m32 = 0; self.m33 = 1
}
public init(
_ a00: T, _ a01: T, _ a02: T, _ a03: T,
_ a10: T, _ a11: T, _ a12: T, _ a13: T,
_ a20: T, _ a21: T, _ a22: T, _ a23: T,
_ a30: T, _ a31: T, _ a32: T, _ a33: T)
{
self.m00 = a00; self.m01 = a01; self.m02 = a02; self.m03 = a03
self.m10 = a10; self.m11 = a11; self.m12 = a12; self.m13 = a13
self.m20 = a20; self.m21 = a21; self.m22 = a22; self.m23 = a23
self.m30 = a30; self.m31 = a31; self.m32 = a32; self.m33 = a33
}
}
public extension Matrix4x4
{
init(diagonal d: Vector4<T>)
{
self.m00 = d.x; self.m01 = 0; self.m02 = 0; self.m03 = 0
self.m10 = 0; self.m11 = d.y; self.m12 = 0; self.m13 = 0
self.m20 = 0; self.m21 = 0; self.m22 = d.z; self.m23 = 0
self.m30 = 0; self.m31 = 0; self.m32 = 0; self.m33 = d.w
}
init(_ row0: Vector4<T>, _ row1: Vector4<T>, _ row2: Vector4<T>, _ row3: Vector4<T>)
{
self.m00 = row0.x; self.m01 = row0.y; self.m02 = row0.z; self.m03 = row0.w
self.m10 = row1.x; self.m11 = row1.y; self.m12 = row1.z; self.m13 = row1.w
self.m20 = row2.x; self.m21 = row2.y; self.m22 = row2.z; self.m23 = row2.w
self.m30 = row3.x; self.m31 = row3.y; self.m32 = row3.z; self.m33 = row3.w
}
/*
init(_ col0: Vector4<T>, _ col1: Vector4<T>, _ col2: Vector4<T>, _ col3: Vector4<T>)
{
self.m00 = col0.x; self.m01 = col1.x; self.m02 = col2.x; self.m03 = col3.x
self.m10 = col0.y; self.m11 = col1.y; self.m12 = col2.y; self.m13 = col3.y
self.m20 = col0.z; self.m21 = col1.z; self.m22 = col2.z; self.m23 = col3.z
self.m30 = col0.w; self.m31 = col1.w; self.m32 = col2.w; self.m33 = col3.w
}
*/
subscript(index: Int) -> Vector4<T>
{
get
{
switch index
{
case 0: rows.0
case 1: rows.1
case 2: rows.2
case 3: rows.3
default: fatalError()
}
}
}
var rows: (Vector4<T>, Vector4<T>, Vector4<T>, Vector4<T>)
{(
.init(m00, m01, m02, m03),
.init(m10, m11, m12, m13),
.init(m20, m21, m22, m23),
.init(m30, m31, m32, m33))
}
var columns: (Vector4<T>, Vector4<T>, Vector4<T>, Vector4<T>)
{(
.init(m00, m10, m20, m30),
.init(m01, m11, m21, m31),
.init(m02, m12, m22, m32),
.init(m03, m13, m23, m33))
}
var transpose: Self
{
Self(
m00, m10, m20, m30,
m01, m11, m21, m31,
m02, m12, m22, m32,
m03, m13, m23, m33)
}
static func * (lhs: Self, rhs: Self) -> Self
{
let l = rhs, r = lhs
return Self(
l.m00 * r.m00 + l.m01 * r.m10 + l.m02 * r.m20 + l.m03 * r.m30,
l.m00 * r.m01 + l.m01 * r.m11 + l.m02 * r.m21 + l.m03 * r.m31,
l.m00 * r.m02 + l.m01 * r.m12 + l.m02 * r.m22 + l.m03 * r.m32,
l.m00 * r.m03 + l.m01 * r.m13 + l.m02 * r.m23 + l.m03 * r.m33,
l.m10 * r.m00 + l.m11 * r.m10 + l.m12 * r.m20 + l.m13 * r.m30,
l.m10 * r.m01 + l.m11 * r.m11 + l.m12 * r.m21 + l.m13 * r.m31,
l.m10 * r.m02 + l.m11 * r.m12 + l.m12 * r.m22 + l.m13 * r.m32,
l.m10 * r.m03 + l.m11 * r.m13 + l.m12 * r.m23 + l.m13 * r.m33,
l.m20 * r.m00 + l.m21 * r.m10 + l.m22 * r.m20 + l.m23 * r.m30,
l.m20 * r.m01 + l.m21 * r.m11 + l.m22 * r.m21 + l.m23 * r.m31,
l.m20 * r.m02 + l.m21 * r.m12 + l.m22 * r.m22 + l.m23 * r.m32,
l.m20 * r.m03 + l.m21 * r.m13 + l.m22 * r.m23 + l.m23 * r.m33,
l.m30 * r.m00 + l.m31 * r.m10 + l.m32 * r.m20 + l.m33 * r.m30,
l.m30 * r.m01 + l.m31 * r.m11 + l.m32 * r.m21 + l.m33 * r.m31,
l.m30 * r.m02 + l.m31 * r.m12 + l.m32 * r.m22 + l.m33 * r.m32,
l.m30 * r.m03 + l.m31 * r.m13 + l.m32 * r.m23 + l.m33 * r.m33)
}
}
#endif
#if USE_SIMD
public extension Matrix4x4
{
init(quat q: Quaternion<T>)
{
self = .init(simd_quatf(ix: q.x, iy: q.y, iz: q.z, r: q.w)).transpose
/*
let FUCK = simd_matrix4x4(simd_quatf(ix: q.x, iy: q.y, iz: q.z, r: q.w))
self = .init(
FUCK.columns.0.x, FUCK.columns.1.x, FUCK.columns.2.x, FUCK.columns.3.x,
FUCK.columns.0.y, FUCK.columns.1.y, FUCK.columns.2.y, FUCK.columns.3.y,
FUCK.columns.0.z, FUCK.columns.1.z, FUCK.columns.2.z, FUCK.columns.3.z,
FUCK.columns.0.w, FUCK.columns.1.w, FUCK.columns.2.w, FUCK.columns.3.w).transpose
*/
}
}
#else
public extension Matrix4x4 where T == Float
{
init(_ m3: Matrix3x3<T>)
{
self.m00 = m3.m00; self.m01 = m3.m01; self.m02 = m3.m02; self.m03 = 0
self.m10 = m3.m10; self.m11 = m3.m11; self.m12 = m3.m12; self.m13 = 0
self.m20 = m3.m20; self.m21 = m3.m21; self.m22 = m3.m22; self.m23 = 0
self.m30 = 0; self.m31 = 0; self.m32 = 0; self.m33 = 1
}
init(quat q: Quaternion<T>)
{
/*
let qq = Quaternion<T>(q.w * q.w, q.x * q.x, q.y * q.y, q.z * q.z)
return .init(
.init(
qq.w + qq.x - qq.y - qq.z,
2.0 * (q.x * q.y - q.w * q.z),
2.0 * (q.x * q.z + q.w * q.y),
0.0),
.init(
2.0 * (q.x * q.y + q.w * q.z),
qq.w - qq.x + qq.y - qq.z,
2.0 * (q.y * q.z - q.w * q.x),
0.0),
.init(
2.0 * (q.x * q.z - q.w * q.y),
2.0 * (q.y * q.z + q.w * q.x),
qq.w - qq.x - qq.y + qq.z,
0.0),
.init(0.0, 0.0, 0.0, 1.0))
*/
/*
let qww = q.w * q.w
let qxx = q.x * q.x
let qyy = q.y * q.y
let qzz = q.z * q.z
.init(
.init(
qqw + qqx - qqy - qqz,
2.0 * (q.x * q.y - q.w * q.z),
2.0 * (q.x * q.z + q.w * q.y),
0.0),
.init(
2.0 * (q.x * q.y + q.w * q.z),
qqw - qqx + qqy - qqz,
2.0 * (q.y * q.z - q.w * q.x),
0.0),
.init(
2.0 * (q.x * q.z - q.w * q.y),
2.0 * (q.y * q.z + q.w * q.x),
qqw - qqx - qqy + qqz,
0.0),
.init(0.0, 0.0, 0.0, 1.0))
*/
let qww = q.w * q.w, qxx = q.x * q.x, qyy = q.y * q.y, qzz = q.z * q.z
let qxy = q.x * q.y, qxz = q.x * q.z, qyz = q.y * q.z
let qwx = q.w * q.x, qwy = q.w * q.y, qwz = q.w * q.z
self = .init(
.init(qww + qxx - qyy - qzz, 2 * (qxy - qwz), 2 * (qxz + qwy), 0),
.init( 2 * (qxy + qwz), qww - qxx + qyy - qzz, 2 * (qyz - qwx), 0),
.init( 2 * (qxz - qwy), 2 * (qyz + qwx), qww - qxx - qyy + qzz, 0),
.init( 0, 0, 0, 1)).transpose
}
@inline(__always) static var identity: Self { Self(diagonal: .one) }
}
#endif
public extension Matrix4x4
{
// what the fuck lmao
@inline(__always) static func * (lhs: Self, rhs: Vector3<T>) -> Vector3<T>
{
return .init(
lhs.columns.0.x * rhs.x + lhs.columns.0.y * rhs.y + lhs.columns.0.z * rhs.z,
lhs.columns.1.x * rhs.x + lhs.columns.1.y * rhs.y + lhs.columns.1.z * rhs.z,
lhs.columns.2.x * rhs.x + lhs.columns.2.y * rhs.y + lhs.columns.2.z * rhs.z)
}
}
#if USE_SIMD
public extension Matrix4x4
{
@inline(__always) static func translate(_ v: Vector3<T>) -> Self
{
Self(
.init( 1, 0, 0, 0),
.init( 0, 1, 0, 0),
.init( 0, 0, 1, 0),
.init(v.x, v.y, v.z, 1))
}
@inline(__always) static func scale(_ v: Vector3<T>) -> Self
{
Self(
.init(v.x, 0, 0, 0),
.init( 0, v.y, 0, 0),
.init( 0, 0, v.z, 0),
.init( 0, 0, 0, 1))
}
@inline(__always) static func scale(scalar v: T) -> Self
{
Self(
.init(v, 0, 0, 0),
.init(0, v, 0, 0),
.init(0, 0, v, 0),
.init(0, 0, 0, 1))
}
@inline(__always) static func rotate(yawPitch: Vector2<T>) -> Self { return rotate(yaw: yawPitch.x, pitch: yawPitch.y) }
static func rotate(yaw ytheta: T, pitch xtheta: T) -> Self
{
let xc = xtheta.cosine, xs = xtheta.sine
let yc = ytheta.cosine, ys = ytheta.sine
return Self(
.init(yc, ys * xs, -ys * xc, 0),
.init( 0, xc, xs, 0),
.init(ys, yc * -xs, yc * xc, 0),
.init( 0, 0, 0, 1))
}
@inline(__always) static func rotate(x theta: T) -> Self
{
let c = theta.cosine, s = theta.sine
return Self(
.init(1, 0, 0, 0),
.init(0, c, s, 0),
.init(0, -s, c, 0),
.init(0, 0, 0, 1))
}
@inline(__always) static func rotate(y theta: T) -> Self
{
let c = theta.cosine, s = theta.sine
return Self(
.init(c, 0, -s, 0),
.init(0, 1, 0, 0),
.init(s, 0, c, 0),
.init(0, 0, 0, 1))
}
@inline(__always) static func rotate(z theta: T) -> Self
{
let c = theta.cosine, s = theta.sine
return Self(
.init(c, -s, 0, 0),
.init(s, c, 0, 0),
.init(0, 0, 1, 0),
.init(0, 0, 0, 1))
}
static func perspective(fovY: T, aspect: T, zNear: T, zFar: T) -> Self
{
let h = 1 / (fovY * T(0.5)).tangent
let w = h / aspect
let invClipRange = 1 / (zFar - zNear)
let z = -(zFar + zNear) * invClipRange
let z2 = -(2 * zFar * zNear) * invClipRange
return Self(
.init(w, 0, 0, 0),
.init(0, h, 0, 0),
.init(0, 0, z, -1),
.init(0, 0, z2, 0))
}
static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
{
let forward = (to - from).normalised
let bitangent = forward.cross(up).normalised
let tangent = bitangent.cross(forward).normalised
let normal = -forward
return Self(
.init(bitangent, 0),
.init( tangent, 0),
.init( normal, 0),
.init(0, 0, 0, 1))
}
}
#else
public extension Matrix4x4 where T == Float
{
@inline(__always) static func translate(_ v: Vector3<T>) -> Self
{
Self(
.init( 1, 0, 0, 0),
.init( 0, 1, 0, 0),
.init( 0, 0, 1, 0),
.init(v.x, v.y, v.z, 1))
}
@inline(__always) static func scale(_ v: Vector3<T>) -> Self
{
Self(
.init(v.x, 0, 0, 0),
.init( 0, v.y, 0, 0),
.init( 0, 0, v.z, 0),
.init( 0, 0, 0, 1))
}
@inline(__always) static func scale(scalar v: T) -> Self
{
Self(
.init(v, 0, 0, 0),
.init(0, v, 0, 0),
.init(0, 0, v, 0),
.init(0, 0, 0, 1))
}
static func rotate(axis v: Vector3<T>, angle theta: T) -> Self
{
//FIXME: THIS IS FUCKED UP FOR EVERYTHING OTHER THAN X AXIS ROTATION LOL
/*
let vv = v * v
let xy = v.x * v.y, xz = v.x * v.z, yz = v.y * v.z
let ts = sin(theta), tc = cos(theta)
return Self(
.init(
vv.x + (tc * (1 - vv.x)),
(xz - (tc * xy)) + (ts * v.z),
(xz - (tc * xz)) - (ts * v.y), 0),
.init(
(xy - (tc * xy)) - (ts * v.z),
vv.y + (tc * (1 - vv.y)),
(xz - (tc * xz)) + (ts * v.x), 0),
.init(
(xz - (tc * xz)) + (ts * v.y),
(xz - (tc * xz)) - (ts * v.y),
vv.z + (tc * (1 - vv.z)), 1),
.init(0, 0, 0, 1))
*/
let vxx = v.x * v.x, vxy = v.x * v.y, vxz = v.x * v.z
let vyy = v.y * v.y, vyz = v.y * v.z
let vzz = v.z * v.z
let ts = theta.sine, tc = theta.cosine
let ic = 1 - tc
return Self(
.init(
ic * vxx + tc,
ic * vxy - v.z * ts,
ic * vxz + v.z * ts,
0),
.init(
ic * vxy + v.z * ts,
ic * vyy + tc,
ic * vyz - v.x * ts,
0),
.init(
ic * vxz - v.y * ts,
ic * vyz + v.x * ts,
ic * vzz + tc,
0),
.init(0, 0, 0, 1))
}
@inline(__always) static func rotate(yawPitch: Vector2<T>) -> Self { return rotate(yaw: yawPitch.x, pitch: yawPitch.y) }
static func rotate(yaw ytheta: T, pitch xtheta: T) -> Self
{
let xc = xtheta.cosine, xs = xtheta.sine
let yc = ytheta.cosine, ys = ytheta.sine
return Self(
.init(yc, ys * xs, -ys * xc, 0),
.init( 0, xc, xs, 0),
.init(ys, yc * -xs, yc * xc, 0),
.init( 0, 0, 0, 1))
}
static func rotate(yaw ytheta: T, pitch xtheta: T, roll ztheta: T) -> Self
{
//FIXME: this doesn't null against control
let xc = xtheta.cosine, xs = xtheta.sine
let yc = ytheta.cosine, ys = ytheta.sine
let zc = ztheta.cosine, zs = ztheta.sine
let ysxs = ys * xs, ycxs = yc * xs
let result = Mat4f(
.init(yc * zc + ysxs * zs, yc * -zs + ysxs * zc, -ys * xc, 0),
.init( xc * zs, xc * zc, xs, 0),
.init(ys * zc - ycxs * zs, ys * -zs - ycxs * zc, yc * xc, 0),
.init( 0, 0, 0, 1))
let shouldBe = .rotate(z: ztheta) * .rotate(x: xtheta) * .rotate(y: ytheta)
let epsilon: Float = .ulpOfOne
if (result != shouldBe)
{
assert(abs(result[0][0] - shouldBe[0][0]) <= epsilon) // epsilon
assert(result[1][0] == shouldBe[1][0])
assert(abs(result[2][0] - shouldBe[2][0]) <= epsilon) // epsilon
assert(result[3][0] == shouldBe[3][0])
assert(abs(result[0][1] - shouldBe[0][1]) <= epsilon) // epsilon
assert(result[1][1] == shouldBe[1][1])
assert(abs(result[2][1] - shouldBe[2][1]) <= epsilon) // epsilon
assert(result[3][1] == shouldBe[3][1])
assert(result[0][2] == shouldBe[0][2])
assert(result[1][2] == shouldBe[1][2])
assert(result[2][2] == shouldBe[2][2])
assert(result[3][2] == shouldBe[3][2])
assert(result[0][3] == shouldBe[0][3])
assert(result[1][3] == shouldBe[1][3])
assert(result[2][3] == shouldBe[2][3])
assert(result[3][3] == shouldBe[3][3])
}
return result
}
@inline(__always) static func rotate(x theta: T) -> Self
{
let c = theta.cosine, s = theta.sine
return Self(
.init(1, 0, 0, 0),
.init(0, c, s, 0),
.init(0, -s, c, 0),
.init(0, 0, 0, 1))
}
@inline(__always) static func rotate(y theta: T) -> Self
{
let c = theta.cosine, s = theta.sine
return Self(
.init(c, 0, -s, 0),
.init(0, 1, 0, 0),
.init(s, 0, c, 0),
.init(0, 0, 0, 1))
}
@inline(__always) static func rotate(z theta: T) -> Self
{
let c = theta.cosine, s = theta.sine
return Self(
.init(c, -s, 0, 0),
.init(s, c, 0, 0),
.init(0, 0, 1, 0),
.init(0, 0, 0, 1))
}
static func perspective(fovY: T, aspect: T, zNear: T, zFar: T) -> Self
{
let h = 1 / (fovY * 0.5).tangent
let w = h / aspect
let invClipRange = 1 / (zFar - zNear)
let z = -(zFar + zNear) * invClipRange
let z2 = -(2 * zFar * zNear) * invClipRange
return Self(
.init(w, 0, 0, 0),
.init(0, h, 0, 0),
.init(0, 0, z, -1),
.init(0, 0, z2, 0))
}
static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
{
let forward = (to - from).normalised
let bitangent = forward.cross(up).normalised
let tangent = bitangent.cross(forward).normalised
let normal = -forward
return Self(
.init(bitangent, 0),
.init( tangent, 0),
.init( normal, 0),
.init(0, 0, 0, 1))
}
}
#endif
#if USE_SIMD
public typealias Mat4f = Matrix4x4
#else
public typealias Mat4f = Matrix4x4<Float>
public typealias Mat4d = Matrix4x4<Double>
#endif