249 lines
6.8 KiB
Lua
249 lines
6.8 KiB
Lua
local function RoundTo(val, num_decimal_points)
|
|
local pow = 1
|
|
|
|
for i = 1, num_decimal_points do
|
|
pow = pow * 10
|
|
end
|
|
|
|
return (math.Round(val * pow) / pow)
|
|
end
|
|
|
|
|
|
local AngMeta = FindMetaTable("Angle")
|
|
|
|
function AngMeta:ToDeg()
|
|
self.p = math.deg(self.p)
|
|
self.y = math.deg(self.y)
|
|
self.r = math.deg(self.r)
|
|
end
|
|
|
|
function AngMeta:ToRad()
|
|
self.p = math.rad(self.p)
|
|
self.y = math.rad(self.y)
|
|
self.r = math.rad(self.r)
|
|
end
|
|
|
|
function AngMeta:Quaternion(quaternion_output)
|
|
local ang = self * 1
|
|
ang:ToRad()
|
|
|
|
local cp = math.cos(ang.p * .5)
|
|
local cy = math.cos(ang.y * .5)
|
|
local cr = math.cos(ang.r * .5)
|
|
|
|
local sp = math.sin(ang.p * .5)
|
|
local sy = math.sin(ang.y * .5)
|
|
local sr = math.sin(ang.r * .5)
|
|
|
|
local cpcy = cp * cy;
|
|
local spsy = sp * sy;
|
|
|
|
if not quaternion_output then return Quaternion(Vector(sr * cpcy - cr * spsy,
|
|
cr * sp * cy + sr * cp * sy,
|
|
cr * cp * sy - sr * sp * cy),
|
|
cr * cpcy + sr * spsy) end
|
|
|
|
quaternion_output.Vec.x = sr * cpcy - cr * spsy
|
|
quaternion_output.Vec.y = cr * sp * cy + sr * cp * sy
|
|
quaternion_output.Vec.z = cr * cp * sy - sr * sp * cy
|
|
|
|
quaternion_output.Rotation = cr * cpcy + sr * spsy
|
|
end
|
|
|
|
local callmeta = {}
|
|
|
|
function callmeta:__call(vec, rot)
|
|
return Quaternion:New(vec, rot)
|
|
end
|
|
|
|
Quaternion = {}
|
|
setmetatable(Quaternion, callmeta)
|
|
|
|
local opsmeta = {}
|
|
opsmeta.__index = Quaternion
|
|
|
|
function Quaternion:New(vec, rot)
|
|
local obj = {}
|
|
--obj.__index = Quaternion
|
|
setmetatable(obj, opsmeta)
|
|
|
|
obj.Vec = vec or Vector(0, 0, 0)
|
|
obj.Rotation = math.rad(rot) or 1
|
|
|
|
return obj
|
|
end
|
|
|
|
function Quaternion:Reset(make_into_identity)
|
|
self.Vec.x = 0
|
|
self.Vec.y = 0
|
|
self.Vec.z = 0
|
|
|
|
self.Rotation = 1
|
|
end
|
|
|
|
function Quaternion:IsIdentity()
|
|
return tobool((self.Vec.x == 0) and
|
|
(self.Vec.y == 0) and
|
|
(self.Vec.z == 0) and
|
|
(self.Rotation == 1))
|
|
end
|
|
|
|
function opsmeta:__eq(quaternion, epsilon)
|
|
if not epsilon then
|
|
return tobool((self.Vec.x == quaternion.Vec.x) and
|
|
(self.Vec.y == quaternion.Vec.y) and
|
|
(self.Vec.z == quaternion.Vec.z) and
|
|
(self.Rotation == quaternion.Rotation))
|
|
end
|
|
|
|
return tobool((math.abs(self.Vec.x - quaternion.Vec.x) < epsilon) and
|
|
(math.abs(self.Vec.y - quaternion.Vec.y) < epsilon) and
|
|
(math.abs(self.Vec.z - quaternion.Vec.z) < epsilon) and
|
|
(math.abs(self.Rotation - quaternion.Rotation) < epsilon))
|
|
end
|
|
|
|
function opsmeta:__add(quaternion)
|
|
return Quaternion(Vector(self.Vec.x + quaternion.Vec.x, self.Vec.y + quaternion.Vec.y, self.Vec.z + quaternion.Vec.z), self.Rotation + quaternion.Rotation)
|
|
end
|
|
|
|
function opsmeta:__unm()
|
|
self.Vec = self.Vec * -1
|
|
end
|
|
|
|
function opsmeta:__mul(inval)
|
|
return ((type(inval) == "number") and self:MultiplyScalar(inval) or self:MultiplyQuaternion(inval))
|
|
end
|
|
|
|
function Quaternion:MultiplyQuaternion(quaternion)
|
|
local vec = Vector(self.Rotation * quaternion.Vec.x + self.Vec.x * quaternion.Rotation + self.Vec.y * quaternion.Vec.z - self.Vec.z * quaternion.Vec.y,
|
|
self.Rotation * quaternion.Vec.y + self.Vec.y * quaternion.Rotation + self.Vec.z * quaternion.Vec.x - self.Vec.x * quaternion.Vec.z,
|
|
self.Rotation * quaternion.Vec.z + self.Vec.z * quaternion.Rotation + self.Vec.x * quaternion.Vec.y - self.Vec.y * quaternion.Vec.x)
|
|
|
|
local rot = self.Rotation * quaternion.Rotation - self.Vec.x * quaternion.Vec.x - self.Vec.y * quaternion.Vec.y - self.Vec.z * quaternion.Vec.z
|
|
|
|
return Quaternion(vec, rot)
|
|
end
|
|
|
|
function Quaternion:MultiplyScalar(val)
|
|
return Quaternion(self.Vec * val, self.Rotation * val)
|
|
end
|
|
|
|
function Quaternion:SetAxis(vec, deg_rotation)
|
|
return self:SetAxisRad(vec, math.rad(deg_rotation))
|
|
end
|
|
|
|
function Quaternion:SetAxisRad(vec, rad_rotation)
|
|
self.Vec = self.Vec * math.sin(rad_rotation)
|
|
self.Rotation = math.cos(rad_rotation)
|
|
end
|
|
|
|
function Quaternion:Dot(quaternion)
|
|
return ((self.Vec.x * quaternion.Vec.x) + (self.Vec.y * quaternion.Vec.y) + (self.Vec.z * quaternion.Vec.z) + (self.Rotation * quaternion.Rotation))
|
|
end
|
|
|
|
function Quaternion:FromAngle(angle)
|
|
angle = angle * 1
|
|
angle:ToRad()
|
|
|
|
local cp = math.cos(angle.p * .5)
|
|
local cy = math.cos(angle.y * .5)
|
|
local cr = math.cos(angle.r * .5)
|
|
|
|
local sp = math.sin(angle.p * .5)
|
|
local sy = math.sin(angle.y * .5)
|
|
local sr = math.sin(angle.r * .5)
|
|
|
|
local cpcy = cp * cy;
|
|
local spsy = sp * sy;
|
|
|
|
self.Vec = Vector(sr * cpcy - cr * spsy,
|
|
cr * sp * cy + sr * cp * sy,
|
|
cr * cp * sy - sr * sp * cy)
|
|
|
|
self.Rotation = cr * cpcy + sr * spsy
|
|
end
|
|
|
|
function Quaternion:ToAngle(angle)
|
|
angle = angle or Angle()
|
|
|
|
local singularity_checks = (self.Vec.y * self.Vec.z) + (self.Vec.x * self.Rotation)
|
|
|
|
if singularity_checks > 0.499 then -- singularity at north pole
|
|
angle.p = math.pi * .5
|
|
angle.y = 2 * math.atan2(self.Vec.y, self.Rotation)
|
|
angle.r = 0
|
|
elseif singularity_checks < -0.499 then -- singularity at south pole
|
|
angle.p = math.pi * -.5
|
|
angle.y = -2 * math.atan2(self.Vec.y, self.Rotation)
|
|
angle.r = 0
|
|
else
|
|
local x_2 = 1 - (2 * self.Vec.x^2)
|
|
|
|
angle.p = math.asin( 2 * singularity_checks)
|
|
angle.y = math.atan2((2 * self.Vec.z * self.Rotation) - (2 * self.Vec.y * self.Vec.x), (x_2 - (2 * self.Vec.z^2)))
|
|
angle.r = math.atan2((2 * self.Vec.y * self.Rotation) - (2 * self.Vec.z * self.Vec.x), (x_2 - (2 * self.Vec.y^2)))
|
|
end
|
|
|
|
angle:ToDeg()
|
|
print(angle)
|
|
return angle
|
|
end
|
|
|
|
function Quaternion:Normalize()
|
|
local scale = (self.Vec.x ^ 2) + (self.Vec.y ^ 2) + (self.Vec.z ^ 2) + (self.Rotation ^ 2)
|
|
|
|
if (scale == 0) or (scale == 1.0) then return (scale == 1.0) end -- Because it might be a normalized already!
|
|
|
|
scale = 1 / math.sqrt(scale)
|
|
|
|
self.Vec.x = self.Vec.x * scale
|
|
self.Vec.y = self.Vec.y * scale
|
|
self.Vec.z = self.Vec.z * scale
|
|
|
|
self.Rotation = self.Rotation * scale
|
|
|
|
return true
|
|
end
|
|
|
|
function Quaternion:AimZAxis(point_a, point_b)
|
|
local vAim = (point_b - point_a):Normalize()
|
|
|
|
self.Vec.x = vAim.y
|
|
self.Vec.y = -vAim.x
|
|
self.Vec.z = 0
|
|
self.Rotation = 1 + vAim.z
|
|
|
|
if (self.Vec.x == 0) and (self.Vec.y == 0) and (self.Vec.z == 0) and (self.Rotation == 0) then -- can't norm this
|
|
return self:Reset()
|
|
else
|
|
return self:Normalize()
|
|
end
|
|
end
|
|
|
|
-- Creates a value from spherical linear interpolation
|
|
function QuaternionSlerp(start_quat, end_quat, perc) -- THIS IS SLOWER THEN NLERP!!!
|
|
if start_quat == end_quat then return start_quat end
|
|
|
|
local perc_a = 1 - perc
|
|
local perc_b = perc
|
|
|
|
local theta = math.acos(a.Dot(b));
|
|
local sin_theta = math.sin(theta);
|
|
|
|
if sin_theta > 0.001 then
|
|
perc_a = math.sin((1 - perc) * theta ) / sin_theta
|
|
perc_b = math.sin(perc * theta) / sin_theta
|
|
end
|
|
|
|
return ((a * perc_a) + (b * perc_b))
|
|
end
|
|
|
|
-- Unlike spherical interpolation, this does not rotate at a constant velocity, and it's faster to do
|
|
function QuaternionNLerp(start_quat, end_quat, perc)
|
|
if start_quat == end_quat then return start_quat end
|
|
|
|
local new_quat = (start_quat * 1) + (end_quat * perc)
|
|
new_quat:Normalize()
|
|
|
|
return new_quat
|
|
end
|