Refactored quat and added doc comments. Fixed some small typos and doc comments in vec2.

This commit is contained in:
Matthew Blanchard 2015-12-25 20:06:31 -05:00
parent c60a324010
commit 85655e65af
2 changed files with 252 additions and 201 deletions

View File

@ -1,87 +1,149 @@
--- A quaternion and associated utilities.
-- @module quat
local current_folder = (...):gsub('%.[^%.]+$', '') .. "."
local constants = require(current_folder .. "constants")
local vec3 = require(current_folder .. "vec3")
local ffi = require "ffi"
local DOT_THRESHOLD = constants.DOT_THRESHOLD
local FLT_EPSILON = constants.FLT_EPSILON
local abs = math.abs
local acos = math.acos
local asin = math.asin
local atan2 = math.atan2
local cos = math.cos
local sin = math.sin
local min = math.min
local max = math.max
local pi = math.pi
local sqrt = math.sqrt
ffi.cdef[[
typedef struct {
double x, y, z, w;
} cpml_quat;
]]
local abs, acos, asin, atan2 = math.abs, math.acos, math.asin, math.atan2
local cos, sin, min, max, pi = math.cos, math.sin, math.min, math.max, math.pi
local sqrt = math.sqrt
local quat = {}
local cpml_quat = ffi.typeof("cpml_quat")
quat.new = cpml_quat
function quat.identity(out)
out.x = 0
out.y = 0
out.z = 0
out.w = 1
-- Private constructor.
local function new(x, y, z, w)
local q = {}
q.x, q.y, q.z, q.w = x, y, z, w
return setmetatable(q, quat_mt)
end
-- Do the check to see if JIT is enabled. If so use the optimized FFI structs.
local status, ffi
if type(jit) == "table" and jit.status() then
status, ffi = pcall(require, "ffi")
if status then
ffi.cdef "typedef struct { double x, y, z, w;} cpml_quat;"
new = ffi.typeof("cpml_quat")
end
end
--- The public constructor.
-- @param x Can be of two types: </br>
-- number x component
-- table {x, y, z, w} or {x = x, y = y, z = z, w = w}
-- @tparam number y y component
-- @tparam number z z component
-- @tparam number w w component
function quat.new(x, y, z, w)
-- number, number, number, number
if x and y and z and w then
assert(type(x) == "number", "new: Wrong argument type for x (<number> expected)")
assert(type(y) == "number", "new: Wrong argument type for y (<number> expected)")
assert(type(z) == "number", "new: Wrong argument type for z (<number> expected)")
assert(type(w) == "number", "new: Wrong argument type for w (<number> expected)")
return new(x, y. z, w)
-- {x=x, y=y, z=z, w=w} or {x, y, z, w}
elseif type(x) == "table" then
local x, y, z, w = x.x or x[1], x.y or x[2], x.z or x[3], x.w or x[4]
assert(type(x) == "number", "new: Wrong argument type for x (<number> expected)")
assert(type(y) == "number", "new: Wrong argument type for y (<number> expected)")
assert(type(z) == "number", "new: Wrong argument type for z (<number> expected)")
assert(type(w) == "number", "new: Wrong argument type for w (<number> expected)")
return new(x, y, z, w)
else
return new(0, 0, 0, 1)
end
end
--- Create a quaternion from an axis, angle pair.
-- @tparam vec3 axis
-- @tparam number angle
-- @treturn quat
function quat.from_axis_angle(axis, angle)
local len = vec3.len(axis)
local s = sin(angle * 0.5)
local c = cos(angle * 0.5)
return quat.new(axis.x*s, axis.y*s, axis.z*s, c)
end
--- Create a quaternion from a normalized, up vector pair.
-- @tparam vec3 normal
-- @tparam vec3 up
-- @treturn quat
function quat.from_direction(normal, up)
local d = vec3.dot(up, normal)
local a = vec3()
vec3.cross(a, up, normal)
return quat.new(a.x, a.y, a.z, d+1)
end
--- Clone a quaternion.
-- @tparam quat a
-- @treturn quat clone
function quat.clone(a)
local out = quat.new()
ffi.copy(out, a, ffi.sizeof(cpml_quat))
return out
new(a.x, a.y, a.z, a.w)
end
--- Component-wise add a quaternion.
-- @tparam quat out
-- @tparam quat a
-- @tparam quat b
-- @treturn quat out
function quat.add(out, a, b)
out.x = a.x + b.x
out.y = a.y + b.y
out.z = a.z + b.z
out.w = a.w + b.w
return out
end
--- Component-wise subtract a quaternion.
-- @tparam quat out
-- @tparam quat a
-- @tparam quat b
-- @treturn quat out
function quat.sub(out, a, b)
out.x = a.x - b.x
out.y = a.y - b.y
out.z = a.z - b.z
out.w = a.w - b.w
return out
end
--- Perform a quaternion multiplication.
-- @tparam quat out
-- @tparam quat a
-- @tparam quat b
-- @treturn quat out
function quat.mul(out, a, b)
if type(b) == "table" and b.x and b.y and b.z and b.w then
out.x = a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y
out.y = a.y * b.w + a.w * b.y + a.z * b.x - a.x * b.z
out.z = a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x
out.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
elseif type(b) == "table" and b.x and b.y and b.z then
local qv = vec3(a.x, a.y, a.z)
local uv, uuv = vec3(), vec3()
vec3.cross(uv, qv, b)
vec3.cross(uuv, qv, uv)
vec3.mul(out, uv, a.w)
vec3.add(out, out, uuv)
vec3.mul(out, out, 2)
vec3.add(out, b, out)
end
end
function quat.div(out, a, b)
if type(b) == "number" then
quat.scale(out, a, 1 / b)
elseif type(b) == "table" and b.x and b.y and b.z and b.w then
quat.reciprocal(out, b)
quat.mul(out, a, out)
end
out.x = a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y
out.y = a.y * b.w + a.w * b.y + a.z * b.x - a.x * b.z
out.z = a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x
out.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
return out
end
--- Pow a quaternion by an exponent
-- @tparam quat out
-- @tparam quat a
-- @tparam number n
-- @treturn quat out
function quat.pow(out, a, n)
if n == 0 then
quat.identity(out)
out.x = 0
out.y = 0
out.z = 0
out.w = 1
elseif n > 0 then
out.x = a.x^(n-1)
out.y = a.y^(n-1)
@ -95,46 +157,77 @@ function quat.pow(out, a, n)
out.z = out.z^(-n)
out.w = out.w^(-n)
end
return out
end
function quat.cross(out, a, b)
out.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y
out.y = a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z
out.z = a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x
out.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
--- Component-wise scale a quaternion by a scalar.
-- @tparam quat out
-- @tparam quat a
-- @tparam number s
-- @treturn quat out
function quat.scale(out, a, s)
out.x = a.x * s
out.y = a.y * s
out.z = a.z * s
out.w = a.w * s
return out
end
function quat.dot(a, b)
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
--- Return the conjugate of a quaternion.
-- @tparam quat out
-- @tparam quat a
-- @treturn quat out
function quat.conjugate(out, a)
out.x = -a.x
out.y = -a.y
out.z = -a.z
out.w = a.w
return out
end
function quat.normalize(out, a)
if quat.is_zero(a) then
error("Cannot normalize a zero-length quaternion.")
return false
end
local l = 1 / quat.len(a)
quat.scale(out, a, l)
--- Return the inverse of a quaternion.
-- @tparam quat out
-- @tparam quat a
-- @treturn quat out
function quat.inverse(out, a)
quat.conjugate(out, a)
quat.normalize(out, out)
return out
end
function quat.len(a)
return sqrt(a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w)
end
function quat.len2(a)
return a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w
--- Return the reciprocal of a quaternion.
-- @tparam quat out
-- @tparam quat a
-- @treturn quat out
function quat.reciprocal(out, a)
local l = quat.len2(a)
quat.conjugate(out, a)
quat.scale(out, out, 1 / l)
return out
end
--- Linearly interpolate from one quaternion to the next.
-- @tparam quat out
-- @tparam quat a
-- @tparam quat b
-- @tparam number s 0-1 range number; 0 = a 1 = b
-- @treturn quat out
function quat.lerp(out, a, b, s)
quat.sub(out, b, a)
quat.mul(out, out, s)
quat.add(out, a, out)
quat.normalize(out, out)
return out
end
--- Slerp from one quaternion to the next.
-- @tparam quat out
-- @tparam quat a
-- @tparam quat b
-- @tparam number s 0-1 range number; 0 = a 1 = b
-- @treturn quat out
function quat.slerp(out, a, b, s)
local function clamp(n, low, high) return min(max(n, low), high) end
local dot = quat.dot(a, b)
if dot < 0 then
@ -147,7 +240,7 @@ function quat.slerp(out, a, b, s)
return
end
clamp(dot, -1, 1)
dot = min(max(dot, -1), 1)
local temp = quat.new()
local theta = acos(dot) * s
@ -157,149 +250,96 @@ function quat.slerp(out, a, b, s)
quat.scale(out, out, sin(theta))
quat.scale(temp, a, cos(theta))
quat.add(out, temp, out)
return out
end
function quat.rotate(out, angle, axis)
local len = vec3.len(axis)
if abs(len - 1) > FLT_EPSILON then
axis.x = axis.x / len
axis.y = axis.y / len
axis.z = axis.z / len
end
local s = sin(angle * 0.5)
local c = cos(angle * 0.5)
out.x = axis.x * s
out.y = axis.y * s
out.z = axis.z * s
out.w = c
--- Normalize a quaternion.
-- @tparam quat out
-- @tparam quat a
-- @treturn quat out
function quat.normalize(out, a)
local l = 1 / quat.len(a)
quat.scale(out, a, l)
return out
end
function quat.scale(out, a, s)
out.x = a.x * s
out.y = a.y * s
out.z = a.z * s
out.w = a.w * s
end
function quat.conjugate(out, a)
out.x = -a.x
out.y = -a.y
out.z = -a.z
out.w = a.w
end
function quat.inverse(out, a)
quat.conjugate(out, a)
quat.normalize(out, out)
end
function quat.reciprocal(out, a)
if quat.is_zero(a) then
error("Cannot reciprocate a zero-length quaternion.")
return false
end
local l = quat.len2(a)
quat.conjugate(out, a)
quat.scale(out, out, 1 / l)
end
function quat.is_zero(a)
return a.x == 0 and a.y == 0 and a.z == 0 and a.w == 0 then
end
function quat.is_real(a)
return a.x == 0 and a.y == 0 and a.z == 0 then
end
function quat.is_imaginary(a)
return a.w == 0 then
--- Return the imaginary part of the quaternion as a vec3.
-- @tparam vec3 out
-- @tparam quat a
-- @treturn quat out
function quat.imaginary(out, a)
out.x = a.x
out.y = a.y
out.z = a.z
return out
end
--- Return the real part of a quaternion.
-- @tparam quat a
-- @treturn number real
function quat.real(a)
return a.w
end
function quat.imaginary(a)
return vec3(a.x, a.y, a.z)
--- Return the inner angle between two quaternions.
-- @tparam quat a
-- @tparam quat b
-- @treturn number angle
function quat.dot(a, b)
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
end
function quat.from_direction(out, normal, up)
local d = vec3.dot(up, normal)
local a = vec3()
vec3.cross(a, up, normal)
out.x = a.x
out.y = a.y
out.z = a.z
out.w = d + 1
--- Return the length of a quaternion.
-- @tparam quat a
-- @treturn number len
function quat.len(a)
return sqrt(a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w)
end
function quat.to_angle_axis(a)
if a.w > 1 or a.w < -1 then
quat.normalize(a, a)
end
local angle = 2 * acos(a.w)
local s = sqrt(1 - a.w * a.w)
local x, y, z
if s < FLT_EPSILON then
x = a.x
y = a.y
z = a.z
else
x = a.x / s
y = a.y / s
z = a.z / s
end
return angle, vec3(x, y, z)
end
function quat.to_euler(a)
local sqx = a.x * a.x
local sqy = a.y * a.y
local sqz = a.z * a.z
local sqw = a.w * a.w
local unit = sqx + sqy + sqz + sqw
local test = a.x * a.y + a.z * a.w
local pitch, yaw, roll
if test > 0.499 * unit then
pitch = pi / 2
yaw = 2 * atan2(a.x, a.w)
roll = 0
elseif test < -0.499 * unit then
pitch = -pi / 2
yaw = -2 * atan2(a.x, a.w)
roll = 0
else
pitch = asin(2 * test / unit)
yaw = atan2(2 * a.y * a.w - 2 * a.x * a.z, sqx - sqy - sqz + sqw)
roll = atan2(2 * a.x * a.w - 2 * a.y * a.z, -sqx + sqy - sqz + sqw)
end
return pitch, yaw, roll
--- Return the squared length of a quaternion.
-- @tparam quat a
-- @treturn number len
function quat.len2(a)
return a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w
end
--- Unpack a quaternion into form x,y,z,w.
-- @tparam quat a
-- @treturn number x
-- @treturn number y
-- @treturn number z
-- @treturn number w
function quat.unpack(a)
return a.x, a.y, a.z, a.w
end
--- Return a string formatted "{x, y, z, w}"
-- @tparam quat a
-- @treturn string
function quat.tostring(a)
return string.format("(%+0.3f,%+0.3f,%+0.3f,%+0.3f)", a.x, a.y, a.z, a.w)
end
--- Return a boolean showing if a table is or is not a quat
-- @param q object to be tested
-- @treturn boolean
function quat.isquat(q)
return type(v) == "table" and
type(v.x) == "number" and
type(v.y) == "number" and
type(v.z) == "number" and
type(v.w) == "number"
end
local quat_mt = {}
quat_mt.__index = quat
quat_mt.__call = quat.new
quat_mt.__tostring = quat.tostring
function quat_mt.__call(self, x, y, z)
return quat.new(x, y, z, w)
end
function quat_mt.__unm(a)
local temp = quat.new()
quat.scale(temp, a, -1)
@ -307,32 +347,40 @@ function quat_mt.__unm(a)
end
function quat_mt.__eq(a,b)
assert(quat.isquat(a), "__eq: Wrong argument type for left hand operant. (<cpml.quat> expected)")
assert(quat.isquat(b), "__eq: Wrong argument type for right hand operant. (<cpml.quat> expected)")
return a.x == b.x and a.y == b.y and a.z == b.z and a.w == b.w
end
function quat_mt.__add(a, b)
assert(quat.isquat(a), "__eq: Wrong argument type for left hand operant. (<cpml.quat> expected)")
assert(quat.isquat(b), "__eq: Wrong argument type for right hand operant. (<cpml.quat> expected)")
local temp = quat.new()
quat.add(temp, a, b)
return temp
end
function quat_mt.__mul(a, b)
assert(quat.isquat(a), "__eq: Wrong argument type for left hand operant. (<cpml.quat> expected)")
assert(quat.isquat(b), "__eq: Wrong argument type for right hand operant. (<cpml.quat> expected)")
local temp = quat.new()
quat.mul(temp, a, b)
return temp
end
function quat_mt.__div(a, b)
local temp = quat.new()
quat.div(temp, a, b)
return temp
end
function quat_mt.__pow(a, n)
assert(quat.isquat(a), "__eq: Wrong argument type for left hand operant. (<cpml.quat> expected)")
assert(type(b) == "number", "__eq: Wrong argument type for right hand operant. (<number> expected)")
local temp = quat.new()
quat.pow(temp, a, n)
return temp
end
ffi.metatype(cpml_quat, quat_mt)
if status then
ffi.metatype(new, quat_mt)
end
return setmetatable({}, quat_mt)

