excessive / cpml / 1

local modules   = (...):gsub('%.[^%.]+$', '') .. "."

local constants = require(modules .. "constants")

local vec2      = require(modules .. "vec2")

local vec3      = require(modules .. "vec3")

local quat      = require(modules .. "quat")

local utils     = require(modules .. "utils")

local precond   = require(modules .. "_private_precond")

local private   = require(modules .. "_private_utils")

local sqrt      = math.sqrt

local cos       = math.cos

local sin       = math.sin

local tan       = math.tan

local rad       = math.rad

local mat4      = {}

local mat4_mt   = {}

        m = m or {

                0, 0, 0, 0,

                0, 0, 0, 0,

                0, 0, 0, 0,

                0, 0, 0, 0

        }

        m._m = m

        return setmetatable(m, mat4_mt)

end

        m[1],  m[2],  m[3],  m[4]  = 1, 0, 0, 0

        m[5],  m[6],  m[7],  m[8]  = 0, 1, 0, 0

        m[9],  m[10], m[11], m[12] = 0, 0, 1, 0

        m[13], m[14], m[15], m[16] = 0, 0, 0, 1

        return m

end

if type(jit) == "table" and jit.status() then

   --  status, ffi = pcall(require, "ffi")

local tmp = new()

local tm4 = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }

local tv4 = { 0, 0, 0, 0 }

-- @treturn mat4 out

function mat4.new(a)

        local out = new()

        -- 4x4 matrix

        if type(a) == "table" and #a == 16 then

                for i = 1, 16 do

                        out[i] = tonumber(a[i])

                end

        -- 3x3 matrix

        elseif type(a) == "table" and #a == 9 then

                out[1], out[2],  out[3]  = a[1], a[2], a[3]

                out[5], out[6],  out[7]  = a[4], a[5], a[6]

                out[9], out[10], out[11] = a[7], a[8], a[9]

                out[16] = 1

        -- 4 vec4s

        elseif type(a) == "table" and type(a[1]) == "table" then

                local idx = 1

                for i = 1, 4 do

                        for j = 1, 4 do

                                out[idx] = a[i][j]

                                idx = idx + 1

                        end

        else

                out[1]  = 1

                out[6]  = 1

                out[11] = 1

                out[16] = 1

        end

        return out

end

-- @treturn mat4 out

function mat4.identity(a)

        return identity(a or new())

end

-- @treturn mat4 out

function mat4.from_angle_axis(angle, axis)

        local l = axis:len()

-- @treturn mat4 out

function mat4.from_quaternion(q)

        return mat4.from_angle_axis(q:to_angle_axis())

-- @treturn mat4 out

function mat4.from_direction(direction, up)

        local forward = vec3.normalize(direction)

-- @treturn mat4 out

function mat4.from_transform(trans, rot, scale)

        local rx, ry, rz, rw = rot.x, rot.y, rot.z, rot.w

        local sm = new {

                scale.x, 0,       0,       0,

                0,       scale.y, 0,       0,

                0,       0,       scale.z, 0,

                0,       0,       0,       1,

        }

        local rm = new {

                1-2*(ry*ry+rz*rz), 2*(rx*ry-rz*rw), 2*(rx*rz+ry*rw), 0,

                2*(rx*ry+rz*rw), 1-2*(rx*rx+rz*rz), 2*(ry*rz-rx*rw), 0,

                2*(rx*rz-ry*rw), 2*(ry*rz+rx*rw), 1-2*(rx*rx+ry*ry), 0,

                0, 0, 0, 1

        }

        local rsm = rm * sm

        rsm[13] = trans.x

        rsm[14] = trans.y

        rsm[15] = trans.z

        return rsm

end

-- @treturn mat4 out

function mat4.from_ortho(left, right, top, bottom, near, far)

        local out = new()

-- @treturn mat4 out

function mat4.from_perspective(fovy, aspect, near, far)

        assert(aspect ~= 0)

        assert(near   ~= far)

        local t   = tan(rad(fovy) / 2)

        local out = new()

        out[1]    =  1 / (t * aspect)

        out[6]    =  1 / t

        out[11]   = -(far + near) / (far - near)

        out[12]   = -1

        out[15]   = -(2 * far * near) / (far - near)

        out[16]   =  0

        return out

