diff --git a/modules/quat.lua b/modules/quat.lua index 61c6063..8a03a5d 100644 --- a/modules/quat.lua +++ b/modules/quat.lua @@ -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:
+-- 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 ( expected)") + assert(type(y) == "number", "new: Wrong argument type for y ( expected)") + assert(type(z) == "number", "new: Wrong argument type for z ( expected)") + assert(type(w) == "number", "new: Wrong argument type for w ( 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 ( expected)") + assert(type(y) == "number", "new: Wrong argument type for y ( expected)") + assert(type(z) == "number", "new: Wrong argument type for z ( expected)") + assert(type(w) == "number", "new: Wrong argument type for w ( 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. ( expected)") + assert(quat.isquat(b), "__eq: Wrong argument type for right hand operant. ( 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. ( expected)") + assert(quat.isquat(b), "__eq: Wrong argument type for right hand operant. ( 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. ( expected)") + assert(quat.isquat(b), "__eq: Wrong argument type for right hand operant. ( 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. ( expected)") + assert(type(b) == "number", "__eq: Wrong argument type for right hand operant. ( 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) diff --git a/modules/vec2.lua b/modules/vec2.lua index ed7b160..046f15e 100644 --- a/modules/vec2.lua +++ b/modules/vec2.lua @@ -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:
-- 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 ( 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"