334 lines
8.5 KiB
Swift
334 lines
8.5 KiB
Swift
import Foundation
|
|
import simd
|
|
|
|
|
|
public struct Quaternion<T: FloatingPoint>: Equatable
|
|
{
|
|
public var w: T, x: T, y: T, z: T
|
|
|
|
public init() { self.x = 0; self.y = 0; self.z = 0; self.w = 1 }
|
|
public init(_ w: T, _ x: T, _ y: T, _ z: T) { self.w = w; self.x = x; self.y = y; self.z = z }
|
|
|
|
public static var identity: Self { .init(1, 0, 0, 0) }
|
|
}
|
|
|
|
public extension Quaternion where T: SIMDScalar
|
|
{
|
|
init(real w: T, imaginary i: Vector3<T>) { self.w = w; self.x = i.x; self.y = i.y; self.z = i.z }
|
|
}
|
|
|
|
#if USE_SIMD
|
|
|
|
public extension Quaternion where T == Float
|
|
{
|
|
init(_ q: simd_quatf)
|
|
{
|
|
self.init(real: q.real, imaginary: q.imag)
|
|
}
|
|
|
|
static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
|
|
{
|
|
let m3 = Matrix3x3.lookAt(from: from, to: to, up: up)
|
|
let et_blee_eve_nyu = simd_quatf(m3)
|
|
return Self(real: et_blee_eve_nyu.real, imaginary: .init(
|
|
et_blee_eve_nyu.imag.x,
|
|
et_blee_eve_nyu.imag.y,
|
|
et_blee_eve_nyu.imag.z))
|
|
}
|
|
}
|
|
|
|
#else
|
|
|
|
public extension Quaternion where T == Float
|
|
{
|
|
init(_ m3: Matrix3x3<T>)
|
|
{
|
|
if m3.m22 < 0
|
|
{
|
|
if m3.m00 > m3.m11
|
|
{
|
|
let t = 1 + m3.m00 - m3.m11 - m3.m22
|
|
self.x = t.squareRoot() * 0.5
|
|
if m3.m21 < m3.m12 { self.x = -self.x }
|
|
let ix4 = 1 / (4 * self.x)
|
|
self.w = (m3.m21 - m3.m12) * ix4
|
|
self.y = (m3.m10 + m3.m01) * ix4
|
|
self.z = (m3.m02 + m3.m20) * ix4
|
|
if t == 1 && self.w == 0 && self.y == 0 && self.z == 0 { self.x = 1 }
|
|
}
|
|
else
|
|
{
|
|
let t = 1 - m3.m00 + m3.m11 - m3.m22
|
|
self.y = t.squareRoot() * 0.5
|
|
if m3.m02 < m3.m20 { self.y = -self.y }
|
|
let iy4 = 1 / (4 * self.y)
|
|
self.w = (m3.m02 - m3.m20) * iy4
|
|
self.x = (m3.m10 + m3.m01) * iy4
|
|
self.z = (m3.m21 + m3.m12) * iy4
|
|
if t == 1 && self.w == 0 && self.x == 0 && self.z == 0 { self.y = 1 }
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if m3.m00 < -m3.m11
|
|
{
|
|
let t = 1 - m3.m00 - m3.m11 + m3.m22
|
|
self.z = t.squareRoot() * 0.5
|
|
if m3.m10 < m3.m01 { self.z = -self.z }
|
|
let iz4 = 1 / (4 * self.z)
|
|
self.w = (m3.m10 - m3.m01) * iz4
|
|
self.x = (m3.m02 + m3.m20) * iz4
|
|
self.y = (m3.m21 + m3.m12) * iz4
|
|
if t == 1 && self.w == 0 && self.x == 0 && self.y == 0 { self.z = 1 }
|
|
}
|
|
else
|
|
{
|
|
let t = 1 + m3.m00 + m3.m11 + m3.m22
|
|
self.w = t.squareRoot() * 0.5
|
|
let iw4 = 1 / (4 * self.w)
|
|
self.x = (m3.m21 - m3.m12) * iw4
|
|
self.y = (m3.m02 - m3.m20) * iw4
|
|
self.z = (m3.m10 - m3.m01) * iw4
|
|
if t == 1 && self.x == 0 && self.y == 0 && self.z == 0 { self.w = 1 }
|
|
}
|
|
}
|
|
|
|
/*
|
|
let m3 = m3FUCK.transpose
|
|
if m3.m22 < 0
|
|
{
|
|
if m3.m00 > m3.m11
|
|
{
|
|
let t = 1 + m3.m00 - m3.m11 - m3.m22
|
|
self.x = t.squareRoot() * 0.5
|
|
if m3.m12 < m3.m21 { self.x = -self.x }
|
|
let ix4 = 1 / (4 * self.x)
|
|
self.w = (m3.m12 - m3.m21) * ix4
|
|
self.y = (m3.m01 + m3.m10) * ix4
|
|
self.z = (m3.m20 + m3.m02) * ix4
|
|
if t == 1 && self.w == 0 && self.y == 0 && self.z == 0 { self.x = 1 }
|
|
}
|
|
else
|
|
{
|
|
let t = 1 - m3.m00 + m3.m11 - m3.m22
|
|
self.y = t.squareRoot() * 0.5
|
|
if m3.m20 < m3.m02 { self.y = -self.y }
|
|
let iy4 = 1 / (4 * self.y)
|
|
self.w = (m3.m20 - m3.m02) * iy4
|
|
self.x = (m3.m01 + m3.m10) * iy4
|
|
self.z = (m3.m12 + m3.m21) * iy4
|
|
if t == 1 && self.w == 0 && self.x == 0 && self.z == 0 { self.y = 1 }
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if m3.m00 < -m3.m11
|
|
{
|
|
let t = 1 - m3.m00 - m3.m11 + m3.m22
|
|
self.z = t.squareRoot() * 0.5
|
|
if m3.m01 < m3.m10 { self.z = -self.z }
|
|
let iz4 = 1 / (4 * self.z)
|
|
self.w = (m3.m01 - m3.m10) * iz4
|
|
self.x = (m3.m20 + m3.m02) * iz4
|
|
self.y = (m3.m12 + m3.m21) * iz4
|
|
if t == 1 && self.w == 0 && self.x == 0 && self.y == 0 { self.z = 1 }
|
|
}
|
|
else
|
|
{
|
|
let t = 1 + m3.m00 + m3.m11 + m3.m22
|
|
self.w = t.squareRoot() * 0.5
|
|
let iw4 = 1 / (4 * self.w)
|
|
self.x = (m3.m12 - m3.m21) * iw4
|
|
self.y = (m3.m20 - m3.m02) * iw4
|
|
self.z = (m3.m01 - m3.m10) * iw4
|
|
if t == 1 && self.x == 0 && self.y == 0 && self.z == 0 { self.w = 1 }
|
|
}
|
|
}
|
|
*/
|
|
|
|
|
|
/*
|
|
self.w = (1 + m3.m00 + m3.m11 + m3.m22).squareRoot() * 0.5
|
|
let iw4 = 1 / (4 * self.w)
|
|
self.x = (m3.m21 - m3.m12) * iw4
|
|
self.y = (m3.m02 - m3.m20) * iw4
|
|
self.z = (m3.m10 - m3.m01) * iw4
|
|
*/
|
|
/*
|
|
if m3.m22 < 0
|
|
{
|
|
if m3.m00 > m3.m11
|
|
{
|
|
self.w = (1 + m3.m00 - m3.m11 - m3.m22).squareRoot() * 0.5
|
|
let iw4 = 1 / (4 * self.w)
|
|
//self.x = (m3.m01 + m3.m10) * iw4
|
|
//self.y = (m3.m20 + m3.m02) * iw4
|
|
//self.z = (m3.m12 - m3.m21) * iw4
|
|
self.x = (m3.m21 - m3.m12) * iw4
|
|
self.y = (m3.m02 - m3.m20) * iw4
|
|
self.z = (m3.m10 - m3.m01) * iw4
|
|
}
|
|
else
|
|
{
|
|
self.x = (1 - m3.m00 + m3.m11 - m3.m22).squareRoot() * 0.5
|
|
let ix4 = 1 / (4 * self.x)
|
|
//self.w = (m3.m01 + m3.m10) * ix4
|
|
//self.y = (m3.m12 + m3.m21) * ix4
|
|
//self.z = (m3.m20 - m3.m02) * ix4
|
|
self.z = (m3.m02 - m3.m20) * ix4
|
|
self.y = (m3.m21 - m3.m12) * ix4
|
|
self.w = (m3.m10 - m3.m01) * ix4
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if m3.m00 < -m3.m11
|
|
{
|
|
self.y = (1 - m3.m00 - m3.m11 + m3.m22).squareRoot() * 0.5
|
|
let iy4 = 1 / (4 * self.y)
|
|
//self.w = (m3.m20 + m3.m02) * iy4
|
|
//self.x = (m3.m12 + m3.m21) * iy4
|
|
//self.z = (m3.m01 - m3.m10) * iy4
|
|
self.z = (m3.m10 - m3.m01) * iy4
|
|
self.x = (m3.m21 - m3.m12) * iy4
|
|
self.w = (m3.m02 - m3.m20) * iy4
|
|
}
|
|
else
|
|
{
|
|
self.z = (1 + m3.m00 + m3.m11 + m3.m22).squareRoot() * 0.5
|
|
let iz4 = 1 / (4 * self.z)
|
|
//self.w = (m3.m12 - m3.m21) * iz4
|
|
//self.x = (m3.m20 - m3.m02) * iz4
|
|
//self.y = (m3.m01 - m3.m10) * iz4
|
|
self.y = (m3.m10 - m3.m01) * iz4
|
|
self.x = (m3.m02 - m3.m20) * iz4
|
|
self.w = (m3.m21 - m3.m12) * iz4
|
|
}
|
|
}
|
|
*/
|
|
}
|
|
|
|
static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
|
|
{
|
|
Self(Matrix3x3<T>.lookAt(from: from, to: to, up: up))
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
public extension Quaternion where T == Float
|
|
{
|
|
init(axis v: Vector3<T>, angle theta: T)
|
|
{
|
|
let tc = (theta * 0.5).cosine, ts = (theta * 0.5).sine
|
|
self.init(real: tc, imaginary: v * ts)
|
|
}
|
|
|
|
@inline(__always) func slerp(_ rhs: Self, _ theta: T) -> Self
|
|
{
|
|
var b = rhs
|
|
var cosHalfTheta = self.dot(b)
|
|
if abs(cosHalfTheta) >= 1
|
|
{
|
|
// if a == b or a == -b then theta == 0
|
|
return self
|
|
}
|
|
else if cosHalfTheta < 0
|
|
{
|
|
b = -b
|
|
cosHalfTheta = -cosHalfTheta
|
|
}
|
|
let halfTheta = acos(cosHalfTheta)
|
|
let sinHalfTheta = (1 - cosHalfTheta * cosHalfTheta).squareRoot()
|
|
if abs(sinHalfTheta) <= .ulpOfOne
|
|
{
|
|
// if theta == 180 degrees then result is not fully defined
|
|
// we could rotate around any axis normal to a or b
|
|
return (self + b) * 0.5
|
|
}
|
|
let ratioA = ((1 - theta) * halfTheta).sine / sinHalfTheta
|
|
let ratioB = (theta * halfTheta).sine / sinHalfTheta
|
|
return self * ratioA + b * ratioB
|
|
//let piss = simd_quatf(ix: x, iy: y, iz: z, r: w)
|
|
//let shit = simd_quatf(ix: b.x, iy: b.y, iz: b.z, r: b.w)
|
|
//let cunt = simd_slerp(piss, shit, x)
|
|
//return .init(real: cunt.real, imaginary: cunt.imag)
|
|
}
|
|
|
|
// Because swift type checking brain injury
|
|
@inline(__always) static func * (a: Self, b: Self) -> Self
|
|
{
|
|
.init(
|
|
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
|
|
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
|
|
a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
|
|
a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w)
|
|
}
|
|
}
|
|
|
|
public extension Quaternion where T: FloatingPoint
|
|
{
|
|
@inline(__always) static prefix func - (this: Self) -> Self
|
|
{
|
|
.init(-this.w, -this.x, -this.y, -this.z)
|
|
}
|
|
|
|
@inline(__always) static func + (lhs: Self, rhs: Self) -> Self
|
|
{
|
|
.init(lhs.w + rhs.w, lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z)
|
|
}
|
|
|
|
@inline(__always) static func * (lhs: Self, rhs: T) -> Self
|
|
{
|
|
.init(lhs.w * rhs, lhs.x * rhs, lhs.y * rhs, lhs.z * rhs)
|
|
}
|
|
|
|
@inline(__always) mutating func normalise()
|
|
{
|
|
let invMag = 1 / self.dot(self).squareRoot()
|
|
self.w *= invMag
|
|
self.x *= invMag
|
|
self.y *= invMag
|
|
self.z *= invMag
|
|
}
|
|
|
|
@inline(__always) var normalised: Self
|
|
{
|
|
let invMag = 1 / self.dot(self).squareRoot()
|
|
return self * invMag
|
|
}
|
|
|
|
@inline(__always) func dot(_ b: Self) -> T { x * b.x + y * b.y + z * b.z + w * b.w }
|
|
}
|
|
|
|
|
|
#if USE_SIMD
|
|
|
|
public extension Vector3 where Scalar == Float
|
|
{
|
|
@inline(__always) static func * (v: Self, q: Quaternion<Scalar>) -> Self
|
|
{
|
|
return v * simd_float3x3(simd_quatf(ix: q.x, iy: q.y, iz: q.z, r: q.w))
|
|
}
|
|
}
|
|
|
|
#else
|
|
|
|
public extension Vector3 where T: BinaryFloatingPoint
|
|
{
|
|
@inline(__always) static func * (v: Self, q: Quaternion<T>) -> Self
|
|
{
|
|
let qw = q.w, qv = Self(q.x, q.y, q.z)
|
|
|
|
var out = qv * 2.0 * qv.dot(v)
|
|
out += v * (qw * qw - qv.dot(qv))
|
|
return out + qv.cross(v) * 2.0 * qw
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
public typealias Quatf = Quaternion<Float>
|
|
public typealias Quatd = Quaternion<Double>
|