------------------------------------------------
--作者： xihan
--日期： 2019-05-10
--文件： Quaternion.lua
--模块： Quaternion
--描述： Quaternion的运算
------------------------------------------------
local Vector3 = require("Common.CustomLib.Utility.Vector3")

local L_Rawget = rawget
local L_Rawset = rawset
local L_Setmetatable = setmetatable
local L_Getmetatable = getmetatable
local L_Math = math
local L_Sin = L_Math.sin
local L_Cos = L_Math.cos
local L_Acos = L_Math.acos
local L_Asin = L_Math.asin
local L_Sqrt = L_Math.sqrt
local L_Min = L_Math.min
local L_Max = L_Math.max
local L_Sign = L_Math.Sign
local L_Atan = L_Math.atan
local L_Clamp = L_Math.Clamp
local L_Abs = L_Math.abs
local L_Rad2Deg = L_Math.Rad2Deg
local L_HalfDegToRad = 0.5 * L_Math.Deg2Rad
local L_Forward = Vector3.forward
local L_Up = Vector3.up
local L_Next = {2, 3, 1}

local Quaternion = {}

local function New(x, y, z, w)
    return L_Setmetatable({x = x or 0, y = y or 0, z = z or 0, w = w or 0}, Quaternion)
end

local function NewByEuler(x, y, z)
    if y == nil and z == nil then
        y = x.y
        z = x.z
        x = x.x
    end

    x = x * L_HalfDegToRad
    y = y * L_HalfDegToRad
    z = z * L_HalfDegToRad

    local _sinX = L_Sin(x)
    local _cosX = L_Cos(x)
    local _sinY = L_Sin(y)
    local _cosY = L_Cos(y)
    local _sinZ = L_Sin(z)
    local _cosZ = L_Cos(z)

    local _w = _cosY * _cosX * _cosZ + _sinY * _sinX * _sinZ
    x = _cosY * _sinX * _cosZ + _sinY * _cosX * _sinZ
    y = _sinY * _cosX * _cosZ - _cosY * _sinX * _sinZ
    z = _cosY * _cosX * _sinZ - _sinY * _sinX * _cosZ
    return New(x, y, z, _w)
end

function Quaternion:Set(x, y, z, w)
    self.x = x or 0
    self.y = y or 0
    self.z = z or 0
    self.w = w or 0
end

function Quaternion:Get()
    return self.x, self.y, self.z, self.w
end

function Quaternion:Clone()
    return New(self.x, self.y, self.z, self.w)
end

function Quaternion:GetNormalize()
    return self:Clone():Normalize()
end

function Quaternion:Normalize()
    local _n = self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w

    if _n ~= 1 and _n > 0 then
        _n = 1 / L_Sqrt(_n)
        self.x = self.x * _n
        self.y = self.y * _n
        self.z = self.z * _n
        self.w = self.w * _n
    end
    return self
end

function Quaternion:SetIdentity()
    self.x = 0
    self.y = 0
    self.z = 0
    self.w = 1
end

function Quaternion:Inverse()
    local _quat = New()
    _quat.x = -self.x
    _quat.y = -self.y
    _quat.z = -self.z
    _quat.w = self.w
    return _quat
end

local function Approximately(f0, f1)
    return L_Abs(f0 - f1) < 1e-6
end

function Quaternion:ToAngleAxis()
    local _angle = 2 * L_Acos(self.w)

    if Approximately(_angle, 0) then
        return _angle * 57.29578, Vector3(1, 0, 0)
    end

    local _div = 1 / L_Sqrt(1 - L_Sqrt(self.w))
    return _angle * 57.29578, Vector3(self.x * _div, self.y * _div, self.z * _div)
end

local L_PI = L_Math.pi
local L_Half_PI = L_PI * 0.5
local L_Two_PI = 2 * L_PI
local L_NegativeFlip = -0.0001
local L_PositiveFlip = L_Two_PI - 0.0001

local function SanitizeEuler(euler)
    if euler.x < L_NegativeFlip then
        euler.x = euler.x + L_Two_PI
    elseif euler.x > L_PositiveFlip then
        euler.x = euler.x - L_Two_PI
    end

    if euler.y < L_NegativeFlip then
        euler.y = euler.y + L_Two_PI
    elseif euler.y > L_PositiveFlip then
        euler.y = euler.y - L_Two_PI
    end

    if euler.z < L_NegativeFlip then
        euler.z = euler.z + L_Two_PI
    elseif euler.z > L_PositiveFlip then
        euler.z = euler.z + L_Two_PI
    end