end

-- @treturn mat4 out

function mat4.from_hmd_perspective(tanHalfFov, zNear, zFar, flipZ, farAtInfinity)

        -- CPML is right-handed and intended for GL, so these don't need to be arguments.

        local rightHanded = true

        local isOpenGL    = true

        local function CreateNDCScaleAndOffsetFromFov(tanHalfFov)

                local x_scale  = 2 / (tanHalfFov.LeftTan + tanHalfFov.RightTan)

                local x_offset =     (tanHalfFov.LeftTan - tanHalfFov.RightTan) * x_scale * 0.5

                local y_scale  = 2 / (tanHalfFov.UpTan   + tanHalfFov.DownTan )

                local y_offset =     (tanHalfFov.UpTan   - tanHalfFov.DownTan ) * y_scale * 0.5

                local result = {

                        Scale  = vec2(x_scale, y_scale),

                        Offset = vec2(x_offset, y_offset)

                }

                -- whereas this is from NDC which is Y=down.

                 return result

        end

        if not flipZ and farAtInfinity then

                print("Error: Cannot push Far Clip to Infinity when Z-order is not flipped")

                farAtInfinity = false

         -- A projection matrix is very like a scaling from NDC, so we can start with that.

        local scaleAndOffset  = CreateNDCScaleAndOffsetFromFov(tanHalfFov)

        local handednessScale = rightHanded and -1.0 or 1.0

        local projection      = new()

        -- Produces X result, mapping clip edges to [-w,+w]

        projection[1] = scaleAndOffset.Scale.x

        projection[2] = 0

        projection[3] = handednessScale * scaleAndOffset.Offset.x

        projection[4] = 0

        -- Produces Y result, mapping clip edges to [-w,+w]

        -- whereas this is derived from an NDC scaling, which is Y=down.

        projection[5] = 0

        projection[6] = scaleAndOffset.Scale.y

        projection[7] = handednessScale * -scaleAndOffset.Offset.y

        projection[8] = 0

        -- Produces Z-buffer result - app needs to fill this in with whatever Z range it wants.

        -- We'll just use some defaults for now.

        projection[9]  = 0

        projection[10] = 0

        if farAtInfinity then

                if isOpenGL then

                        -- It's not clear this makes sense for OpenGL - you don't get the same precision benefits you do in D3D.

        else

                if isOpenGL then

                        -- Clip range is [-w,+w], so 0 is at the middle of the range.

                        projection[11] = -handednessScale * (flipZ and -1.0 or 1.0) * (zNear + zFar) / (zNear - zFar)

                        projection[12] = 2.0 * ((flipZ and -zFar or zFar) * zNear) / (zNear - zFar)

                else

                        -- Clip range is [0,+w], so 0 is at the start of the range.

        -- Produces W result (= Z in)

        projection[13] = 0

        projection[14] = 0

        projection[15] = handednessScale

        projection[16] = 0

        return projection:transpose(projection)

end

-- @treturn mat4 out

function mat4.clone(a)

        return new(a)

end

-- @treturn mat4 out Matrix equivalent to "apply b, then a"

