294 lines
8.7 KiB
Swift
294 lines
8.7 KiB
Swift
import Foundation
|
|
import simd
|
|
|
|
|
|
public extension FloatingPoint
|
|
{
|
|
@inline(__always) var saturate: Self { min(max(self , 0), 1) }
|
|
|
|
@inline(__always) static func lerp(_ a: Self, _ b: Self, _ x: Self) -> Self { a * (1 - x) + b * x }
|
|
|
|
@inline(__always) static func deg(fromRad: Self) -> Self { fromRad * (180 / Self.pi) }
|
|
@inline(__always) static func rad(fromDeg: Self) -> Self { fromDeg * (Self.pi / 180) }
|
|
|
|
fileprivate func axisDeadzone(_ min: Self, _ max: Self) -> Self
|
|
{
|
|
let xabs = abs(self)
|
|
return if xabs <= min { 0 }
|
|
else if xabs >= max { Self(signOf: self, magnitudeOf: 1) }
|
|
else { Self(signOf: self, magnitudeOf: xabs - min) / (max - min) }
|
|
}
|
|
}
|
|
|
|
public extension SIMD2 where Scalar: FloatingPoint
|
|
{
|
|
@inline(__always) var len2: Scalar { x * x + y * y }
|
|
//@inline(__always) var len2: Scalar { simd_dot(self, self) }
|
|
@inline(__always) var len: Scalar { len2.squareRoot() }
|
|
@inline(__always) var normalised: Self { self / len }
|
|
|
|
@inline(__always) func dot(_ b: Self) -> Scalar { x * b.x + y * b.y }
|
|
@inline(__always) func reflect(_ n: Self) -> Self { self - (n * 2 * self.dot(n)) }
|
|
@inline(__always) func project(_ n: Self) -> Self { n * self.dot(n) }
|
|
|
|
@inline(__always) func cross(_ b: Self) -> Scalar { x * b.y - y * b.x }
|
|
|
|
@inline(__always) func lerp(_ b: Self, _ x: Scalar) -> Self
|
|
{
|
|
let invX = 1 - x
|
|
return Self(self.x * invX + b.x * x, self.y * invX + b.y * x)
|
|
}
|
|
|
|
@inline(__always) func distance(_ b: Self) -> Scalar { return (b - self).len }
|
|
|
|
func cardinalDeadzone(min: Scalar, max: Scalar) -> Self
|
|
{
|
|
Self(self.x.axisDeadzone(min, max), self.y.axisDeadzone(min, max))
|
|
}
|
|
|
|
func radialDeadzone(min: Scalar, max: Scalar) -> Self
|
|
{
|
|
let magnitude = self.len
|
|
if magnitude == .zero || magnitude < min { return Self.zero }
|
|
if magnitude > max { return self / magnitude }
|
|
let rescale = (magnitude - min) / (max - min)
|
|
return self / magnitude * rescale
|
|
}
|
|
}
|
|
|
|
public extension SIMD3 where Scalar: FloatingPoint
|
|
{
|
|
@inline(__always) static var X: Self { Self(1, 0, 0) }
|
|
@inline(__always) static var Y: Self { Self(0, 1, 0) }
|
|
@inline(__always) static var Z: Self { Self(0, 0, 1) }
|
|
|
|
@inline(__always) static var up: Self { Y }
|
|
@inline(__always) static var down: Self { -Y }
|
|
@inline(__always) static var left: Self { -X }
|
|
@inline(__always) static var right: Self { X }
|
|
@inline(__always) static var forward: Self { Z }
|
|
@inline(__always) static var back: Self { -Z }
|
|
|
|
@inline(__always) var len2: Scalar { x * x + y * y + z * z }
|
|
@inline(__always) var len: Scalar { len2.squareRoot() }
|
|
@inline(__always) var normalised: Self { self / len }
|
|
|
|
@inline(__always) mutating func normalise() { self /= len }
|
|
|
|
@inline(__always) func lerp(_ b: Self, _ x: Scalar) -> Self
|
|
{
|
|
let invX = 1 - x
|
|
return Self(self.x * invX + b.x * x, self.y * invX + b.y * x, self.z * invX + b.z * x)
|
|
}
|
|
|
|
@inline(__always) func dot(_ b: Self) -> Scalar { x * b.x + y * b.y + z * b.z }
|
|
@inline(__always) func cross(_ b: Self) -> Self { Self(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x) }
|
|
|
|
@inline(__always) func project(_ n: Self) -> Self { n * self.dot(n) }
|
|
}
|
|
|
|
public extension SIMD4 where Scalar: FloatingPoint
|
|
{
|
|
@inline(__always) static var X: Self { Self(1, 0, 0, 0) }
|
|
@inline(__always) static var Y: Self { Self(0, 1, 0, 0) }
|
|
@inline(__always) static var Z: Self { Self(0, 0, 1, 0) }
|
|
@inline(__always) static var W: Self { Self(0, 0, 0, 1) }
|
|
}
|
|
|
|
public extension simd_float4x4
|
|
{
|
|
@inline(__always) static var identity: Self { Self(diagonal: .one) }
|
|
|
|
@inline(__always) static func translate(_ v: Vec3f) -> 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: Vec3f) -> 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: Float) -> 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: Vec3f, angle theta: Float) -> 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(angle), tc = cos(angle)
|
|
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 = sin(theta), tc = cos(theta)
|
|
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: Vec2f) -> Self { return rotate(yaw: yawPitch.x, pitch: yawPitch.y) }
|
|
|
|
static func rotate(yaw ytheta: Float, pitch xtheta: Float) -> Self
|
|
{
|
|
let xc = cos(xtheta), xs = sin(xtheta)
|
|
let yc = cos(ytheta), ys = sin(ytheta)
|
|
|
|
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: Float, pitch xtheta: Float, roll ztheta: Float) -> Self
|
|
{
|
|
//FIXME: this doesn't null against control
|
|
let xc = cos(xtheta), xs = sin(xtheta)
|
|
let yc = cos(ytheta), ys = sin(ytheta)
|
|
let zc = cos(ztheta), zs = sin(ztheta)
|
|
|
|
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: Float) -> Self
|
|
{
|
|
let c = cos(theta), s = sin(theta)
|
|
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: Float) -> Self
|
|
{
|
|
let c = cos(theta), s = sin(theta)
|
|
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: Float) -> Self
|
|
{
|
|
let c = cos(theta), s = sin(theta)
|
|
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: Float, aspect: Float, zNear: Float, zFar: Float) -> Self
|
|
{
|
|
let h = 1 / tanf(fovY * 0.5)
|
|
let w = h / aspect
|
|
let invClipRange = 1 / (zFar - zNear)
|
|
let z = -(zFar + zNear) * invClipRange
|
|
let z2 = -(2 * zFar * zNear) * invClipRange
|
|
return simd_matrix(
|
|
.init(w, 0, 0, 0),
|
|
.init(0, h, 0, 0),
|
|
.init(0, 0, z, -1),
|
|
.init(0, 0, z2, 0))
|
|
}
|
|
|
|
static func lookAt(from: Vec3f = .zero, to: Vec3f, up: Vec3f = .up) -> Self
|
|
{
|
|
let forward = (to - from).normalised
|
|
let bitangent = forward.cross(up).normalised
|
|
let tangent = bitangent.cross(forward).normalised
|
|
let normal = -forward
|
|
return simd_matrix(
|
|
.init(bitangent.x, tangent.x, normal.x, 0.0),
|
|
.init(bitangent.y, tangent.y, normal.y, 0.0),
|
|
.init(bitangent.z, tangent.z, normal.z, 0.0),
|
|
.init( 0.0, 0.0, 0.0, 1.0))
|
|
}
|
|
}
|
|
|
|
|
|
public typealias Vec2f = SIMD2<Float>
|
|
public typealias Vec2d = SIMD2<Double>
|
|
public typealias Vec3f = SIMD3<Float>
|
|
public typealias Vec3d = SIMD3<Double>
|
|
public typealias Vec4f = SIMD4<Float>
|
|
public typealias Vec4d = SIMD4<Double>
|
|
public typealias Mat4f = simd_float4x4
|