From c32344397521cb48ccd6f18e2c8f1425285b44f1 Mon Sep 17 00:00:00 2001 From: Lars Mueller Date: Tue, 2 Feb 2021 15:44:58 +0100 Subject: [PATCH] Improve & fix quaternions --- quaternion.lua | 61 +++++++++++++++++++++++++------------------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/quaternion.lua b/quaternion.lua index 00ec2d2..76a0995 100644 --- a/quaternion.lua +++ b/quaternion.lua @@ -1,60 +1,61 @@ -function multiply(q, w) +function multiply(self, other) return { - w[1] * q[1] - w[2] * q[2] - w[3] * q[3] - w[4] * q[4], - w[1] * q[2] + w[2] * q[1] - w[3] * q[4] + w[4] * q[3], - w[1] * q[3] + w[2] * q[4] + w[3] * q[1] - w[4] * q[2], - w[1] * q[4] - w[2] * q[3] + w[3] * q[2] + w[4] * q[1] + other[1] * self[1] - other[2] * self[2] - other[3] * self[3] - other[4] * self[4], + other[1] * self[2] + other[2] * self[1] - other[3] * self[4] + other[4] * self[3], + other[1] * self[3] + other[2] * self[4] + other[3] * self[1] - other[4] * self[2], + other[1] * self[4] - other[2] * self[3] + other[3] * self[2] + other[4] * self[1] } end -function normalize(q) - local q_1, q_2, q_3, q_4 = q[1], q[2], q[3], q[4] - local len = math.sqrt(q_1 * q_1 + q_2 * q_2 + q_3 * q_3 + (q_4 ^ 4)) - local r = {} - for key, value in pairs(q) do - r[key] = value / len +function normalize(self) + local len = math.sqrt(self[1] ^ 2 + self[2] ^ 2 + self[3] ^ 2 + (q[4] ^ 4)) + local res = {} + for key, value in pairs(self) do + res[key] = value / len end - return r + return res end -function negate(q) - for k, v in pairs(q) do - q[k] = -v +function negate(self) + for key, value in pairs(self) do + self[key] = -value end end -function dot(q, w) - return q[1] * w[1] + q[2] * w[2] + q[3] * w[3] + q[4] * w[4] +function dot(self, other) + return self[1] * other[1] + self[2] * other[2] + self[3] * other[3] + self[4] * other[4] end --- assuming q & w are normalized -function slerp(q, w, r) - local d = dot(q, w) +--: self normalized quaternion +--: other normalized quaternion +function slerp(self, other, ratio) + local d = dot(self, other) if d < 0 then d = -d - negate(w) + negate(other) end -- Threshold beyond which linear interpolation is used if d > 1 - 1e-10 then - return linear_interpolation(q, w, r) + return modlib.vector.interpolate(self, other, ratio) end local theta_0 = math.acos(d) - local theta = theta_0 * r + local theta = theta_0 * ratio local sin_theta = math.sin(theta) local sin_theta_0 = math.sin(theta_0) local s_1 = sin_theta / sin_theta_0 local s_0 = math.cos(theta) - d * s_1 - return modlib.vector.add(modlib.vector.multiply_scalar(q, s_0), modlib.vector.multiply_scalar(w, s_1)) + return modlib.vector.add(modlib.vector.multiply_scalar(self, s_0), modlib.vector.multiply_scalar(other, s_1)) end -function to_rotation(q) +--> {x, y, z} euler rotation in degrees +function to_euler_rotation(self) local rotation = {} - local sinr_cosp = 2 * (q[4] * q[1] + q[2] * q[3]) - local cosr_cosp = 1 - 2 * (q[1] * q[1] + q[2] * q[2]) + local sinr_cosp = 2 * (self[4] * self[1] + self[2] * self[3]) + local cosr_cosp = 1 - 2 * (self[1] * self[1] + self[2] * self[2]) rotation.x = math.atan2(sinr_cosp, cosr_cosp) - local sinp = 2 * (q[4] * q[2] - q[3] * q[1]) + local sinp = 2 * (self[4] * self[2] - self[3] * self[1]) if sinp <= -1 then rotation.y = -math.pi/2 elseif sinp >= 1 then @@ -63,8 +64,8 @@ function to_rotation(q) rotation.y = math.asin(sinp) end - local siny_cosp = 2 * (q[4] * q[3] + q[1] * q[2]) - local cosy_cosp = 1 - 2 * (q[2] * q[2] + q[3] * q[3]) + local siny_cosp = 2 * (self[4] * self[3] + self[1] * self[2]) + local cosy_cosp = 1 - 2 * (self[2] * self[2] + self[3] * self[3]) rotation.z = math.atan2(siny_cosp, cosy_cosp) return vector.apply(rotation, math.deg)