View File

@ -1,3 +1,6 @@
--- A 2 component vector.
-- @module vec2
local sqrt= math.sqrt
local ffi = require "ffi"
@ -23,10 +26,10 @@ end
--- The public constructor.
-- @param x Can be of three types: </br>
-- number x component
-- table {x, y, z} or {x = x, y = y}
-- table {x, y} or {x = x, y = y}
-- scalar to fill the vector eg. {x, x}
-- @tparam number y y component
function vec2.new(x, y, z)
function vec2.new(x, y)
-- number, number, number
if x and y then
assert(type(x) == "number", "new: Wrong argument type for x (<number> expected)")
@ -122,7 +125,7 @@ end
--- Get the cross product of two vectors.
-- @tparam vec2 a Left hand operant
-- @tparam vec2 b Right hand operant
-- @tparam number
-- @treturn number magnitude of cross product in 3d
function vec2.cross(a, b)
return a.x * b.y - a.y * b.x
end
@ -175,7 +178,7 @@ end
-- @tparam vec3 b second vector
-- @tparam number s step value
-- @treturn vec3
function vec3.lerp(out, a, b, s)
function vec2.lerp(out, a, b, s)
vec2.sub(out, b, a)
vec2.mul(out, out, s)
vec2.add(out, out, a)
@ -200,7 +203,7 @@ end
--- Return a boolean showing if a table is or is not a vec2
-- @param v the object to be tested
-- @treturn boolean
function vec3.isvector(v)
function vec2.isvector(v)
return type(v) == "table" and
type(v.x) == "number" and
type(v.y) == "number"