function mat4.mul(out, a, b)

        tm4[1]  = b[1]  * a[1] + b[2]  * a[5] + b[3]  * a[9]  + b[4]  * a[13]

        tm4[2]  = b[1]  * a[2] + b[2]  * a[6] + b[3]  * a[10] + b[4]  * a[14]

        tm4[3]  = b[1]  * a[3] + b[2]  * a[7] + b[3]  * a[11] + b[4]  * a[15]

        tm4[4]  = b[1]  * a[4] + b[2]  * a[8] + b[3]  * a[12] + b[4]  * a[16]

        tm4[5]  = b[5]  * a[1] + b[6]  * a[5] + b[7]  * a[9]  + b[8]  * a[13]

        tm4[6]  = b[5]  * a[2] + b[6]  * a[6] + b[7]  * a[10] + b[8]  * a[14]

        tm4[7]  = b[5]  * a[3] + b[6]  * a[7] + b[7]  * a[11] + b[8]  * a[15]

        tm4[8]  = b[5]  * a[4] + b[6]  * a[8] + b[7]  * a[12] + b[8]  * a[16]

        tm4[9]  = b[9]  * a[1] + b[10] * a[5] + b[11] * a[9]  + b[12] * a[13]

        tm4[10] = b[9]  * a[2] + b[10] * a[6] + b[11] * a[10] + b[12] * a[14]

        tm4[11] = b[9]  * a[3] + b[10] * a[7] + b[11] * a[11] + b[12] * a[15]

        tm4[12] = b[9]  * a[4] + b[10] * a[8] + b[11] * a[12] + b[12] * a[16]

        tm4[13] = b[13] * a[1] + b[14] * a[5] + b[15] * a[9]  + b[16] * a[13]

        tm4[14] = b[13] * a[2] + b[14] * a[6] + b[15] * a[10] + b[16] * a[14]

        tm4[15] = b[13] * a[3] + b[14] * a[7] + b[15] * a[11] + b[16] * a[15]

        tm4[16] = b[13] * a[4] + b[14] * a[8] + b[15] * a[12] + b[16] * a[16]

        for i=1, 16 do

                out[i] = tm4[i]

        end

        return out

end

-- @treturn mat4 out

function mat4.mul_vec4(out, a, b)

        tv4[1] = b[1] * a[1] + b[2] * a[5] + b [3] * a[9]  + b[4] * a[13]

        tv4[2] = b[1] * a[2] + b[2] * a[6] + b [3] * a[10] + b[4] * a[14]

        tv4[3] = b[1] * a[3] + b[2] * a[7] + b [3] * a[11] + b[4] * a[15]

        tv4[4] = b[1] * a[4] + b[2] * a[8] + b [3] * a[12] + b[4] * a[16]

        for i=1, 4 do

                out[i] = tv4[i]

        end

        return out

end

-- @treturn mat4 out

function mat4.invert(out, a)

        tm4[1]  =  a[6] * a[11] * a[16] - a[6] * a[12] * a[15] - a[10] * a[7] * a[16] + a[10] * a[8] * a[15] + a[14] * a[7] * a[12] - a[14] * a[8] * a[11]

        tm4[2]  = -a[2] * a[11] * a[16] + a[2] * a[12] * a[15] + a[10] * a[3] * a[16] - a[10] * a[4] * a[15] - a[14] * a[3] * a[12] + a[14] * a[4] * a[11]

        tm4[3]  =  a[2] * a[7]  * a[16] - a[2] * a[8]  * a[15] - a[6]  * a[3] * a[16] + a[6]  * a[4] * a[15] + a[14] * a[3] * a[8]  - a[14] * a[4] * a[7]

        tm4[4]  = -a[2] * a[7]  * a[12] + a[2] * a[8]  * a[11] + a[6]  * a[3] * a[12] - a[6]  * a[4] * a[11] - a[10] * a[3] * a[8]  + a[10] * a[4] * a[7]

        tm4[5]  = -a[5] * a[11] * a[16] + a[5] * a[12] * a[15] + a[9]  * a[7] * a[16] - a[9]  * a[8] * a[15] - a[13] * a[7] * a[12] + a[13] * a[8] * a[11]

        tm4[6]  =  a[1] * a[11] * a[16] - a[1] * a[12] * a[15] - a[9]  * a[3] * a[16] + a[9]  * a[4] * a[15] + a[13] * a[3] * a[12] - a[13] * a[4] * a[11]

        tm4[7]  = -a[1] * a[7]  * a[16] + a[1] * a[8]  * a[15] + a[5]  * a[3] * a[16] - a[5]  * a[4] * a[15] - a[13] * a[3] * a[8]  + a[13] * a[4] * a[7]

        tm4[8]  =  a[1] * a[7]  * a[12] - a[1] * a[8]  * a[11] - a[5]  * a[3] * a[12] + a[5]  * a[4] * a[11] + a[9]  * a[3] * a[8]  - a[9]  * a[4] * a[7]

        tm4[9]  =  a[5] * a[10] * a[16] - a[5] * a[12] * a[14] - a[9]  * a[6] * a[16] + a[9]  * a[8] * a[14] + a[13] * a[6] * a[12] - a[13] * a[8] * a[10]

        tm4[10] = -a[1] * a[10] * a[16] + a[1] * a[12] * a[14] + a[9]  * a[2] * a[16] - a[9]  * a[4] * a[14] - a[13] * a[2] * a[12] + a[13] * a[4] * a[10]

        tm4[11] =  a[1] * a[6]  * a[16] - a[1] * a[8]  * a[14] - a[5]  * a[2] * a[16] + a[5]  * a[4] * a[14] + a[13] * a[2] * a[8]  - a[13] * a[4] * a[6]

        tm4[12] = -a[1] * a[6]  * a[12] + a[1] * a[8]  * a[10] + a[5]  * a[2] * a[12] - a[5]  * a[4] * a[10] - a[9]  * a[2] * a[8]  + a[9]  * a[4] * a[6]

        tm4[13] = -a[5] * a[10] * a[15] + a[5] * a[11] * a[14] + a[9]  * a[6] * a[15] - a[9]  * a[7] * a[14] - a[13] * a[6] * a[11] + a[13] * a[7] * a[10]

        tm4[14] =  a[1] * a[10] * a[15] - a[1] * a[11] * a[14] - a[9]  * a[2] * a[15] + a[9]  * a[3] * a[14] + a[13] * a[2] * a[11] - a[13] * a[3] * a[10]

        tm4[15] = -a[1] * a[6]  * a[15] + a[1] * a[7]  * a[14] + a[5]  * a[2] * a[15] - a[5]  * a[3] * a[14] - a[13] * a[2] * a[7]  + a[13] * a[3] * a[6]

        tm4[16] =  a[1] * a[6]  * a[11] - a[1] * a[7]  * a[10] - a[5]  * a[2] * a[11] + a[5]  * a[3] * a[10] + a[9]  * a[2] * a[7]  - a[9]  * a[3] * a[6]

        for i=1, 16 do

                out[i] = tm4[i]

        end

        local det = a[1] * out[1] + a[2] * out[5] + a[3] * out[9] + a[4] * out[13]

        if det == 0 then return a end

        det = 1 / det

        for i = 1, 16 do

                out[i] = out[i] * det

        end

        return out

