modlib/quaternion.lua

129 lines
3.4 KiB
Lua
Raw Normal View History

2021-02-06 12:01:47 +01:00
-- TODO OOP, extend vector
function from_euler_rotation(rotation)
rotation = vector.divide(rotation, 2)
local cos = vector.apply(rotation, math.cos)
local sin = vector.apply(rotation, math.sin)
return {
sin.z * cos.x * cos.y - cos.z * sin.x * sin.y,
cos.z * sin.x * cos.y + sin.z * cos.x * sin.y,
cos.z * cos.x * sin.y - sin.z * sin.x * cos.y,
cos.z * cos.x * cos.y + sin.z * sin.x * sin.y
}
end
2021-02-02 15:44:58 +01:00
function multiply(self, other)
2021-02-02 10:38:21 +01:00
return {
2021-02-02 15:44:58 +01:00
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]
2021-02-02 10:38:21 +01:00
}
end
2021-02-02 15:44:58 +01:00
function normalize(self)
2021-02-02 23:37:15 +01:00
local len = math.sqrt(self[1] ^ 2 + self[2] ^ 2 + self[3] ^ 2 + (self[4] ^ 4))
2021-02-02 15:44:58 +01:00
local res = {}
for key, value in pairs(self) do
res[key] = value / len
2021-02-02 10:38:21 +01:00
end
2021-02-02 15:44:58 +01:00
return res
2021-02-02 10:38:21 +01:00
end
2021-02-03 12:49:23 +01:00
function conjugate(self)
return {
-self[1],
-self[2],
-self[3],
self[4]
}
end
function inverse(self)
return modlib.vector.divide_scalar(conjugate(self), self[1] ^ 2 + self[2] ^ 2 + self[3] ^ 2 + self[4] ^ 2)
end
2021-02-02 15:44:58 +01:00
function negate(self)
for key, value in pairs(self) do
self[key] = -value
2021-02-02 10:38:21 +01:00
end
end
2021-02-02 15:44:58 +01:00
function dot(self, other)
return self[1] * other[1] + self[2] * other[2] + self[3] * other[3] + self[4] * other[4]
2021-02-02 10:38:21 +01:00
end
2021-02-02 15:44:58 +01:00
--: self normalized quaternion
--: other normalized quaternion
function slerp(self, other, ratio)
local d = dot(self, other)
2021-02-02 10:38:21 +01:00
if d < 0 then
d = -d
2021-02-02 15:44:58 +01:00
negate(other)
2021-02-02 10:38:21 +01:00
end
-- Threshold beyond which linear interpolation is used
if d > 1 - 1e-10 then
2021-02-02 15:44:58 +01:00
return modlib.vector.interpolate(self, other, ratio)
2021-02-02 10:38:21 +01:00
end
local theta_0 = math.acos(d)
2021-02-02 15:44:58 +01:00
local theta = theta_0 * ratio
2021-02-02 10:38:21 +01:00
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
2021-02-02 15:44:58 +01:00
return modlib.vector.add(modlib.vector.multiply_scalar(self, s_0), modlib.vector.multiply_scalar(other, s_1))
2021-02-02 10:38:21 +01:00
end
2021-02-06 12:01:47 +01:00
--> {x = pitch, y = yaw, z = roll} euler rotation in degrees
2021-02-02 15:44:58 +01:00
function to_euler_rotation(self)
2021-02-02 10:38:21 +01:00
local rotation = {}
2021-02-02 15:44:58 +01:00
local sinr_cosp = 2 * (self[4] * self[1] + self[2] * self[3])
2021-02-06 11:57:34 +01:00
local cosr_cosp = 1 - 2 * (self[1] ^ 2 + self[2] ^ 2)
2021-02-02 10:38:21 +01:00
rotation.x = math.atan2(sinr_cosp, cosr_cosp)
2021-02-02 15:44:58 +01:00
local sinp = 2 * (self[4] * self[2] - self[3] * self[1])
2021-02-02 10:38:21 +01:00
if sinp <= -1 then
2021-02-06 11:57:34 +01:00
rotation.z = -math.pi/2
2021-02-02 10:38:21 +01:00
elseif sinp >= 1 then
2021-02-06 11:57:34 +01:00
rotation.z = math.pi/2
2021-02-02 10:38:21 +01:00
else
2021-02-06 11:57:34 +01:00
rotation.z = math.asin(sinp)
end
2021-02-02 10:38:21 +01:00
2021-02-02 15:44:58 +01:00
local siny_cosp = 2 * (self[4] * self[3] + self[1] * self[2])
2021-02-06 11:57:34 +01:00
local cosy_cosp = 1 - 2 * (self[2] ^ 2 + self[3] ^ 2)
rotation.y = math.atan2(siny_cosp, cosy_cosp)
2021-02-02 10:38:21 +01:00
return vector.apply(rotation, math.deg)
2021-02-06 11:57:34 +01:00
end
-- See https://github.com/zaki/irrlicht/blob/master/include/quaternion.h#L652
function to_euler_rotation_irrlicht(self)
local x, y, z, w = unpack(self)
local test = 2 * (y * w - x * z)
local function _calc()
if math.abs(test - 1) <= 1e-6 then
return {
z = -2 * math.atan2(x, w),
x = 0,
y = math.pi/2
}
end
if math.abs(test + 1) <= 1e-6 then
return {
z = 2 * math.atan2(x, w),
x = 0,
y = math.pi/-2
}
end
return {
z = math.atan2(2 * (x * y + z * w), x ^ 2 - y ^ 2 - z ^ 2 + w ^ 2),
x = math.atan2(2 * (y * z + x * w), -x ^ 2 - y ^ 2 + z ^ 2 + w ^ 2),
y = math.asin(math.min(math.max(test, -1), 1))
}
end
return vector.apply(_calc(), math.deg)
2021-02-02 10:38:21 +01:00
end