improved terrain and collision detection

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

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)