diff --git a/automobiles_lib/entities.lua b/automobiles_lib/entities.lua index 385755a..9010e6d 100644 --- a/automobiles_lib/entities.lua +++ b/automobiles_lib/entities.lua @@ -739,6 +739,7 @@ function automobiles_lib.on_step(self, dtime) local newroll = 0 if self._is_flying == 1 then + newpitch = 0 local turn_effect_speed = longit_speed if turn_effect_speed > 10 then turn_effect_speed = 10 end newroll = (-self._steering_angle/100)*(turn_effect_speed/10) @@ -775,6 +776,7 @@ function automobiles_lib.on_step(self, dtime) newroll = tilt_effect * -1 self.front_suspension:set_rotation({x=0,y=0,z=tilt_effect}) self.rear_suspension:set_rotation({x=0,y=0,z=tilt_effect}) + newroll = newroll + (self._roll or 0) if (noded and noded.drawtype ~= 'airlike') then diff --git a/automobiles_lib/ground_detection.lua b/automobiles_lib/ground_detection.lua index b851658..0744d01 100644 --- a/automobiles_lib/ground_detection.lua +++ b/automobiles_lib/ground_detection.lua @@ -26,6 +26,125 @@ function automobiles_lib.pid_controller(current_value, setpoint, last_error, del end +-- Função para calcular ângulos de pitch e roll +local function get_vehicle_pitch_roll(pos, wheels, yaw) + local world = vector.new() + --core.chat_send_all(dump(wheels)) + + --rotacionando as posições das rodas + for i = 1,4,1 do + local orig_x = wheels[i].x + local orig_z = wheels[i].z + wheels[i].x = (orig_x * math.cos(yaw)) - (orig_z * math.sin(yaw)) + wheels[i].z = (orig_x * math.sin(yaw)) + (orig_z * math.cos(yaw)) + end + --core.chat_send_all(dump(wheels)) + + local points = {} + + -- Obter altura do terreno para cada roda + for i, wheel in ipairs(wheels) do + world.x = pos.x + wheel.x + world.y = pos.y + wheel.y + world.z = pos.z + wheel.z + local height = automobiles_lib.get_obstacle({x=world.x,y=world.y,z=world.z}).y or 0 + table.insert(points, {x = world.x, y = height, z = world.z}) + end + + -- Calcular os coeficientes do plano Ax + By + Cz + D = 0 + local x1, y1, z1 = points[1].x, points[1].y, points[1].z + local x2, y2, z2 = points[2].x, points[2].y, points[2].z + local x3, y3, z3 = points[3].x, points[3].y, points[3].z + local x4, y4, z4 = points[4].x, points[4].y, points[4].z + + -- Criamos dois vetores a partir das quatro rodas + local v1 = {x = x2 - x1, y = y2 - y1, z = z2 - z1} + local v2 = {x = x3 - x1, y = y3 - y1, z = z3 - z1} + local v3 = {x = x4 - x1, y = y4 - y1, z = z4 - z1} + + -- Tiramos a média do produto vetorial entre os três vetores + local normal1 = { + x = v1.y * v2.z - v1.z * v2.y, + y = v1.z * v2.x - v1.x * v2.z, + z = v1.x * v2.y - v1.y * v2.x + } + + local normal2 = { + x = v1.y * v3.z - v1.z * v3.y, + y = v1.z * v3.x - v1.x * v3.z, + z = v1.x * v3.y - v1.y * v3.x + } + + -- Média das duas normais para melhor precisão + local normal = { + x = (normal1.x + normal2.x) / 2, + y = (normal1.y + normal2.y) / 2, + z = (normal1.z + normal2.z) / 2 + } + + -- Normalizar o vetor normal + local magnitude = math.sqrt(normal.x^2 + normal.y^2 + normal.z^2) + if magnitude == 0 then + return 0, 0 -- Retorna ângulos neutros se o cálculo falhar + end + + normal.x = normal.x / magnitude + normal.y = normal.y / magnitude + normal.z = normal.z / magnitude + --core.chat_send_all("normal2 "..dump(normal).."\nmagnitude: "..magnitude) + + -- Criar um vetor de direção do veículo (da traseira para a frente) + local forward = { + x = (wheels[1].x + wheels[2].x) / 2 - (wheels[3].x + wheels[4].x) / 2, + z = (wheels[1].z + wheels[2].z) / 2 - (wheels[3].z + wheels[4].z) / 2 + } + + -- Normalizar o vetor de direção + local magnitude_forward = math.sqrt(forward.x^2 + forward.z^2) + forward.x = forward.x / magnitude_forward + forward.z = forward.z / magnitude_forward + + -- Descobrir se o veículo está apontando para frente ou para trás no eixo Z + local direction_z = forward.z >= 0 and -1 or 1 + -- Descobrir se o veículo está apontando para direita/esquerda no eixo X + local direction_x = forward.x >= 0 and -1 or 1 + + -- Verificar qual eixo está "apontado para frente" + local dominantX = math.abs(forward.x) > math.abs(forward.z) + + local pitch, roll + local m, n + + if dominantX then + -- Calcular ângulos de pitch e roll + m = normal.x/normal.y + pitch = math.atan(m) * direction_x + if pitch ~= pitch then pitch = 0 end + n = normal.z/normal.y + roll = math.atan(n) * direction_x + if roll ~= roll then roll = 0 end + else + -- Calcular ângulos de pitch e roll + m = normal.z/normal.y + pitch = math.atan(m) * direction_z + if pitch ~= pitch then pitch = 0 end + n = normal.x/normal.y + roll = math.atan(n) * direction_z * -1 + if roll ~= roll then roll = 0 end + end + + --core.chat_send_all(dump("pitch "..math.deg(pitch).." - roll "..math.deg(roll))) + + return pitch, roll +end + +local function get_nodedef_field(nodename, fieldname) + if not minetest.registered_nodes[nodename] then + return nil + end + return minetest.registered_nodes[nodename][fieldname] +end + --lets assume that the rear axis is at object center, so we will use the distance only for front wheels function automobiles_lib.ground_get_distances(self, radius, axis_distance) --local mid_axis = (axis_length / 2)/10 @@ -40,98 +159,85 @@ function automobiles_lib.ground_get_distances(self, radius, axis_distance) yaw = math.rad(deg_yaw) local pos = self.object:get_pos() + local front_wheel_x = self._front_wheel_xpos / 10 + if math.abs(front_wheel_x) < 0.01 then front_wheel_x = 0.01 end --pra não anular as normals + local rear_wheel_x = self._rear_wheel_xpos / 10 + if math.abs(rear_wheel_x) < 0.01 then rear_wheel_x = 0.01 end --pra não anular as normals - local r_x, r_z = automobiles_lib.get_xz_from_hipotenuse(pos.x, pos.z, yaw, 0) - local r_y = pos.y - local rear_axis = {x=r_x, y=r_y, z=r_z} + local wheels = { + {x = -front_wheel_x, y = 0, z = axis_distance}, -- Roda frontal esquerda + {x = front_wheel_x, y = 0, z = axis_distance}, -- Roda frontal direita + {x = -rear_wheel_x, y = 0, z = 0}, -- Roda traseira esquerda + {x = rear_wheel_x, y = 0, z = 0} -- Roda traseira direita + } - local rear_obstacle_level = automobiles_lib.get_obstacle(rear_axis) - --minetest.chat_send_all("rear"..dump(rear_obstacle_level)) + --retornando pitch e roll + local pitch, roll = get_vehicle_pitch_roll(pos, wheels, yaw) - if not self._last_front_detection then - self._last_front_detection = rear_obstacle_level.y - end - if not self._last_pitch then - self._last_pitch = 0 - end + if not self._last_pitch then self._last_pitch = pitch end - local f_x, f_z = automobiles_lib.get_xz_from_hipotenuse(pos.x, pos.z, yaw, hip) - local f_y = pos.y - --local x, f_y = automobiles_lib.get_xz_from_hipotenuse(f_x, r_y, pitch - math.rad(90), hip) --the x is only a mock - --minetest.chat_send_all("r: "..r_y.." f: "..f_y .." - "..math.deg(pitch)) - local front_axis = {x=f_x, y=f_y, z=f_z} - local front_obstacle_level = automobiles_lib.get_obstacle(front_axis) - --minetest.chat_send_all("front"..dump(front_obstacle_level)) +--[[ +* `minetest.raycast(pos1, pos2, objects, liquids)`: returns `Raycast` + * Creates a `Raycast` object. + * `pos1`: start of the ray + * `pos2`: end of the ray + * `objects`: if false, only nodes will be returned. Default is `true`. + * `liquids`: if false, liquid nodes won't be returned. Default is `false`. +]]-- - --lets try to get the pitch - if front_obstacle_level.y ~= nil and rear_obstacle_level.y ~= nil then - local deltaX = axis_distance; - local deltaY = front_obstacle_level.y - rear_obstacle_level.y; - --minetest.chat_send_all("deutaY "..deltaY) - local m = (deltaY/deltaX) - local new_pitch = math.atan(m) --math.atan2(deltaY, deltaX); - --local deg_angle = math.deg(new_pitch) - --first test front wheels before - if (front_obstacle_level.y-self._last_front_detection) <= self.initial_properties.stepheight then - pitch = new_pitch --math.atan2(deltaY, deltaX); - self._last_pitch = new_pitch - self._last_front_detection = front_obstacle_level.y - else - --until 20 deg, climb - if math.deg(new_pitch - self._last_pitch) <= 20 then + if 1 then + local pos_elevation = 1 + local f_x, f_z = automobiles_lib.get_xz_from_hipotenuse(pos.x, pos.z, yaw, axis_distance) + local f_y = pos.y + pos_elevation + local end_pos = {x=f_x, y=f_y, z=f_z} + local initial_pos = vector.new(pos) + initial_pos.y = initial_pos.y + pos_elevation - --[[if self._last_error == nil then self._last_error = 0 end -- Último erro registrado - -- Estado atual do sistema - local output, last_error = automobiles_lib.pid_controller(self._last_pitch, new_pitch, self._last_error, self.dtime/2, 0.5) - self._last_error = last_error - new_pitch = output - local conversion = automobiles_lib.putAngleOnRange(math.deg(new_pitch)) - new_pitch = math.rad(conversion) - minetest.chat_send_all("last: "..self._last_pitch.." - new: "..new_pitch)]]-- + local cast = minetest.raycast(initial_pos, end_pos, true, false) + local thing = cast:next() + while thing do + if thing.type == "node" then + local pos = thing.intersection_point + if pos then + local nodename = minetest.get_node(thing.under).name + local drawtype = get_nodedef_field(nodename, "drawtype") - pitch = new_pitch - self._last_pitch = new_pitch - self._last_front_detection = front_obstacle_level.y - else - --no climb here - self._last_front_detection = rear_obstacle_level.y - --here we need to set the collision effect - self.object:set_acceleration({x=0,y=0,z=0}) - local oldvel = self.object:get_velocity() - self.object:add_velocity(vector.subtract(vector.new(), oldvel)) + if drawtype ~= "plantlike" then + self.object:set_acceleration({x=0,y=0,z=0}) + local oldvel = self.object:get_velocity() + self.object:add_velocity(vector.subtract(vector.new(), oldvel)) + pitch = math.rad(0) + break + end + end end + thing = cast:next() end - else - pitch = math.rad(0) end - self._pitch = pitch + self._pitch = pitch -- math.rad(90) + self._roll = roll -- math.rad(90) + --core.chat_send_all(dump("pitch "..pitch.." - roll "..roll)) end -function automobiles_lib.get_obstacle(ref_pos) +function automobiles_lib.get_obstacle(ref_pos, ammount) + ammount = ammount or -4 --lets clone the table local retval = {x=ref_pos.x, y=ref_pos.y, z=ref_pos.z} --minetest.chat_send_all("aa y: " .. dump(retval.y)) local i_pos = {x=ref_pos.x, y=ref_pos.y + 1, z=ref_pos.z} --minetest.chat_send_all("bb y: " .. dump(i_pos.y)) - local y = automobiles_lib.eval_interception(i_pos, {x=i_pos.x, y=i_pos.y - 4, z=i_pos.z}) + local y = automobiles_lib.eval_interception(i_pos, {x=i_pos.x, y=i_pos.y + ammount, z=i_pos.z}) retval.y = y --minetest.chat_send_all("y: " .. dump(ref_pos.y) .. " ye: ".. dump(retval.y)) return retval end -local function get_nodedef_field(nodename, fieldname) - if not minetest.registered_nodes[nodename] then - return nil - end - return minetest.registered_nodes[nodename][fieldname] -end - function automobiles_lib.eval_interception(initial_pos, end_pos) local ret_y = nil local cast = minetest.raycast(initial_pos, end_pos, true, false)