end

-- @treturn mat4 out

function mat4.scale(out, a, s)

        identity(tmp)

        tmp[1]  = s.x

        tmp[6]  = s.y

        tmp[11] = s.z

        return out:mul(tmp, a)

end

-- @treturn mat4 out

function mat4.rotate(out, a, angle, axis)

        if type(angle) == "table" or type(angle) == "cdata" then

                angle, axis = angle:to_angle_axis()

        end

        local l = axis:len()

        if l == 0 then

                return a

        end

        local x, y, z = axis.x / l, axis.y / l, axis.z / l

        local c = cos(angle)

        local s = sin(angle)

        identity(tmp)

        tmp[1]  = x * x * (1 - c) + c

        tmp[2]  = y * x * (1 - c) + z * s

        tmp[3]  = x * z * (1 - c) - y * s

        tmp[5]  = x * y * (1 - c) - z * s

        tmp[6]  = y * y * (1 - c) + c

        tmp[7]  = y * z * (1 - c) + x * s

        tmp[9]  = x * z * (1 - c) + y * s

        tmp[10] = y * z * (1 - c) - x * s

        tmp[11] = z * z * (1 - c) + c

        return out:mul(tmp, a)

end

--- Translate a matrix.

-- @treturn mat4 out

function mat4.translate(out, a, t)

        identity(tmp)

        tmp[13] = t.x

        tmp[14] = t.y

        tmp[15] = t.z

        return out:mul(tmp, a)

end

--- Shear a matrix.

-- @treturn mat4 out

function mat4.shear(out, a, yx, zx, xy, zy, xz, yz)

        identity(tmp)

        tmp[2]  = yx or 0

        tmp[3]  = zx or 0

        tmp[5]  = xy or 0

        tmp[7]  = zy or 0

        tmp[9]  = xz or 0

        tmp[10] = yz or 0

        return out:mul(tmp, a)

end

--- Reflect a matrix across a plane.

-- @tparam vec3 normal The (normalized!) normal vector of the plane

