-------------------------------------------------------------------------------- -- 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