2021-06-11 20:47:29 +02:00
|
|
|
-- Localize globals
|
|
|
|
local math, modlib, pairs, unpack, vector = math, modlib, pairs, unpack, vector
|
|
|
|
|
2021-06-17 19:45:08 +02:00
|
|
|
-- Set environment
|
|
|
|
local _ENV = {}
|
|
|
|
setfenv(1, _ENV)
|
|
|
|
|
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 {
|
2021-03-27 20:10:49 +01:00
|
|
|
cos.z * sin.x * cos.y + sin.z * cos.x * sin.y,
|
2021-02-06 12:01:47 +01:00
|
|
|
cos.z * cos.x * sin.y - sin.z * sin.x * cos.y,
|
2021-08-29 11:42:03 +02:00
|
|
|
sin.z * cos.x * cos.y - cos.z * sin.x * sin.y,
|
2021-02-06 12:01:47 +01:00
|
|
|
cos.z * cos.x * cos.y + sin.z * sin.x * sin.y
|
|
|
|
}
|
|
|
|
end
|
|
|
|
|
2022-01-22 16:40:49 +01:00
|
|
|
function from_euler_rotation_deg(rotation)
|
|
|
|
return from_euler_rotation(vector.apply(rotation, math.rad))
|
|
|
|
end
|
|
|
|
|
2021-02-02 15:44:58 +01:00
|
|
|
function multiply(self, other)
|
2022-05-12 10:44:18 +02:00
|
|
|
local X, Y, Z, W = unpack(self)
|
|
|
|
return normalize{
|
|
|
|
(other[4] * X) + (other[1] * W) + (other[2] * Z) - (other[3] * Y);
|
|
|
|
(other[4] * Y) + (other[2] * W) + (other[3] * X) - (other[1] * Z);
|
|
|
|
(other[4] * Z) + (other[3] * W) + (other[1] * Y) - (other[2] * X);
|
|
|
|
(other[4] * W) - (other[1] * X) - (other[2] * Y) - (other[3] * Z);
|
2021-02-02 10:38:21 +01:00
|
|
|
}
|
|
|
|
end
|
|
|
|
|
2022-01-22 16:31:23 +01:00
|
|
|
function compose(self, other)
|
|
|
|
return multiply(other, self)
|
|
|
|
end
|
|
|
|
|
2023-02-26 14:46:13 +01:00
|
|
|
function len(self)
|
|
|
|
return (self[1] ^ 2 + self[2] ^ 2 + self[3] ^ 2 + self[4] ^ 2) ^ 0.5
|
|
|
|
end
|
|
|
|
|
2021-02-02 15:44:58 +01:00
|
|
|
function normalize(self)
|
2023-02-26 14:46:13 +01:00
|
|
|
local l = len(self)
|
2021-02-02 15:44:58 +01:00
|
|
|
local res = {}
|
|
|
|
for key, value in pairs(self) do
|
2023-02-26 14:46:13 +01:00
|
|
|
res[key] = value / l
|
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-08-27 10:45:59 +02:00
|
|
|
--> axis, angle
|
|
|
|
function to_axis_angle(self)
|
|
|
|
local axis = modlib.vector.new{self[1], self[2], self[3]}
|
|
|
|
local len = axis:length()
|
|
|
|
-- HACK invert axis for correct rotation in Minetest
|
|
|
|
return len == 0 and axis or axis:divide_scalar(-len), 2 * math.atan2(len, self[4])
|
|
|
|
end
|
|
|
|
|
2022-01-22 16:40:49 +01:00
|
|
|
function to_euler_rotation_rad(self)
|
2021-03-27 20:10:49 +01:00
|
|
|
local rotation = {}
|
|
|
|
|
|
|
|
local sinr_cosp = 2 * (self[4] * self[1] + self[2] * self[3])
|
|
|
|
local cosr_cosp = 1 - 2 * (self[1] ^ 2 + self[2] ^ 2)
|
|
|
|
rotation.x = math.atan2(sinr_cosp, cosr_cosp)
|
|
|
|
|
|
|
|
local sinp = 2 * (self[4] * self[2] - self[3] * self[1])
|
|
|
|
if sinp <= -1 then
|
2021-04-01 01:04:59 +02:00
|
|
|
rotation.y = -math.pi/2
|
2021-03-27 20:10:49 +01:00
|
|
|
elseif sinp >= 1 then
|
2021-04-01 01:04:59 +02:00
|
|
|
rotation.y = math.pi/2
|
2021-03-27 20:10:49 +01:00
|
|
|
else
|
2021-04-01 01:04:59 +02:00
|
|
|
rotation.y = math.asin(sinp)
|
2021-02-06 11:57:34 +01:00
|
|
|
end
|
2021-02-02 10:38:21 +01:00
|
|
|
|
2021-03-27 20:10:49 +01:00
|
|
|
local siny_cosp = 2 * (self[4] * self[3] + self[1] * self[2])
|
|
|
|
local cosy_cosp = 1 - 2 * (self[2] ^ 2 + self[3] ^ 2)
|
2021-04-01 01:04:59 +02:00
|
|
|
rotation.z = math.atan2(siny_cosp, cosy_cosp)
|
2021-02-02 10:38:21 +01:00
|
|
|
|
2022-01-22 16:40:49 +01:00
|
|
|
return rotation
|
|
|
|
end
|
|
|
|
|
|
|
|
-- TODO rename this to to_euler_rotation_deg eventually (breaking change)
|
|
|
|
--> {x = pitch, y = yaw, z = roll} euler rotation in degrees
|
|
|
|
function to_euler_rotation(self)
|
|
|
|
return vector.apply(to_euler_rotation_rad(self), 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-06-17 19:45:08 +02:00
|
|
|
end
|
|
|
|
|
|
|
|
-- Export environment
|
|
|
|
return _ENV
|