63 lines
1.9 KiB
Lua
63 lines
1.9 KiB
Lua
|
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;
|