miduo_client/Assets/ManagedResources/~Lua/UnityEngine/Quaternion.lua

616 lines
14 KiB
Lua

--------------------------------------------------------------------------------
-- Copyright (c) 2015 , 蒙占志(topameng) topameng@gmail.com
-- All rights reserved.
-- Use, modification and distribution are subject to the "MIT License"
--------------------------------------------------------------------------------
local math = math
local sin = math.sin
local cos = math.cos
local acos = math.acos
local asin = math.asin
local sqrt = math.sqrt
local min = math.min
local max = math.max
local sign = math.sign
local atan2 = math.atan2
local clamp = Mathf.Clamp
local abs = math.abs
local setmetatable = setmetatable
local getmetatable = getmetatable
local rawget = rawget
local rawset = rawset
local Vector3 = Vector3
local rad2Deg = Mathf.Rad2Deg
local halfDegToRad = 0.5 * Mathf.Deg2Rad
local _forward = Vector3.forward
local _up = Vector3.up
local _next = { 2, 3, 1 }
local Quaternion = {}
local get = tolua.initget(Quaternion)
Quaternion.__index = function(t, k)
local var = rawget(Quaternion, k)
if var == nil then
var = rawget(get, k)
if var ~= nil then
return var(t)
end
end
return var
end
Quaternion.__newindex = function(t, name, k)
if name == "eulerAngles" then
t:SetEuler(k)
else
rawset(t, name, k)
end
end
function Quaternion.New(x, y, z, w)
local t = {x = x or 0, y = y or 0, z = z or 0, w = w or 0}
setmetatable(t, Quaternion)
return t
end
local _new = Quaternion.New
Quaternion.__call = function(t, x, y, z, w)
local t = {x = x or 0, y = y or 0, z = z or 0, w = w or 0}
setmetatable(t, Quaternion)
return t
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:Clone()
return _new(self.x, self.y, self.z, self.w)
end
function Quaternion:Get()
return self.x, self.y, self.z, self.w
end
function Quaternion.Dot(a, b)
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
end
function Quaternion.Angle(a, b)
local dot = Quaternion.Dot(a, b)
if dot < 0 then dot = -dot end
return acos(min(dot, 1)) * 2 * 57.29578
end
function Quaternion.AngleAxis(angle, axis)
local normAxis = axis:Normalize()
angle = angle * halfDegToRad
local s = sin(angle)
local w = cos(angle)
local x = normAxis.x * s
local y = normAxis.y * s
local z = normAxis.z * s
return _new(x,y,z,w)
end
function Quaternion.Equals(a, b)
return a.x == b.x and a.y == b.y and a.z == b.z and a.w == b.w
end
function Quaternion.Euler(x, y, z)
if y == nil and z == nil then
y = x.y
z = x.z
x = x.x
end
x = x * 0.0087266462599716
y = y * 0.0087266462599716
z = z * 0.0087266462599716
local sinX = sin(x)
x = cos(x)
local sinY = sin(y)
y = cos(y)
local sinZ = sin(z)
z = cos(z)
local q = {x = y * sinX * z + sinY * x * sinZ, y = sinY * x * z - y * sinX * sinZ, z = y * x * sinZ - sinY * sinX * z, w = y * x * z + sinY * sinX * sinZ}
setmetatable(q, Quaternion)
return q
end
function Quaternion:SetEuler(x, y, z)
if y == nil and z == nil then
y = x.y
z = x.z
x = x.x
end
x = x * 0.0087266462599716
y = y * 0.0087266462599716
z = z * 0.0087266462599716
local sinX = sin(x)
local cosX = cos(x)
local sinY = sin(y)
local cosY = cos(y)
local sinZ = sin(z)
local cosZ = cos(z)
self.w = cosY * cosX * cosZ + sinY * sinX * sinZ
self.x = cosY * sinX * cosZ + sinY * cosX * sinZ
self.y = sinY * cosX * cosZ - cosY * sinX * sinZ
self.z = cosY * cosX * sinZ - sinY * sinX * cosZ
return self
end
function Quaternion:Normalize()
local quat = self:Clone()
quat:SetNormalize()
return quat
end
function Quaternion:SetNormalize()
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 / sqrt(n)
self.x = self.x * n
self.y = self.y * n
self.z = self.z * n
self.w = self.w * n
end
end
--产生一个新的从from到to的四元数
function Quaternion.FromToRotation(from, to)
local quat = Quaternion.New()
quat:SetFromToRotation(from, to)
return quat
end
--设置当前四元数为 from 到 to的旋转, 注意from和to同 forward平行会同unity不一致
function Quaternion:SetFromToRotation1(from, to)
local v0 = from:Normalize()
local v1 = to:Normalize()
local d = Vector3.Dot(v0, v1)
if d > -1 + 1e-6 then
local s = sqrt((1+d) * 2)
local invs = 1 / s
local c = Vector3.Cross(v0, v1) * invs
self:Set(c.x, c.y, c.z, s * 0.5)
elseif d > 1 - 1e-6 then
return _new(0, 0, 0, 1)
else
local axis = Vector3.Cross(Vector3.right, v0)
if axis:SqrMagnitude() < 1e-6 then
axis = Vector3.Cross(Vector3.forward, v0)
end
self:Set(axis.x, axis.y, axis.z, 0)
return self
end
return self
end
local function MatrixToQuaternion(rot, quat)
local trace = rot[1][1] + rot[2][2] + rot[3][3]
if trace > 0 then
local s = 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:SetNormalize()
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 = _next[i]
local k = _next[j]
local t = rot[i][i] - rot[j][j] - rot[k][k] + 1
local s = 0.5 / 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:SetNormalize()
end
end
function Quaternion:SetFromToRotation(from, to)
from = from:Normalize()
to = to:Normalize()
local e = Vector3.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/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 = Vector3.Cross(from, to)
local h = (1 - e) / Vector3.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
function Quaternion:Inverse()
local quat = Quaternion.New()
quat.x = -self.x
quat.y = -self.y
quat.z = -self.z
quat.w = self.w
return quat
end
function Quaternion.Lerp(q1, q2, t)
t = clamp(t, 0, 1)
local q = {x = 0, y = 0, z = 0, w = 1}
if Quaternion.Dot(q1, q2) < 0 then
q.x = q1.x + t * (-q2.x -q1.x)
q.y = q1.y + t * (-q2.y -q1.y)
q.z = q1.z + t * (-q2.z -q1.z)
q.w = q1.w + t * (-q2.w -q1.w)
else
q.x = q1.x + (q2.x - q1.x) * t
q.y = q1.y + (q2.y - q1.y) * t
q.z = q1.z + (q2.z - q1.z) * t
q.w = q1.w + (q2.w - q1.w) * t
end
Quaternion.SetNormalize(q)
setmetatable(q, Quaternion)
return q
end
function Quaternion.LookRotation(forward, up)
local mag = forward:Magnitude()
if mag < 1e-6 then
error("error input forward to Quaternion.LookRotation"..tostring(forward))
return nil
end
forward = forward / mag
up = up or _up
local right = Vector3.Cross(up, forward)
right:SetNormalize()
up = Vector3.Cross(forward, right)
right = Vector3.Cross(up, forward)
--[[ local quat = _new(0,0,0,1)
local rot =
{
{right.x, up.x, forward.x},
{right.y, up.y, forward.y},
{right.z, up.z, forward.z},
}
MatrixToQuaternion(rot, quat)
return quat--]]
local t = right.x + up.y + forward.z
if t > 0 then
local x, y, z, w
t = t + 1
local s = 0.5 / sqrt(t)
w = s * t
x = (up.z - forward.y) * s
y = (forward.x - right.z) * s
z = (right.y - up.x) * s
local ret = _new(x, y, z, w)
ret:SetNormalize()
return ret
else
local rot =
{
{right.x, up.x, forward.x},
{right.y, up.y, forward.y},
{right.z, up.z, forward.z},
}
local q = {0, 0, 0}
local i = 1
if up.y > right.x then
i = 2
end
if forward.z > rot[i][i] then
i = 3
end
local j = _next[i]
local k = _next[j]
local t = rot[i][i] - rot[j][j] - rot[k][k] + 1
local s = 0.5 / 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
local ret = _new(q[1], q[2], q[3], w)
ret:SetNormalize()
return ret
end
end
function Quaternion:SetIdentity()
self.x = 0
self.y = 0
self.z = 0
self.w = 1
end
local function UnclampedSlerp(q1, q2, t)
local dot = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w
if dot < 0 then
dot = -dot
q2 = setmetatable({x = -q2.x, y = -q2.y, z = -q2.z, w = -q2.w}, Quaternion)
end
if dot < 0.95 then
local angle = acos(dot)
local invSinAngle = 1 / sin(angle)
local t1 = sin((1 - t) * angle) * invSinAngle
local t2 = sin(t * angle) * invSinAngle
q1 = {x = q1.x * t1 + q2.x * t2, y = q1.y * t1 + q2.y * t2, z = q1.z * t1 + q2.z * t2, w = q1.w * t1 + q2.w * t2}
setmetatable(q1, Quaternion)
return q1
else
q1 = {x = q1.x + t * (q2.x - q1.x), y = q1.y + t * (q2.y - q1.y), z = q1.z + t * (q2.z - q1.z), w = q1.w + t * (q2.w - q1.w)}
Quaternion.SetNormalize(q1)
setmetatable(q1, Quaternion)
return q1
end
end
function Quaternion.Slerp(from, to, t)
if t < 0 then
t = 0
elseif t > 1 then
t = 1
end
return UnclampedSlerp(from, to, t)
end
function Quaternion.RotateTowards(from, to, maxDegreesDelta)
local angle = Quaternion.Angle(from, to)
if angle == 0 then
return to
end
local t = min(1, maxDegreesDelta / angle)
return UnclampedSlerp(from, to, t)
end
local function Approximately(f0, f1)
return abs(f0 - f1) < 1e-6
end
function Quaternion:ToAngleAxis()
local angle = 2 * acos(self.w)
if Approximately(angle, 0) then
return angle * 57.29578, Vector3.New(1, 0, 0)
end
local div = 1 / sqrt(1 - sqrt(self.w))
return angle * 57.29578, Vector3.New(self.x * div, self.y * div, self.z * div)
end
local pi = Mathf.PI
local half_pi = pi * 0.5
local two_pi = 2 * pi
local negativeFlip = -0.0001
local positiveFlip = two_pi - 0.0001
local function SanitizeEuler(euler)
if euler.x < negativeFlip then
euler.x = euler.x + two_pi
elseif euler.x > positiveFlip then
euler.x = euler.x - two_pi
end
if euler.y < negativeFlip then
euler.y = euler.y + two_pi
elseif euler.y > positiveFlip then
euler.y = euler.y - two_pi
end
if euler.z < negativeFlip then
euler.z = euler.z + two_pi
elseif euler.z > positiveFlip then
euler.z = euler.z + two_pi
end
end
--from http://www.geometrictools.com/Documentation/EulerAngles.pdf
--Order of rotations: YXZ
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.New( -asin(check),
atan2(2 * (x * z + w * y), 1 - 2 * (x * x + y * y)),
atan2(2 * (x * y + w * z), 1 - 2 * (x * x + z * z)))
SanitizeEuler(v)
v:Mul(rad2Deg)
return v
else
local v = Vector3.New(half_pi, atan2(2 * (x * y - w * z), 1 - 2 * (y * y + z * z)), 0)
SanitizeEuler(v)
v:Mul(rad2Deg)
return v
end
else
local v = Vector3.New(-half_pi, atan2(-2 * (x * y - w * z), 1 - 2 * (y * y + z * z)), 0)
SanitizeEuler(v)
v:Mul(rad2Deg)
return v
end
end
function Quaternion:Forward()
return self:MulVec3(_forward)
end
function Quaternion.MulVec3(self, point)
local vec = Vector3.New()
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
Quaternion.__mul = function(lhs, rhs)
if Quaternion == getmetatable(rhs) then
return Quaternion.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 == getmetatable(rhs) then
return lhs:MulVec3(rhs)
end
end
Quaternion.__unm = function(q)
return Quaternion.New(-q.x, -q.y, -q.z, -q.w)
end
Quaternion.__eq = function(lhs,rhs)
return Quaternion.Dot(lhs, rhs) > 0.999999
end
Quaternion.__tostring = function(self)
return "["..self.x..","..self.y..","..self.z..","..self.w.."]"
end
get.identity = function() return _new(0, 0, 0, 1) end
get.eulerAngles = Quaternion.ToEulerAngles
UnityEngine.Quaternion = Quaternion
setmetatable(Quaternion, Quaternion)
return Quaternion