From 7cdca018aa33632be335c866832bd997a8aed8d6 Mon Sep 17 00:00:00 2001 From: Colby Klein Date: Fri, 10 Jul 2015 12:25:39 -0700 Subject: [PATCH] Add quat.cross, fix quat.rotate, cleanup. --- modules/quat.lua | 163 +++++++++++++++++++++++++++++------------------ 1 file changed, 101 insertions(+), 62 deletions(-) diff --git a/modules/quat.lua b/modules/quat.lua index 62fc1a7..18e12f0 100644 --- a/modules/quat.lua +++ b/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 })