Add quat.cross, fix quat.rotate, cleanup.
This commit is contained in:
Colby Klein
parent
6a0e56d799
commit
7cdca018aa
163
modules/quat.lua
163
modules/quat.lua
@ -22,65 +22,77 @@ 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
|
||||
local arg = { select(1, ...) or 0, select(2, ...) or 0, select(3, ...) or 0, select(4, ...) or 0 }
|
||||
local n = select('#', ...)
|
||||
if n == 1 and type(arg[1]) == "table" 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[1].w or arg[1][4]
|
||||
-- four numbers
|
||||
elseif #arg == 4 then
|
||||
elseif n == 4 then
|
||||
x = arg[1]
|
||||
y = arg[2]
|
||||
z = arg[3]
|
||||
w = arg[4]
|
||||
-- real number plus vector
|
||||
elseif #arg == 2 then
|
||||
elseif n == 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
|
||||
print(string.format("%s %s %s %s", select(1, ...), select(2, ...), select(3, ...), select(4, ...)))
|
||||
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)
|
||||
return setmetatable({ x = x or 0, y = y or 0, z = z or 0, w = w or 1 }, 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)
|
||||
function quaternion.__add(a, b)
|
||||
if type(b) == "number" then
|
||||
return new(a.x, a.y, a.z, a.w + b)
|
||||
end
|
||||
|
||||
return new(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w)
|
||||
end
|
||||
|
||||
function quaternion:__sub(q)
|
||||
return new(self.x - q.x, self.y - q.y, self.z - q.z, self.w - q.w)
|
||||
function quaternion.__sub(a, b)
|
||||
return new(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.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)
|
||||
function quaternion.__mul(a, b)
|
||||
-- quat * number
|
||||
if type(b) == "number" then
|
||||
return a:scale(b)
|
||||
-- quat * quat
|
||||
elseif type(b) == "table" and b.w then
|
||||
local x, y, z, w
|
||||
|
||||
x = a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y
|
||||
y = a.y * b.w + a.w * b.y + a.z * b.x - a.x * b.z
|
||||
z = a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x
|
||||
w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
|
||||
|
||||
return new(x, y, z, w)
|
||||
else
|
||||
local qv = vec3(a.x, a.y, a.z)
|
||||
local uv = qv:cross(b)
|
||||
local uuv = qv:cross(uv)
|
||||
|
||||
return b + ((uv * a.w) + uuv) * 2
|
||||
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()
|
||||
function quaternion.__div(a, b)
|
||||
if type(b) == "number" then
|
||||
return a:scale(1 / b)
|
||||
elseif type(b) == "table" then
|
||||
return a * b:reciprocal()
|
||||
end
|
||||
end
|
||||
|
||||
@ -94,38 +106,41 @@ function quaternion:__pow(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
|
||||
function quaternion.__eq(a, b)
|
||||
if a.x ~= b.x or a.y ~= b.y or a.z ~= b.z or a.w ~= b.w then
|
||||
return false
|
||||
end
|
||||
|
||||
return true
|
||||
end
|
||||
|
||||
function quaternion:__tostring()
|
||||
return "("..tonumber(self.x)..","..tonumber(self.y)..","..tonumber(self.z)..","..tonumber(self.w)..")"
|
||||
return string.format("(%0.3f,%0.3f,%0.3f,%0.3f)", self.x, self.y, self.z, self.x)
|
||||
end
|
||||
|
||||
function quaternion.unit()
|
||||
return new(0,0,0,1)
|
||||
return new(0, 0, 0, 1)
|
||||
end
|
||||
|
||||
function quaternion:to_axis_angle()
|
||||
local tmp = self
|
||||
if tmp.w > 1 then
|
||||
tmp = tmp:normalize()
|
||||
if self.w > 1 then
|
||||
self = self:normalize()
|
||||
end
|
||||
local angle = 2 * math.acos(tmp.w)
|
||||
local s = math.sqrt(1-tmp.w*tmp.w)
|
||||
|
||||
local angle = 2 * math.acos(self.w)
|
||||
local s = math.sqrt(1-self.w*self.w)
|
||||
local x, y, z
|
||||
|
||||
if s < constants.FLT_EPSILON then
|
||||
x = tmp.x
|
||||
y = tmp.y
|
||||
z = tmp.z
|
||||
x = self.x
|
||||
y = self.y
|
||||
z = self.z
|
||||
else
|
||||
x = tmp.x / s -- normalize axis
|
||||
y = tmp.y / s
|
||||
z = tmp.z / s
|
||||
x = self.x / s -- normalize axis
|
||||
y = self.y / s
|
||||
z = self.z / s
|
||||
end
|
||||
|
||||
return angle, { x, y, z }
|
||||
end
|
||||
|
||||
@ -135,6 +150,7 @@ function quaternion:is_zero()
|
||||
if self.x ~= 0 or self.y ~= 0 or self.z ~= 0 or self.w ~= 0 then
|
||||
return false
|
||||
end
|
||||
|
||||
return true
|
||||
end
|
||||
|
||||
@ -144,6 +160,7 @@ function quaternion:is_real()
|
||||
if self.x ~= 0 or self.y ~= 0 or self.z ~= 0 then
|
||||
return false
|
||||
end
|
||||
|
||||
return true
|
||||
end
|
||||
|
||||
@ -153,6 +170,7 @@ function quaternion:is_imaginary()
|
||||
if self.w ~= 0 then
|
||||
return false
|
||||
end
|
||||
|
||||
return true
|
||||
end
|
||||
|
||||
@ -161,6 +179,15 @@ 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.cross(a, b)
|
||||
return new(
|
||||
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
|
||||
a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
|
||||
a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
|
||||
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
|
||||
)
|
||||
end
|
||||
|
||||
-- Length of a quaternion
|
||||
function quaternion:len()
|
||||
return math.sqrt(self:len2())
|
||||
@ -168,7 +195,7 @@ 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
|
||||
return self:dot(self)
|
||||
end
|
||||
|
||||
-- Normalize a quaternion to have length 1
|
||||
@ -177,13 +204,14 @@ function quaternion:normalize()
|
||||
error("Unable to normalize a zero-length quaternion")
|
||||
return false
|
||||
end
|
||||
local l = 1/self:len()
|
||||
|
||||
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)
|
||||
return new(self.x * l, self.y * l, self.z * l, self.w * l)
|
||||
end
|
||||
|
||||
-- Conjugation (corresponds to inverting a rotation)
|
||||
@ -191,15 +219,21 @@ function quaternion:conjugate()
|
||||
return new(-self.x, -self.y, -self.z, self.w)
|
||||
end
|
||||
|
||||
function quaternion:inverse()
|
||||
return self:conjugate():normalize()
|
||||
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)
|
||||
q = q:scale(1 / l)
|
||||
|
||||
return q
|
||||
end
|
||||
|
||||
@ -222,15 +256,19 @@ 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
|
||||
local function rotate(angle, axis)
|
||||
local len = axis:len()
|
||||
|
||||
if math.abs(len - 1) > 0.001 then
|
||||
axis.x = axis.x / len
|
||||
axis.y = axis.y / len
|
||||
axis.z = axis.z / len
|
||||
end
|
||||
|
||||
local sin = math.sin(angle * 0.5)
|
||||
local cos = math.cos(angle * 0.5)
|
||||
|
||||
return new(axis.x * sin, axis.y * sin, axis.z * sin, cos)
|
||||
end
|
||||
|
||||
function quaternion:to_euler()
|
||||
@ -260,9 +298,10 @@ function quaternion:to_euler()
|
||||
roll = 0
|
||||
return pitch, yaw, roll
|
||||
end
|
||||
yaw = math.atan2(2*self.y*self.w-2*self.x*self.z , sqx - sqy - sqz + sqw)
|
||||
|
||||
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)
|
||||
roll = math.atan2(2*self.x*self.w-2*self.y*self.z , -sqx + sqy - sqz + sqw)
|
||||
|
||||
return pitch, roll, yaw
|
||||
end
|
||||
@ -298,5 +337,5 @@ end
|
||||
|
||||
-- return quaternion
|
||||
-- the module
|
||||
return setmetatable({ new = new },
|
||||
return setmetatable({ new = new, rotate = rotate },
|
||||
{ __call = function(_, ...) return new(...) end })
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user