472 lines
11 KiB
Lua
472 lines
11 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 acos = math.acos
|
|
local sqrt = math.sqrt
|
|
local max = math.max
|
|
local min = math.min
|
|
local clamp = Mathf.Clamp
|
|
local cos = math.cos
|
|
local sin = math.sin
|
|
local abs = math.abs
|
|
local sign = Mathf.Sign
|
|
local setmetatable = setmetatable
|
|
local rawset = rawset
|
|
local rawget = rawget
|
|
local type = type
|
|
|
|
local rad2Deg = 57.295779513082
|
|
local deg2Rad = 0.017453292519943
|
|
|
|
local Vector3 = {}
|
|
local get = tolua.initget(Vector3)
|
|
|
|
Vector3.__index = function(t,k)
|
|
local var = rawget(Vector3, k)
|
|
|
|
if var == nil then
|
|
var = rawget(get, k)
|
|
|
|
if var ~= nil then
|
|
return var(t)
|
|
end
|
|
end
|
|
|
|
return var
|
|
end
|
|
|
|
function Vector3.New(x, y, z)
|
|
local t = {x = x or 0, y = y or 0, z = z or 0}
|
|
setmetatable(t, Vector3)
|
|
return t
|
|
end
|
|
|
|
local _new = Vector3.New
|
|
|
|
Vector3.__call = function(t,x,y,z)
|
|
local t = {x = x or 0, y = y or 0, z = z or 0}
|
|
setmetatable(t, Vector3)
|
|
return t
|
|
end
|
|
|
|
function Vector3:Set(x,y,z)
|
|
self.x = x or 0
|
|
self.y = y or 0
|
|
self.z = z or 0
|
|
end
|
|
|
|
function Vector3.Get(v)
|
|
return v.x, v.y, v.z
|
|
end
|
|
|
|
function Vector3:Clone()
|
|
return setmetatable({x = self.x, y = self.y, z = self.z}, Vector3)
|
|
end
|
|
|
|
function Vector3.Distance(va, vb)
|
|
return sqrt((va.x - vb.x)^2 + (va.y - vb.y)^2 + (va.z - vb.z)^2)
|
|
end
|
|
|
|
function Vector3.Dot(lhs, rhs)
|
|
return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z
|
|
end
|
|
|
|
function Vector3.Lerp(from, to, t)
|
|
t = clamp(t, 0, 1)
|
|
return _new(from.x + (to.x - from.x) * t, from.y + (to.y - from.y) * t, from.z + (to.z - from.z) * t)
|
|
end
|
|
|
|
function Vector3:Magnitude()
|
|
return sqrt(self.x * self.x + self.y * self.y + self.z * self.z)
|
|
end
|
|
|
|
function Vector3.Max(lhs, rhs)
|
|
return _new(max(lhs.x, rhs.x), max(lhs.y, rhs.y), max(lhs.z, rhs.z))
|
|
end
|
|
|
|
function Vector3.Min(lhs, rhs)
|
|
return _new(min(lhs.x, rhs.x), min(lhs.y, rhs.y), min(lhs.z, rhs.z))
|
|
end
|
|
|
|
function Vector3.Normalize(v)
|
|
local x,y,z = v.x, v.y, v.z
|
|
local num = sqrt(x * x + y * y + z * z)
|
|
|
|
if num > 1e-5 then
|
|
return setmetatable({x = x / num, y = y / num, z = z / num}, Vector3)
|
|
end
|
|
|
|
return setmetatable({x = 0, y = 0, z = 0}, Vector3)
|
|
end
|
|
|
|
function Vector3:SetNormalize()
|
|
local num = sqrt(self.x * self.x + self.y * self.y + self.z * self.z)
|
|
|
|
if num > 1e-5 then
|
|
self.x = self.x / num
|
|
self.y = self.y / num
|
|
self.z = self.z /num
|
|
else
|
|
self.x = 0
|
|
self.y = 0
|
|
self.z = 0
|
|
end
|
|
|
|
return self
|
|
end
|
|
|
|
function Vector3:SqrMagnitude()
|
|
return self.x * self.x + self.y * self.y + self.z * self.z
|
|
end
|
|
|
|
local dot = Vector3.Dot
|
|
|
|
function Vector3.Angle(from, to)
|
|
return acos(clamp(dot(from:Normalize(), to:Normalize()), -1, 1)) * rad2Deg
|
|
end
|
|
|
|
function Vector3:ClampMagnitude(maxLength)
|
|
if self:SqrMagnitude() > (maxLength * maxLength) then
|
|
self:SetNormalize()
|
|
self:Mul(maxLength)
|
|
end
|
|
|
|
return self
|
|
end
|
|
|
|
|
|
function Vector3.OrthoNormalize(va, vb, vc)
|
|
va:SetNormalize()
|
|
vb:Sub(vb:Project(va))
|
|
vb:SetNormalize()
|
|
|
|
if vc == nil then
|
|
return va, vb
|
|
end
|
|
|
|
vc:Sub(vc:Project(va))
|
|
vc:Sub(vc:Project(vb))
|
|
vc:SetNormalize()
|
|
return va, vb, vc
|
|
end
|
|
|
|
function Vector3.MoveTowards(current, target, maxDistanceDelta)
|
|
local delta = target - current
|
|
local sqrDelta = delta:SqrMagnitude()
|
|
local sqrDistance = maxDistanceDelta * maxDistanceDelta
|
|
|
|
if sqrDelta > sqrDistance then
|
|
local magnitude = sqrt(sqrDelta)
|
|
|
|
if magnitude > 1e-6 then
|
|
delta:Mul(maxDistanceDelta / magnitude)
|
|
delta:Add(current)
|
|
return delta
|
|
else
|
|
return current:Clone()
|
|
end
|
|
end
|
|
|
|
return target:Clone()
|
|
end
|
|
|
|
function ClampedMove(lhs, rhs, clampedDelta)
|
|
local delta = rhs - lhs
|
|
|
|
if delta > 0 then
|
|
return lhs + min(delta, clampedDelta)
|
|
else
|
|
return lhs - min(-delta, clampedDelta)
|
|
end
|
|
end
|
|
|
|
local overSqrt2 = 0.7071067811865475244008443621048490
|
|
|
|
local function OrthoNormalVector(vec)
|
|
local res = _new()
|
|
|
|
if abs(vec.z) > overSqrt2 then
|
|
local a = vec.y * vec.y + vec.z * vec.z
|
|
local k = 1 / sqrt (a)
|
|
res.x = 0
|
|
res.y = -vec.z * k
|
|
res.z = vec.y * k
|
|
else
|
|
local a = vec.x * vec.x + vec.y * vec.y
|
|
local k = 1 / sqrt (a)
|
|
res.x = -vec.y * k
|
|
res.y = vec.x * k
|
|
res.z = 0
|
|
end
|
|
|
|
return res
|
|
end
|
|
|
|
function Vector3.RotateTowards(current, target, maxRadiansDelta, maxMagnitudeDelta)
|
|
local len1 = current:Magnitude()
|
|
local len2 = target:Magnitude()
|
|
|
|
if len1 > 1e-6 and len2 > 1e-6 then
|
|
local from = current / len1
|
|
local to = target / len2
|
|
local cosom = dot(from, to)
|
|
|
|
if cosom > 1 - 1e-6 then
|
|
return Vector3.MoveTowards (current, target, maxMagnitudeDelta)
|
|
elseif cosom < -1 + 1e-6 then
|
|
local axis = OrthoNormalVector(from)
|
|
local q = Quaternion.AngleAxis(maxRadiansDelta * rad2Deg, axis)
|
|
local rotated = q:MulVec3(from)
|
|
local delta = ClampedMove(len1, len2, maxMagnitudeDelta)
|
|
rotated:Mul(delta)
|
|
return rotated
|
|
else
|
|
local angle = acos(cosom)
|
|
local axis = Vector3.Cross(from, to)
|
|
axis:SetNormalize ()
|
|
local q = Quaternion.AngleAxis(min(maxRadiansDelta, angle) * rad2Deg, axis)
|
|
local rotated = q:MulVec3(from)
|
|
local delta = ClampedMove(len1, len2, maxMagnitudeDelta)
|
|
rotated:Mul(delta)
|
|
return rotated
|
|
end
|
|
end
|
|
|
|
return Vector3.MoveTowards(current, target, maxMagnitudeDelta)
|
|
end
|
|
|
|
function Vector3.SmoothDamp(current, target, currentVelocity, smoothTime)
|
|
local maxSpeed = Mathf.Infinity
|
|
local deltaTime = Time.deltaTime
|
|
smoothTime = max(0.0001, smoothTime)
|
|
local num = 2 / smoothTime
|
|
local num2 = num * deltaTime
|
|
local num3 = 1 / (1 + num2 + 0.48 * num2 * num2 + 0.235 * num2 * num2 * num2)
|
|
local vector2 = target:Clone()
|
|
local maxLength = maxSpeed * smoothTime
|
|
local vector = current - target
|
|
vector:ClampMagnitude(maxLength)
|
|
target = current - vector
|
|
local vec3 = (currentVelocity + (vector * num)) * deltaTime
|
|
currentVelocity = (currentVelocity - (vec3 * num)) * num3
|
|
local vector4 = target + (vector + vec3) * num3
|
|
|
|
if Vector3.Dot(vector2 - current, vector4 - vector2) > 0 then
|
|
vector4 = vector2
|
|
currentVelocity:Set(0,0,0)
|
|
end
|
|
|
|
return vector4, currentVelocity
|
|
end
|
|
|
|
function Vector3.Scale(a, b)
|
|
local x = a.x * b.x
|
|
local y = a.y * b.y
|
|
local z = a.z * b.z
|
|
return _new(x, y, z)
|
|
end
|
|
|
|
function Vector3.Cross(lhs, rhs)
|
|
local x = lhs.y * rhs.z - lhs.z * rhs.y
|
|
local y = lhs.z * rhs.x - lhs.x * rhs.z
|
|
local z = lhs.x * rhs.y - lhs.y * rhs.x
|
|
return _new(x,y,z)
|
|
end
|
|
|
|
function Vector3:Equals(other)
|
|
return self.x == other.x and self.y == other.y and self.z == other.z
|
|
end
|
|
|
|
function Vector3.Reflect(inDirection, inNormal)
|
|
local num = -2 * dot(inNormal, inDirection)
|
|
inNormal = inNormal * num
|
|
inNormal:Add(inDirection)
|
|
return inNormal
|
|
end
|
|
|
|
|
|
function Vector3.Project(vector, onNormal)
|
|
local num = onNormal:SqrMagnitude()
|
|
|
|
if num < 1.175494e-38 then
|
|
return _new(0,0,0)
|
|
end
|
|
|
|
local num2 = dot(vector, onNormal)
|
|
local v3 = onNormal:Clone()
|
|
v3:Mul(num2/num)
|
|
return v3
|
|
end
|
|
|
|
function Vector3.ProjectOnPlane(vector, planeNormal)
|
|
local v3 = Vector3.Project(vector, planeNormal)
|
|
v3:Mul(-1)
|
|
v3:Add(vector)
|
|
return v3
|
|
end
|
|
|
|
function Vector3.Slerp(from, to, t)
|
|
local omega, sinom, scale0, scale1
|
|
|
|
if t <= 0 then
|
|
return from:Clone()
|
|
elseif t >= 1 then
|
|
return to:Clone()
|
|
end
|
|
|
|
local v2 = to:Clone()
|
|
local v1 = from:Clone()
|
|
local len2 = to:Magnitude()
|
|
local len1 = from:Magnitude()
|
|
v2:Div(len2)
|
|
v1:Div(len1)
|
|
|
|
local len = (len2 - len1) * t + len1
|
|
local cosom = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z
|
|
|
|
if cosom > 1 - 1e-6 then
|
|
scale0 = 1 - t
|
|
scale1 = t
|
|
elseif cosom < -1 + 1e-6 then
|
|
local axis = OrthoNormalVector(from)
|
|
local q = Quaternion.AngleAxis(180.0 * t, axis)
|
|
local v = q:MulVec3(from)
|
|
v:Mul(len)
|
|
return v
|
|
else
|
|
omega = acos(cosom)
|
|
sinom = sin(omega)
|
|
scale0 = sin((1 - t) * omega) / sinom
|
|
scale1 = sin(t * omega) / sinom
|
|
end
|
|
|
|
v1:Mul(scale0)
|
|
v2:Mul(scale1)
|
|
v2:Add(v1)
|
|
v2:Mul(len)
|
|
return v2
|
|
end
|
|
|
|
|
|
function Vector3:Mul(q)
|
|
if type(q) == "number" then
|
|
self.x = self.x * q
|
|
self.y = self.y * q
|
|
self.z = self.z * q
|
|
else
|
|
self:MulQuat(q)
|
|
end
|
|
|
|
return self
|
|
end
|
|
|
|
function Vector3:Div(d)
|
|
self.x = self.x / d
|
|
self.y = self.y / d
|
|
self.z = self.z / d
|
|
|
|
return self
|
|
end
|
|
|
|
function Vector3:Add(vb)
|
|
self.x = self.x + vb.x
|
|
self.y = self.y + vb.y
|
|
self.z = self.z + vb.z
|
|
|
|
return self
|
|
end
|
|
|
|
function Vector3:Sub(vb)
|
|
self.x = self.x - vb.x
|
|
self.y = self.y - vb.y
|
|
self.z = self.z - vb.z
|
|
|
|
return self
|
|
end
|
|
|
|
function Vector3:MulQuat(quat)
|
|
local num = quat.x * 2
|
|
local num2 = quat.y * 2
|
|
local num3 = quat.z * 2
|
|
local num4 = quat.x * num
|
|
local num5 = quat.y * num2
|
|
local num6 = quat.z * num3
|
|
local num7 = quat.x * num2
|
|
local num8 = quat.x * num3
|
|
local num9 = quat.y * num3
|
|
local num10 = quat.w * num
|
|
local num11 = quat.w * num2
|
|
local num12 = quat.w * num3
|
|
|
|
local x = (((1 - (num5 + num6)) * self.x) + ((num7 - num12) * self.y)) + ((num8 + num11) * self.z)
|
|
local y = (((num7 + num12) * self.x) + ((1 - (num4 + num6)) * self.y)) + ((num9 - num10) * self.z)
|
|
local z = (((num8 - num11) * self.x) + ((num9 + num10) * self.y)) + ((1 - (num4 + num5)) * self.z)
|
|
|
|
self:Set(x, y, z)
|
|
return self
|
|
end
|
|
|
|
function Vector3.AngleAroundAxis (from, to, axis)
|
|
from = from - Vector3.Project(from, axis)
|
|
to = to - Vector3.Project(to, axis)
|
|
local angle = Vector3.Angle (from, to)
|
|
return angle * (Vector3.Dot (axis, Vector3.Cross (from, to)) < 0 and -1 or 1)
|
|
end
|
|
|
|
|
|
Vector3.__tostring = function(self)
|
|
return "["..self.x..","..self.y..","..self.z.."]"
|
|
end
|
|
|
|
Vector3.__div = function(va, d)
|
|
return _new(va.x / d, va.y / d, va.z / d)
|
|
end
|
|
|
|
Vector3.__mul = function(va, d)
|
|
if type(d) == "number" then
|
|
return _new(va.x * d, va.y * d, va.z * d)
|
|
else
|
|
local vec = va:Clone()
|
|
vec:MulQuat(d)
|
|
return vec
|
|
end
|
|
end
|
|
|
|
Vector3.__add = function(va, vb)
|
|
return _new(va.x + vb.x, va.y + vb.y, va.z + vb.z)
|
|
end
|
|
|
|
Vector3.__sub = function(va, vb)
|
|
return _new(va.x - vb.x, va.y - vb.y, va.z - vb.z)
|
|
end
|
|
|
|
Vector3.__unm = function(va)
|
|
return _new(-va.x, -va.y, -va.z)
|
|
end
|
|
|
|
Vector3.__eq = function(a,b)
|
|
local v = a - b
|
|
local delta = v:SqrMagnitude()
|
|
return delta < 1e-10
|
|
end
|
|
|
|
get.up = function() return _new(0,1,0) end
|
|
get.down = function() return _new(0,-1,0) end
|
|
get.right = function() return _new(1,0,0) end
|
|
get.left = function() return _new(-1,0,0) end
|
|
get.forward = function() return _new(0,0,1) end
|
|
get.back = function() return _new(0,0,-1) end
|
|
get.zero = function() return _new(0,0,0) end
|
|
get.one = function() return _new(1,1,1) end
|
|
|
|
get.magnitude = Vector3.Magnitude
|
|
get.normalized = Vector3.Normalize
|
|
get.sqrMagnitude= Vector3.SqrMagnitude
|
|
|
|
UnityEngine.Vector3 = Vector3
|
|
setmetatable(Vector3, Vector3)
|
|
return Vector3
|