function mat4.reflect(out, a, position, normal)

        local nx, ny, nz = normal:unpack()

        local d = -position:dot(normal)

        tmp[1] = 1 - 2 * nx ^ 2

        tmp[2] = 2 * nx * ny

        tmp[3] = -2 * nx * nz

        tmp[4] = 0

        tmp[5] = -2 * nx * ny

        tmp[6] = 1 - 2 * ny ^ 2

        tmp[7] = -2 * ny * nz

        tmp[8] = 0

        tmp[9] = -2 * nx * nz

        tmp[10] = -2 * ny * nz

        tmp[11] = 1 - 2 * nz ^ 2

        tmp[12] = 0

        tmp[13] = -2 * nx * d

        tmp[14] = -2 * ny * d

        tmp[15] = -2 * nz * d

        tmp[16] = 1

        return out:mul(tmp, a)

end

--- Transform matrix to look at a point.

-- @treturn mat4 out

function mat4.look_at(out, a, eye, look_at, up)

        local z_axis = (eye - look_at):normalize()

        local x_axis = up:cross(z_axis):normalize()

        local y_axis = z_axis:cross(x_axis)

        out[1] = x_axis.x

        out[2] = y_axis.x

        out[3] = z_axis.x

        out[4] = 0

        out[5] = x_axis.y

        out[6] = y_axis.y

        out[7] = z_axis.y

        out[8] = 0

        out[9] = x_axis.z

        out[10] = y_axis.z

        out[11] = z_axis.z

        out[12] = 0

        out[13] = -out[  1]*eye.x - out[4+1]*eye.y - out[8+1]*eye.z

        out[14] = -out[  2]*eye.x - out[4+2]*eye.y - out[8+2]*eye.z

        out[15] = -out[  3]*eye.x - out[4+3]*eye.y - out[8+3]*eye.z

        out[16] = -out[  4]*eye.x - out[4+4]*eye.y - out[8+4]*eye.z + 1

  return out

end

--- Transpose a matrix.

-- @treturn mat4 out

function mat4.transpose(out, a)

        tm4[1]  = a[1]

        tm4[2]  = a[5]

        tm4[3]  = a[9]

        tm4[4]  = a[13]

        tm4[5]  = a[2]

        tm4[6]  = a[6]

        tm4[7]  = a[10]

        tm4[8]  = a[14]

        tm4[9]  = a[3]

        tm4[10] = a[7]

        tm4[11] = a[11]

        tm4[12] = a[15]

        tm4[13] = a[4]

        tm4[14] = a[8]

        tm4[15] = a[12]

        tm4[16] = a[16]

        for i=1, 16 do

                out[i] = tm4[i]

        end

        return out

end

-- https://github.com/g-truc/glm/blob/master/glm/gtc/matrix_transform.inl#L518

-- @treturn vec3 win

function mat4.project(obj, view, projection, viewport)

        local position = { obj.x, obj.y, obj.z, 1 }

        mat4.mul_vec4(position, view,       position)

        mat4.mul_vec4(position, projection, position)

        position[1] = position[1] / position[4] * 0.5 + 0.5

        position[2] = position[2] / position[4] * 0.5 + 0.5

        position[3] = position[3] / position[4] * 0.5 + 0.5

        position[1] = position[1] * viewport[3] + viewport[1]

        position[2] = position[2] * viewport[4] + viewport[2]

        return vec3(position[1], position[2], position[3])

end

-- https://github.com/g-truc/glm/blob/master/glm/gtc/matrix_transform.inl#L544

-- @treturn vec3 obj

function mat4.unproject(win, view, projection, viewport)

        local position = { win.x, win.y, win.z, 1 }

        position[1] = (position[1] - viewport[1]) / viewport[3]

        position[2] = (position[2] - viewport[2]) / viewport[4]

        position[1] = position[1] * 2 - 1

        position[2] = position[2] * 2 - 1

        position[3] = position[3] * 2 - 1

        tmp:mul(projection, view):invert(tmp)

        mat4.mul_vec4(position, tmp, position)

        position[1] = position[1] / position[4]

        position[2] = position[2] / position[4]

        position[3] = position[3] / position[4]

        return vec3(position[1], position[2], position[3])

end

--- Return a boolean showing if a table is or is not a mat4.

-- @treturn boolean is_mat4

