function getHeadingBetweenTwoPoints(x1, y1, x2, y2)
local theta = math.atan2(x2 - x1, y1 - y2)
if theta < 0 then
theta = theta + 6.2831853071795865
end
local beta = 57.2957795130823209 * theta
return beta < 180 and beta + 180 or beta - 180
end
-- https://github.com/openmultiplayer/open.mp/blob/master/SDK/include/gtaquat.hpp
function quat(w, x, y, z)
local EPSILON = 0.00000202655792236328125
local mt = {}
local obj = {
w = w or 1.0,
x = x or 0.0,
y = y or 0.0,
z = z or 0.0,
}
function obj:get()
return self.w, self.x, self.y, self.z
end
function obj:getData()
return { [0] = self.w, [1] = self.x, [2] = self.y, [3] = self.z }
end
function obj:fromEuler(degrees)
assert(type(degrees) == "table", "vector3d expected, got "..type(degrees))
local radians = degrees * deg2rad
local c = vector3d(math.cos(radians.x * -0.5), math.cos(radians.y * -0.5), math.cos(radians.z * -0.5))
local s = vector3d(math.sin(radians.x * -0.5), math.sin(radians.y * -0.5), math.sin(radians.z * -0.5))
self.w = c.x * c.y * c.z + s.x * s.y * s.z
self.x = c.x * s.y * s.z + s.x * c.y * c.z
self.y = c.x * s.y * c.z - s.x * c.y * s.z
self.z = c.x * c.y * s.z - s.x * s.y * c.z
end
function obj:toEuler()
local temp = 2 * self.y * self.z - 2 * self.x * self.w
local rx, ry, rz
if temp >= 1.0 - EPSILON then
rx = 90.0
ry = -math.deg(math.atan2(math.clamp(self.y, -1.0, 1.0), math.clamp(self.w, -1.0, 1.0)))
rz = -math.deg(math.atan2(math.clamp(self.z, -1.0, 1.0), math.clamp(self.w, -1.0, 1.0)))
elseif -temp >= 1.0 - EPSILON then
rx = -90.0
ry = -math.deg(math.atan2(math.clamp(self.y, -1.0, 1.0), math.clamp(self.w, -1.0, 1.0)))
rz = -math.deg(math.atan2(math.clamp(self.z, -1.0, 1.0), math.clamp(self.w, -1.0, 1.0)))
else
rx = math.deg(math.asin(math.clamp(temp, -1.0, 1.0)))
ry = -math.deg(math.atan2(math.clamp(self.x * self.z + self.y * self.w, -1.0, 1.0), math.clamp(0.5 - self.x * self.x - self.y * self.y, -1.0, 1.0)))
rz = -math.deg(math.atan2(math.clamp(self.x * self.y + self.z * self.w, -1.0, 1.0), math.clamp(0.5 - self.x * self.x - self.z * self.z, -1.0, 1.0)))
end
-- Keep each component in the [0, 360) interval
-- return mod(Vector3(rx, ry, rz), 360.0f); -- хуй знает что вообще
return vector3d(math.fmod(rx, 360.0), math.fmod(ry, 360.0), math.fmod(rz, 360.0))
end
function obj:faceTo(to, from, vertical) -- ?
assert(type(to) == "table", "vector3d expected, got "..type(to))
-- vertical is not implemented yet
if not from then from = vector3d(getBotPosition()) end
local a = getHeadingBetweenTwoPoints(from.x, from.y, to.x, to.y)
self:fromEuler(vector3d(0.0, 0.0, a))
end
setmetatable(obj, mt)
return obj
end
function faceBotTo(target)
local q = quat()
q:faceTo(target)
setBotQuaternion(q:get())
end