QuatHelper = {}
-- 四元数结构体
QuatHelper = {}
QuatHelper.__index = QuatHelper

-- 创建四元数
function QuatHelper.new(W, X, Y, Z)
    return setmetatable({W = W, X = X, Y = Y, Z = Z}, QuatHelper)
end

function QuatHelper.RotToQuat(Rot)
    return STExtraBlueprintFunctionLibrary.RotToQuat(Rot)
end

function QuatHelper.QuatToRot(q)
    return STExtraBlueprintFunctionLibrary.QuatToRot(q)
end

-- 四元数乘法 (q1 * q2)
function QuatHelper.mul(q1, q2)
    return QuatHelper.new(
        q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z,
        q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y,
        q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X,
        q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W
    )
end

-- 四元数与向量乘法 (q * v)，假设向量v是{x, y, z}
function QuatHelper.mulVec(q, v)
    local qv = QuatHelper.new(0, v.x, v.y, v.z)
    local q_conjugate = QuatHelper.conjugate(q)
    local result = QuatHelper.mul(QuatHelper.mul(q, qv), q_conjugate)
    return {x = result.X, y = result.Y, z = result.Z}
end

-- 四元数单位化
function QuatHelper.normalize(q)
    local magnitude = math.sqrt(q.W * q.W + q.X * q.X + q.Y * q.Y + q.Z * q.Z)
    return QuatHelper.new(q.W / magnitude, q.X / magnitude, q.Y / magnitude, q.Z / magnitude)
end

-- 四元数共轭
function QuatHelper.conjugate(q)
    return QuatHelper.new(q.W, -q.X, -q.Y, -q.Z)
end

-- 四元数逆
function QuatHelper.inverse(q)
    local conjugate = QuatHelper.conjugate(q)
    local magnitude_squared = q.W * q.W + q.X * q.X + q.Y * q.Y + q.Z * q.Z
    return QuatHelper.new(conjugate.W / magnitude_squared, conjugate.X / magnitude_squared, conjugate.Y / magnitude_squared, conjugate.Z / magnitude_squared)
end

-- 生成四元数字符串表示 (用于调试)
function QuatHelper.toString(q)
    return string.format("(%f, %f, %f, %f)", q.W, q.X, q.Y, q.Z)
end




return QuatHelper;