more mods

mods/automobiles_pck/automobiles_lib/ground_detection.lua

@@ -0,0 +1,278 @@
+function automobiles_lib.putAngleOnRange(angle)
+    local n_angle = angle/360
+    n_angle = (n_angle - math.floor(n_angle))*360
+    if n_angle < -180 then n_angle = n_angle + 360 end
+    if n_angle > 180 then n_angle = n_angle - 360 end
+    return n_angle
+end
+
+function automobiles_lib.pid_controller(current_value, setpoint, last_error, delta_t, kp, ki, kd)
+    kp = kp or 0
+    ki = ki or 0.00000000000001
+    kd = kd or 0.05
+    delta_t = delta_t or 0.100;
+
+    ti = kp/ki
+    td = kd/kp
+
+    local _error = setpoint - current_value
+    local derivative = _error - last_error
+    --local output = kpv*erro + (kpv/Tiv)*I + kpv*Tdv*((erro - erro_passado)/delta_t);
+    if integrative == nil then integrative = 0 end
+    integrative = integrative + (((_error + last_error)/delta_t)/2);
+    local output = kp*_error + (kp/ti)*integrative + kp * td*((_error - last_error)/delta_t)
+    last_error = _error
+    return output, last_error
+
+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
+    local hip = axis_distance
+    --minetest.chat_send_all("entre-eixo "..hip)
+    local pitch = self._pitch --+90 for the calculations
+
+    local yaw = self.object:get_yaw()
+    local deg_yaw = math.deg(yaw)
+    local yaw_turns = math.floor(deg_yaw / 360)
+    deg_yaw = deg_yaw - (yaw_turns * 360)
+    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 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
+    }
+
+    --retornando pitch e roll
+    local pitch, roll = get_vehicle_pitch_roll(pos, wheels, yaw)
+
+    if not self._last_pitch then self._last_pitch = pitch end
+
+--[[
+* `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`.
+]]--
+
+
+    if 1 then
+        local pos_y_elevation = 1.6
+        local pos_elevation = (math.tan(pitch)*axis_distance) + pos_y_elevation
+        --core.chat_send_all(dump(pos_elevation))
+        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
+
+	    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")
+
+                    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
+
+    end
+
+    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, ammount)
+    ammount = ammount or -3
+    --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=ref_pos.y + ammount, z=i_pos.z})
+    if y then
+        retval.y = y
+    end
+
+    --minetest.chat_send_all("y: " .. dump(ref_pos.y) .. " ye: ".. dump(retval.y))
+    return retval    
+end
+
+function automobiles_lib.eval_interception(initial_pos, end_pos)
+    local ret_y = nil
+	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")
+
+                if drawtype ~= "plantlike" and drawtype ~= nil then
+                    if initial_pos.y >= pos.y then 
+                        ret_y = pos.y
+                        --minetest.chat_send_all("ray intercection: " .. dump(pos.y) .. " -- " .. nodename)
+                    end
+                    break
+                end
+            end
+        end
+        thing = cast:next()
+    end
+    return ret_y
+end
+
+function automobiles_lib.get_node_below(pos, dist)
+    local node = minetest.get_node(pos)
+    local pos_below = pos
+    pos_below.y = pos_below.y - (dist + 0.1)
+    local node_below = minetest.get_node(pos_below)
+    return node_below
+end
+
+