299 lines
6.7 KiB
Lua
299 lines
6.7 KiB
Lua
-- quaternions
|
|
-- Author: Andrew Stacey
|
|
-- Website: http://www.math.ntnu.no/~stacey/HowDidIDoThat/iPad/Codea.html
|
|
-- Licence: CC0 http://wiki.creativecommons.org/CC0
|
|
|
|
--[[
|
|
This is a class for handling quaternion numbers. It was originally
|
|
designed as a way of encoding rotations of 3 dimensional space.
|
|
--]]
|
|
|
|
local current_folder = (...):gsub('%.[^%.]+$', '') .. "."
|
|
local constants = require(current_folder .. "constants")
|
|
local vec3 = require(current_folder .. "vec3")
|
|
local quaternion = {}
|
|
quaternion.__index = quaternion
|
|
|
|
--[[
|
|
A quaternion can either be specified by giving the four coordinates as
|
|
real numbers or by giving the scalar part and the vector part.
|
|
--]]
|
|
|
|
local function new(...)
|
|
local x, y, z, w
|
|
-- copy
|
|
local arg = {...}
|
|
if #arg == 1 and type(arg[1]) == "table" then
|
|
x = arg[1].x
|
|
y = arg[1].y
|
|
z = arg[1].z
|
|
w = arg[1].w
|
|
-- four numbers
|
|
elseif #arg == 4 then
|
|
x = arg[1]
|
|
y = arg[2]
|
|
z = arg[3]
|
|
w = arg[4]
|
|
-- real number plus vector
|
|
elseif #arg == 2 then
|
|
x = arg[1].x or arg[1][1]
|
|
y = arg[1].y or arg[1][2]
|
|
z = arg[1].z or arg[1][3]
|
|
w = arg[2]
|
|
else
|
|
error("Incorrect number of arguments to quaternion")
|
|
end
|
|
|
|
return setmetatable({ x = x or 0, y = y or 0, z = z or 0, w = w or 0 }, quaternion)
|
|
end
|
|
|
|
function quaternion:__add(q)
|
|
if type(q) == "number" then
|
|
return new(self.x, self.y, self.z, self.w + q)
|
|
else
|
|
return new(self.x + q.x, self.y + q.y, self.z + q.z, self.w + q.w)
|
|
end
|
|
end
|
|
|
|
function quaternion:__sub(q)
|
|
return new(self.x - q.x, self.y - q.y, self.z - q.z, self.w - q.w)
|
|
end
|
|
|
|
function quaternion:__unm()
|
|
return self:scale(-1)
|
|
end
|
|
|
|
function quaternion:__mul(q)
|
|
if type(q) == "number" then
|
|
return self:scale(q)
|
|
elseif type(q) == "table" then
|
|
local x,y,z,w
|
|
x = self.w * q.x + self.x * q.w + self.y * q.z - self.z * q.y
|
|
y = self.w * q.y - self.x * q.z + self.y * q.w + self.z * q.x
|
|
z = self.w * q.z + self.x * q.y - self.y * q.x + self.z * q.w
|
|
w = self.w * q.w - self.x * q.x - self.y * q.y - self.z * q.z
|
|
return new(x,y,z,w)
|
|
end
|
|
end
|
|
|
|
function quaternion:__div(q)
|
|
if type(q) == "number" then
|
|
return self:scale(1/q)
|
|
elseif type(q) == "table" then
|
|
return self * q:reciprocal()
|
|
end
|
|
end
|
|
|
|
function quaternion:__pow(n)
|
|
if n == 0 then
|
|
return self.unit()
|
|
elseif n > 0 then
|
|
return self * self^(n-1)
|
|
elseif n < 0 then
|
|
return self:reciprocal()^(-n)
|
|
end
|
|
end
|
|
|
|
function quaternion:__eq(q)
|
|
if self.x ~= q.x or self.y ~= q.y or self.z ~= q.z or self.w ~= q.w then
|
|
return false
|
|
end
|
|
return true
|
|
end
|
|
|
|
function quaternion:__tostring()
|
|
return "("..tonumber(self.x)..","..tonumber(self.y)..","..tonumber(self.z)..","..tonumber(self.w)..")"
|
|
end
|
|
|
|
function quaternion.unit()
|
|
return new(0,0,0,1)
|
|
end
|
|
|
|
function quaternion:to_axis_angle()
|
|
local tmp = self
|
|
if tmp.w > 1 then
|
|
tmp = tmp:normalize()
|
|
end
|
|
local angle = 2 * math.acos(tmp.w)
|
|
local s = math.sqrt(1-tmp.w*tmp.w)
|
|
local x, y, z
|
|
if s < constants.FLT_EPSILON then
|
|
x = tmp.x
|
|
y = tmp.y
|
|
z = tmp.z
|
|
else
|
|
x = tmp.x / s -- normalize axis
|
|
y = tmp.y / s
|
|
z = tmp.z / s
|
|
end
|
|
return angle, { x, y, z }
|
|
end
|
|
|
|
-- Test if we are zero
|
|
function quaternion:is_zero()
|
|
-- are we the zero vector
|
|
if self.x ~= 0 or self.y ~= 0 or self.z ~= 0 or self.w ~= 0 then
|
|
return false
|
|
end
|
|
return true
|
|
end
|
|
|
|
-- Test if we are real
|
|
function quaternion:is_real()
|
|
-- are we the zero vector
|
|
if self.x ~= 0 or self.y ~= 0 or self.z ~= 0 then
|
|
return false
|
|
end
|
|
return true
|
|
end
|
|
|
|
-- Test if the real part is zero
|
|
function quaternion:is_imaginary()
|
|
-- are we the zero vector
|
|
if self.w ~= 0 then
|
|
return false
|
|
end
|
|
return true
|
|
end
|
|
|
|
-- The dot product of two quaternions
|
|
function quaternion.dot(a, b)
|
|
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
|
|
end
|
|
|
|
-- Length of a quaternion
|
|
function quaternion:len()
|
|
return math.sqrt(self:len2())
|
|
end
|
|
|
|
-- Length squared of a quaternion
|
|
function quaternion:len2()
|
|
return self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
|
|
end
|
|
|
|
-- Normalize a quaternion to have length 1
|
|
function quaternion:normalize()
|
|
if self:is_zero() then
|
|
error("Unable to normalize a zero-length quaternion")
|
|
return false
|
|
end
|
|
local l = 1/self:len()
|
|
return self:scale(l)
|
|
end
|
|
|
|
-- Scale the quaternion
|
|
function quaternion:scale(l)
|
|
return new(self.x * l,self.y * l,self.z * l, self.w * l)
|
|
end
|
|
|
|
-- Conjugation (corresponds to inverting a rotation)
|
|
function quaternion:conjugate()
|
|
return new(-self.x, -self.y, -self.z, self.w)
|
|
end
|
|
|
|
-- Reciprocal: 1/q
|
|
function quaternion:reciprocal()
|
|
if self.is_zero() then
|
|
error("Cannot reciprocate a zero quaternion")
|
|
return false
|
|
end
|
|
local q = self:conjugate()
|
|
local l = self:len2()
|
|
q = q:scale(1/l)
|
|
return q
|
|
end
|
|
|
|
-- Returns the real part
|
|
function quaternion:real()
|
|
return self.w
|
|
end
|
|
|
|
-- Returns the vector (imaginary) part as a Vec3 object
|
|
function quaternion:to_vec3()
|
|
return vec3(self.x, self.y, self.z)
|
|
end
|
|
|
|
--[[
|
|
Converts a rotation to a quaternion. The first argument is the angle
|
|
to rotate, the second must specify an axis as a Vec3 object.
|
|
--]]
|
|
|
|
function quaternion:rotate(a,axis)
|
|
local q,c,s
|
|
q = new(axis, 0)
|
|
q = q:normalize()
|
|
c = math.cos(a)
|
|
s = math.sin(a)
|
|
q = q:scale(s)
|
|
q = q + c
|
|
return q
|
|
end
|
|
|
|
function quaternion:to_euler()
|
|
local sqx = self.x*self.x
|
|
local sqy = self.y*self.y
|
|
local sqz = self.z*self.z
|
|
local sqw = self.w*self.w
|
|
|
|
-- if normalised is one, otherwise is correction factor
|
|
local unit = sqx + sqy + sqz + sqw
|
|
local test = self.x*self.y + self.z*self.w
|
|
|
|
local pitch, yaw, roll
|
|
|
|
-- singularity at north pole
|
|
if test > 0.499*unit then
|
|
yaw = 2 * math.atan2(self.x,self.w)
|
|
pitch = math.pi/2
|
|
roll = 0
|
|
return pitch, yaw, roll
|
|
end
|
|
|
|
-- singularity at south pole
|
|
if test < -0.499*unit then
|
|
yaw = -2 * math.atan2(self.x,self.w)
|
|
pitch = -math.pi/2
|
|
roll = 0
|
|
return pitch, yaw, roll
|
|
end
|
|
yaw = math.atan2(2*self.y*self.w-2*self.x*self.z , sqx - sqy - sqz + sqw)
|
|
pitch = math.asin(2*test/unit)
|
|
roll = math.atan2(2*self.x*self.w-2*self.y*self.z , -sqx + sqy - sqz + sqw)
|
|
|
|
return pitch, roll, yaw
|
|
end
|
|
|
|
-- http://keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/
|
|
-- non-normalized rotations do not work out for quats!
|
|
function quaternion.lerp(a, b, s)
|
|
local v = a + (b - a) * s
|
|
return v:normalize()
|
|
end
|
|
|
|
-- http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
|
|
function quaternion.slerp(a, b, s)
|
|
local function clamp(n, low, high) return math.min(math.max(n, low), high) end
|
|
local dot = a:dot(b)
|
|
|
|
-- http://www.gamedev.net/topic/312067-shortest-slerp-path/#entry2995591
|
|
if dot < 0 then
|
|
a = -a
|
|
dot = -dot
|
|
end
|
|
|
|
if dot > constants.DOT_THRESHOLD then
|
|
return quaternion.lerp(a, b, s)
|
|
end
|
|
|
|
clamp(dot, -1, 1)
|
|
local theta = math.acos(dot) * s
|
|
local c = (b - a * dot):normalize()
|
|
|
|
return a * math.cos(theta) + c * math.sin(theta)
|
|
end
|
|
|
|
-- return quaternion
|
|
-- the module
|
|
return setmetatable({ new = new },
|
|
{ __call = function(_, ...) return new(...) end })
|