function mat4.is_mat4(a)

        if type(a) == "cdata" then

                return ffi.istype("cpml_mat4", a)

        end

        if type(a) ~= "table" then

                return false

        end

        for i = 1, 16 do

                if type(a[i]) ~= "number" then

                        return false

                end

        end

        return true

end

--- Return whether any component is NaN

-- @treturn boolean if any component is NaN

function vec2.has_nan(a)

        for i=1, 16 do

                if private.is_nan(a[i]) then

                        return true

-- @treturn string formatted

function mat4.to_string(a)

        local str = "[ "

        for i = 1, 16 do

                str = str .. string.format("%+0.3f", a[i])

                if i < 16 then

                        str = str .. ", "

                end

        end

        str = str .. " ]"

        return str

end

--- Convert a matrix to row vec4s.

-- @treturn table vec4s

function mat4.to_vec4s(a)

        return {

                { a[1],  a[2],  a[3],  a[4]  },

                { a[5],  a[6],  a[7],  a[8]  },

                { a[9],  a[10], a[11], a[12] },

                { a[13], a[14], a[15], a[16] }

        }

end

--- Convert a matrix to col vec4s.

-- @treturn table vec4s

function mat4.to_vec4s_cols(a)

        return {

                { a[1], a[5], a[9],  a[13] },

                { a[2], a[6], a[10], a[14] },

                { a[3], a[7], a[11], a[15] },

                { a[4], a[8], a[12], a[16] }

        }

end

-- http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/

-- @treturn quat out

function mat4.to_quat(a)

        identity(tmp):transpose(a)

        local w     = sqrt(1 + tmp[1] + tmp[6] + tmp[11]) / 2

        local scale = w * 4

        local q     = quat.new(

                tmp[10] - tmp[7] / scale,

                tmp[3]  - tmp[9] / scale,

                tmp[5]  - tmp[2] / scale,

                w

        )

        return q:normalize(q)

end

-- http://www.crownandcutlass.com/features/technicaldetails/frustum.html

-- @treturn frustum out

function mat4.to_frustum(a, infinite)

        local t

        local frustum = {}

        -- Extract the LEFT plane

        frustum.left   = {}

        frustum.left.a = a[4]  + a[1]

        frustum.left.b = a[8]  + a[5]

        frustum.left.c = a[12] + a[9]

        frustum.left.d = a[16] + a[13]

        -- Normalize the result

        t = sqrt(frustum.left.a * frustum.left.a + frustum.left.b * frustum.left.b + frustum.left.c * frustum.left.c)

        frustum.left.a = frustum.left.a / t

        frustum.left.b = frustum.left.b / t

        frustum.left.c = frustum.left.c / t

        frustum.left.d = frustum.left.d / t

        -- Extract the RIGHT plane

        frustum.right   = {}

        frustum.right.a = a[4]  - a[1]

        frustum.right.b = a[8]  - a[5]

        frustum.right.c = a[12] - a[9]

        frustum.right.d = a[16] - a[13]

        -- Normalize the result

        t = sqrt(frustum.right.a * frustum.right.a + frustum.right.b * frustum.right.b + frustum.right.c * frustum.right.c)

        frustum.right.a = frustum.right.a / t

        frustum.right.b = frustum.right.b / t

        frustum.right.c = frustum.right.c / t

        frustum.right.d = frustum.right.d / t

        -- Extract the BOTTOM plane

        frustum.bottom   = {}

        frustum.bottom.a = a[4]  + a[2]

        frustum.bottom.b = a[8]  + a[6]

        frustum.bottom.c = a[12] + a[10]

        frustum.bottom.d = a[16] + a[14]

        -- Normalize the result

        t = sqrt(frustum.bottom.a * frustum.bottom.a + frustum.bottom.b * frustum.bottom.b + frustum.bottom.c * frustum.bottom.c)

        frustum.bottom.a = frustum.bottom.a / t

        frustum.bottom.b = frustum.bottom.b / t

        frustum.bottom.c = frustum.bottom.c / t

        frustum.bottom.d = frustum.bottom.d / t

        -- Extract the TOP plane

        frustum.top   = {}

        frustum.top.a = a[4]  - a[2]

        frustum.top.b = a[8]  - a[6]