end

function Quaternion:ToEulerAngles()
    local _x = self.x
    local _y = self.y
    local _z = self.z
    local _w = self.w

    local _check = 2 * (_y * _z - _w * _x)

    if _check < 0.999 then
        if _check > -0.999 then
            local _v =
                Vector3(
                -L_Asin(_check),
                L_Atan(2 * (_x * _z + _w * _y), 1 - 2 * (_x * _x + _y * _y)),
                L_Atan(2 * (_x * _y + _w * _z), 1 - 2 * (_x * _x + _z * _z))
            )
            SanitizeEuler(_v)
            _v:Mul(L_Rad2Deg)
            return _v
        else
            local _v = Vector3(L_Half_PI, L_Atan(2 * (_x * _y - _w * _z), 1 - 2 * (_y * _y + _z * _z)), 0)
            SanitizeEuler(_v)
            _v:Mul(L_Rad2Deg)
            return _v
        end
    else
        local _v = Vector3(-L_Half_PI, L_Atan(-2 * (_x * _y - _w * _z), 1 - 2 * (_y * _y + _z * _z)), 0)
        SanitizeEuler(_v)
        _v:Mul(L_Rad2Deg)
        return _v
    end
end

function Quaternion:Forward()
    return self:MulVec3(Vector3.forward)
end

function Quaternion:MulVec3(point)
    local _vec = Vector3()

    local _num = self.x * 2
    local _num2 = self.y * 2
    local _num3 = self.z * 2
    local _num4 = self.x * _num
    local _num5 = self.y * _num2
    local _num6 = self.z * _num3
    local _num7 = self.x * _num2
    local _num8 = self.x * _num3
    local _num9 = self.y * _num3
    local _num10 = self.w * _num
    local _num11 = self.w * _num2
    local _num12 = self.w * _num3

    _vec.x = (((1 - (_num5 + _num6)) * point.x) + ((_num7 - _num12) * point.y)) + ((_num8 + _num11) * point.z)
    _vec.y = (((_num7 + _num12) * point.x) + ((1 - (_num4 + _num6)) * point.y)) + ((_num9 - _num10) * point.z)
    _vec.z = (((_num8 - _num11) * point.x) + ((_num9 + _num10) * point.y)) + ((1 - (_num4 + _num5)) * point.z)
    return _vec
end

local function MatrixToQuaternion(rot, quat)
    local _trace = rot[1][1] + rot[2][2] + rot[3][3]

    if _trace > 0 then
        local _s = L_Sqrt(_trace + 1)
        quat.w = 0.5 * _s
        _s = 0.5 / _s
        quat.x = (rot[3][2] - rot[2][3]) * _s
        quat.y = (rot[1][3] - rot[3][1]) * _s
        quat.z = (rot[2][1] - rot[1][2]) * _s
         --]]
        quat:Normalize()
    else
        local _i = 1
        local _q = {0, 0, 0}

        if rot[2][2] > rot[1][1] then
            _i = 2
        end

        if rot[3][3] > rot[_i][_i] then
            _i = 3
        end

        local _j = L_Next[_i]
        local _k = L_Next[_j]

        local _t = rot[_i][_i] - rot[_j][_j] - rot[_k][_k] + 1
        local _s = 0.5 / L_Sqrt(_t)
        _q[_i] = _s * _t
        local _w = (rot[_k][_j] - rot[_j][_k]) * _s
        _q[_j] = (rot[_j][_i] + rot[_i][_j]) * _s
        _q[_k] = (rot[_k][_i] + rot[_i][_k]) * _s

        quat:Set(_q[1], _q[2], _q[3], _w)
        quat:Normalize()
    end
end

--创建一个从formDirection到toDirection的Quaternion实例
function Quaternion:GetFromToRotation(from, to)
    local _q = self:Clone()
    _q:FromToRotation(from, to)
    return _q
