616 lines
14 KiB
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 |