V3.swift

enum Math<F> where F:FloatingPoint
{
    typealias V2 = (x:F, y:F)
    typealias V3 = (x:F, y:F, z:F)

    @inline(__always)
    static
    func add(_ v1:V3, _ v2:V3) -> V3
    {
        return (v1.x + v2.x, v1.y + v2.y, v1.z + v2.z)
    }

    @inline(__always)
    static
    func sub(_ v1:V3, _ v2:V3) -> V3
    {
        return (v1.x - v2.x, v1.y - v2.y, v1.z - v2.z)
    }

    @inline(__always)
    static
    func mult(_ v1:V3, _ v2:V3) -> V3
    {
        return (v1.x * v2.x, v1.y * v2.y, v1.z * v2.z)
    }

    @inline(__always)
    static
    func neg(_ v:V3) -> V3
    {
        return (-v.x, -v.y, -v.z)
    }

    @inline(__always)
    static
    func madd(_ v1:V3, _ v2:V3, _ v3:V3) -> V3
    {
        return (v1.x.addingProduct(v2.x, v3.x), v1.y.addingProduct(v2.y, v3.y), v1.z.addingProduct(v2.z, v3.z))
    }

    @inline(__always)
    static
    func scadd(_ v1:V3, _ v2:V3, _ c:F) -> V3
    {
        return (v1.x.addingProduct(v2.x, c), v1.y.addingProduct(v2.y, c), v1.z.addingProduct(v2.z, c))
    }

    @inline(__always)
    static
    func scale(_ v:V2, by c:F) -> V2
    {
        return (v.x * c, v.y * c)
    }

    @inline(__always)
    static
    func scale(_ v:V3, by c:F) -> V3
    {
        return (v.x * c, v.y * c, v.z * c)
    }

    @inline(__always)
    static
    func dot(_ v1:V3, _ v2:V3) -> F
    {
        return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z
    }

    @inline(__always)
    static
    func cross(_ v1:V3, _ v2:V3) -> V3
    {
        return (v1.y*v2.z - v2.y*v1.z, v1.z*v2.x - v2.z*v1.x, v1.x*v2.y - v2.x*v1.y)
    }

    @inline(__always)
    static
    func length(_ v:V3) -> F
    {
        return Math.dot(v, v).squareRoot()
    }

    @inline(__always)
    static
    func normalize(_ v:V3) -> V3
    {
        let factor:F = 1 / Math.dot(v, v).squareRoot()
        return Math.scale(v, by: factor)
    }

    @inline(__always)
    static
    func copy(_ v:V2, to ptr:UnsafeMutablePointer<F>)
    {
        ptr[0] = v.x
        ptr[1] = v.y
    }

    @inline(__always)
    static
    func copy(_ v:V3, to ptr:UnsafeMutablePointer<F>)
    {
        ptr[0] = v.x
        ptr[1] = v.y
        ptr[2] = v.z
    }
}

extension Math where F == Double
{
    @inline(__always)
    static
    func cast_float(_ v:V3) -> Math<Float>.V3
    {
        return (Float(v.x), Float(v.y), Float(v.z))
    }
}