end
--从from到to的四元数
function Quaternion:FromToRotation(from, to)
    from = from:Normalize()
    to = to:Normalize()

    local _e = Utils.Dot(from, to)

    if _e > 1 - 1e-6 then
        self:Set(0, 0, 0, 1)
    elseif _e < -1 + 1e-6 then
        local _left = {0, from.z, from.y}
        local _mag = _left[2] * _left[2] + _left[3] * _left[3] --+ left[1] * left[1] = 0

        if _mag < 1e-6 then
            _left[1] = -from.z
            _left[2] = 0
            _left[3] = from.x
            _mag = _left[1] * _left[1] + _left[3] * _left[3]
        end

        local _invlen = 1 / L_Sqrt(_mag)
        _left[1] = _left[1] * _invlen
        _left[2] = _left[2] * _invlen
        _left[3] = _left[3] * _invlen

        local _up = {0, 0, 0}
        _up[1] = _left[2] * from.z - _left[3] * from.y
        _up[2] = _left[3] * from.x - _left[1] * from.z
        _up[3] = _left[1] * from.y - _left[2] * from.x

        local _fxx = -from.x * from.x
        local _fyy = -from.y * from.y
        local _fzz = -from.z * from.z

        local _fxy = -from.x * from.y
        local _fxz = -from.x * from.z
        local _fyz = -from.y * from.z

        local _uxx = _up[1] * _up[1]
        local _uyy = _up[2] * _up[2]
        local _uzz = _up[3] * _up[3]
        local _uxy = _up[1] * _up[2]
        local _uxz = _up[1] * _up[3]
        local _uyz = _up[2] * _up[3]

        local _lxx = -_left[1] * _left[1]
        local _lyy = -_left[2] * _left[2]
        local _lzz = -_left[3] * _left[3]
        local _lxy = -_left[1] * _left[2]
        local _lxz = -_left[1] * _left[3]
        local _lyz = -_left[2] * _left[3]

        local _rot = {
            {_fxx + _uxx + _lxx, _fxy + _uxy + _lxy, _fxz + _uxz + _lxz},
            {_fxy + _uxy + _lxy, _fyy + _uyy + _lyy, _fyz + _uyz + _lyz},
            {_fxz + _uxz + _lxz, _fyz + _uyz + _lyz, _fzz + _uzz + _lzz}
        }

        MatrixToQuaternion(_rot, self)
    else
        local _v = Utils.Cross(from, to)
        local _h = (1 - _e) / Utils.Dot(_v, _v)

        local _hx = _h * _v.x
        local _hz = _h * _v.z
        local _hxy = _hx * _v.y
        local _hxz = _hx * _v.z
        local _hyz = _hz * _v.y

        local _rot = {
            {_e + _hx * _v.x, _hxy - _v.z, _hxz + _v.y},
            {_hxy + _v.z, _e + _h * _v.y * _v.y, _hyz - _v.x},
            {_hxz - _v.y, _hyz + _v.x, _e + _hz * _v.z}
        }
        MatrixToQuaternion(_rot, self)
    end
end

Quaternion.__mul =
    function(lhs, rhs)
    if Quaternion == L_Getmetatable(rhs) then
        return New(
            (((lhs.w * rhs.x) + (lhs.x * rhs.w)) + (lhs.y * rhs.z)) - (lhs.z * rhs.y),
            (((lhs.w * rhs.y) + (lhs.y * rhs.w)) + (lhs.z * rhs.x)) - (lhs.x * rhs.z),
            (((lhs.w * rhs.z) + (lhs.z * rhs.w)) + (lhs.x * rhs.y)) - (lhs.y * rhs.x),
            (((lhs.w * rhs.w) - (lhs.x * rhs.x)) - (lhs.y * rhs.y)) - (lhs.z * rhs.z)
        )
    elseif Vector3 == L_Getmetatable(rhs) then
        return lhs:MulVec3(rhs)
    end
end

Quaternion.__unm = function(q)
    return New(-q.x, -q.y, -q.z, -q.w)
end

Quaternion.__eq = function(lhs, rhs)
    return Utils.Dot(lhs, rhs) > 0.999999
end

Quaternion.__tostring = function(self)
    return "[" .. self.x .. "," .. self.y .. "," .. self.z .. "," .. self.w .. "]"
end

Quaternion.__index = function(t, k)
    return L_Rawget(Quaternion, k)
end

local L_Metatable = {
    identity = function()
        return New(0, 0, 0, 1)
    end
}

L_Metatable.__index = function(t, k)
    if L_Metatable[k] then
        return L_Metatable[k]()
    end
end

L_Metatable.__call = function(t, x, y, z, w)
    if type(x) == "number" or type(x) == "nil" then
        return New(x, y, z, w)
    else
        if x.w then
            return New(x.x, x.y, x.z, x.w)
        else
            return NewByEuler(x.x, x.y, x.z)
        end
    end
end

L_Setmetatable(Quaternion, L_Metatable)
return Quaternion