From e52e02f088434491bb7779132eb0d24c07741b11 Mon Sep 17 00:00:00 2001 From: Iwan1803 <47528836+Iwan1803@users.noreply.github.com> Date: Thu, 7 Mar 2024 01:30:37 +0100 Subject: [PATCH 01/44] Update README.md 2.0.1.7-RC --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index d066aa1c..b73767b6 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # FS22_AutoDrive FS22 version of the AutoDrive mod -### Latest Release: 2.0.1.6 +### Latest Release: 2.0.1.7-RC ![GitHub all releases](https://img.shields.io/github/downloads/Stephan-S/FS22_AutoDrive/total?label=Downloads&style=plastic) Direct Download: https://github.com/Stephan-S/FS22_AutoDrive/releases/latest/download/FS22_AutoDrive.zip From e9da28a5f6a20d50aefd363cb1d88aff40e900e8 Mon Sep 17 00:00:00 2001 From: Iwan1803 <47528836+Iwan1803@users.noreply.github.com> Date: Thu, 7 Mar 2024 01:30:51 +0100 Subject: [PATCH 02/44] Update modDesc.xml 2.0.1.7-RC --- modDesc.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modDesc.xml b/modDesc.xml index 968ce961..988ceac8 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -32,7 +32,7 @@ Différents modes d'utilisation ont été ajoutés depuis les premières version - 2.0.1.6 + 2.0.1.7-RC icon.dds From b6b381c4daccccab6caf059709b32d22be555bf7 Mon Sep 17 00:00:00 2001 From: Iwan1803 <47528836+Iwan1803@users.noreply.github.com> Date: Thu, 7 Mar 2024 01:31:03 +0100 Subject: [PATCH 03/44] Update AutoDrive.lua 2.0.1.7-RC --- scripts/AutoDrive.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index 76cfe98d..f031fdc4 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -1,5 +1,5 @@ AutoDrive = {} -AutoDrive.version = "2.0.1.6" +AutoDrive.version = "2.0.1.7-RC" AutoDrive.directory = g_currentModDirectory From cd9df13ed95a6e1a76b0fccae6b9123678d5e1cd Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Thu, 16 May 2024 19:54:17 +0200 Subject: [PATCH 04/44] possible FPS improvement might improve FPS in editor mode depending on map, network, CPU, GPU --- modDesc.xml | 2 +- scripts/AutoDrive.lua | 2 +- scripts/Manager/DrawingManager.lua | 342 +++++++++++++++++++++++++---- scripts/Manager/GraphManager.lua | 4 + scripts/Specialization.lua | 34 +-- 5 files changed, 326 insertions(+), 58 deletions(-) diff --git a/modDesc.xml b/modDesc.xml index 968ce961..c91c25ee 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -32,7 +32,7 @@ Différents modes d'utilisation ont été ajoutés depuis les premières version - 2.0.1.6 + 2.0.1.7-RC1 icon.dds diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index 76cfe98d..ef8e8e8e 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -1,5 +1,5 @@ AutoDrive = {} -AutoDrive.version = "2.0.1.6" +AutoDrive.version = "2.0.1.7-RC1" AutoDrive.directory = g_currentModDirectory diff --git a/scripts/Manager/DrawingManager.lua b/scripts/Manager/DrawingManager.lua index cbfc6b49..99aaf1c3 100644 --- a/scripts/Manager/DrawingManager.lua +++ b/scripts/Manager/DrawingManager.lua @@ -1,4 +1,5 @@ ADDrawingManager = {} +ADDrawingManager.enableDebug = false ADDrawingManager.i3DBaseDir = "drawing/" ADDrawingManager.yOffset = 0 ADDrawingManager.emittivity = 0 @@ -10,6 +11,8 @@ ADDrawingManager.lines.fileName = "line.i3d" ADDrawingManager.lines.buffer = Buffer:new() ADDrawingManager.lines.objects = FlaggedTable:new() ADDrawingManager.lines.tasks = {} +ADDrawingManager.lines.lastTaskCount = 0 +ADDrawingManager.lines.currentTask = 0 ADDrawingManager.lines.itemIDs = {} ADDrawingManager.lines.lastDrawZero = true @@ -18,6 +21,8 @@ ADDrawingManager.arrows.fileName = "arrow.i3d" ADDrawingManager.arrows.buffer = Buffer:new() ADDrawingManager.arrows.objects = FlaggedTable:new() ADDrawingManager.arrows.tasks = {} +ADDrawingManager.arrows.lastTaskCount = 0 +ADDrawingManager.arrows.currentTask = 0 ADDrawingManager.arrows.itemIDs = {} ADDrawingManager.arrows.lastDrawZero = true ADDrawingManager.arrows.position = {} @@ -29,6 +34,8 @@ ADDrawingManager.sSphere.fileName = "sphere_small.i3d" ADDrawingManager.sSphere.buffer = Buffer:new() ADDrawingManager.sSphere.objects = FlaggedTable:new() ADDrawingManager.sSphere.tasks = {} +ADDrawingManager.sSphere.lastTaskCount = 0 +ADDrawingManager.sSphere.currentTask = 0 ADDrawingManager.sSphere.itemIDs = {} ADDrawingManager.sSphere.lastDrawZero = true @@ -37,6 +44,8 @@ ADDrawingManager.sphere.fileName = "sphere.i3d" ADDrawingManager.sphere.buffer = Buffer:new() ADDrawingManager.sphere.objects = FlaggedTable:new() ADDrawingManager.sphere.tasks = {} +ADDrawingManager.sphere.lastTaskCount = 0 +ADDrawingManager.sphere.currentTask = 0 ADDrawingManager.sphere.itemIDs = {} ADDrawingManager.sphere.lastDrawZero = true @@ -48,6 +57,8 @@ ADDrawingManager.markers.fileNames[3] = "marker_3.i3d" ADDrawingManager.markers.buffer = Buffer:new() ADDrawingManager.markers.objects = FlaggedTable:new() ADDrawingManager.markers.tasks = {} +ADDrawingManager.markers.lastTaskCount = 0 +ADDrawingManager.markers.currentTask = 0 ADDrawingManager.markers.itemIDs = {} ADDrawingManager.markers.lastDrawZero = true ADDrawingManager.markers.lastDrawFileUsed = 1 @@ -58,6 +69,8 @@ ADDrawingManager.cross.fileName = "cross.i3d" ADDrawingManager.cross.buffer = Buffer:new() ADDrawingManager.cross.objects = FlaggedTable:new() ADDrawingManager.cross.tasks = {} +ADDrawingManager.cross.lastTaskCount = 0 +ADDrawingManager.cross.currentTask = 0 ADDrawingManager.cross.itemIDs = {} ADDrawingManager.cross.lastDrawZero = true @@ -89,46 +102,199 @@ end function ADDrawingManager:addLineTask(sx, sy, sz, ex, ey, ez, scale, r, g, b) -- storing task - local hash = 0 - -- local hash = string.format("l%.2f%.2f%.2f%.2f%.2f%.2f%.2f%.2f%.2f%.1f", sx, sy, sz, ex, ey, ez, r, g, b, self.yOffset) - table.insert(self.lines.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, hash = hash}) + -- local hash = 0 + -- table.insert(self.lines.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, hash = hash}) + scale = scale or 1 + self.lines.currentTask = self.lines.currentTask + 1 + if self.lines.tasks[self.lines.currentTask] == nil then + -- add new task + table.insert(self.lines.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, taskChanged = true}) + elseif + self.lines.tasks[self.lines.currentTask].sx ~= sx or + self.lines.tasks[self.lines.currentTask].sy ~= sy or + self.lines.tasks[self.lines.currentTask].sz ~= sz or + self.lines.tasks[self.lines.currentTask].ex ~= ex or + self.lines.tasks[self.lines.currentTask].ey ~= ey or + self.lines.tasks[self.lines.currentTask].ez ~= ez or + self.lines.tasks[self.lines.currentTask].scale ~= scale or + self.lines.tasks[self.lines.currentTask].r ~= r or + self.lines.tasks[self.lines.currentTask].g ~= g or + self.lines.tasks[self.lines.currentTask].b ~= b + then + -- task changed + self.lines.tasks[self.lines.currentTask].sx = sx + self.lines.tasks[self.lines.currentTask].sy = sy + self.lines.tasks[self.lines.currentTask].sz = sz + self.lines.tasks[self.lines.currentTask].ex = ex + self.lines.tasks[self.lines.currentTask].ey = ey + self.lines.tasks[self.lines.currentTask].ez = ez + self.lines.tasks[self.lines.currentTask].scale = scale + self.lines.tasks[self.lines.currentTask].r = r + self.lines.tasks[self.lines.currentTask].g = g + self.lines.tasks[self.lines.currentTask].b = b + self.lines.tasks[self.lines.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.lines.tasks[self.lines.currentTask].taskChanged = false + end end function ADDrawingManager:addArrowTask(sx, sy, sz, ex, ey, ez, scale, position, r, g, b) -- storing task - local hash = 0 - -- local hash = string.format("a%.2f%.2f%.2f%.2f%.2f%.2f%d%.2f%.2f%.2f%.1f", sx, sy, sz, ex, ey, ez, position, r, g, b, self.yOffset) - table.insert(self.arrows.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, position = position, hash = hash}) + scale = scale or 1 + -- local hash = 0 + -- table.insert(self.arrows.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, position = position, hash = hash}) + self.arrows.currentTask = self.arrows.currentTask + 1 + if self.arrows.tasks[self.arrows.currentTask] == nil then + -- add new task + table.insert(self.arrows.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, position = position, r = r, g = g, b = b, taskChanged = true}) + elseif + self.arrows.tasks[self.arrows.currentTask].sx ~= sx or + self.arrows.tasks[self.arrows.currentTask].sy ~= sy or + self.arrows.tasks[self.arrows.currentTask].sz ~= sz or + self.arrows.tasks[self.arrows.currentTask].ex ~= ex or + self.arrows.tasks[self.arrows.currentTask].ey ~= ey or + self.arrows.tasks[self.arrows.currentTask].ez ~= ez or + self.arrows.tasks[self.arrows.currentTask].scale ~= scale or + self.arrows.tasks[self.arrows.currentTask].position ~= position or + self.arrows.tasks[self.arrows.currentTask].r ~= r or + self.arrows.tasks[self.arrows.currentTask].g ~= g or + self.arrows.tasks[self.arrows.currentTask].b ~= b + then + -- task changed + self.arrows.tasks[self.arrows.currentTask].sx = sx + self.arrows.tasks[self.arrows.currentTask].sy = sy + self.arrows.tasks[self.arrows.currentTask].sz = sz + self.arrows.tasks[self.arrows.currentTask].ex = ex + self.arrows.tasks[self.arrows.currentTask].ey = ey + self.arrows.tasks[self.arrows.currentTask].ez = ez + self.arrows.tasks[self.arrows.currentTask].scale = scale + self.arrows.tasks[self.arrows.currentTask].position = position + self.arrows.tasks[self.arrows.currentTask].r = r + self.arrows.tasks[self.arrows.currentTask].g = g + self.arrows.tasks[self.arrows.currentTask].b = b + self.arrows.tasks[self.arrows.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.arrows.tasks[self.arrows.currentTask].taskChanged = false + end end function ADDrawingManager:addSmallSphereTask(x, y, z, r, g, b) -- storing task - local hash = 0 - -- local hash = string.format("ss%.2f%.2f%.2f%.2f%.2f%.2f%.1f", x, y, z, r, g, b, self.yOffset) - table.insert(self.sSphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, hash = hash}) + -- local hash = 0 + -- table.insert(self.sSphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, hash = hash}) + self.sSphere.currentTask = self.sSphere.currentTask + 1 + if self.sSphere.tasks[self.sSphere.currentTask] == nil then + -- add new task + table.insert(self.sSphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, taskChanged = true}) + elseif + self.sSphere.tasks[self.sSphere.currentTask].x ~= x or + self.sSphere.tasks[self.sSphere.currentTask].y ~= y or + self.sSphere.tasks[self.sSphere.currentTask].z ~= z or + self.sSphere.tasks[self.sSphere.currentTask].r ~= r or + self.sSphere.tasks[self.sSphere.currentTask].g ~= g or + self.sSphere.tasks[self.sSphere.currentTask].b ~= b + then + -- task changed + self.sSphere.tasks[self.sSphere.currentTask].x = x + self.sSphere.tasks[self.sSphere.currentTask].y = y + self.sSphere.tasks[self.sSphere.currentTask].z = z + self.sSphere.tasks[self.sSphere.currentTask].r = r + self.sSphere.tasks[self.sSphere.currentTask].g = g + self.sSphere.tasks[self.sSphere.currentTask].b = b + self.sSphere.tasks[self.sSphere.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.sSphere.tasks[self.sSphere.currentTask].taskChanged = false + end end function ADDrawingManager:addMarkerTask(x, y, z) -- storing task - local hash = 0 - -- local hash = string.format("m%.2f%.2f%.2f%.1f", x, y, z, self.yOffset) - table.insert(self.markers.tasks, {x = x, y = y, z = z, hash = hash}) + -- local hash = 0 + -- table.insert(self.markers.tasks, {x = x, y = y, z = z, hash = hash}) + self.markers.currentTask = self.markers.currentTask + 1 + if self.markers.tasks[self.markers.currentTask] == nil then + -- add new task + table.insert(self.markers.tasks, {x = x, y = y, z = z, taskChanged = true}) + elseif + self.markers.tasks[self.markers.currentTask].x ~= x or + self.markers.tasks[self.markers.currentTask].y ~= y or + self.markers.tasks[self.markers.currentTask].z ~= z + then + -- task changed + self.markers.tasks[self.markers.currentTask].x = x + self.markers.tasks[self.markers.currentTask].y = y + self.markers.tasks[self.markers.currentTask].z = z + self.markers.tasks[self.markers.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.markers.tasks[self.markers.currentTask].taskChanged = false + end end function ADDrawingManager:addCrossTask(x, y, z, scale) -- storing task - local hash = 0 - -- local hash = string.format("c%.2f%.2f%.2f%.1f", x, y, z, self.yOffset) - table.insert(self.cross.tasks, {x = x, y = y, z = z, scale = scale, hash = hash}) + -- local hash = 0 + -- table.insert(self.cross.tasks, {x = x, y = y, z = z, scale = scale, hash = hash}) + scale = scale or 1 + self.cross.currentTask = self.cross.currentTask + 1 + if self.cross.tasks[self.cross.currentTask] == nil then + -- add new task + table.insert(self.cross.tasks, {x = x, y = y, z = z, scale = scale, taskChanged = true}) + elseif + self.cross.tasks[self.cross.currentTask].x ~= x or + self.cross.tasks[self.cross.currentTask].y ~= y or + self.cross.tasks[self.cross.currentTask].z ~= z or + self.cross.tasks[self.cross.currentTask].scale ~= scale + then + -- task changed + self.cross.tasks[self.cross.currentTask].x = x + self.cross.tasks[self.cross.currentTask].y = y + self.cross.tasks[self.cross.currentTask].z = z + self.cross.tasks[self.cross.currentTask].scale = scale + self.cross.tasks[self.cross.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.cross.tasks[self.cross.currentTask].taskChanged = false + end end function ADDrawingManager:addSphereTask(x, y, z, scale, r, g, b, a) + -- storing task + -- local hash = 0 + -- table.insert(self.sphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, a = a, scale = scale, hash = hash}) scale = scale or 1 a = a or 0 - -- storing task - local hash = 0 - -- local hash = string.format("s%.2f%.2f%.2f%.3f%.2f%.2f%.2f%.2f%.1f", x, y, z, scale, r, g, b, a, self.yOffset) - table.insert(self.sphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, a = a, scale = scale, hash = hash}) + self.sphere.currentTask = self.sphere.currentTask + 1 + if self.sphere.tasks[self.sphere.currentTask] == nil then + -- add new task + table.insert(self.sphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, a = a, scale = scale, taskChanged = true}) + elseif + self.sphere.tasks[self.sphere.currentTask].x ~= x or + self.sphere.tasks[self.sphere.currentTask].y ~= y or + self.sphere.tasks[self.sphere.currentTask].z ~= z or + self.sphere.tasks[self.sphere.currentTask].r ~= r or + self.sphere.tasks[self.sphere.currentTask].g ~= g or + self.sphere.tasks[self.sphere.currentTask].b ~= b or + self.sphere.tasks[self.sphere.currentTask].a ~= a or + self.sphere.tasks[self.sphere.currentTask].scale ~= scale + then + -- task changed + self.sphere.tasks[self.sphere.currentTask].x = x + self.sphere.tasks[self.sphere.currentTask].y = y + self.sphere.tasks[self.sphere.currentTask].z = z + self.sphere.tasks[self.sphere.currentTask].r = r + self.sphere.tasks[self.sphere.currentTask].g = g + self.sphere.tasks[self.sphere.currentTask].b = b + self.sphere.tasks[self.sphere.currentTask].a = a + self.sphere.tasks[self.sphere.currentTask].scale = scale + self.sphere.tasks[self.sphere.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.sphere.tasks[self.sphere.currentTask].taskChanged = false + end end function ADDrawingManager:draw() @@ -181,8 +347,109 @@ function ADDrawingManager:draw() end end +function ADDrawingManager:drawObjects_alternative2(obj, dFunc, iFunc) + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 start...") + local stats = {} + stats["Tasks"] = {Total = obj.currentTask} + stats["itemIDs"] = {Total = #obj.itemIDs} + + if (obj.currentTask == 0 and obj.lastTaskCount > 0) then -- disabled obj -> set all invisible + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 disabled objects %s", tostring(obj.fileName)) + -- set all invisible + for _, itemID in ipairs (obj.itemIDs) do + setVisibility(itemID, false) + end + obj.lastTaskCount = obj.currentTask + return stats + end + + if obj.currentTask == 0 then -- nothing to draw -> exit + -- if (g_updateLoopIndex % 60 == 0) then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 nothing to draw %s", tostring(obj.fileName)) + -- end + obj.lastTaskCount = obj.currentTask + return stats + end + + local fileToUse + if obj.usesSelection then + if obj.lastDrawFileUsed ~= AutoDrive.getSetting("iconSetToUse") then + -- cleaning up not needed objects + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 iconSetToUse changed %s", tostring(obj.fileName)) + for _, id in pairs(obj.itemIDs) do + -- make invisible unused items + setVisibility(id, false) + end + obj.itemIDs = {} + end + fileToUse = obj.fileNames[AutoDrive.getSetting("iconSetToUse")] + obj.lastDrawFileUsed = AutoDrive.getSetting("iconSetToUse") + else + fileToUse = obj.fileName + end +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 fileToUse %s ", tostring(fileToUse)) + + -- check if enougth objects are available - add missing amount + if obj.currentTask > #obj.itemIDs then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %s #obj.itemIDs %s %s", tostring(obj.currentTask), tostring(#obj.itemIDs), tostring(obj.fileName)) + for i = 1, obj.currentTask - #obj.itemIDs do + -- loading new i3ds + local itemID = g_i3DManager:loadSharedI3DFile(self.i3DBaseDir .. fileToUse, false, false) + table.insert(obj.itemIDs,iFunc(itemID)) + end + end +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %s #obj.itemIDs %s ", tostring(obj.currentTask), tostring(#obj.itemIDs)) + + -- handle visibility + if obj.currentTask > 0 then + if obj.currentTask < obj.lastTaskCount then + -- set not needed items invisible + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %d < lastTaskCount %d %s", obj.currentTask, obj.lastTaskCount, tostring(obj.fileName)) + for i = obj.currentTask + 1, obj.lastTaskCount do + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 setVisibility itemID %s ", tostring(itemID)) + if obj.itemIDs[i] ~= nil then + setVisibility(obj.itemIDs[i], false) + end + end + elseif obj.currentTask > obj.lastTaskCount then + -- set previous invisible items visible + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %d > lastTaskCount %d %s", obj.currentTask, obj.lastTaskCount, tostring(obj.fileName)) + for i = obj.lastTaskCount, obj.currentTask do + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 setVisibility itemID %s ", tostring(itemID)) + if obj.itemIDs[i] ~= nil then + setVisibility(obj.itemIDs[i], true) + end + end + else + -- number of visible items not changed + -- if (g_updateLoopIndex % 60 == 0) then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask not changed %s", tostring(obj.fileName)) + -- end + end + end + + +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 drawing... ") + -- drawing tasks + local index = 1 + for _, task in pairs(obj.tasks) do + local itemId = obj.itemIDs[index] +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 index %s itemId %s ", tostring(index), tostring(itemId)) + if task.taskChanged then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 dFunc %s", tostring(obj.fileName)) + dFunc(self, itemId, task) + task.taskChanged = false + end + index = index + 1 + end +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 end ") + obj.lastTaskCount = obj.currentTask + obj.currentTask = 0 + return stats +end + function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects start...") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative start...") local taskCount = #obj.tasks local stats = {} stats["Tasks"] = {Total = taskCount} @@ -191,13 +458,13 @@ function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) if taskCount > 0 or obj.lastDrawZero == false then -- set all invisible for _, itemID in ipairs (obj.itemIDs) do - -- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects setVisibility itemID %s ", tostring(itemID)) + -- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative setVisibility itemID %s ", tostring(itemID)) setVisibility(itemID, false) end end if taskCount == 0 then -- nothing to draw --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects nothing to draw ") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative nothing to draw ") obj.lastDrawZero = true return stats end @@ -217,9 +484,9 @@ function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) else fileToUse = obj.fileName end --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects fileToUse %s ", tostring(fileToUse)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative fileToUse %s ", tostring(fileToUse)) --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) -- check if enougth objects are available - add missing amount if taskCount > #obj.itemIDs then for i = 1, taskCount - #obj.itemIDs do @@ -230,19 +497,19 @@ function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) table.insert(obj.itemIDs,iFunc(itemID)) end end --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects drawing... ") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative drawing... ") -- drawing tasks local index = 1 for _, task in pairs(obj.tasks) do local itemId = obj.itemIDs[index] --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects index %s itemId %s ", tostring(index), tostring(itemId)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative index %s itemId %s ", tostring(index), tostring(itemId)) dFunc(self, itemId, task) index = index + 1 end obj.tasks = {} --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects end ") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative end ") obj.lastDrawZero = taskCount == 0 return stats end @@ -328,7 +595,8 @@ function ADDrawingManager:drawObjects_original(obj, dFunc, iFunc) end function ADDrawingManager:drawObjects(obj, dFunc, iFunc) - return self:drawObjects_alternative(obj, dFunc, iFunc) + return self:drawObjects_alternative2(obj, dFunc, iFunc) + -- return self:drawObjects_alternative(obj, dFunc, iFunc) -- return self:drawObjects_original(obj, dFunc, iFunc) end @@ -356,9 +624,6 @@ function ADDrawingManager:drawLine(id, task) -- Update line color setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity, false) - - -- Update line visibility - setVisibility(id, true) end function ADDrawingManager:drawArrow(id, task) @@ -395,32 +660,31 @@ function ADDrawingManager:drawArrow(id, task) -- Update arrow color setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity, false) - - -- Update arrow visibility - setVisibility(id, true) end function ADDrawingManager:drawSmallSphere(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity, false) - setVisibility(id, true) end function ADDrawingManager:drawMarker(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) - setVisibility(id, true) end function ADDrawingManager:drawCross(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) local scaleLines = (AutoDrive.getSetting("scaleLines") or 1) * (task.scale or 1) setScale(id, scaleLines, scaleLines, scaleLines) - setVisibility(id, true) end function ADDrawingManager:drawSphere(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) setScale(id, task.scale, task.scale, task.scale) setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity + task.a, false) - setVisibility(id, true) +end + +function ADDrawingManager.debugMsg(vehicle, debugText, ...) + if ADDrawingManager.enableDebug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + end end diff --git a/scripts/Manager/GraphManager.lua b/scripts/Manager/GraphManager.lua index 7f761fb2..86873271 100644 --- a/scripts/Manager/GraphManager.lua +++ b/scripts/Manager/GraphManager.lua @@ -120,10 +120,12 @@ function ADGraphManager:setMapMarkers(mapMarkers) -- create debug markers, debug markers are not saved, so no need to delete them or update map hotspots required -- notifyDestinationListeners is called from caller function -> argument is false self:createDebugMarkers(false) + self:markChanges() end function ADGraphManager:setMapMarker(mapMarker) self.mapMarkers[mapMarker.markerIndex] = mapMarker + self:markChanges() end function ADGraphManager:getPathTo(vehicle, waypointId, startPoint) @@ -1137,6 +1139,7 @@ function ADGraphManager:removeDebugMarkers() end end end + self:markChanges() end -- create debug markers for waypoints issues @@ -1298,6 +1301,7 @@ function ADGraphManager:createDebugMarkers(updateMap) if shouldUpdateMap == true then AutoDrive:notifyDestinationListeners() end + self:markChanges() end function ADGraphManager:checkForWrongReverseStart(wp_ref, wp_current, wp_ahead) diff --git a/scripts/Specialization.lua b/scripts/Specialization.lua index 53ce4e91..ca7c07c0 100644 --- a/scripts/Specialization.lua +++ b/scripts/Specialization.lua @@ -361,17 +361,6 @@ function AutoDrive:onUpdateTick(dt, isActiveForInput, isActiveForInputIgnoreSele -- self:resetClosestWayPoint() -- if we want to update distances every frame, when lines drawing is enabled, we can move this at the end of onDraw function - if AutoDrive.isEditorShowEnabled() then - local x, y, z = getWorldTranslation(self.components[1].node) - local distance = MathUtil.vector2Length(x - self.ad.lastDrawPosition.x, z - self.ad.lastDrawPosition.z) - if distance > AutoDrive.drawDistance / 2 then - self.ad.lastDrawPosition = {x = x, z = z} - self:resetWayPointsDistance() - end - else - self:resetWayPointsDistance() - end - if self.isServer then self.ad.recordingModule:updateTick(dt, isActiveForInput, isActiveForInputIgnoreSelection, isSelected) @@ -761,6 +750,7 @@ function AutoDrive:onEnterVehicle(isControlling) self.ad.stateModule:setActualFarmId(self.ad.stateModule:getPlayerFarmId()) -- onEnterVehicle end end + self:resetWayPointsDistance() end function AutoDrive:onLeaveVehicle(wasEntered) @@ -810,6 +800,20 @@ function AutoDrive:onDrawEditorMode() and not AutoDrive.leftALTmodifierKeyPressed and not AutoDrive.rightSHIFTmodifierKeyPressed + if AutoDrive.isEditorShowEnabled() or AutoDrive.isInExtendedEditorMode() then + local x, y, z = getWorldTranslation(self.components[1].node) + local distance = MathUtil.vector2Length(x - self.ad.lastDrawPosition.x, z - self.ad.lastDrawPosition.z) + if distance > AutoDrive.drawDistance / 2 then + self.ad.lastDrawPosition = {x = x, z = z} + self:resetWayPointsDistance() + end + end + + if AutoDrive:getIsEntered(self) and ADGraphManager:hasChanges() then + self:resetWayPointsDistance() + ADGraphManager:resetChanges() + end + --Draw close destinations for _, marker in pairs(ADGraphManager:getMapMarkers()) do local wp = ADGraphManager:getWayPointById(marker.id) @@ -1455,9 +1459,7 @@ function AutoDrive:getClosestWayPoint(noUpdate) end function AutoDrive:getClosestNotReversedWayPoint() - if self.ad.distances.closestNotReverse.wayPoint == -1 then - self:updateWayPointsDistance() - end + self:updateWayPointsDistance() if self.ad.distances.closestNotReverse.wayPoint ~= nil then return self.ad.distances.closestNotReverse.wayPoint.id, self.ad.distances.closestNotReverse.distance end @@ -1478,9 +1480,7 @@ function AutoDrive:getWayPointsInRange(minDistance, maxDistance) end function AutoDrive:getWayPointIdsInRange(minDistance, maxDistance) - if self.ad.distances.wayPoints == nil then - self:updateWayPointsDistance() - end + self:updateWayPointsDistance() local inRange = {} for _, elem in pairs(self.ad.distances.wayPoints) do if elem.distance >= minDistance and elem.distance <= maxDistance and elem.wayPoint.id > 0 then From 8b92562af00d91a19d8e15271c27a747226b5b5d Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 19 May 2024 22:27:56 +0200 Subject: [PATCH 05/44] improved harvester unloader cooperation - unload CP harvester only if requested - more intelligent clear crop --- modDesc.xml | 2 +- scripts/AutoDrive.lua | 2 +- scripts/ExternalInterface.lua | 3 +- scripts/Manager/HarvestManager.lua | 48 ++++--- scripts/Modes/CombineUnloaderMode.lua | 77 +++++------ scripts/Tasks/CatchCombinePipeTask.lua | 74 +++++++---- scripts/Tasks/ClearCropTask.lua | 170 +++++++++++++++++++------ scripts/Tasks/EmptyHarvesterTask.lua | 61 +++++++-- scripts/Tasks/FollowCombineTask.lua | 152 +++++++++++++--------- 9 files changed, 399 insertions(+), 190 deletions(-) diff --git a/modDesc.xml b/modDesc.xml index c91c25ee..4f65d788 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -32,7 +32,7 @@ Différents modes d'utilisation ont été ajoutés depuis les premières version - 2.0.1.7-RC1 + 2.0.1.7-RC2 icon.dds diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index ef8e8e8e..7864f26c 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -1,5 +1,5 @@ AutoDrive = {} -AutoDrive.version = "2.0.1.7-RC1" +AutoDrive.version = "2.0.1.7-RC2" AutoDrive.directory = g_currentModDirectory diff --git a/scripts/ExternalInterface.lua b/scripts/ExternalInterface.lua index 70ac2923..2638b3e4 100644 --- a/scripts/ExternalInterface.lua +++ b/scripts/ExternalInterface.lua @@ -209,8 +209,7 @@ function AutoDrive:notifyDestinationListeners() end function AutoDrive:combineIsCallingDriver(combine) --only for CoursePlay - local openPipe,_ = ADHarvestManager.getOpenPipePercent(combine) - return openPipe or ADHarvestManager.doesHarvesterNeedUnloading(combine, true) + return AutoDrive:getIsCPWaitingForUnload(combine) end function AutoDrive:getCombineOpenPipePercent(combine) --for AIVE diff --git a/scripts/Manager/HarvestManager.lua b/scripts/Manager/HarvestManager.lua index f5249885..cd90f573 100644 --- a/scripts/Manager/HarvestManager.lua +++ b/scripts/Manager/HarvestManager.lua @@ -1,4 +1,5 @@ ADHarvestManager = {} +ADHarvestManager.debug = false ADHarvestManager.MAX_PREDRIVE_LEVEL = 0.85 ADHarvestManager.MAX_SEARCH_RANGE = 300 @@ -12,9 +13,9 @@ function ADHarvestManager:load() end function ADHarvestManager:registerHarvester(harvester) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerHarvester") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:registerHarvester") if not table.contains(self.idleHarvesters, harvester) and not table.contains(self.harvesters, harvester) then - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerHarvester - inserted") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:registerHarvester - inserted") if harvester ~= nil and harvester.ad ~= nil then local rootVehicle = harvester:getRootVehicle() rootVehicle.ad.isRegisterdHarvester = true @@ -26,7 +27,7 @@ function ADHarvestManager:registerHarvester(harvester) end function ADHarvestManager:unregisterHarvester(harvester) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:unregisterHarvester") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:unregisterHarvester") if harvester ~= nil and harvester.ad ~= nil then local rootVehicle = harvester:getRootVehicle() rootVehicle.ad.isRegisterdHarvester = false @@ -35,22 +36,22 @@ function ADHarvestManager:unregisterHarvester(harvester) if table.contains(self.idleHarvesters, harvester) then local index = table.indexOf(self.idleHarvesters, harvester) local harvester = table.remove(self.idleHarvesters, index) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:unregisterHarvester - removed - idleHarvesters") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:unregisterHarvester - removed - idleHarvesters") end if table.contains(self.harvesters, harvester) then local index = table.indexOf(self.harvesters, harvester) local harvester = table.remove(self.harvesters, index) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:unregisterHarvester - removed - harvesters") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:unregisterHarvester - removed - harvesters") end end end function ADHarvestManager:registerAsUnloader(vehicle) - AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerAsUnloader") + ADHarvestManager.debugMsg(vehicle, "ADHarvestManager:registerAsUnloader") --remove from active and idle list self:unregisterAsUnloader(vehicle) if not table.contains(self.idleUnloaders, vehicle) then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerAsUnloader - inserted") + ADHarvestManager.debugMsg(vehicle, "ADHarvestManager:registerAsUnloader - inserted") table.insert(self.idleUnloaders, vehicle) end end @@ -79,14 +80,20 @@ function ADHarvestManager:unregisterAsUnloader(vehicle) end end -function ADHarvestManager:fireUnloader(unloader) +function ADHarvestManager:fireUnloader(unloader, harvester) if unloader.ad.stateModule:isActive() then local follower = unloader.ad.modes[AutoDrive.MODE_UNLOAD]:getFollowingUnloader() if follower ~= nil then follower.ad.taskModule:abortAllTasks() + if AutoDrive.getSetting("parkInField", follower) then + follower.ad.taskModule:addTask(ClearCropTask:new(follower, harvester)) + end follower.ad.taskModule:addTask(StopAndDisableADTask:new(follower, ADTaskModule.DONT_PROPAGATE, true)) end unloader.ad.taskModule:abortAllTasks() + if AutoDrive.getSetting("parkInField", unloader) then + unloader.ad.taskModule:addTask(ClearCropTask:new(unloader, harvester)) + end unloader.ad.taskModule:addTask(StopAndDisableADTask:new(unloader, ADTaskModule.DONT_PROPAGATE, true)) end self:unregisterAsUnloader(unloader) @@ -115,7 +122,7 @@ function ADHarvestManager:update(dt) local unloader = self:getAssignedUnloader(harvester) if unloader ~= nil then - self:fireUnloader(unloader) + self:fireUnloader(unloader, harvester) end end end @@ -207,7 +214,6 @@ end function ADHarvestManager.doesHarvesterNeedUnloading(harvester, ignorePipe) local ret = false - local _, maxCapacity, _, leftCapacity = AutoDrive.getObjectFillLevels(harvester) local cpIsCalling = AutoDrive:getIsCPWaitingForUnload(harvester) @@ -215,18 +221,19 @@ function ADHarvestManager.doesHarvesterNeedUnloading(harvester, ignorePipe) ret = ( ( ( - (maxCapacity > 0 and leftCapacity / maxCapacity < 0.2) - -- (maxCapacity > 0 and leftCapacity < 1.0) - or cpIsCalling ) - and - -- (pipeOut or ignorePipe) - (pipeOut or (ignorePipe == true)) + or + (pipeOut) ) and harvester.ad.noMovementTimer.elapsedTime > 5000 ) + ADHarvestManager.debugMsg(harvester, "ADHarvestManager.doesHarvesterNeedUnloading cpIsCalling %s pipeOut %s noMovementTimer %s" + , tostring(cpIsCalling) + , tostring(pipeOut) + , tostring(harvester.ad.noMovementTimer.elapsedTime > 5000) + ) return ret end @@ -265,6 +272,7 @@ function ADHarvestManager.isHarvesterActive(harvester) if manuallyControlled then return AutoDrive.isPipeOut(harvester) end + ADHarvestManager.debugMsg(harvester, "ADHarvestManager.isHarvesterActive reachedPreCallLevel %s isAlmostFull %s allowedToChase %s", reachedPreCallLevel, isAlmostFull, allowedToChase) return reachedPreCallLevel and (not isAlmostFull) and allowedToChase end @@ -375,3 +383,11 @@ function ADHarvestManager.getOpenPipePercent(harvester) end return openPipe, pipePercent end + +function ADHarvestManager.debugMsg(vehicle, debugText, ...) + if ADHarvestManager.debug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) + end +end diff --git a/scripts/Modes/CombineUnloaderMode.lua b/scripts/Modes/CombineUnloaderMode.lua index e90529bb..ad312cb2 100644 --- a/scripts/Modes/CombineUnloaderMode.lua +++ b/scripts/Modes/CombineUnloaderMode.lua @@ -1,4 +1,5 @@ CombineUnloaderMode = ADInheritsFrom(AbstractMode) +CombineUnloaderMode.debug = false CombineUnloaderMode.STATE_INIT = {} CombineUnloaderMode.STATE_WAIT_TO_BE_CALLED = {} @@ -43,7 +44,7 @@ function CombineUnloaderMode:reset() end function CombineUnloaderMode:start(user) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:start start self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:start start self.state %s", tostring(self:getStateName())) if not self.vehicle.ad.stateModule:isActive() then self.vehicle:startAutoDrive() end @@ -58,13 +59,13 @@ function CombineUnloaderMode:start(user) if self.activeTask ~= nil then self.vehicle.ad.taskModule:addTask(self.activeTask) end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:start end self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:start end self.state %s", tostring(self:getStateName())) end function CombineUnloaderMode:monitorTasks(dt) if self.combine ~= nil and (self.state == self.STATE_DRIVE_TO_START or self.state == self.STATE_DRIVE_TO_UNLOAD or self.state == self.STATE_EXIT_FIELD) then if AutoDrive.getDistanceBetween(self.vehicle, self.combine) > 25 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:monitorTasks -> unregisterAsUnloader") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:monitorTasks -> unregisterAsUnloader") ADHarvestManager:unregisterAsUnloader(self.vehicle) self.followingUnloader = nil self.combine = nil @@ -75,7 +76,7 @@ function CombineUnloaderMode:monitorTasks(dt) end --We are stuck if self.failedPathFinder >= 5 or ((self.vehicle.ad.specialDrivingModule:shouldStopMotor() or self.vehicle.ad.specialDrivingModule.stoppedTimer:done()) and self.vehicle.ad.specialDrivingModule.isBlocked) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:monitorTasks() - detected stuck vehicle - try reversing out of it now") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:monitorTasks() - detected stuck vehicle - try reversing out of it now") self.vehicle.ad.specialDrivingModule:releaseVehicle() self.vehicle.ad.taskModule:abortAllTasks() self.activeTask = ReverseFromBadLocationTask:new(self.vehicle) @@ -90,7 +91,7 @@ function CombineUnloaderMode:monitorTasks(dt) end function CombineUnloaderMode:notifyAboutFailedPathfinder() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:notifyAboutFailedPathfinder self.failedPathFinder %s ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20 %s", tostring(self.failedPathFinder), tostring(ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:notifyAboutFailedPathfinder self.failedPathFinder %s ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20 %s", tostring(self.failedPathFinder), tostring(ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20)) --print("CombineUnloaderMode:notifyAboutFailedPathfinder() - blocked: " .. tostring(self.vehicle.ad.pathFinderModule.completelyBlocked) .. " distance: " .. ADGraphManager:getDistanceFromNetwork(self.vehicle)) if self.vehicle.ad.pathFinderModule.completelyBlocked and ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20 then self.failedPathFinder = self.failedPathFinder + 1 @@ -134,14 +135,14 @@ function CombineUnloaderMode:promoteFollowingUnloader(combine) end function CombineUnloaderMode:handleFinishedTask() - -- AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:handleFinishedTask") + -- CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:handleFinishedTask") self.vehicle.ad.trailerModule:reset() self.lastTask = self.activeTask self.activeTask = self:getNextTask() if self.activeTask ~= nil then self.vehicle.ad.taskModule:addTask(self.activeTask) end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:handleFinishedTask self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:handleFinishedTask self.state %s", tostring(self:getStateName())) end function CombineUnloaderMode:stop() @@ -163,15 +164,15 @@ function CombineUnloaderMode:continue() end if self.state == self.STATE_DRIVE_TO_UNLOAD then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue self.STATE_DRIVE_TO_UNLOAD") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue self.STATE_DRIVE_TO_UNLOAD") self.activeTask:continue() else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue self.state" .. tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue self.state" .. tostring(self:getStateName())) self.vehicle.ad.taskModule:abortCurrentTask() if AutoDrive.checkIsOnField(x, y, z) and distanceToStart > 30 then -- is activated on a field - use ExitFieldTask to leave field according to setting - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue ExitFieldTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue ExitFieldTask...") self.activeTask = ExitFieldTask:new(self.vehicle) self.state = self.STATE_EXIT_FIELD else @@ -181,7 +182,7 @@ function CombineUnloaderMode:continue() self.vehicle.ad.stateModule:setSecondMarker(nextTarget) end end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue UnloadAtDestinationTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue UnloadAtDestinationTask...") self.activeTask = UnloadAtDestinationTask:new(self.vehicle, self.vehicle.ad.stateModule:getSecondMarker().id) self.state = self.STATE_DRIVE_TO_UNLOAD end @@ -194,7 +195,7 @@ function CombineUnloaderMode:continue() end function CombineUnloaderMode:getNextTask() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask start self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask start self.state %s", tostring(self:getStateName())) local nextTask local x, y, z = getWorldTranslation(self.vehicle.components[1].node) @@ -202,7 +203,7 @@ function CombineUnloaderMode:getNextTask() local _, _, filledToUnload, _ = AutoDrive.getAllFillLevels(self.trailers) if self.state == self.STATE_INIT then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask STATE_INIT filledToUnload %s", tostring(filledToUnload)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask STATE_INIT filledToUnload %s", tostring(filledToUnload)) if filledToUnload then -- fill level above setting unload level if (AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_ONLYDELIVER or AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_PICKUPANDDELIVER) and AutoDrive.getSetting("useFolders") then local nextTarget = ADMultipleTargetsManager:getNextTarget(self.vehicle, false) @@ -224,9 +225,9 @@ function CombineUnloaderMode:getNextTask() self:setToWaitForCall() end end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_INIT end self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_INIT end self.state %s", tostring(self:getStateName())) elseif self.state == self.STATE_DRIVE_TO_COMBINE then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_COMBINE") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_COMBINE") -- we finished the precall to combine route -- check if we should wait / pull up to combines pipe nextTask = FollowCombineTask:new(self.vehicle, self.combine) @@ -234,14 +235,14 @@ function CombineUnloaderMode:getNextTask() self.breadCrumbs = Queue:new() self.lastBreadCrumb = nil elseif self.state == self.STATE_DRIVE_TO_PIPE or self.state == self.STATE_REVERSE_FROM_BAD_LOCATION then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_PIPE | STATE_REVERSE_FROM_BAD_LOCATION") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_PIPE | STATE_REVERSE_FROM_BAD_LOCATION") --Drive to pipe can be finished when combine is emptied or when vehicle has reached 'old' pipe position and should switch to active mode nextTask = self:getTaskAfterUnload(filledToUnload) elseif self.state == self.STATE_LEAVE_CROP then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_LEAVE_CROP") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_LEAVE_CROP") self:setToWaitForCall() elseif self.state == self.STATE_DRIVE_TO_UNLOAD then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_UNLOAD") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_UNLOAD") nextTask = DriveToDestinationTask:new(self.vehicle, self.vehicle.ad.stateModule:getFirstMarker().id) if (AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_ONLYDELIVER or AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_PICKUPANDDELIVER) and AutoDrive.getSetting("useFolders") then local nextTarget = ADMultipleTargetsManager:getNextTarget(self.vehicle, true) @@ -251,10 +252,10 @@ function CombineUnloaderMode:getNextTask() end self.state = self.STATE_DRIVE_TO_START elseif self.state == self.STATE_DRIVE_TO_START then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_START") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_START") self:setToWaitForCall() elseif self.state == self.STATE_ACTIVE_UNLOAD_COMBINE then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_ACTIVE_UNLOAD_COMBINE") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_ACTIVE_UNLOAD_COMBINE") if filledToUnload then nextTask = self:getTaskAfterUnload(filledToUnload) else @@ -262,13 +263,13 @@ function CombineUnloaderMode:getNextTask() self.state = self.STATE_DRIVE_TO_COMBINE end elseif self.state == self.STATE_FOLLOW_CURRENT_UNLOADER then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_FOLLOW_CURRENT_UNLOADER") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_FOLLOW_CURRENT_UNLOADER") if self.targetUnloader ~= nil then self.targetUnloader.ad.modes[AutoDrive.MODE_UNLOAD]:unregisterFollowingUnloader() end nextTask = self:getTaskAfterUnload(filledToUnload) elseif self.state == self.STATE_EXIT_FIELD then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_EXIT_FIELD") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_EXIT_FIELD") if (AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_ONLYDELIVER or AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_PICKUPANDDELIVER) and AutoDrive.getSetting("useFolders") then local nextTarget = ADMultipleTargetsManager:getNextTarget(self.vehicle, false) if nextTarget ~= nil then @@ -279,19 +280,19 @@ function CombineUnloaderMode:getNextTask() self.state = self.STATE_DRIVE_TO_UNLOAD end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask end self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask end self.state %s", tostring(self:getStateName())) return nextTask end function CombineUnloaderMode:setToWaitForCall(keepCombine) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:setToWaitForCall start self.state %s keepCombine %s", tostring(self.state), tostring(keepCombine)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:setToWaitForCall start self.state %s keepCombine %s", tostring(self:getStateName()), tostring(keepCombine)) -- We just have to wait to be wait to be called (again) self.state = self.STATE_WAIT_TO_BE_CALLED self.vehicle.ad.taskModule:addTask(WaitForCallTask:new(self.vehicle)) if self.combine ~= nil and self.combine.ad ~= nil and (keepCombine == nil or keepCombine ~= true) then self.combine = nil end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:setToWaitForCall end self.state %s self.combine %s", tostring(self:getStateName()), tostring(self.combine)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:setToWaitForCall end self.state %s self.combine %s", tostring(self:getStateName()), tostring(self.combine)) end function CombineUnloaderMode:assignToHarvester(harvester) @@ -336,7 +337,7 @@ end function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) local nextTask - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload start filledToUnload %s", tostring(filledToUnload)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload start filledToUnload %s", tostring(filledToUnload)) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) local point = nil @@ -358,11 +359,11 @@ function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) --self.combine = nil if AutoDrive.checkIsOnField(x, y, z) and distanceToStart > 30 then -- is activated on a field - use ExitFieldTask to leave field according to setting - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload ExitFieldTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload ExitFieldTask...") nextTask = ExitFieldTask:new(self.vehicle) self.state = self.STATE_EXIT_FIELD else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload UnloadAtDestinationTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload UnloadAtDestinationTask...") nextTask = UnloadAtDestinationTask:new(self.vehicle, self.vehicle.ad.stateModule:getSecondMarker().id) self.state = self.STATE_DRIVE_TO_UNLOAD end @@ -371,11 +372,11 @@ function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) if (self.combine and self.combine.ad.isChopper) or (AutoDrive.getSetting("parkInField", self.vehicle) or (self.lastTask ~= nil and self.lastTask.stayOnField)) then -- If we are in fruit, we should clear it if AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload ClearCropTask...") - nextTask = ClearCropTask:new(self.vehicle) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload ClearCropTask...") + nextTask = ClearCropTask:new(self.vehicle, self.combine) self.state = self.STATE_LEAVE_CROP else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload setToWaitForCall") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload setToWaitForCall") self:setToWaitForCall() end else @@ -384,7 +385,7 @@ function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) self.state = self.STATE_DRIVE_TO_START end end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload end") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload end") return nextTask end @@ -543,18 +544,18 @@ function CombineUnloaderMode:getPipeChaseWayPoint(offsetX, offsetZ) local wayPoint = {x = 0, y = 0, z = 0} local trailer = g_currentMission.nodeToObject[self.targetFillNode] if trailer == nil then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint trailer == nil") + CombineUnloaderMode.debugMsg(self.vehicle, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint trailer == nil") trailer = self.vehicle end local _, trailerDistY, _ = localToLocal(self.targetFillNode, trailer.components[1].node, 0, 0, 0) local dischargeNode = AutoDrive.getDischargeNode(self.combine) if dischargeNode == nil then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint dischargeNode == nil") + CombineUnloaderMode.debugMsg(self.vehicle, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint dischargeNode == nil") dischargeNode = self.combine.components[1].node end local combinePipeRootNode = AutoDrive.getPipeRoot(self.combine) if combinePipeRootNode == nil then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint combinePipeRootNode == nil") + CombineUnloaderMode.debugMsg(self.vehicle, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint combinePipeRootNode == nil") combinePipeRootNode = self.combine.components[1].node end local targetX, targetY, targetZ = getWorldTranslation(dischargeNode) @@ -717,7 +718,7 @@ function CombineUnloaderMode:getPipeChasePosition(planningPhase) if self.combine.ad.isChopper then -- any chopper - --AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getPipeChasePosition=IsBufferCombine") + --CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getPipeChasePosition=IsBufferCombine") local leftChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, sideChaseTermX + self:getPipeSlopeCorrection(), sideChaseTermZ - 2) local rightChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, -(sideChaseTermX + self:getPipeSlopeCorrection()), sideChaseTermZ - 2) local rearChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, 0, rearChaseTermZ) @@ -754,7 +755,7 @@ function CombineUnloaderMode:getPipeChasePosition(planningPhase) end else -- harveser with bunker - --AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getPipeChasePosition:IsNormalCombine") + --CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getPipeChasePosition:IsNormalCombine") local rearChaseTermX = self:getRearChaseOffsetX(leftBlocked, rightBlocked) local sideChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, self.pipeSide * (sideChaseTermX + self:getPipeSlopeCorrection()), sideChaseTermZ) @@ -873,5 +874,7 @@ end function CombineUnloaderMode.debugMsg(vehicle, debugText, ...) if CombineUnloaderMode.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end diff --git a/scripts/Tasks/CatchCombinePipeTask.lua b/scripts/Tasks/CatchCombinePipeTask.lua index e25d9fae..933a2188 100644 --- a/scripts/Tasks/CatchCombinePipeTask.lua +++ b/scripts/Tasks/CatchCombinePipeTask.lua @@ -1,13 +1,14 @@ CatchCombinePipeTask = ADInheritsFrom(AbstractTask) +CatchCombinePipeTask.debug = false CatchCombinePipeTask.TARGET_DISTANCE = 15 -CatchCombinePipeTask.STATE_PATHPLANNING = 1 -CatchCombinePipeTask.STATE_DRIVING = 2 -CatchCombinePipeTask.STATE_REVERSING = 3 -CatchCombinePipeTask.STATE_DELAY_PATHPLANNING = 4 -CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH = 5 -CatchCombinePipeTask.STATE_FINISHED = 6 +CatchCombinePipeTask.STATE_PATHPLANNING = {} +CatchCombinePipeTask.STATE_DRIVING = {} +CatchCombinePipeTask.STATE_REVERSING = {} +CatchCombinePipeTask.STATE_DELAY_PATHPLANNING = {} +CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH = {} +CatchCombinePipeTask.STATE_FINISHED = {} CatchCombinePipeTask.MAX_REVERSE_DISTANCE = 18 CatchCombinePipeTask.MIN_COMBINE_DISTANCE = 25 @@ -29,11 +30,12 @@ function CatchCombinePipeTask:new(vehicle, combine) o.taskType = "CatchCombinePipeTask" o.newPathFindingCounter = 0 o.trailers = nil + CatchCombinePipeTask.setStateNames(o) return o end function CatchCombinePipeTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:setUp()") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:setUp()") local angleToCombineHeading = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombineHeading() local angleToCombine = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombine() @@ -50,17 +52,21 @@ function CatchCombinePipeTask:update(dt) self:finished() return end + if self.lastState ~= self.state then + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + end if self.state == CatchCombinePipeTask.STATE_PATHPLANNING then if self.vehicle.ad.pathFinderModule:hasFinished() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_PATHPLANNING finished") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_PATHPLANNING finished") self.newPathFindingCounter = 0 self.wayPoints = self.vehicle.ad.pathFinderModule:getPath() if self.wayPoints == nil or #self.wayPoints < 1 then --restart self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() --AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.WARN, "$l10n_AD_Driver_of; %s $l10n_AD_cannot_find_path; %s", 5000, self.vehicle.ad.stateModule:getName(), self.combine.ad.stateModule:getName()) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_PATHPLANNING restarting path finder - with delay") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_PATHPLANNING restarting path finder - with delay") self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING return else @@ -74,7 +80,7 @@ function CatchCombinePipeTask:update(dt) self.vehicle.ad.specialDrivingModule:update(dt) end elseif self.state == CatchCombinePipeTask.STATE_DELAY_PATHPLANNING then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DELAY_PATHPLANNING") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DELAY_PATHPLANNING") if self.waitForCheckTimer:timer(true, 1000, dt) then if self:startNewPathFinding() then self.vehicle.ad.pathFinderModule:addDelayTimer(6000) @@ -99,7 +105,7 @@ function CatchCombinePipeTask:update(dt) -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE then -- got stuck or to close to combine -> reverse - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DRIVING stuck") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING stuck") self.stuckTimer:timer(false) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} @@ -108,23 +114,23 @@ function CatchCombinePipeTask:update(dt) end if combineTravelDistance > 85 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - combine travelled - recalculate path") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - combine travelled - recalculate path") self.waitForCheckTimer.elapsedTime = 4000 self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING else if self.vehicle.ad.drivePathModule:isTargetReached() then -- check if we have actually reached the target or not -- accept current location if we are in a good position to start chasing: distance and angle are important here - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DRIVING TargetReached") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING TargetReached") local angleToCombine = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombineHeading() local isCorrectSide = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() if angleToCombine < 35 and AutoDrive.getDistanceBetween(self.vehicle, self.combine) < 80 and isCorrectSide then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DRIVING -> STATE_FINISHED") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING -> STATE_FINISHED") self.state = CatchCombinePipeTask.STATE_FINISHED return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - angle or distance to combine too high - recalculate path now") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - angle or distance to combine too high - recalculate path now") self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING end else @@ -142,7 +148,7 @@ function CatchCombinePipeTask:update(dt) self.vehicle.ad.specialDrivingModule:driveReverse(dt, 15, 1, self.vehicle.ad.trailerModule:canBeHandledInReverse()) end elseif self.state == CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_WAIT_BEFORE_FINISH") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_WAIT_BEFORE_FINISH") self.waitTimer:timer(true, self.MAX_REVERSE_TIME, dt) if self.waitTimer:done() then self.waitTimer:timer(false) @@ -153,7 +159,7 @@ function CatchCombinePipeTask:update(dt) self.vehicle.ad.specialDrivingModule:update(dt) end elseif self.state == CatchCombinePipeTask.STATE_FINISHED then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_FINISHED") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_FINISHED") self:finished() return end @@ -163,12 +169,12 @@ function CatchCombinePipeTask:abort() end function CatchCombinePipeTask:finished(propagate) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:finished()") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:finished()") self.vehicle.ad.taskModule:setCurrentTaskFinished(propagate) end function CatchCombinePipeTask:startNewPathFinding() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding()") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding()") local pipeChasePos, pipeChaseSide = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getPipeChasePosition(true) local x, _, z = getWorldTranslation(self.combine.components[1].node) local targetFieldId = g_farmlandManager:getFarmlandIdAtWorldPosition(pipeChasePos.x, pipeChasePos.z) @@ -181,7 +187,7 @@ function CatchCombinePipeTask:startNewPathFinding() local cFillRatio = cfillCapacity > 0 and cfillLevel / cfillCapacity or 0 if cFillRatio > 0.91 or cfillFreeCapacity < 0.1 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding() - Combine is almost full - dont chase for active unloading anymore") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding() - Combine is almost full - dont chase for active unloading anymore") self:finished(ADTaskModule.DONT_PROPAGATE) self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:setToWaitForCall() return false @@ -192,14 +198,14 @@ function CatchCombinePipeTask:startNewPathFinding() if self.combine.ad.isChopper or (pipeChaseSide ~= AutoDrive.CHASEPOS_REAR or (targetFieldId == combineFieldId and cFillRatio <= 0.85)) then -- if self.combine:getIsBufferCombine() or (pipeChaseSide ~= AutoDrive.CHASEPOS_REAR and targetFieldId == combineFieldId and cFillRatio <= 0.85) then -- is chopper or chase not rear and harvester on correct field and filled < 85% - i.e. combine pipe not in fruit - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks good - calculate path to it...") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks good - calculate path to it...") self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) -- use false to enable pathfinder fallback self.combinesStartLocation = {} self.combinesStartLocation.x, self.combinesStartLocation.y, self.combinesStartLocation.z = getWorldTranslation(self.combine.components[1].node) return true else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks bad, is not on the same field or combine's fill level is approaching limit - aborting for now") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks bad, is not on the same field or combine's fill level is approaching limit - aborting for now") self.waitForCheckTimer:timer(false) end return false @@ -213,6 +219,28 @@ function CatchCombinePipeTask:getExcludedVehiclesForCollisionCheck() return excludedVehicles end +function CatchCombinePipeTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(CatchCombinePipeTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function CatchCombinePipeTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] CatchCombinePipeTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" +end + function CatchCombinePipeTask:getI18nInfo() local text = "$l10n_AD_task_catch_up_with_combine;" if self.state == CatchCombinePipeTask.STATE_PATHPLANNING then @@ -231,5 +259,7 @@ end function CatchCombinePipeTask.debugMsg(vehicle, debugText, ...) if CatchCombinePipeTask.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end diff --git a/scripts/Tasks/ClearCropTask.lua b/scripts/Tasks/ClearCropTask.lua index 819b69a4..d4719017 100644 --- a/scripts/Tasks/ClearCropTask.lua +++ b/scripts/Tasks/ClearCropTask.lua @@ -1,24 +1,34 @@ ClearCropTask = ADInheritsFrom(AbstractTask) +ClearCropTask.debug = false ClearCropTask.TARGET_DISTANCE_SIDE = 10 ClearCropTask.TARGET_DISTANCE_FRONT_STEP = 10 -ClearCropTask.STATE_CLEARING = 1 -ClearCropTask.STATE_REVERSING = 2 +ClearCropTask.MAX_HARVESTER_DISTANCE = 50 +ClearCropTask.WAIT_TIME = 10000 +ClearCropTask.STUCK_TIME = 60000 +ClearCropTask.STATE_CLEARING_FIRST = {} +ClearCropTask.STATE_CLEARING_SECOND = {} +ClearCropTask.STATE_REVERSING = {} +ClearCropTask.STATE_WAITING = {} ClearCropTask.LEFT = 1 ClearCropTask.RIGHT = -1 -function ClearCropTask:new(vehicle) +function ClearCropTask:new(vehicle, harvester) local o = ClearCropTask:create() o.vehicle = vehicle + o.harvester = harvester + o.waitTimer = AutoDriveTON:new() o.stuckTimer = AutoDriveTON:new() - o.state = ClearCropTask.STATE_CLEARING + o.state = ClearCropTask.STATE_WAITING o.reverseStartLocation = nil + o.vehicleTrainLength = AutoDrive.getTractorTrainLength(vehicle, true, false) + ClearCropTask.setStateNames(o) return o end function ClearCropTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "Setting up ClearCropTask") + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:setUp") local leftBlocked = self.vehicle.ad.sensors.leftSensorFruit:pollInfo() or self.vehicle.ad.sensors.leftSensor:pollInfo() local rightBlocked = self.vehicle.ad.sensors.rightSensorFruit:pollInfo() or self.vehicle.ad.sensors.rightSensor:pollInfo() @@ -28,15 +38,6 @@ function ClearCropTask:setUp() leftBlocked = leftBlocked or leftFrontBlocked rightBlocked = rightBlocked or rightFrontBlocked - -- prefer side where front is also free - if (not leftBlocked) and (not rightBlocked) then - if (not leftFrontBlocked) and rightFrontBlocked then - rightBlocked = true - elseif leftFrontBlocked and (not rightFrontBlocked) then - leftBlocked = true - end - end - local cleartowards = ClearCropTask.RIGHT if leftBlocked and rightBlocked then cleartowards = ClearCropTask.RIGHT @@ -53,37 +54,96 @@ function ClearCropTask:setUp() table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 3)) table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 4)) - self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) + self.harvesterWayPoints = {} + + if self.harvester then + table.insert(self.harvesterWayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 1)) + table.insert(self.harvesterWayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 2)) + table.insert(self.harvesterWayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 3)) + local distance = AutoDrive.getDistanceBetween(self.vehicle, self.harvester) + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:setUp distance %.0f" + , distance + ) + if distance < ClearCropTask.MAX_HARVESTER_DISTANCE then + self.vehicle.ad.drivePathModule:setWayPoints(self.harvesterWayPoints) + else + self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) + end + else + self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) + end end function ClearCropTask:update(dt) - if self.state == ClearCropTask.STATE_CLEARING then - -- Check if the driver and trailers have left the crop yet - if not AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then + if self.lastState ~= self.state then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + end + + -- Check if the driver and trailers have left the crop yet + if not AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update not isVehicleOrTrailerInCrop") + self:finished() + return + end + self.stuckTimer:timer(true, ClearCropTask.STUCK_TIME, dt) + if self.stuckTimer:done() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update stuckTimer:done") + self:finished() + return + end + self.waitTimer:timer(true, ClearCropTask.WAIT_TIME, dt) + + if self.state == ClearCropTask.STATE_WAITING then + if self.waitTimer:done() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update STATE_WAITING - done waiting - clear now...") + self.waitTimer:timer(false) + self.state = ClearCropTask.STATE_CLEARING_FIRST + return + end + elseif self.state == ClearCropTask.STATE_CLEARING_FIRST then + if self.vehicle.ad.drivePathModule:isTargetReached() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update isTargetReached") self:finished() + return + elseif self.waitTimer:done() then + self.waitTimer:timer(false) + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + self.state = ClearCropTask.STATE_REVERSING + return else - if self.vehicle.ad.drivePathModule:isTargetReached() then - self:finished() - elseif self.stuckTimer:done() then - local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.reverseStartLocation = {x = x, y = y, z = z} - self.state = ClearCropTask.STATE_REVERSING - else - self.stuckTimer:timer(true, 30000, dt) - self.vehicle.ad.drivePathModule:update(dt) - end + self.vehicle.ad.drivePathModule:update(dt) end elseif self.state == ClearCropTask.STATE_REVERSING then local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.stuckTimer:timer(false) local distanceToReversStart = MathUtil.vector2Length(x - self.reverseStartLocation.x, z - self.reverseStartLocation.z) - if not AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then - self:finished() - elseif distanceToReversStart > 20 then - self.state = ClearCropTask.STATE_CLEARING + if (g_updateLoopIndex % 60 == 0) then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update distanceToReversStart %.0f" + , distanceToReversStart + ) + end + if distanceToReversStart > 20 then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update distanceToReversStart > 20") + self.waitTimer:timer(false) + self.state = ClearCropTask.STATE_CLEARING_SECOND + return else self.vehicle.ad.specialDrivingModule:driveReverse(dt, 15, 1, self.vehicle.ad.trailerModule:canBeHandledInReverse()) end + elseif self.state == ClearCropTask.STATE_CLEARING_SECOND then + if self.vehicle.ad.drivePathModule:isTargetReached() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update isTargetReached") + self:finished() + return + elseif self.waitTimer:done() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update waitTimer:done") + self.waitTimer:timer(false) + self:finished() + return + else + self.vehicle.ad.drivePathModule:update(dt) + end end end @@ -91,14 +151,50 @@ function ClearCropTask:abort() end function ClearCropTask:finished() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ClearCropTask:finished()") + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:finished") self.vehicle.ad.taskModule:setCurrentTaskFinished() end -function ClearCropTask:getInfoText() - return g_i18n:getText("AD_task_clearcrop") +function ClearCropTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(ClearCropTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function ClearCropTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] ClearCropTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" end function ClearCropTask:getI18nInfo() - return "$l10n_AD_task_clearcrop;" + local text = "$l10n_AD_task_clearcrop;" + if self.state == ClearCropTask.STATE_CLEARING_FIRST then + text = text .. " - 1/2" + elseif self.state == ClearCropTask.STATE_CLEARING_SECOND then + text = text .. " - 2/2" + elseif self.state == ClearCropTask.STATE_REVERSING then + text = text .. " - " .. "$l10n_AD_task_reversing_from_combine;" + elseif self.state == ClearCropTask.STATE_WAITING then + text = text .. " - " .. "$l10n_AD_task_waiting_for_room;" + end + return text +end + +function ClearCropTask.debugMsg(vehicle, debugText, ...) + if ClearCropTask.debug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) + end end diff --git a/scripts/Tasks/EmptyHarvesterTask.lua b/scripts/Tasks/EmptyHarvesterTask.lua index 9dcd7531..b85f4c31 100644 --- a/scripts/Tasks/EmptyHarvesterTask.lua +++ b/scripts/Tasks/EmptyHarvesterTask.lua @@ -1,11 +1,12 @@ EmptyHarvesterTask = ADInheritsFrom(AbstractTask) +EmptyHarvesterTask.debug = false -EmptyHarvesterTask.STATE_PATHPLANNING = 1 -EmptyHarvesterTask.STATE_DRIVING = 2 -EmptyHarvesterTask.STATE_UNLOADING = 3 -EmptyHarvesterTask.STATE_REVERSING = 4 -EmptyHarvesterTask.STATE_WAITING = 5 -EmptyHarvesterTask.STATE_UNLOADING_FINISHED = 6 +EmptyHarvesterTask.STATE_PATHPLANNING = {} +EmptyHarvesterTask.STATE_DRIVING = {} +EmptyHarvesterTask.STATE_UNLOADING = {} +EmptyHarvesterTask.STATE_REVERSING = {} +EmptyHarvesterTask.STATE_WAITING = {} +EmptyHarvesterTask.STATE_UNLOADING_FINISHED = {} EmptyHarvesterTask.REVERSE_TIME = 30000 EmptyHarvesterTask.WAITING_TIME = 7000 @@ -23,11 +24,12 @@ function EmptyHarvesterTask:new(vehicle, combine) o.trailers = nil o.trailerCount = 0 o.tractorTrainLength = 0 + EmptyHarvesterTask.setStateNames(o) return o end function EmptyHarvesterTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "Setting up EmptyHarvesterTask") + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:setUp") self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) self.trailers, self.trailerCount = AutoDrive.getAllUnits(self.vehicle) self.tractorTrainLength = AutoDrive.getTractorTrainLength(self.vehicle, true, false) @@ -39,6 +41,10 @@ function EmptyHarvesterTask:update(dt) self:finished() return end + if self.lastState ~= self.state then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + end if self.state == EmptyHarvesterTask.STATE_PATHPLANNING then if self.vehicle.ad.pathFinderModule:hasFinished() then @@ -50,13 +56,13 @@ function EmptyHarvesterTask:update(dt) self:finished(ADTaskModule.DONT_PROPAGATE) self.vehicle:stopAutoDrive() AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.WARN, "$l10n_AD_Driver_of; %s $l10n_AD_cannot_find_path; %s", 5000, self.vehicle.ad.stateModule:getName(), self.combine.ad.stateModule:getName()) + return else self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) self.vehicle.ad.pathFinderModule:addDelayTimer(10000) end else - --AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_DRIVING") self.state = EmptyHarvesterTask.STATE_DRIVING end else @@ -65,9 +71,7 @@ function EmptyHarvesterTask:update(dt) self.vehicle.ad.specialDrivingModule:update(dt) end elseif self.state == EmptyHarvesterTask.STATE_DRIVING then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update EmptyHarvesterTask.STATE_DRIVING") if self.vehicle.ad.drivePathModule:isTargetReached() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_UNLOADING") self.state = EmptyHarvesterTask.STATE_UNLOADING else self.vehicle.ad.drivePathModule:update(dt) @@ -78,9 +82,13 @@ function EmptyHarvesterTask:update(dt) AutoDrive:holdCPCombine(self.combine) --Check if the combine is moving / has already moved away and we are supposed to actively unload if self.combine.ad.driveForwardTimer.elapsedTime > 100 then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update driveForwardTimer.elapsedTime > 100") if AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update isVehicleOrTrailerInCrop") self:finished() + return elseif self.combine.ad.driveForwardTimer.elapsedTime > 4000 then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update driveForwardTimer.elapsedTime > 4000") self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD].state = CombineUnloaderMode.STATE_ACTIVE_UNLOAD_COMBINE self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD].breadCrumbs = Queue:new() self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD].lastBreadCrumb = nil @@ -102,19 +110,20 @@ function EmptyHarvesterTask:update(dt) if combineFillLevel <= 0.1 or filledToUnload then local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_UNLOADING_FINISHED") self.state = EmptyHarvesterTask.STATE_UNLOADING_FINISHED else -- Drive forward with collision checks active and only for a limited distance if distanceToCombine > 30 then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update distanceToCombine > 30") self:finished() + return else self.vehicle.ad.specialDrivingModule:driveForward(dt) end end end elseif self.state == EmptyHarvesterTask.STATE_UNLOADING_FINISHED then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - STATE_UNLOADING_FINISHED getIsCPCombineInPocket %s", tostring(AutoDrive:getIsCPCombineInPocket(self.combine))) + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update - STATE_UNLOADING_FINISHED getIsCPCombineInPocket %s", tostring(AutoDrive:getIsCPCombineInPocket(self.combine))) if AutoDrive:getIsCPCombineInPocket(self.combine) or AutoDrive.combineIsTurning(self.combine) then -- reverse if CP unload in a pocket or pullback position -- reverse if combine is turning @@ -147,7 +156,6 @@ function EmptyHarvesterTask:update(dt) end self.reverseTimer:timer(true, EmptyHarvesterTask.REVERSE_TIME, dt) if (distanceToReversStart > overallLength) or self.reverseTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_WAITING") self.holdCPCombineTimer:timer(false) self.reverseTimer:timer(false) self.state = EmptyHarvesterTask.STATE_WAITING @@ -164,6 +172,7 @@ function EmptyHarvesterTask:update(dt) if self.waitTimer:done() then self.waitTimer:timer(false) self:finished() + return else self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) @@ -175,7 +184,7 @@ function EmptyHarvesterTask:abort() end function EmptyHarvesterTask:finished(propagate) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:finished()") + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:finished()") self.vehicle.ad.specialDrivingModule.motorShouldNotBeStopped = false self.vehicle.ad.taskModule:setCurrentTaskFinished(propagate) end @@ -188,6 +197,28 @@ function EmptyHarvesterTask:getExcludedVehiclesForCollisionCheck() return excludedVehicles end +function EmptyHarvesterTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(EmptyHarvesterTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function EmptyHarvesterTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] EmptyHarvesterTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" +end + function EmptyHarvesterTask:getI18nInfo() local text = "$l10n_AD_task_unloading_combine;" if self.state == EmptyHarvesterTask.STATE_PATHPLANNING then @@ -208,5 +239,7 @@ end function EmptyHarvesterTask.debugMsg(vehicle, debugText, ...) if EmptyHarvesterTask.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end diff --git a/scripts/Tasks/FollowCombineTask.lua b/scripts/Tasks/FollowCombineTask.lua index f21799a7..1dcc6f80 100644 --- a/scripts/Tasks/FollowCombineTask.lua +++ b/scripts/Tasks/FollowCombineTask.lua @@ -1,18 +1,18 @@ FollowCombineTask = ADInheritsFrom(AbstractTask) FollowCombineTask.debug = false -FollowCombineTask.STATE_CHASING = 1 -FollowCombineTask.STATE_WAIT_FOR_TURN = 2 -FollowCombineTask.STATE_REVERSING = 3 -FollowCombineTask.STATE_REVERSING_FROM_CHOPPER = 12 -FollowCombineTask.STATE_WAIT_FOR_PASS_BY = 4 -FollowCombineTask.STATE_CIRCLING_PATHPLANNING = 5 -FollowCombineTask.STATE_CIRCLING = 6 -FollowCombineTask.STATE_FINISHED = 7 -FollowCombineTask.STATE_WAIT_BEFORE_FINISH = 8 -FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY = 9 -FollowCombineTask.STATE_GENERATE_UTURN_PATH = 10 -FollowCombineTask.STATE_DRIVE_UTURN_PATH = 11 +FollowCombineTask.STATE_CHASING = {} +FollowCombineTask.STATE_WAIT_FOR_TURN = {} +FollowCombineTask.STATE_REVERSING = {} +FollowCombineTask.STATE_REVERSING_FROM_CHOPPER = {} +FollowCombineTask.STATE_WAIT_FOR_PASS_BY = {} +FollowCombineTask.STATE_CIRCLING_PATHPLANNING = {} +FollowCombineTask.STATE_CIRCLING = {} +FollowCombineTask.STATE_FINISHED = {} +FollowCombineTask.STATE_WAIT_BEFORE_FINISH = {} +FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY = {} +FollowCombineTask.STATE_GENERATE_UTURN_PATH = {} +FollowCombineTask.STATE_DRIVE_UTURN_PATH = {} FollowCombineTask.MAX_REVERSE_DISTANCE = 20 FollowCombineTask.MIN_COMBINE_DISTANCE = 25 @@ -43,11 +43,12 @@ function FollowCombineTask:new(vehicle, combine) o.angleToCombine = vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombine() o.trailers = nil o.activeUnloading = AutoDrive.getSetting("activeUnloading", self.combine) + FollowCombineTask.setStateNames(o) return o end function FollowCombineTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask setUp") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask setUp") self.lastChaseSide = self.chaseSide self.trailers, _ = AutoDrive.getAllUnits(self.vehicle) self.activeUnloading = AutoDrive.getSetting("activeUnloading", self.combine) @@ -62,6 +63,11 @@ function FollowCombineTask:update(dt) self:updateStates(dt) + if self.lastState ~= self.state then + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + end + if self.state == FollowCombineTask.STATE_CHASING then self.chaseTimer:timer(true, 4000, dt) self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, self.MAX_STUCK_TIME, dt) @@ -69,18 +75,18 @@ function FollowCombineTask:update(dt) if self.combine.ad.isChopper then if self.filled and self.chaseSide ~= nil and self.chaseSide ~= AutoDrive.CHASEPOS_REAR then --skip reversing - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - filled chopper") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - filled chopper") self.state = FollowCombineTask.STATE_FINISHED -- finish immediate return elseif self.filledToUnload and self.chaseSide ~= nil and self.chaseSide == AutoDrive.CHASEPOS_REAR then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - filledToUnload chopper") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - filledToUnload chopper") local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester return end elseif self.filled or (self.combine.ad.isHarvester and self.combineFillPercent <= 0.1 and (not self.activeUnloading)) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - filled harvester") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - filled harvester") self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH -- unload after some time to let harvester drive away return end @@ -92,7 +98,7 @@ function FollowCombineTask:update(dt) FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING stuckTimer:done -> STATE_REVERSING") end self.stuckTimer:timer(false) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - stuck -> stuckTimer:%s angleWrongTimer:%s" + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - stuck -> stuckTimer:%s angleWrongTimer:%s" , tostring(self.stuckTimer:done()), tostring(self.angleWrongTimer.elapsedTime > 15000)) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} @@ -105,7 +111,7 @@ function FollowCombineTask:update(dt) end if not self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - not UnloaderOnCorrectSide -> finished") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - not UnloaderOnCorrectSide -> finished") self:finished() return end @@ -114,7 +120,7 @@ function FollowCombineTask:update(dt) and AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE -- if to close -> reverse then -- Stop chasing and wait for a normal unload call while standing - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - to close to harvester -> reverse") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - to close to harvester -> reverse") local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester @@ -123,7 +129,7 @@ function FollowCombineTask:update(dt) if (not self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide(self.chaseSide)) and (not AutoDrive.combineIsTurning(self.combine)) then if self.lastChaseSide ~= CombineUnloaderMode.CHASEPOS_REAR then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - switching chase side from side to elsewhere - let's wait for passby next") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - switching chase side from side to elsewhere - let's wait for passby next") self.state = FollowCombineTask.STATE_WAIT_FOR_PASS_BY end end @@ -131,7 +137,7 @@ function FollowCombineTask:update(dt) if AutoDrive.combineIsTurning(self.combine) then -- harvester turns --print("Waiting for turn now - 1- t:" .. tostring(AutoDrive.combineIsTurning(self.combine)) .. " anglewrongtimer: " .. tostring(self.angleWrongTimer.elapsedTime > 10000)) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - combineIsTurning") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - combineIsTurning") self.state = FollowCombineTask.STATE_WAIT_FOR_TURN return elseif ((self.combine.lastSpeedReal * self.combine.movingDirection) <= -0.00005) then @@ -140,17 +146,17 @@ function FollowCombineTask:update(dt) self:followChasePoint(dt) end elseif self.state == FollowCombineTask.STATE_WAIT_FOR_TURN then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN") self.waitForTurnTimer:timer(true, self.MAX_TURN_TIME, dt) if self.waitForTurnTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn took to long - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn took to long - set finished now") self.waitForTurnTimer:timer(false) self.state = FollowCombineTask.STATE_FINISHED return end if AutoDrive.combineIsTurning(self.combine) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsTurning") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsTurning") if self.combine.ad.isHarvester and (self.distanceToCombine < ((self.vehicle.size.length + self.combine.size.length) / 2 + 10)) then -- harvester -- if combine drive reverse to turn -> reverse to keep distance @@ -161,43 +167,44 @@ function FollowCombineTask:update(dt) local isdrivingReverse = ((self.combine.lastSpeedReal * self.combine.movingDirection) <= -0.00051) local combineIsDriving = (self.combine.lastSpeedReal > 0.001) self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, 15000, dt) + if isdrivingReverse then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> self:reverse") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> self:reverse") self:reverse(dt) elseif self.stuckTimer:done() or (not combineIsDriving and (self:getAngleToCobine() > 45)) then -- if stuck with harvester - try reverse - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - stuck / getAngleToCobine() > 45 -> STATE_REVERSING_FROM_CHOPPER combineIsDriving %s", tostring(combineIsDriving)) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - stuck / getAngleToCobine() > 45 -> STATE_REVERSING_FROM_CHOPPER combineIsDriving %s", tostring(combineIsDriving)) self.stuckTimer:timer(false) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER -- reverse to get room from harvester return elseif combineIsDriving then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsDriving -> stopVehicle") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsDriving -> stopVehicle") self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint self.chaseSide %s", tostring(self.chaseSide)) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint self.chaseSide %s", tostring(self.chaseSide)) self:followChasePoint(dt) end else -- isFixedPipeChopper - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - noMovementTimer %d", self.combine.ad.noMovementTimer.elapsedTime) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - noMovementTimer %d", self.combine.ad.noMovementTimer.elapsedTime) local dischargeState = self.combine:getDischargeState() self.fillingTimer:timer(not self.combine.spec_combine.isFilling, 100, dt) if self.fillingTimer:done() and self.combine.ad.noMovementTimer.elapsedTime < 5000 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - fillingTimer:done") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - fillingTimer:done") -- harvested to end of row AutoDrive:holdCPCombine(self.combine) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint no AutoAimingChopper") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint no AutoAimingChopper") self:followChasePoint(dt) end self.dischargeTimer:timer(dischargeState ~= Dischargeable.DISCHARGE_STATE_OBJECT , 500, dt) if self.dischargeTimer:done() and self.fillingTimer:done() and self.combine.ad.noMovementTimer.elapsedTime < 5000 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - dischargeTimer:done") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - dischargeTimer:done") self.fillingTimer:timer(false) self.dischargeTimer:timer(false) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) @@ -207,7 +214,7 @@ function FollowCombineTask:update(dt) end end else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> stopVehicle") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> stopVehicle") -- stop while combine is turning self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) @@ -227,27 +234,27 @@ function FollowCombineTask:update(dt) or self.waitForTurnTimer.elapsedTime > 15000 -- turn longer than 15 sec ) then if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide(self.chaseSide) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks good - start chasing again") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks good - start chasing again") self.waitForTurnTimer:timer(false) self.chaseTimer:timer(false) self.state = FollowCombineTask.STATE_CHASING return elseif self.angleToCombineHeading > 150 and self.angleToCombineHeading < 210 and self.distanceToCombine < 80 and AutoDrive.experimentalFeatures.UTurn == true and self.combine.ad.isHarvester then -- Instead of directly trying a long way around to get behind the harvester, let's wait for him to pass us by and then U-turn - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading inverted - wait for passby, then U-turn") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading inverted - wait for passby, then U-turn") self.state = FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY self.waitForTurnTimer:timer(false) self.chaseTimer:timer(false) self.waitForPassByTimer:timer(false) else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks bad - stop to be able to start pathfinder") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks bad - stop to be able to start pathfinder") self.stayOnField = true self.state = FollowCombineTask.STATE_FINISHED return end end elseif self.state == FollowCombineTask.STATE_WAIT_FOR_PASS_BY then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY") self.waitForPassByTimer:timer(true, 2200, dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) @@ -255,24 +262,24 @@ function FollowCombineTask:update(dt) self.waitForPassByTimer:timer(false) self.chaseTimer:timer(false) if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks good - chasing again") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks good - chasing again") self.state = FollowCombineTask.STATE_CHASING return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks bad - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks bad - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return end end elseif self.state == FollowCombineTask.STATE_REVERSING then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING") local x, y, z = getWorldTranslation(self.vehicle.components[1].node) local distanceToReverseStart = MathUtil.vector2Length(x - self.reverseStartLocation.x, z - self.reverseStartLocation.z) self.reverseTimer:timer(true, self.MAX_REVERSE_TIME, dt) local doneReversing = distanceToReverseStart > self.MAX_REVERSE_DISTANCE or (not self.startedChasing) if doneReversing or self.reverseTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING - done reversing - set finished") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING - done reversing - set finished") self.reverseTimer:timer(false) self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return @@ -280,7 +287,7 @@ function FollowCombineTask:update(dt) self:reverse(dt) end elseif self.state == FollowCombineTask.STATE_REVERSING_FROM_CHOPPER then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER") local cx, _, cz = getWorldTranslation(self.combine.components[1].node) local vx, _, vz = getWorldTranslation(self.vehicle.components[1].node) local distanceToReverseStart = MathUtil.vector2Length(vx - self.reverseStartLocation.x, vz - self.reverseStartLocation.z) @@ -288,7 +295,7 @@ function FollowCombineTask:update(dt) self.reverseTimer:timer(true, self.MAX_REVERSE_TIME, dt) local doneReversing = distanceToReverseStart > self.MAX_REVERSE_DISTANCE or distanceToCombine > self.MAX_REVERSE_DISTANCE if doneReversing or self.reverseTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - done reversing - set finished") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - done reversing - set finished") self.reverseTimer:timer(false) if self.combine.ad.isFixedPipeChopper and AutoDrive:getIsCPActive(self.combine) then -- wait for CP to finish a turn maneuver before invoke pathfinder @@ -299,18 +306,18 @@ function FollowCombineTask:update(dt) return else if self.combine.ad.isFixedPipeChopper then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - not AutoAimingChopper -> holdCPCombine") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - not AutoAimingChopper -> holdCPCombine") AutoDrive:holdCPCombine(self.combine) else if self:getAngleToCobine() > 30 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - AngleToCobine > 30 -> holdCPCombine") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - AngleToCobine > 30 -> holdCPCombine") AutoDrive:holdCPCombine(self.combine) end end self:reverse(dt) end elseif self.state == FollowCombineTask.STATE_WAIT_BEFORE_FINISH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_BEFORE_FINISH") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_BEFORE_FINISH") -- wait for CP to finish a turn maneuver before invoke pathfinder -- TODO: check if following is useful! local combineIsDriving = self.combine.ad.isFixedPipeChopper and AutoDrive:getIsCPActive(self.combine) and (self.combine.lastSpeedReal > 0.001) @@ -324,12 +331,12 @@ function FollowCombineTask:update(dt) self.vehicle.ad.specialDrivingModule:update(dt) end elseif self.state == FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY") self.waitForPassByTimer:timer(true, 15000, dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) if self.waitForPassByTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - passby timer elapsed - heading looks bad - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - passby timer elapsed - heading looks bad - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return @@ -337,7 +344,7 @@ function FollowCombineTask:update(dt) local cx, cy, cz = getWorldTranslation(self.combine.components[1].node) local _, _, offsetZ = worldToLocal(self.vehicle.components[1].node, cx, cy, cz) if offsetZ <= -10 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - combine passed us. Calculate U-turn now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - combine passed us. Calculate U-turn now") self.state = FollowCombineTask.STATE_GENERATE_UTURN_PATH local cx, cy, cz = getWorldTranslation(self.combine.components[1].node) local offsetX, _, _ = worldToLocal(self.vehicle.components[1].node, cx, cy, cz) @@ -346,30 +353,30 @@ function FollowCombineTask:update(dt) end end elseif self.state == FollowCombineTask.STATE_GENERATE_UTURN_PATH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH") if self.vehicle.ad.uTurn ~= nil and self.vehicle.ad.uTurn.inProgress then self.vehicle:generateUTurn(true) elseif self.vehicle.ad.uTurn ~= nil and not self.vehicle.ad.uTurn.inProgress then if self.vehicle.ad.uTurn.colliFound or self.vehicle.ad.uTurn.points == nil or #self.vehicle.ad.uTurn.points < 5 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation failed due to collision - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation failed due to collision - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation finished - passing points to drivePathModule now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation finished - passing points to drivePathModule now") self.vehicle.ad.drivePathModule:setWayPoints(self.vehicle.ad.uTurn.points) self.state = FollowCombineTask.STATE_DRIVE_UTURN_PATH end end elseif self.state == FollowCombineTask.STATE_DRIVE_UTURN_PATH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH") if self.vehicle.ad.drivePathModule:isTargetReached() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - U-Turn finished") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - U-Turn finished") if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks good - chasing again") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks good - chasing again") self.state = FollowCombineTask.STATE_CHASING return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks bad - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks bad - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return @@ -378,7 +385,7 @@ function FollowCombineTask:update(dt) self.vehicle.ad.drivePathModule:update(dt) end elseif self.state == FollowCombineTask.STATE_FINISHED then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_FINISHED") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_FINISHED") self:finished() return end @@ -434,7 +441,7 @@ end function FollowCombineTask:followChasePoint(dt) if self:shouldWaitForChasePos(dt) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:followChasePoint getAngleToChasePos %.0f -> stopVehicle", self:getAngleToChasePos()) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:followChasePoint getAngleToChasePos %.0f -> stopVehicle", self:getAngleToChasePos()) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) else @@ -507,7 +514,7 @@ function FollowCombineTask:abort() end function FollowCombineTask:finished() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:finished()") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:finished()") self.vehicle.ad.taskModule:setCurrentTaskFinished() end @@ -519,6 +526,29 @@ function FollowCombineTask:getExcludedVehiclesForCollisionCheck() return excludedVehicles end +function FollowCombineTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(FollowCombineTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function FollowCombineTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] FollowCombineTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" +end + + function FollowCombineTask:getI18nInfo() local text = "$l10n_AD_task_chasing_combine;" if self.state == FollowCombineTask.STATE_CHASING then @@ -534,7 +564,7 @@ function FollowCombineTask:getI18nInfo() elseif self.chaseSide == AutoDrive.CHASEPOS_RIGHT then text = text .. " - " .. "$l10n_AD_task_chase_side_right;" end - elseif self.state == FollowCombineTask.STATE_REVERSING_FROM_CHOPPER then + elseif self.state == FollowCombineTask.STATE_REVERSING_FROM_CHOPPER or self.state == FollowCombineTask.STATE_WAIT_FOR_TURN then text = text .. " - " .. "$l10n_AD_task_wait_for_combine_turn;" elseif self.state == FollowCombineTask.STATE_REVERSING then text = text .. " - " .. "$l10n_AD_task_reversing_from_combine;" @@ -549,5 +579,7 @@ end function FollowCombineTask.debugMsg(vehicle, debugText, ...) if FollowCombineTask.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end From 5eceb6ea7de59fd65a2015b1944eaae62947a1e8 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 15 Jun 2024 18:06:07 +0200 Subject: [PATCH 06/44] correct some programming issues no functional change --- scripts/ExternalInterface.lua | 6 +++--- scripts/Hud.lua | 2 +- scripts/Manager/GraphManager.lua | 2 +- scripts/Manager/TriggerManager.lua | 2 +- scripts/Modules/PathFinderModule.lua | 8 ++++---- scripts/Sensors/VirtualSensors.lua | 6 +++--- scripts/Tasks/FollowCombineTask.lua | 2 +- scripts/Utils/AutoDriveVehicleData.lua | 4 ++-- scripts/Utils/CombineUtil.lua | 2 +- scripts/Utils/UtilFuncs.lua | 5 ++--- 10 files changed, 19 insertions(+), 20 deletions(-) diff --git a/scripts/ExternalInterface.lua b/scripts/ExternalInterface.lua index 2638b3e4..b79de0f9 100644 --- a/scripts/ExternalInterface.lua +++ b/scripts/ExternalInterface.lua @@ -827,9 +827,9 @@ function AutoDrive:getALFillTypes(object) -- used by PullDownList, getSupportedF -- spec_universalAutoload local spec = object.spec_universalAutoload if spec and AutoDrive:hasAL(object) then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") + AutoDrive.debugPrint(object, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") end - AutoDrive.debugPrint(vehicle, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALFillTypes #fillTypes %s", tostring(#fillTypes)) + AutoDrive.debugPrint(object, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALFillTypes #fillTypes %s", tostring(#fillTypes)) return fillTypes end @@ -846,7 +846,7 @@ function AutoDrive:getALCurrentFillType(object) -- used by onEnterVehicle, onPos -- spec_universalAutoload local spec = object.spec_universalAutoload if spec and AutoDrive:hasAL(object) then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") + AutoDrive.debugPrint(object, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") end return nil end diff --git a/scripts/Hud.lua b/scripts/Hud.lua index c1d03d95..69814f5e 100644 --- a/scripts/Hud.lua +++ b/scripts/Hud.lua @@ -438,7 +438,7 @@ function AutoDriveHud:mouseEvent(vehicle, posX, posY, isDown, isUp, button) for i = #self.hudElements, 1, -1 do local element = self.hudElements[i] local layer = element.layer - mouseEventHandled, silent = element:mouseEvent(vehicle, posX, posY, isDown, isUp, button, layer) + local mouseEventHandled, silent = element:mouseEvent(vehicle, posX, posY, isDown, isUp, button, layer) if mouseEventHandled then -- Maybe a PullDownList have been expanded/collapsed, so need to refresh layer sequence self:refreshHudElementsLayerSequence() diff --git a/scripts/Manager/GraphManager.lua b/scripts/Manager/GraphManager.lua index 86873271..6cf9647c 100644 --- a/scripts/Manager/GraphManager.lua +++ b/scripts/Manager/GraphManager.lua @@ -1233,7 +1233,7 @@ function ADGraphManager:createDebugMarkers(updateMap) -- mark wayPoint without incoming connection if not wp.foundError and wp.out ~= nil then for _, wp_out in pairs(wp.out) do - local missingIncoming = self:checkForMissingIncoming(wp, self:getWayPointById(wp_out)) + local missingIncoming = self:checkForMissingIncoming(wp) if missingIncoming then local debugMapMarkerName = "3_" .. tostring(count3) diff --git a/scripts/Manager/TriggerManager.lua b/scripts/Manager/TriggerManager.lua index fbdb4595..5cdcdd41 100644 --- a/scripts/Manager/TriggerManager.lua +++ b/scripts/Manager/TriggerManager.lua @@ -382,7 +382,7 @@ function ADTriggerManager:getBestPickupLocationFor(vehicle, trailer, fillType) local aifillTypes = loadingStation:getAISupportedFillTypes() if aifillTypes[fillType] and loadingStation:getFillLevel(fillType, farmId) > 0 then if loadingStation.getAITargetPositionAndDirection ~= nil then - x, z, xDir, zDir = loadingStation:getAITargetPositionAndDirection(FillType.UNKNOWN) + local x, z, xDir, zDir = loadingStation:getAITargetPositionAndDirection(FillType.UNKNOWN) table.insert(validLoadingStations, loadingStation) end diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 232fd50e..a0f9ea86 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -906,7 +906,7 @@ function PathFinderModule:checkGridCell(cell) string.format("PFM checkGridCell isRestricted self.restrictToField %s self.fallBackMode1 %s isOnField %s x,z %d %d", tostring(self.restrictToField), tostring(self.fallBackMode1), - tostring(isOnField), + tostring(cell.isOnField), math.floor(worldPos.x), math.floor(worldPos.z) ) @@ -1196,7 +1196,7 @@ function PathFinderModule:drawDebugForPF() local color_green = 0.1 local color_blue = 0.1 local color_count = 0 - index = 0 + local index = 0 for _, cell in pairs(self.grid) do index = index + 1 @@ -2089,11 +2089,11 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) return false, angleBetween end -function PathFinderModule.debugVehicleMsg(vehicle, msg, ...) +function PathFinderModule.debugVehicleMsg(vehicle, msg) -- collect output for single vehicle - help to examine sequences for a single vehicle if vehicle ~= nil and vehicle.ad ~= nil and vehicle.ad.debug ~= nil then if AutoDrive.debugVehicleMsg ~= nil then - AutoDrive.debugVehicleMsg(vehicle, msg, ...) + AutoDrive.debugVehicleMsg(vehicle, msg) end end end diff --git a/scripts/Sensors/VirtualSensors.lua b/scripts/Sensors/VirtualSensors.lua index a689b4c6..dff752b1 100644 --- a/scripts/Sensors/VirtualSensors.lua +++ b/scripts/Sensors/VirtualSensors.lua @@ -234,8 +234,8 @@ function ADSensor:getLocationByPosition() --local frontToolLength = 0 --AutoDrive.getFrontToolLength(self.vehicle) --lengthOffset = frontToolLength / 2 --end - location = self:getRotatedFront() - if location == nil then + local front = self:getRotatedFront() + if front == nil then location = {x = 0, z = vehicle.size.length / 2} end --location.z = location.z + AutoDrive.getVehicleLeadingEdge(vehicle) @@ -387,7 +387,7 @@ function ADSensor:updateSensor(dt) end end -function ADSensor:onUpdate() +function ADSensor:onUpdate(dt) Logging.warning("[AutoDrive] ADSensor:onUpdate() called - Please override this in instance class") end diff --git a/scripts/Tasks/FollowCombineTask.lua b/scripts/Tasks/FollowCombineTask.lua index 1dcc6f80..fb2cd577 100644 --- a/scripts/Tasks/FollowCombineTask.lua +++ b/scripts/Tasks/FollowCombineTask.lua @@ -459,7 +459,7 @@ function FollowCombineTask:followChasePoint(dt) end function FollowCombineTask:shouldWaitForChasePos(dt) - local angle = self:getAngleToChasePos(dt) + local angle = self:getAngleToChasePos() self.angleWrongTimer:timer(angle > 50, 3000, dt) local _, _, diffZ = worldToLocal(self.vehicle.components[1].node, self.chasePos.x, self.chasePos.y, self.chasePos.z) return self.angleWrongTimer:done() or diffZ <= -1 --or (not self.combine.ad.sensors.frontSensorFruit:pollInfo()) diff --git a/scripts/Utils/AutoDriveVehicleData.lua b/scripts/Utils/AutoDriveVehicleData.lua index f6d847d6..e6ed9002 100644 --- a/scripts/Utils/AutoDriveVehicleData.lua +++ b/scripts/Utils/AutoDriveVehicleData.lua @@ -41,7 +41,7 @@ function AutoDriveVehicleData.registerEventListeners(vehicleType) end end -function AutoDrive.registerFunctions(vehicleType) +function AutoDriveVehicleData.registerFunctions(vehicleType) SpecializationUtil.registerFunction(vehicleType, "getParkDestination", AutoDriveVehicleData.getParkDestination) SpecializationUtil.registerFunction(vehicleType, "setParkDestination", AutoDriveVehicleData.setParkDestination) end @@ -88,7 +88,7 @@ function AutoDriveVehicleData:onEnterVehicle() local actualparkDestination = -1 if g_dedicatedServer == nil then -- only send the client event to server - local selectedWorkTool = AutoDrive.getSelectedWorkTool(self, true) + local selectedWorkTool = AutoDrive.getSelectedWorkTool(self) if selectedWorkTool ~= nil and selectedWorkTool.advd ~= nil then actualparkDestination = selectedWorkTool.advd.parkDestination else diff --git a/scripts/Utils/CombineUtil.lua b/scripts/Utils/CombineUtil.lua index 30fbe521..2a499fd3 100644 --- a/scripts/Utils/CombineUtil.lua +++ b/scripts/Utils/CombineUtil.lua @@ -164,7 +164,7 @@ function AutoDrive.getPipeLength(combine) if (combine.ad.isFixedPipeChopper or combine.ad.isHarvester) and AutoDrive.isPipeOut(combine) then local combineNode = AutoDrive.getPipeRoot(combine) local dischargeX, dichargeY, dischargeZ = getWorldTranslation(AutoDrive.getDischargeNode(combine)) - diffX, _, _ = worldToLocal(combineNode, dischargeX, dichargeY, dischargeZ) + local diffX, _, _ = worldToLocal(combineNode, dischargeX, dichargeY, dischargeZ) length = math.abs(diffX) -- Store pipe length for 'normal' harvesters diff --git a/scripts/Utils/UtilFuncs.lua b/scripts/Utils/UtilFuncs.lua index 2868f412..c73072a4 100644 --- a/scripts/Utils/UtilFuncs.lua +++ b/scripts/Utils/UtilFuncs.lua @@ -105,7 +105,7 @@ function AutoDrive.streamWriteStringOrEmpty(streamId, string) end function AutoDrive.streamReadUIntNList(streamId, numberOfBits) - list = {} + local list = {} local len = streamReadUIntN(streamId, numberOfBits) for i = 1, len do local v = streamReadUIntN(streamId, numberOfBits) @@ -288,7 +288,7 @@ end function table:concatNil(sep, i, j) local res = table.concat(self, sep, i, j) if res == "" then - res = nil + return nil end return res end @@ -1186,7 +1186,6 @@ function AutoDrive.checkWaypointsMultipleSameOut(correctit) for j, linkedNodeId_1 in ipairs(network[i].out) do local wp_2 = network[linkedNodeId_1] if wp_2 ~= nil then - found = false for k, linkedNodeId_2 in ipairs(network[i].out) do if k>j then local wp_3 = network[linkedNodeId_2] From be030ea10acaaee53bd65798b95b6768f8145329 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 15 Jun 2024 18:09:20 +0200 Subject: [PATCH 07/44] ignore folder vscode --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..3f2c92b2 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ + +/.vscode From 4ac2f9b24dfe65a59fddd96cc4cff5f41c98991a Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 15 Jun 2024 18:59:12 +0200 Subject: [PATCH 08/44] BSM: do not hold empty vehicles --- scripts/Manager/BunkerSiloManager.lua | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/scripts/Manager/BunkerSiloManager.lua b/scripts/Manager/BunkerSiloManager.lua index 12bfc0d7..9157d57c 100644 --- a/scripts/Manager/BunkerSiloManager.lua +++ b/scripts/Manager/BunkerSiloManager.lua @@ -54,7 +54,11 @@ function ADBunkerSiloManager:update(dt) if triggerX ~= nil then local distance = MathUtil.vector2Length(triggerX - vehicleX, triggerZ - vehicleZ) if distance < bsmRange then - if AutoDrive.isVehicleInBunkerSiloArea(vehicle) or bunkerSilo.adClosestVehicle == vehicle then + local fillLevel, _, _, _ = AutoDrive.getAllFillLevels(AutoDrive.getAllUnits(vehicle)) + if AutoDrive.isVehicleInBunkerSiloArea(vehicle) + or bunkerSilo.adClosestVehicle == vehicle + or fillLevel < 0.1 + then -- IMPORTANT: DO NOT SET setUnPaused to avoid crash with CP silo compacter !!! -- vehicle.ad.drivePathModule:setUnPaused() else From a4dbc787bf3379eec7847e8e7b8a00c1a4306ebc Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 15 Jun 2024 19:01:04 +0200 Subject: [PATCH 09/44] give 5 tries to exit a field --- scripts/Tasks/ExitFieldTask.lua | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/scripts/Tasks/ExitFieldTask.lua b/scripts/Tasks/ExitFieldTask.lua index c1598b51..d582b5db 100644 --- a/scripts/Tasks/ExitFieldTask.lua +++ b/scripts/Tasks/ExitFieldTask.lua @@ -11,6 +11,7 @@ function ExitFieldTask:new(vehicle) local o = ExitFieldTask:create() o.vehicle = vehicle o.trailers = nil + o.failedPathFinder = 0 return o end @@ -27,9 +28,12 @@ function ExitFieldTask:update(dt) if self.vehicle.ad.pathFinderModule:hasFinished() then self.wayPoints = self.vehicle.ad.pathFinderModule:getPath() if self.wayPoints == nil or #self.wayPoints == 0 then - self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() - self:selectNextStrategy() - if self.vehicle.ad.pathFinderModule:isTargetBlocked() then + self.failedPathFinder = self.failedPathFinder + 1 + if self.failedPathFinder > 5 then + self.failedPathFinder = 0 + self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() + self:selectNextStrategy() + elseif self.vehicle.ad.pathFinderModule:isTargetBlocked() then -- If the selected field exit isn't reachable, try the next strategy and restart without delay self:startPathPlanning() elseif self.vehicle.ad.pathFinderModule:timedOut() or self.vehicle.ad.pathFinderModule:isBlocked() then From 8bca02f1d26cd463958636554cff5fed1cff2793 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 15 Jun 2024 19:09:10 +0200 Subject: [PATCH 10/44] give the task to leave crop more time --- scripts/Tasks/ClearCropTask.lua | 63 +++++++++++++++++---------------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/scripts/Tasks/ClearCropTask.lua b/scripts/Tasks/ClearCropTask.lua index d4719017..496dc0ec 100644 --- a/scripts/Tasks/ClearCropTask.lua +++ b/scripts/Tasks/ClearCropTask.lua @@ -5,6 +5,7 @@ ClearCropTask.TARGET_DISTANCE_SIDE = 10 ClearCropTask.TARGET_DISTANCE_FRONT_STEP = 10 ClearCropTask.MAX_HARVESTER_DISTANCE = 50 ClearCropTask.WAIT_TIME = 10000 +ClearCropTask.DRIVE_TIME = 30000 ClearCropTask.STUCK_TIME = 60000 ClearCropTask.STATE_CLEARING_FIRST = {} ClearCropTask.STATE_CLEARING_SECOND = {} @@ -19,6 +20,7 @@ function ClearCropTask:new(vehicle, harvester) o.vehicle = vehicle o.harvester = harvester o.waitTimer = AutoDriveTON:new() + o.driveTimer = AutoDriveTON:new() o.stuckTimer = AutoDriveTON:new() o.state = ClearCropTask.STATE_WAITING o.reverseStartLocation = nil @@ -48,30 +50,23 @@ function ClearCropTask:setUp() end self.wayPoints = {} - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, (ClearCropTask.TARGET_DISTANCE_SIDE / 2) * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 0.5)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 1)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 2)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 3)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 4)) - - self.harvesterWayPoints = {} - - if self.harvester then - table.insert(self.harvesterWayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 1)) - table.insert(self.harvesterWayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 2)) - table.insert(self.harvesterWayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 3)) - local distance = AutoDrive.getDistanceBetween(self.vehicle, self.harvester) - ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:setUp distance %.0f" - , distance - ) - if distance < ClearCropTask.MAX_HARVESTER_DISTANCE then - self.vehicle.ad.drivePathModule:setWayPoints(self.harvesterWayPoints) - else - self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) - end + + local distance = AutoDrive.getDistanceBetween(self.vehicle, self.harvester) + ClearCropTask.debugMsg(self.harvester, "ClearCropTask:setUp distance %.0f" + , distance + ) + if self.harvester and distance < ClearCropTask.MAX_HARVESTER_DISTANCE then + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 1)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 2)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 3)) else - self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, (ClearCropTask.TARGET_DISTANCE_SIDE / 2) * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 0.5)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 1)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 2)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 3)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 4)) end + self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) end function ClearCropTask:update(dt) @@ -92,22 +87,29 @@ function ClearCropTask:update(dt) self:finished() return end - self.waitTimer:timer(true, ClearCropTask.WAIT_TIME, dt) if self.state == ClearCropTask.STATE_WAITING then + self.waitTimer:timer(true, ClearCropTask.WAIT_TIME, dt) if self.waitTimer:done() then ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update STATE_WAITING - done waiting - clear now...") self.waitTimer:timer(false) + self.driveTimer:timer(false) + self.stuckTimer:timer(false) + self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) self.state = ClearCropTask.STATE_CLEARING_FIRST return end elseif self.state == ClearCropTask.STATE_CLEARING_FIRST then + self.driveTimer:timer(true, ClearCropTask.DRIVE_TIME, dt) if self.vehicle.ad.drivePathModule:isTargetReached() then - ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update isTargetReached") + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update 1 isTargetReached") self:finished() return - elseif self.waitTimer:done() then + elseif self.driveTimer:done() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update 1 driveTimer:done") self.waitTimer:timer(false) + self.driveTimer:timer(false) + self.stuckTimer:timer(false) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = ClearCropTask.STATE_REVERSING @@ -126,6 +128,9 @@ function ClearCropTask:update(dt) if distanceToReversStart > 20 then ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update distanceToReversStart > 20") self.waitTimer:timer(false) + self.driveTimer:timer(false) + self.stuckTimer:timer(false) + self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) self.state = ClearCropTask.STATE_CLEARING_SECOND return else @@ -133,12 +138,7 @@ function ClearCropTask:update(dt) end elseif self.state == ClearCropTask.STATE_CLEARING_SECOND then if self.vehicle.ad.drivePathModule:isTargetReached() then - ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update isTargetReached") - self:finished() - return - elseif self.waitTimer:done() then - ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update waitTimer:done") - self.waitTimer:timer(false) + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update 2 isTargetReached") self:finished() return else @@ -148,6 +148,7 @@ function ClearCropTask:update(dt) end function ClearCropTask:abort() + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:abort") end function ClearCropTask:finished() From 4b7acb4e94abe363a5d5b34aad411473c95e594c Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 15 Jun 2024 21:09:27 +0200 Subject: [PATCH 11/44] small fix --- scripts/Hud.lua | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/scripts/Hud.lua b/scripts/Hud.lua index 69814f5e..25c241c0 100644 --- a/scripts/Hud.lua +++ b/scripts/Hud.lua @@ -429,6 +429,7 @@ function AutoDriveHud:mouseEvent(vehicle, posX, posY, isDown, isUp, button) if mouseActiveForAutoDrive then local mouseEventHandled = false + local silent = false if AutoDrive.splineInterpolation ~= nil then AutoDrive.splineInterpolation.valid = false end @@ -438,7 +439,7 @@ function AutoDriveHud:mouseEvent(vehicle, posX, posY, isDown, isUp, button) for i = #self.hudElements, 1, -1 do local element = self.hudElements[i] local layer = element.layer - local mouseEventHandled, silent = element:mouseEvent(vehicle, posX, posY, isDown, isUp, button, layer) + mouseEventHandled, silent = element:mouseEvent(vehicle, posX, posY, isDown, isUp, button, layer) if mouseEventHandled then -- Maybe a PullDownList have been expanded/collapsed, so need to refresh layer sequence self:refreshHudElementsLayerSequence() From c8039f1e2ff60d1fd0fcb82033ea7d7a84461c4c Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 16 Jun 2024 16:48:37 +0200 Subject: [PATCH 12/44] small fix --- scripts/Sensors/VirtualSensors.lua | 2 ++ 1 file changed, 2 insertions(+) diff --git a/scripts/Sensors/VirtualSensors.lua b/scripts/Sensors/VirtualSensors.lua index dff752b1..ddaeece4 100644 --- a/scripts/Sensors/VirtualSensors.lua +++ b/scripts/Sensors/VirtualSensors.lua @@ -237,6 +237,8 @@ function ADSensor:getLocationByPosition() local front = self:getRotatedFront() if front == nil then location = {x = 0, z = vehicle.size.length / 2} + else + location = front end --location.z = location.z + AutoDrive.getVehicleLeadingEdge(vehicle) --location.z = location.z + 2 From 0653d4596c26ddd4bd61b8da5584cc73bbb831df Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 14 Jul 2024 18:47:45 +0200 Subject: [PATCH 13/44] fix show reverse roads in helper GUI --- scripts/AutoDrive.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index 7864f26c..b21afcd1 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -440,7 +440,7 @@ function AutoDrive.drawNetworkOnMap() if isSubPrio(outNode) then r, g, b, a = unpack(AutoDrive.currentColors.ad_color_subPrioDualConnection) end - elseif ADGraphManager:isReverseRoad(start, target) then + elseif ADGraphManager:isReverseRoad(node, outNode) then r, g, b, a = unpack(AutoDrive.currentColors.ad_color_reverseConnection) end setOverlayColor( AutoDrive.courseOverlayId, r, g, b, a) From 1fce136e54d9cc68299b6453ebcb37e5a0c804c0 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 14 Jul 2024 19:04:36 +0200 Subject: [PATCH 14/44] TWEAK: loop count for pickup and deliver reset loops done: - only if started/stopped by user - job completed - no longer if AD started by itself when refuel, repair --- scripts/Manager/InputManager.lua | 4 +++- scripts/Modes/PickupAndDeliverMode.lua | 3 +++ scripts/Specialization.lua | 2 -- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/scripts/Manager/InputManager.lua b/scripts/Manager/InputManager.lua index ff762d8a..23aa01e6 100644 --- a/scripts/Manager/InputManager.lua +++ b/scripts/Manager/InputManager.lua @@ -262,13 +262,14 @@ function ADInputManager:input_start_stop(vehicle, farmId) end if vehicle.ad.stateModule:isActive() then vehicle.ad.isStoppingWithError = true + vehicle.ad.stateModule:setLoopsDone(0) vehicle:stopAutoDrive() else if farmId and farmId > 0 then -- set the farmId for the vehicle controlled by AD vehicle.ad.stateModule:setActualFarmId(farmId) -- input_start_stop end - + vehicle.ad.stateModule:setLoopsDone(0) vehicle.ad.stateModule:getCurrentMode():start(AutoDrive.USER_PLAYER) if AutoDrive.rightSHIFTmodifierKeyPressed then @@ -278,6 +279,7 @@ function ADInputManager:input_start_stop(vehicle, farmId) if otherVehicle.ad.stateModule.activeBeforeSave then -- g_currentMission:requestToEnterVehicle(otherVehicle) + otherVehicle.ad.stateModule:setLoopsDone(0) otherVehicle.ad.stateModule:getCurrentMode():start(AutoDrive.USER_PLAYER) end if otherVehicle.ad.stateModule.AIVEActiveBeforeSave and otherVehicle.acParameters ~= nil then diff --git a/scripts/Modes/PickupAndDeliverMode.lua b/scripts/Modes/PickupAndDeliverMode.lua index 89632306..f967fdc1 100644 --- a/scripts/Modes/PickupAndDeliverMode.lua +++ b/scripts/Modes/PickupAndDeliverMode.lua @@ -229,18 +229,21 @@ function PickupAndDeliverMode:getNextTask(forced) elseif self.state == PickupAndDeliverMode.STATE_RETURN_TO_START then -- job done - stop AD and tasks AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PickupAndDeliverMode:getNextTask STATE_RETURN_TO_START StopAndDisableADTask...") + self.vehicle.ad.stateModule:setLoopsDone(0) nextTask = StopAndDisableADTask:new(self.vehicle, ADTaskModule.DONT_PROPAGATE) self.state = PickupAndDeliverMode.STATE_FINISHED AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.INFO, "$l10n_AD_Driver_of; %s $l10n_AD_has_reached; %s", 5000, self.vehicle.ad.stateModule:getName(), self.vehicle.ad.stateModule:getFirstMarkerName()) elseif self.state == PickupAndDeliverMode.STATE_PARK then -- job done - drive to park position and stop AD and tasks AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PickupAndDeliverMode:getNextTask STATE_RETURN_TO_START StopAndDisableADTask...") + self.vehicle.ad.stateModule:setLoopsDone(0) nextTask = StopAndDisableADTask:new(self.vehicle, ADTaskModule.DONT_PROPAGATE) self.state = PickupAndDeliverMode.STATE_FINISHED -- message for reached park position send by ParkTask as only there the correct destination is known else -- error path - should never appear! AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PickupAndDeliverMode:getNextTask self.state %s NO nextTask assigned !!!", tostring(self.state)) + self.vehicle.ad.stateModule:setLoopsDone(0) nextTask = StopAndDisableADTask:new(self.vehicle, ADTaskModule.DONT_PROPAGATE) self.state = PickupAndDeliverMode.STATE_FINISHED AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.ERROR, "$l10n_AD_Driver_of; %s $l10n_AD_has_reached; %s", 5000, self.vehicle.ad.stateModule:getName(), "???") diff --git a/scripts/Specialization.lua b/scripts/Specialization.lua index ca7c07c0..7e15817e 100644 --- a/scripts/Specialization.lua +++ b/scripts/Specialization.lua @@ -1038,7 +1038,6 @@ function AutoDrive:startAutoDrive() if self.isServer then if not self.ad.stateModule:isActive() then self.ad.stateModule:setActive(true) - self.ad.stateModule:setLoopsDone(0) self.ad.isStoppingWithError = false self.ad.onRouteToPark = false @@ -1141,7 +1140,6 @@ function AutoDrive:stopAutoDrive() self.spec_aiVehicle.aiTrafficCollisionTranslation[2] = 0 end - self.ad.stateModule:setLoopsDone(0) self.ad.stateModule:setActive(false) self.ad.taskModule:abortAllTasks() From 77642a2ac59e1b12502bb59b82c1b977913f3c68 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 14 Jul 2024 19:11:42 +0200 Subject: [PATCH 15/44] TWEAK: add vehicle.id to log to distinguish between vehicles with same name/type --- scripts/Utils/UtilFuncs.lua | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/scripts/Utils/UtilFuncs.lua b/scripts/Utils/UtilFuncs.lua index c73072a4..bc3f445d 100644 --- a/scripts/Utils/UtilFuncs.lua +++ b/scripts/Utils/UtilFuncs.lua @@ -789,11 +789,11 @@ function AutoDrive.debugMsg(vehicle, debugText, ...) local printText = "[AD] " .. tostring(g_updateLoopIndex) .. " " if vehicle ~= nil then if vehicle.ad ~= nil and vehicle.ad.stateModule ~= nil and vehicle == vehicle:getRootVehicle() then - printText = printText .. vehicle.ad.stateModule:getName() .. ": " + printText = printText .. vehicle.ad.stateModule:getName() .. " (" .. tostring(vehicle.id or 0) .. ") : " elseif vehicle.getName ~= nil then - printText = printText .. vehicle:getName() .. ": " + printText = printText .. vehicle:getName() .. " (" .. tostring(vehicle.id or 0) .. ") : " else - printText = printText .. tostring(vehicle) .. ": " + printText = printText .. tostring(vehicle) .. " (" .. tostring(vehicle.id or 0) .. ") : " end end From 2a7cd881e746592e3a13d34572d068f82c21d18b Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Mon, 15 Jul 2024 19:47:31 +0200 Subject: [PATCH 16/44] TWEAK: Scheduler - reduce delay time - shortcut for removed vehicle if active --- scripts/Manager/Scheduler.lua | 38 +++++++++++++++++++++++------------ 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/scripts/Manager/Scheduler.lua b/scripts/Manager/Scheduler.lua index 27d87f2b..f708834b 100644 --- a/scripts/Manager/Scheduler.lua +++ b/scripts/Manager/Scheduler.lua @@ -2,7 +2,7 @@ ADScheduler = {} ADScheduler.UPDATE_TIME = 3000 -- time interval for calculation/check ADScheduler.FRAMES_TO_CHECK_FOR_ACTUAL_FPS = 10 -- number of frames to calculate actual FPS ADScheduler.FRAMES_TO_CHECK_FOR_AVERAGE_FPS = 60 * 60 -- number of frames to calculate average FPS -ADScheduler.MIN_STEPS_PER_FRAME = 8 -- min steps for pathfinder per frame +ADScheduler.MIN_STEPS_PER_FRAME = 2 -- min steps for pathfinder per frame ADScheduler.MAX_STEPS_PER_FRAME = 8 -- max steps for pathfinder per frame ADScheduler.FPS_DIFFERENCE = 0.1 -- 10 % difference to average for calculation/check ADScheduler.MIN_FPS = 20 -- min FPS where stepsPerFrame will always be decreased @@ -108,9 +108,9 @@ function ADScheduler:addPathfinderVehicle(vehicle) table.insert(self.pathFinderVehicles, vehicle) -- set new vehicle a delay if table.count(self.pathFinderVehicles) > 1 then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, "Scheduler addPathfinderVehicle addDelayTimer 10000") + AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, "Scheduler addPathfinderVehicle addDelayTimer 5000") -- if already vehicle in table, pause this new one - vehicle.ad.pathFinderModule:addDelayTimer(10000) + vehicle.ad.pathFinderModule:addDelayTimer(5000) end end end @@ -120,25 +120,37 @@ function ADScheduler:removePathfinderVehicle(vehicle) if table.contains(self.pathFinderVehicles, vehicle) then AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, "Scheduler removePathfinderVehicle") table.removeValue(self.pathFinderVehicles, vehicle) + if self.activePathFinderVehicle == vehicle then + self.updateTimer:timer(true, ADScheduler.UPDATE_TIME, ADScheduler.UPDATE_TIME) -- shoutcut timer + end end end function ADScheduler:updateActiveVehicle() - if self.activePathFinderVehicle ~= nil then - -- pause current vehicle - self.activePathFinderVehicle.ad.pathFinderModule:addDelayTimer(10000) + -- pause all vehicles + for _, vehicle in pairs(self.pathFinderVehicles) do + if vehicle and vehicle.ad and vehicle.ad.pathFinderModule then + vehicle.ad.pathFinderModule:addDelayTimer(5000) + end + end + if self.activePathFinderVehicle and table.contains(self.pathFinderVehicles, self.activePathFinderVehicle) then + -- add this vehicle to end of queue + local vehicle = self.activePathFinderVehicle + self.activePathFinderVehicle = nil + self:removePathfinderVehicle(vehicle) + self:addPathfinderVehicle(vehicle) end -- get next vehicle - self.activePathFinderVehicle = self.pathFinderVehicles[1] + local nextVehicle = self.pathFinderVehicles[1] - if self.activePathFinderVehicle ~= nil then - AutoDrive.debugPrint(self.activePathFinderVehicle, AutoDrive.DC_PATHINFO, "Scheduler updateActiveVehicle activePathFinderVehicle") + if nextVehicle ~= nil then + AutoDrive.debugPrint(nextVehicle, AutoDrive.DC_PATHINFO, "Scheduler updateActiveVehicle activate nextVehicle") -- found vehicle - -- add this vehicle to end of queue - self:removePathfinderVehicle(self.activePathFinderVehicle) - self:addPathfinderVehicle(self.activePathFinderVehicle) -- unpause vehicle to continue pathfinder calculation - self.activePathFinderVehicle.ad.pathFinderModule:addDelayTimer(0) + if nextVehicle.ad and nextVehicle.ad.pathFinderModule then + nextVehicle.ad.pathFinderModule:addDelayTimer(0) + end + self.activePathFinderVehicle = nextVehicle end end From 6a694457b2d2759da00ea45685f6400e8197c6b9 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Mon, 15 Jul 2024 20:24:58 +0200 Subject: [PATCH 17/44] TWEAK: Pathfinder based on Astar - active by default - switch back to previous with Exp Schwitch New Pathfinder - need reset before start pathfinder functions! - running pathfinder does not change previous / new calculation --- modDesc.xml | 2 +- scripts/AutoDrive.lua | 3 +- .../Modules/PathFinderModule - Kopie (15).lua | 2656 +++++++++++++++++ scripts/Modules/PathFinderModule.lua | 743 ++++- scripts/Tasks/CatchCombinePipeTask.lua | 5 +- scripts/Tasks/DriveToDestinationTask.lua | 5 +- scripts/Tasks/DriveToVehicleTask.lua | 5 +- scripts/Tasks/EmptyHarvesterTask.lua | 6 +- scripts/Tasks/ExitFieldTask.lua | 6 +- scripts/Tasks/FollowCombineTask.lua | 5 +- scripts/Tasks/LoadAtDestinationTask.lua | 5 +- scripts/Tasks/ParkTask.lua | 5 +- scripts/Tasks/RefuelTask.lua | 5 +- scripts/Tasks/RepairTask.lua | 5 +- scripts/Tasks/UnloadAtDestinationTask.lua | 12 +- 15 files changed, 3350 insertions(+), 118 deletions(-) create mode 100644 scripts/Modules/PathFinderModule - Kopie (15).lua diff --git a/modDesc.xml b/modDesc.xml index 4f65d788..84b20976 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -32,7 +32,7 @@ Différents modes d'utilisation ont été ajoutés depuis les premières version - 2.0.1.7-RC2 + 2.0.1.7-RC3 icon.dds diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index b21afcd1..ba957160 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -1,5 +1,5 @@ AutoDrive = {} -AutoDrive.version = "2.0.1.7-RC2" +AutoDrive.version = "2.0.1.7-RC3" AutoDrive.directory = g_currentModDirectory @@ -17,6 +17,7 @@ AutoDrive.experimentalFeatures.UTurn = true AutoDrive.experimentalFeatures.FoldImplements = true AutoDrive.experimentalFeatures.RefuelOnlyAtValidStations = true AutoDrive.experimentalFeatures.RecordWhileNotInVehicle = false +AutoDrive.experimentalFeatures.NewPathfinder = true AutoDrive.dynamicChaseDistance = true AutoDrive.smootherDriving = true diff --git a/scripts/Modules/PathFinderModule - Kopie (15).lua b/scripts/Modules/PathFinderModule - Kopie (15).lua new file mode 100644 index 00000000..f9447250 --- /dev/null +++ b/scripts/Modules/PathFinderModule - Kopie (15).lua @@ -0,0 +1,2656 @@ +--[[ +New to Pathfinder: +- 3 Settings are considered: restrictToField, avoidFruit, pathFinderTime + +1. restrictToField +The Pathfinder tries to find a path to target in the limit of the current field borders. +This is only possible if the vehicle is located inside a field. +If disabled, only setting avoidFruit limits or not, the shortest path to target will be calculated! + +2. avoidFruit +The Pathfinder tries to find a path to target without passing through fruit. +This is effective if vehicle is inside a field. +NEW: Working also outside of a field, i.e. if possible a path around a field to network will be searched for! +If disabled, the shortest path to target will be calculated! + +3. pathFinderTime +Is more a factor for the range the Pathfinder will search for path to target, default is 1. +In case fruit avoid on large fields with sufficient headland is not working, you may try to increase. +But be aware the calculation time will increase as well! + +- 3 fallback scenario will automatic take effect: +If setting restrictToField is enabled, fallback 1 and 2 are possible: +fallback 1: +The first fallback will extend the field border by 30+ meters around the field pathfinding was started. +With this the vehicle will search for a path also around the field border! +30+ meters depend on the vehicle + trailer turn radius, if greater then extended wider. +fallback 2: +Second fallback will deactivate all field border restrictions, means setting avoidFruit will limit or not the search range for path. + +fallback 3: +Third fallback will take effect only if setting avoidFruit is enabled. +It will disable fruit avoid automatically if no path was found. + +Inside informations: +This is a calculation with the worst assumption of all cells to be checked: + +Number of cells: +#cells = MAX_PATHFINDER_STEPS_PER_FRAME / 2 * MAX_PATHFINDER_STEPS_TOTAL * 3 (next directions - see determineNextGridCells) + +PathFinderModule.MAX_PATHFINDER_STEPS_PER_FRAME = 10 +PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL = 400 +#cells = 6000 + +with minTurnRadius = 7m calculated area: + +cellsize = 7m * 7m = 49m^2 +overall area = #cells * cellsize * pathFinderTime + +with pathFinderTime = 1: +overall area = 6000 * 49 * 1 = 294000 m^2 +for quadrat field layout: side length ~ 540m + +with pathFinderTime = 2: side length ~ 760m +with pathFinderTime = 3: side length ~ 940m + +This is inclusive of the field border cells! +]] + +PathFinderModule = {} +PathFinderModule.debug = false + +PathFinderModule.PATHFINDER_MAX_RETRIES = 3 +PathFinderModule.MAX_PATHFINDER_STEPS_PER_FRAME = 2 +PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL = 400 +PathFinderModule.MAX_PATHFINDER_STEPS_COMBINE_TURN = 100 +PathFinderModule.PATHFINDER_FOLLOW_DISTANCE = 45 +PathFinderModule.PATHFINDER_TARGET_DISTANCE = 7 +PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE = 16 +PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE_CLOSE = 6 +PathFinderModule.PATHFINDER_START_DISTANCE = 7 +PathFinderModule.MAX_FIELDBORDER_CELLS = 5 + +PathFinderModule.PP_MIN_DISTANCE = 20 +PathFinderModule.PP_CELL_X = 9 +PathFinderModule.PP_CELL_Z = 9 + +PathFinderModule.GRID_SIZE_FACTOR = 0.5 +PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER = 1.1 + +PathFinderModule.PP_MAX_EAGER_LOOKAHEAD_STEPS = 1 + +PathFinderModule.MIN_FRUIT_VALUE = 50 +PathFinderModule.SLOPE_DETECTION_THRESHOLD = math.rad(20) +PathFinderModule.NEW_PF_STEP_FACTOR = 4 +--[[ +from Giants Engine: +AITurnStrategy.SLOPE_DETECTION_THRESHOLD = 0.5235987755983 +]] + +function PathFinderModule:new(vehicle) + local o = {} + setmetatable(o, self) + self.__index = self + o.vehicle = vehicle + PathFinderModule.reset(o) + return o +end + +function PathFinderModule:reset() + PathFinderModule.debugMsg(self.vehicle, "PFM:reset start") + self.mask = AutoDrive.collisionMaskTerrain + self.steps = 0 + self.grid = {} + self.wayPoints = {} + self.initNew = false + self.path = {} + self.diffOverallNetTime = 0 + self.retryCounter = 0 + self.delayTime = 0 + self.restrictToField = false + self.avoidFruitSetting = false + self.destinationId = 0 + self.fallBackMode1 = false + self.fallBackMode2 = false + self.fallBackMode3 = false + self.isFinished = true + self.smoothDone = true + self.fruitAreas = {} + + if AutoDrive.experimentalFeatures.NewPathfinder then + self.PP_UP = 0 + self.PP_UP_LEFT = 1 + self.PP_LEFT = 2 + self.PP_DOWN_LEFT = 3 + self.PP_DOWN = 4 + self.PP_DOWN_RIGHT = 5 + self.PP_RIGHT = 6 + self.PP_UP_RIGHT = 7 + self.direction_to_text = { + "PP_UP", + "PP_UP_LEFT", + "PP_LEFT", + "PP_DOWN_LEFT", + "PP_DOWN", + "PP_DOWN_RIGHT", + "PP_RIGHT", + "PP_UP_RIGHT", + "unknown" + } + self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) + self.isNewPF = true + else + self.PP_UP = 0 + self.PP_UP_RIGHT = 1 + self.PP_RIGHT = 2 + self.PP_DOWN_RIGHT = 3 + self.PP_DOWN = 4 + self.PP_DOWN_LEFT = 5 + self.PP_LEFT = 6 + self.PP_UP_LEFT = 7 + self.direction_to_text = { + "PP_UP", + "PP_UP_RIGHT", + "PP_RIGHT", + "PP_DOWN_RIGHT", + "PP_DOWN", + "PP_DOWN_LEFT", + "PP_LEFT", + "PP_UP_LEFT", + "unknown" + } + self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) * 2 / 3 + self.isNewPF = false + end +end + +function PathFinderModule:hasFinished() + if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then + return false + end + if self.isFinished and self.smoothDone == true then + return true + end + return false +end + +function PathFinderModule:getPath() + return self.wayPoints +end + +function PathFinderModule:startPathPlanningToNetwork(destinationId) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningToNetwork destinationId %s", + tostring(destinationId) + ) + ) + local closest = self.vehicle:getClosestWayPoint() + self:startPathPlanningToWayPoint(closest, destinationId) + self.goingToNetwork = true +end + +function PathFinderModule:startPathPlanningToWayPoint(wayPointId, destinationId) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningToWayPoint wayPointId %s", + tostring(wayPointId) + ) + ) + local targetNode = ADGraphManager:getWayPointById(wayPointId) + local wayPoints = ADGraphManager:pathFromTo(wayPointId, destinationId) + if wayPoints ~= nil and #wayPoints > 1 then + local vecToNextPoint = {x = wayPoints[2].x - targetNode.x, z = wayPoints[2].z - targetNode.z} + self:startPathPlanningTo(targetNode, vecToNextPoint) + self.goingToNetwork = true + self.destinationId = destinationId + self.targetWayPointId = wayPointId + self.appendWayPoints = wayPoints + end + return +end + +function PathFinderModule:startPathPlanningToPipe(combine, chasing) + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:startPathPlanningToPipe") + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningToPipe combine %s", + tostring(combine:getName()) + ) + ) + local _, worldY, _ = getWorldTranslation(combine.components[1].node) + local rx, _, rz = localDirectionToWorld(combine.components[1].node, 0, 0, 1) + if combine.components[2] ~= nil and combine.components[2].node ~= nil then + rx, _, rz = localDirectionToWorld(combine.components[2].node, 0, 0, 1) + end + local combineVector = {x = rx, z = rz} + + local pipeChasePos, pipeChaseSide = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getPipeChasePosition(true) + -- We use the follow distance as a proxy measure for "what works" for the size of the + -- field being worked. + -- local followDistance = AutoDrive.getSetting("followDistance", self.vehicle) + -- Use the length of the tractor-trailer combo to determine how far to drive to straighten + -- the trailer. + -- 2*math.sin(math.pi/8)) is the third side of a 45-67.5-67.5 isosceles triangle with the + -- equal sides being the length of the tractor train + local lengthOffset = combine.size.length / 2 + + AutoDrive.getTractorTrainLength(self.vehicle, true, false) * (2 * math.sin(math.pi / 8)) + -- A bit of a sanity check, in case the vehicle is absurdly long. + --if lengthOffset > self.PATHFINDER_FOLLOW_DISTANCE then + -- lengthOffset = self.PATHFINDER_FOLLOW_DISTANCE + --elseif + if lengthOffset <= self.PATHFINDER_TARGET_DISTANCE then + lengthOffset = self.PATHFINDER_TARGET_DISTANCE + end + + --local target = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} + -- The sugarcane harvester needs extra room or it collides + --if pipeChaseSide ~= CombineUnloaderMode.CHASEPOS_REAR or CombineUnloaderMode:isSugarcaneHarvester(combine) then + -- AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:startPathPlanningToPipe?lengthOffset " .. lengthOffset) + -- local straightenNode = {x = pipeChasePos.x - lengthOffset * rx, y = worldY, z = pipeChasePos.z - lengthOffset * rz} + -- self:startPathPlanningTo(straightenNode, combineVector) + -- table.insert(self.appendWayPoints, target) + --else + -- self:startPathPlanningTo(target, combineVector) + --end + if combine.ad.isAutoAimingChopper then + -- local pathFinderTarget = {x = pipeChasePos.x - (self.PATHFINDER_TARGET_DISTANCE) * rx, y = worldY, z = pipeChasePos.z - (self.PATHFINDER_TARGET_DISTANCE) * rz} + local pathFinderTarget = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} + self:startPathPlanningTo(pathFinderTarget, combineVector) + + elseif combine.ad.isFixedPipeChopper then + local pathFinderTarget = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} + -- only append target points / try to straighten the driver/trailer combination if we are driving up to the pipe not the rear end + if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then + pathFinderTarget = {x = pipeChasePos.x - (combine.size.length) * rx, y = worldY, z = pipeChasePos.z - (combine.size.length) * rz} + end + local appendedNode = {x = pipeChasePos.x - (combine.size.length / 2 * rx), y = worldY, z = pipeChasePos.z - (combine.size.length / 2 * rz)} + + self:startPathPlanningTo(pathFinderTarget, combineVector) + + if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then + table.insert(self.appendWayPoints, appendedNode) + end + else + -- combine.ad.isHarvester + local pathFinderTarget = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} + -- only append target points / try to straighten the driver/trailer combination if we are driving up to the pipe not the rear end + if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then + pathFinderTarget = {x = pipeChasePos.x - (lengthOffset) * rx, y = worldY, z = pipeChasePos.z - (lengthOffset) * rz} + end + local appendedNode = {x = pipeChasePos.x - (combine.size.length / 2 * rx), y = worldY, z = pipeChasePos.z - (combine.size.length / 2 * rz)} + + self:startPathPlanningTo(pathFinderTarget, combineVector) + + if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then + table.insert(self.appendWayPoints, appendedNode) + table.insert(self.appendWayPoints, pipeChasePos) + end + end + + if combine.spec_combine ~= nil and combine.ad.isHarvester then + if combine.spec_combine.fillUnitIndex ~= nil and combine.spec_combine.fillUnitIndex ~= 0 then + local fillType = g_fruitTypeManager:getFruitTypeIndexByFillTypeIndex(combine:getFillUnitFillType(combine.spec_combine.fillUnitIndex)) + if fillType ~= nil then + self.fruitToCheck = fillType + local fruitType = g_fruitTypeManager:getFruitTypeByIndex(fillType) + + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningToPipe self.fruitToCheck %s Fruit name %s title %s", + tostring(self.fruitToCheck), + tostring(fruitType.fillType.name), + tostring(fruitType.fillType.title) + ) + ) + end + end + end + + self.goingToPipe = true + if AutoDrive.getDistanceBetween(self.vehicle, combine) < 50 then + -- shorten path calculation for close combine + self.max_pathfinder_steps = PathFinderModule.MAX_PATHFINDER_STEPS_COMBINE_TURN + end + self.chasingVehicle = chasing +end + +function PathFinderModule:startPathPlanningToVehicle(targetVehicle, targetDistance) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningToVehicle targetVehicle %s", + tostring(targetVehicle:getName()) + ) + ) + local worldX, worldY, worldZ = getWorldTranslation(targetVehicle.components[1].node) + local rx, _, rz = localDirectionToWorld(targetVehicle.components[1].node, 0, 0, 1) + local targetVector = {x = rx, z = rz} + + local wpBehind = {x = worldX - targetDistance * rx, y = worldY, z = worldZ - targetDistance * rz} + self:startPathPlanningTo(wpBehind, targetVector) + + self.goingToPipe = false + self.chasingVehicle = true + self.isSecondChasingVehicle = true + if targetVehicle.ad ~= nil and targetVehicle.ad.pathFinderModule ~= nil and targetVehicle.ad.pathFinderModule.fruitToCheck ~= nil then + self.fruitToCheck = targetVehicle.ad.pathFinderModule.fruitToCheck + end +end + +function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningTo targetPoint x,z %d %d", + math.floor(targetPoint.x), + math.floor(targetPoint.z) + ) + ) + ADScheduler:addPathfinderVehicle(self.vehicle) + self.targetVector = targetVector + local vehicleWorldX, vehicleWorldY, vehicleWorldZ = getWorldTranslation(self.vehicle.components[1].node) + local vehicleRx, _, vehicleRz = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, 1) + local vehicleVector = {x = vehicleRx, z = vehicleRz} + self.startX = vehicleWorldX + self.PATHFINDER_START_DISTANCE * vehicleRx + self.startZ = vehicleWorldZ + self.PATHFINDER_START_DISTANCE * vehicleRz + + local angleRad = math.atan2(targetVector.z, targetVector.x) + angleRad = AutoDrive.normalizeAngle(angleRad) + + self.vectorX = {x = math.cos(angleRad) * self.minTurnRadius, z = math.sin(angleRad) * self.minTurnRadius} + self.vectorZ = {x = - math.sin(angleRad) * self.minTurnRadius, z = math.cos(angleRad) * self.minTurnRadius} + + --Make the target a few meters ahead of the road to the start point + local targetX = targetPoint.x - math.cos(angleRad) * self.PATHFINDER_TARGET_DISTANCE + local targetZ = targetPoint.z - math.sin(angleRad) * self.PATHFINDER_TARGET_DISTANCE + + self.grid = {} + self.steps = 0 + self.retryCounter = 0 + self.isFinished = false + self.fallBackMode1 = false -- disable restrict to field + self.fallBackMode2 = false -- disable restrict to field border + self.fallBackMode3 = false -- disable avoid fruit + self.max_pathfinder_steps = PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL * AutoDrive.getSetting("pathFinderTime") + + self.fruitToCheck = nil + + self.startCell = {x = 0, z = 0} + self.startCell.direction = self:worldDirectionToGridDirection(vehicleVector) + self.startCell.visited = false + self.startCell.out = nil + self.startCell.isRestricted = false + self.startCell.hasCollision = false + self.startCell.hasFruit = false + self.startCell.steps = 0 + self.startCell.bordercells = 0 + self.currentCell = nil + + local vehicleBehindX, _, vehicleBehindZ = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, -self.minTurnRadius) + local vehicleBehindVector = {x = vehicleBehindX, z = vehicleBehindZ} + self.behindStartCell = self:worldLocationToGridLocation(vehicleWorldX + vehicleBehindX, vehicleWorldZ + vehicleBehindZ) + self.behindStartCell.direction = self:worldDirectionToGridDirection(vehicleBehindVector, vehicleVector) + + -- table.insert(self.grid, self.startCell) + local gridKey = string.format("%d|%d|%d", self.startCell.x, self.startCell.z, self.startCell.direction) + self.grid[gridKey] = self.startCell + + self.smoothStep = 0 + self.smoothDone = false + self.target = {x = targetX, z = targetZ} + + local targetCellZ = (((targetX - self.startX) / self.vectorX.x) * self.vectorX.z - targetZ + self.startZ) / (((self.vectorZ.x / self.vectorX.x) * self.vectorX.z) - self.vectorZ.z) + local targetCellX = (targetZ - self.startZ - targetCellZ * self.vectorZ.z) / self.vectorX.z + targetCellX = AutoDrive.round(targetCellX) + targetCellZ = AutoDrive.round(targetCellZ) + self.targetCell = {x = targetCellX, z = targetCellZ, direction = self.PP_UP} + self.targetAhead = {x = targetX + self.vectorX.x, z = targetZ + self.vectorX.z} + self.targetAheadCell = self:worldLocationToGridLocation(self.targetAhead.x, self.targetAhead.z) + + self:determineBlockedCells(self.targetCell) + --self:checkGridCell(self.targetCell) + + self.appendWayPoints = {} + self.appendWayPoints[1] = targetPoint + + self.goingToCombine = false + + self.startIsOnField = AutoDrive.checkIsOnField(vehicleWorldX, vehicleWorldY, vehicleWorldZ) and self.vehicle.ad.sensors.frontSensorField:pollInfo(true) + self.endIsOnField = AutoDrive.checkIsOnField(targetX, vehicleWorldY, targetZ) + self.restrictToField = AutoDrive.getSetting("restrictToField", self.vehicle) and self.startIsOnField and self.endIsOnField + self.goingToPipe = false + self.chasingVehicle = false + self.isSecondChasingVehicle = false + self.goingToNetwork = false + self.destinationId = nil + self.completelyBlocked = false + self.targetBlocked = false --self.targetCell.hasCollision or self.targetCell.isRestricted --> always false TODO: how to use this ? + self.blockedByOtherVehicle = false + self.avoidFruitSetting = AutoDrive.getSetting("avoidFruit", self.vehicle) + -- self.targetFieldId = g_farmlandManager:getFarmlandIdAtWorldPosition(targetX, targetZ) -- only has ID if vector target is onField + -- self.targetFieldId = nil + -- if self.restrictToField and self.targetFieldId ~= nil and self.targetFieldId > 0 then + -- self.reachedFieldBorder = startIsOnField + -- local targetFieldPos = {x = g_farmlandManager.farmlands[self.targetFieldId].xWorldPos, z = g_farmlandManager.farmlands[self.targetFieldId].zWorldPos} + + -- self.fieldCell = self:worldLocationToGridLocation(targetFieldPos.x, targetFieldPos.z) + -- else + -- self.reachedFieldBorder = true + -- end + self.chainStartToTarget = {} + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningTo self.minTurnRadius %s vectorX.x,vectorX.z %s %s vectorZ.x,vectorZ.z %s %s", + tostring(self.minTurnRadius), + tostring(self.vectorX.x), + tostring(self.vectorX.z), + tostring(self.vectorZ.x), + tostring(self.vectorZ.z) + ) + ) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningTo startCell xz %d %d direction %s", + math.floor(self.startCell.x), + math.floor(self.startCell.z), + tostring(self.direction_to_text[self.startCell.direction+1]) + ) + ) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningTo start targetCell xz %d %d direction %s", + math.floor(self.targetCell.x), + math.floor(self.targetCell.z), + tostring(self.direction_to_text[self.targetCell.direction+1]) + ) + ) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningTo restrictToField %s endIsOnField %s", + tostring(self.restrictToField), + tostring(self.endIsOnField) + ) + ) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM startPathPlanningTo self.fruitToCheck %s getSetting avoidFruit %s", + tostring(self.fruitToCheck), + tostring(self.avoidFruitSetting) + ) + ) +end + +function PathFinderModule:restartAtNextWayPoint() + self.targetWayPointId = self.appendWayPoints[2].id + local targetNode = ADGraphManager:getWayPointById(self.targetWayPointId) + local wayPoints = ADGraphManager:pathFromTo(self.targetWayPointId, self.destinationId) + if wayPoints ~= nil and #wayPoints > 1 then + local vecToNextPoint = {x = wayPoints[2].x - targetNode.x, z = wayPoints[2].z - targetNode.z} + local storedRetryCounter = self.retryCounter + local storedTargetWayPointId = self.targetWayPointId + local storedDestinationId = self.destinationId + self:startPathPlanningTo(targetNode, vecToNextPoint) + self.retryCounter = storedRetryCounter + self.destinationId = storedDestinationId + self.fallBackMode1 = false -- disable restrict to field + self.fallBackMode2 = false -- disable restrict to field border + self.fallBackMode3 = false -- disable avoid fruit + self.targetWayPointId = storedTargetWayPointId + if self.targetWayPointId ~= nil then + self.appendWayPoints = ADGraphManager:pathFromTo(self.targetWayPointId, self.destinationId) + end + end + self:autoRestart() +end + +function PathFinderModule:autoRestart() + PathFinderModule.debugMsg(self.vehicle, "PFM:autoRestart start") + self.steps = 0 + self.grid = {} + self.wayPoints = {} + self.initNew = false + self.path = {} + self.diffOverallNetTime = 0 + self.startCell.visited = false + self.startCell.out = nil + self.currentCell = nil + + local gridKey = string.format("%d|%d|%d", self.startCell.x, self.startCell.z, self.startCell.direction) + self.grid[gridKey] = self.startCell + + self:determineBlockedCells(self.targetCell) + self.smoothStep = 0 + self.smoothDone = false + self.completelyBlocked = false + -- self.targetBlocked = false --> always false TODO: how to use this ? +end + +function PathFinderModule:abort() + PathFinderModule.debugMsg(self.vehicle, "PFM:abort start") + self.isFinished = true + self.smoothDone = true + self.wayPoints = {} + ADScheduler:removePathfinderVehicle(self.vehicle) +end + +function PathFinderModule:isBlocked() --> true if no path in grid found -- used in: ExitFieldTask, UnloadAtDestinationTask + return self.completelyBlocked -- or self.targetBlocked --> always false TODO: how to use this ? +end + +function PathFinderModule:isTargetBlocked() --> always false TODO: how to use this ? -- used in: ExitFieldTask, UnloadAtDestinationTask, EmptyHarvesterTask + return self.targetBlocked +end + +-- return the actual and max number of iterations the pathfinder will perform by itself, could be used to show info in HUD +function PathFinderModule:getCurrentState() + local maxStates = 1 + local actualState = 1 + if self.restrictToField then + maxStates = maxStates + 2 + end + if self.avoidFruitSetting then + maxStates = maxStates + 1 + end + if self.destinationId ~= nil then + maxStates = maxStates + 3 + end + + if self.fallBackMode1 then + actualState = actualState + 1 + end + if self.fallBackMode2 then + actualState = actualState + 1 + end + if self.fallBackMode3 then + actualState = actualState + 1 + end + if self.destinationId ~= nil and self.retryCounter > 0 then + actualState = actualState + 1 + end + + return actualState, maxStates, self.steps, self.max_pathfinder_steps +end + +function PathFinderModule:timedOut() --> not self:isBlocked() -- used in: ExitFieldTask, UnloadAtDestinationTask + return not self:isBlocked() +end + +function PathFinderModule:addDelayTimer(delayTime) -- used in: ExitFieldTask, UnloadAtDestinationTask, CatchCombinePipeTask, EmptyHarvesterTask + self.delayTime = delayTime +end + +function PathFinderModule:update(dt) + + --stop if called without prior 'start' method calls + if self.startCell == nil then + self:abort() + end + + self.delayTime = math.max(0, self.delayTime - dt) + if self.delayTime > 0 then + return + end + + if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then + if self.isNewPF then + self:drawDebugNewPF() + else + if self.isFinished and self.smoothDone and self.wayPoints ~= nil and self.chainStartToTarget ~= nil and #self.chainStartToTarget > 0 and self.vehicle.ad.stateModule:getSpeedLimit() > 40 then + self:drawDebugForCreatedRoute() + else + self:drawDebugForPF() + end + end + end + if self.isFinished then + if not self.smoothDone then + if self.isNewPF then + self:createWayPointsNew() + else + self:createWayPoints() + end + end + if self.smoothDone then + ADScheduler:removePathfinderVehicle(self.vehicle) + PathFinderModule.debugMsg(self.vehicle, "PFM:update Complete #self.path %s" + , tostring(#self.path) + ) + end + return + end + + self.steps = self.steps + 1 + if (self.steps % 100) == 0 then + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - self.steps %d #self.grid %d", self.steps, table.count(self.grid)) + end + + if self.completelyBlocked or self.targetBlocked or self.steps > (self.max_pathfinder_steps) then + --[[ We need some better logic here. + Some situations might be solved by the module itself by either + a) 'fallBackMode (ignore fruit and field restrictions)' + b) 'try next wayPoint' + while others should be handled by the calling task, to properly assess the current situation + c) 'retry same location - with or without prior pausing' + d) 'update target location and reinvoke pathfinder if target has moved' + e) 'try different field exit strategy' + --]] + -- Only allow fallback if we are not heading for a moving vehicle + local fallBackModeAllowed1 = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.restrictToField) and (not self.fallBackMode1) -- disable restrict to field + local fallBackModeAllowed2 = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.restrictToField) and (not self.fallBackMode2) -- disable restrict to field border + local fallBackModeAllowed3 = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.avoidFruitSetting) and (not self.fallBackMode3) -- disable avoid fruit + local increaseStepsAllowed = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.max_pathfinder_steps < PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL * AutoDrive.getSetting("pathFinderTime")) -- increase number of steps if possible + + -- Only allow auto restart when planning path to network and we can adjust target wayPoint + local retryAllowed = self.destinationId ~= nil and self.retryCounter < self.PATHFINDER_MAX_RETRIES + + if fallBackModeAllowed1 then + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed1: yes -> going fallback now -> disable restrict to field #self.grid %d", table.count(self.grid)) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed1: yes -> going fallback now -> disable restrict to field #self.grid %d", + table.count(self.grid) + ) + ) + self.fallBackMode1 = true + self:autoRestart() + elseif fallBackModeAllowed2 and not self.isNewPF then + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", table.count(self.grid)) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", + table.count(self.grid) + ) + ) + self.fallBackMode2 = true + self:autoRestart() + elseif fallBackModeAllowed3 then + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed3: yes -> going fallback now -> disable avoid fruit #self.grid %d", table.count(self.grid)) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed3: yes -> going fallback now -> disable avoid fruit #self.grid %d", + table.count(self.grid) + ) + ) + self.fallBackMode3 = true + self:autoRestart() + elseif increaseStepsAllowed then + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> increaseStepsAllowed: yes -> restart #self.grid %d self.max_pathfinder_steps %d", table.count(self.grid), self.max_pathfinder_steps) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - error - retryAllowed: no -> increaseStepsAllowed: yes -> restart -> disable avoid fruit #self.grid %d self.max_pathfinder_steps %d", + table.count(self.grid), + self.max_pathfinder_steps + ) + ) + self.max_pathfinder_steps = self.max_pathfinder_steps + PathFinderModule.MAX_PATHFINDER_STEPS_COMBINE_TURN + self.max_pathfinder_steps = math.min(self.max_pathfinder_steps, PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL * AutoDrive.getSetting("pathFinderTime")) + self.fallBackMode1 = false + self.fallBackMode2 = false + self.fallBackMode3 = false + self:autoRestart() + elseif retryAllowed then + self.retryCounter = self.retryCounter + 1 + --if we are going to the network and can't find a path. Just select the next waypoint for now + if self.appendWayPoints ~= nil and #self.appendWayPoints > 2 then + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: yes -> retry now retryCounter %d", self.retryCounter) + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - error - retryAllowed: yes -> retry now retryCounter %d", + self.retryCounter + ) + ) + self:restartAtNextWayPoint() + else + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: yes -> but no appendWayPoints") + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - error - retryAllowed: yes -> but no appendWayPoints" + ) + ) + self:abort() + end + else + if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then + return + end + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed: no -> aborting now") + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed: no -> aborting now" + ) + ) + self:abort() + end + return + end + + if self.isNewPF then + if not self.isFinished then + if not self.initNew then + self:setupNew(self.behindStartCell, self.startCell,self.targetCell) + if self.nodeBehindStart == self.nodeGoal then + self.completelyBlocked = true + return -- no valid path + end + end + local diffNetTime = netGetTime() + + local current + local add_neighbor_fn = function(neighbor, cost) + if self:isDriveable(neighbor) then + if not self.closedset[neighbor] then + if not cost then cost = self:get_cost(current, neighbor) end + local tentative_g_score = self.g_score[current] + cost + local openset_idx = self.openset[neighbor] + if not openset_idx or tentative_g_score < self.g_score[neighbor] then + self.came_from[neighbor] = current + self.g_score[neighbor] = tentative_g_score + self.h_score[neighbor] = self.h_score[neighbor] or self:estimate_cost(neighbor, self.nodeGoal) + self.f_score[neighbor] = tentative_g_score + self.h_score[neighbor] + self.openset[neighbor] = true + end + end + end + end + local count = 0 + while next(self.openset) do + count = count + 1 + if count > 10000 then + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + AutoDrive.debugMsg(self.vehicle, "PFM:find ERROR exit counter count %d self.diffOverallNetTime %d" + , count + , self.diffOverallNetTime + ) + + self.completelyBlocked = true + return -- no valid path + end + current = self:pop_best_node(self.openset, self.f_score) + if current == self.nodeGoal or self:reachedGoal(current, self.nodeGoal) then + self.came_from[self.nodeGoal] = current + self.path = self:unwind_path({}, self.came_from, self.nodeGoal) + table.insert(self.path, self.nodeGoal) + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + PathFinderModule.debugMsg(self.vehicle, "PFM:update find goal reached self.steps %d diffOverallNetTime %d self.nodeGoal xz %d,%d current xz %d,%d" + , self.steps + , self.diffOverallNetTime + , self.nodeGoal.x, self.nodeGoal.z + , current.x, current.z + ) + + self.isFinished = true + return -- found path + end + if current then self.closedset[current] = true end + local from_node = self.came_from[current] + self:get_neighbors(current, from_node, add_neighbor_fn) + if count > (ADScheduler:getStepsPerFrame() * PathFinderModule.NEW_PF_STEP_FACTOR) then + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + PathFinderModule.debugMsg(self.vehicle, "PFM:find steps in frame count %d diffNetTime %d self.diffOverallNetTime %d" + , count + , diffNetTime + , self.diffOverallNetTime + ) + + return -- shedule + end + end + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + PathFinderModule.debugMsg(self.vehicle, "PFM:find exit end count %d self.diffOverallNetTime %d" + , count + , self.diffOverallNetTime + ) + + self.completelyBlocked = true + return -- no valid path + end + else + --We should see some perfomance increase by localizing the sqrt/pow functions right here + local sqrt = math.sqrt + local distanceFunc = function(a, b) + return sqrt(a * a + b * b) + end + + for i = 1, ADScheduler:getStepsPerFrame(), 1 do + if self.currentCell == nil then + + self.currentCell = self:findClosestCell(self.grid, math.huge) + + if self.currentCell ~= nil and distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) < 1.5 then + + if self.currentCell.out == nil then + self:determineNextGridCells(self.currentCell) + end + + if self:reachedTargetsNeighbor(self.currentCell.out) then + return + end + end + + if self.currentCell == nil then + --Mark process stopped if we have no more cells to check + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - Mark process stopped if we have no more cells to check") + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - Mark process stopped if we have no more cells to check #self.grid %d", + table.count(self.grid) + ) + ) + self.completelyBlocked = true + break + end + else + if self.currentCell.out == nil then + self:determineNextGridCells(self.currentCell) + end + self:testNextCells(self.currentCell) + + --Try shortcutting the process here. We dont have to go through the whole grid if one of the out points is viable and closer than the currenCell which was already closest + local currentDistance = distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) + + local outCells = {} + for _, outCell in pairs(self.currentCell.out) do + local gridKey = string.format("%d|%d|%d", outCell.x, outCell.z, outCell.direction) + if self.grid[gridKey] ~= nil then + table.insert(outCells, self.grid[gridKey]) + end + end + local nextCell = self:findClosestCell(outCells, currentDistance) + + -- Lets again check if we have reached our target already + if self:reachedTargetsNeighbor(self.currentCell.out) then + return + end + + self.currentCell = nextCell + end + end + end +end + +function PathFinderModule:reachedTargetsNeighbor(cells) + for _, outCell in pairs(cells) do + if outCell.x == self.targetCell.x and outCell.z == self.targetCell.z then + self.isFinished = true + self.targetCell.incoming = self.currentCell --.incoming + + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - path found") + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - path found #self.grid %d", + table.count(self.grid) + ) + ) + + return true + end + end + return false +end + +function PathFinderModule:findClosestCell(cells, startDistance) + local cellsToCheck = cells + local sqrt = math.sqrt + local distanceFunc = function(a, b) + return sqrt(a * a + b * b) + end + local minDistance = startDistance + local bestCell = nil + local bestSteps = math.huge + + for _, cell in pairs(cellsToCheck) do + if (not cell.visited) and (not cell.hasCollision) and (not cell.isRestricted) and (cell.bordercells < PathFinderModule.MAX_FIELDBORDER_CELLS) then + local distance = distanceFunc(self.targetCell.x - cell.x, self.targetCell.z - cell.z) + + if (distance < minDistance) or (distance == minDistance and cell.steps < bestSteps) then + minDistance = distance + bestCell = cell + bestSteps = cell.steps + end + end + end + + return bestCell +end + +function PathFinderModule:testNextCells(cell) + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM testNextCells start cell xz %d %d isRestricted %s hasFruit %s direction %s", + math.floor(cell.x), + math.floor(cell.z), + tostring(cell.isRestricted), + tostring(cell.hasFruit), + tostring(self.direction_to_text[cell.direction+1]) + ) + ) + end + for _, location in pairs(cell.out) do + local createPoint = true + local duplicatePointDirection = -1 + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM testNextCells location xz %d %d direction %s", + math.floor(location.x), + math.floor(location.z), + tostring(self.direction_to_text[location.direction+1]) + ) + ) + end + for i = -1, self.PP_UP_LEFT, 1 do -- important: do not break this loop to check for all directions! + local gridKey = string.format("%d|%d|%d", location.x, location.z, i) + if self.grid[gridKey] ~= nil then + -- cell is already in the grid + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM testNextCells gridKey %s cell.x,cell.z %s %s direction %s", + tostring(gridKey), + tostring(self.grid[gridKey].x), + tostring(self.grid[gridKey].z), + tostring(self.direction_to_text[self.grid[gridKey].direction+1]) + ) + ) + end + + if self.grid[gridKey].x == location.x and self.grid[gridKey].z == location.z then -- out cell is already in grid + + if self.grid[gridKey].direction == -1 then + createPoint = false + elseif self.grid[gridKey].direction == location.direction then + createPoint = false + if self.grid[gridKey].steps > (cell.steps + 1) then --found shortcut + self.grid[gridKey].incoming = cell + self.grid[gridKey].steps = cell.steps + 1 + end + --elseif self.grid[gridKey].direction ~= location.direction then + --duplicatePointDirection = self.grid[gridKey].direction -- remember the grid direction + --if self.grid[gridKey].steps > (cell.steps + 1) then --found shortcut -> not true!!! The outgoing angles would be all wrong here. This caused issues with undrivable paths being generated! + --self.grid[gridKey].incoming = cell + --self.grid[gridKey].steps = cell.steps + 1 + --end + end + end + end + end + if createPoint then + local gridKey + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM testNextCells location xz %d %d createPoint %s", + math.floor(location.x), + math.floor(location.z), + tostring(self.direction_to_text[location.direction+1]) + ) + ) + end + if duplicatePointDirection >= 0 then + -- if different direction, it is not necessary to check the cell details again, just add a new entry in grid with known required restrictions + -- Todo : Not true!! If we come from a different direction we ususally have a differently sized collision box to check. There is a difference between a 0° angle when coming from the last cell and a +/- 45° angle. + gridKey = string.format("%d|%d|%d", location.x, location.z, duplicatePointDirection) + location.isRestricted = self.grid[gridKey].isRestricted + location.hasCollision = self.grid[gridKey].hasCollision + location.bordercells = self.grid[gridKey].bordercells + location.hasFruit = self.grid[gridKey].hasFruit + location.fruitValue = self.grid[gridKey].fruitValue + + if not location.isRestricted and not location.hasCollision and location.incoming ~= nil then + -- check for up/down is to big or below water level + -- this is a required check as we come from different direction + local worldPos = self:gridLocationToWorldLocation(location) + local worldPosPrevious = self:gridLocationToWorldLocation(location.incoming) + location.hasCollision = location.hasCollision or self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down is to big or below water level + end + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM testNextCells different direction xz %d %d createPoint %s", + math.floor(location.x), + math.floor(location.z), + tostring(self.direction_to_text[location.direction+1]) + ) + ) + end + else + self:checkGridCell(location) + end + gridKey = string.format("%d|%d|%d", location.x, location.z, location.direction) + self.grid[gridKey] = location + end + end + + cell.visited = true +end + +function PathFinderModule:checkGridCell(cell) + local worldPos = self:gridLocationToWorldLocation(cell) + --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped + + cell.isOnField = AutoDrive.checkIsOnField(worldPos.x, 0, worldPos.z) + if self.restrictToField and (self.fallBackMode1 and not self.fallBackMode2) then + -- limit cells to field border only possible if started on field + cell.bordercells = cell.incoming.bordercells + 1 -- by default we assume the new cell is not on field, so increase the counter + + if cell.incoming.bordercells == 0 then + -- if incoming cell is on field we check if the new is also on field + if cell.isOnField then + -- still on field, so set the current cell counter to 0 + cell.bordercells = 0 + end + end + + if cell.bordercells > 0 then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkGridCell - xz %d %d cell.bordercells %d #self.grid %d", + math.floor(cell.x), + math.floor(cell.z), + math.floor(cell.bordercells), + table.count(self.grid) + ) + ) + end + end + end + + -- check the most probable restrictions on field first to prevent unneccessary checks + if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 and self.fallBackMode2) then + -- in fallBackMode1 we ignore the field restriction + cell.isRestricted = cell.isRestricted or (not cell.isOnField) + + if cell.isRestricted then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkGridCell isRestricted self.restrictToField %s self.fallBackMode1 %s isOnField %s x,z %d %d", + tostring(self.restrictToField), + tostring(self.fallBackMode1), + tostring(cell.isOnField), + math.floor(worldPos.x), + math.floor(worldPos.z) + ) + ) + end + end + end + + local gridFactor = PathFinderModule.GRID_SIZE_FACTOR * 1.3 --> 0.6 + if self.isSecondChasingVehicle then + gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER * 1.6 --> 1.7 + end + local corners = self:getCorners(cell, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) + + if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then + -- check for fruit + self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found + end + + if not cell.isRestricted and cell.incoming ~= nil then + -- check for up/down is to big or below water level + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local angelToSlope, angle = self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level + cell.angle = angle + cell.hasCollision = cell.hasCollision or angelToSlope + end + + if not cell.isRestricted and not cell.hasCollision then + -- check for obstacles + local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height + local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallbackIgnore", nil, self.mask, true, true, true) + cell.hasCollision = cell.hasCollision or (shapes > 0) + if cell.hasCollision then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkGridCell hasCollision = (shapes > 0) %d x,z %d %d x,z %d %d ", + shapes, + math.floor(worldPos.x), + math.floor(worldPos.z), + math.floor(shapeDefinition.x), + math.floor(shapeDefinition.z) + ) + ) + end + end + end + + if not cell.isRestricted and not cell.hasCollision and cell.incoming ~= nil then + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local vectorX = worldPosPrevious.x - worldPos.x + local vectorZ = worldPosPrevious.z - worldPos.z + local dirVec = { x=vectorX, z = vectorZ} + + local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) + cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath + self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkGridCell cellUsedByVehiclePath %s self.blockedByOtherVehicle %s", + tostring(cellUsedByVehiclePath), + tostring(self.blockedByOtherVehicle) + ) + ) + end + end +end + +function PathFinderModule:gridLocationToWorldLocation(cell) + local result = {x = 0, z = 0} + + result.x = self.target.x + (cell.x - self.targetCell.x) * self.vectorX.x + (cell.z - self.targetCell.z) * self.vectorZ.x + result.z = self.target.z + (cell.x - self.targetCell.x) * self.vectorX.z + (cell.z - self.targetCell.z) * self.vectorZ.z + + return result +end + +function PathFinderModule:worldDirectionToGridDirection(vector, baseVector) + local baseVector = baseVector or self.vectorX + local angle = AutoDrive.angleBetween(baseVector, vector) + + local direction = math.floor(angle / 45) + local remainder = angle % 45 + if remainder >= 22.5 then + direction = (direction + 1) + elseif remainder <= -22.5 then + direction = (direction - 1) + end + + if direction < 0 then + direction = 8 + direction + end + + return direction +end + +function PathFinderModule:worldLocationToGridLocation(worldX, worldZ) + local result = {x = 0, z = 0} + + result.z = (((worldX - self.startX) / self.vectorX.x) * self.vectorX.z - worldZ + self.startZ) / (((self.vectorZ.x / self.vectorX.x) * self.vectorX.z) - self.vectorZ.z) + result.x = (worldZ - self.startZ - result.z * self.vectorZ.z) / self.vectorX.z + + result.x = AutoDrive.round(result.x) + result.z = AutoDrive.round(result.z) + + return result +end + +function PathFinderModule:determineBlockedCells(cell) + if (math.abs(cell.x) < 2 and math.abs(cell.z) < 2) then + return + end +--[[ + table.insert(self.grid, {x = cell.x + 1, z = cell.z + 0, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_UP + table.insert(self.grid, {x = cell.x + 1, z = cell.z - 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_UP_LEFT + table.insert(self.grid, {x = cell.x + 0, z = cell.z + 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_RIGHT + table.insert(self.grid, {x = cell.x + 1, z = cell.z + 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_UP_RIGHT + table.insert(self.grid, {x = cell.x + 0, z = cell.z - 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_LEFT +]] + local gridKey = "" + local direction = -1 + local x = 0 + local z = 0 + x = cell.x + 1 + z = cell.z + 0 + gridKey = string.format("%d|%d|%d", x, z, direction) + self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} + x = cell.x + 1 + z = cell.z - 1 + gridKey = string.format("%d|%d|%d", x, z, direction) + self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} + x = cell.x + 0 + z = cell.z + 1 + gridKey = string.format("%d|%d|%d", x, z, direction) + self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} + x = cell.x + 1 + z = cell.z + 1 + gridKey = string.format("%d|%d|%d", x, z, direction) + self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} + x = cell.x + 0 + z = cell.z - 1 + gridKey = string.format("%d|%d|%d", x, z, direction) + self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} +end + +function PathFinderModule:determineNextGridCells(cell) + if cell.out == nil then + cell.out = {} + end + if cell.direction == self.PP_UP then + cell.out[1] = {x = cell.x + 1, z = cell.z - 1} + cell.out[1].direction = self.PP_UP_LEFT + cell.out[2] = {x = cell.x + 1, z = cell.z + 0} + cell.out[2].direction = self.PP_UP + cell.out[3] = {x = cell.x + 1, z = cell.z + 1} + cell.out[3].direction = self.PP_UP_RIGHT + elseif cell.direction == self.PP_UP_RIGHT then + cell.out[1] = {x = cell.x + 1, z = cell.z + 0} + cell.out[1].direction = self.PP_UP + cell.out[2] = {x = cell.x + 1, z = cell.z + 1} + cell.out[2].direction = self.PP_UP_RIGHT + cell.out[3] = {x = cell.x + 0, z = cell.z + 1} + cell.out[3].direction = self.PP_RIGHT + elseif cell.direction == self.PP_RIGHT then + cell.out[1] = {x = cell.x + 1, z = cell.z + 1} + cell.out[1].direction = self.PP_UP_RIGHT + cell.out[2] = {x = cell.x + 0, z = cell.z + 1} + cell.out[2].direction = self.PP_RIGHT + cell.out[3] = {x = cell.x - 1, z = cell.z + 1} + cell.out[3].direction = self.PP_DOWN_RIGHT + elseif cell.direction == self.PP_DOWN_RIGHT then + cell.out[1] = {x = cell.x + 0, z = cell.z + 1} + cell.out[1].direction = self.PP_RIGHT + cell.out[2] = {x = cell.x - 1, z = cell.z + 1} + cell.out[2].direction = self.PP_DOWN_RIGHT + cell.out[3] = {x = cell.x - 1, z = cell.z + 0} + cell.out[3].direction = self.PP_DOWN + elseif cell.direction == self.PP_DOWN then + cell.out[1] = {x = cell.x - 1, z = cell.z + 1} + cell.out[1].direction = self.PP_DOWN_RIGHT + cell.out[2] = {x = cell.x - 1, z = cell.z + 0} + cell.out[2].direction = self.PP_DOWN + cell.out[3] = {x = cell.x - 1, z = cell.z - 1} + cell.out[3].direction = self.PP_DOWN_LEFT + elseif cell.direction == self.PP_DOWN_LEFT then + cell.out[1] = {x = cell.x - 1, z = cell.z - 0} + cell.out[1].direction = self.PP_DOWN + cell.out[2] = {x = cell.x - 1, z = cell.z - 1} + cell.out[2].direction = self.PP_DOWN_LEFT + cell.out[3] = {x = cell.x - 0, z = cell.z - 1} + cell.out[3].direction = self.PP_LEFT + elseif cell.direction == self.PP_LEFT then + cell.out[1] = {x = cell.x - 1, z = cell.z - 1} + cell.out[1].direction = self.PP_DOWN_LEFT + cell.out[2] = {x = cell.x - 0, z = cell.z - 1} + cell.out[2].direction = self.PP_LEFT + cell.out[3] = {x = cell.x + 1, z = cell.z - 1} + cell.out[3].direction = self.PP_UP_LEFT + elseif cell.direction == self.PP_UP_LEFT then + cell.out[1] = {x = cell.x - 0, z = cell.z - 1} + cell.out[1].direction = self.PP_LEFT + cell.out[2] = {x = cell.x + 1, z = cell.z - 1} + cell.out[2].direction = self.PP_UP_LEFT + cell.out[3] = {x = cell.x + 1, z = cell.z + 0} + cell.out[3].direction = self.PP_UP + end + + for _, outGoing in pairs(cell.out) do + outGoing.visited = false + outGoing.isRestricted = false + outGoing.hasCollision = false + outGoing.hasFruit = false + outGoing.incoming = cell + outGoing.steps = cell.steps + 1 + outGoing.bordercells = cell.bordercells + end +end + +function PathFinderModule:cellDistance(cell) + return MathUtil.vector2Length(self.targetCell.x - cell.x, self.targetCell.z - cell.z) +end + +function PathFinderModule:checkForFruitInArea(cell, corners) + + if self.goingToNetwork then + -- on the way to network, check all fruit types + self.fruitToCheck = nil + end + if self.fruitToCheck == nil then + for _, fruitType in pairs(g_fruitTypeManager:getFruitTypes()) do + if not (fruitType == g_fruitTypeManager:getFruitTypeByName("MEADOW")) then + local fruitTypeIndex = fruitType.index + self:checkForFruitTypeInArea(cell, fruitTypeIndex, corners) + end + --stop if cell is already restricted and/or fruit type is now known + if cell.isRestricted ~= false or self.fruitToCheck ~= nil then + break + end + end + else + self:checkForFruitTypeInArea(cell, self.fruitToCheck, corners) + end +end + +function PathFinderModule:checkForFruitTypeInArea(cell, fruitTypeIndex, corners) + local fruitValue = 0 + fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(fruitTypeIndex, corners[1].x, corners[1].z, corners[2].x, corners[2].z, corners[3].x, corners[3].z, true, true) + + if (self.fruitToCheck == nil or self.fruitToCheck < 1) and (fruitValue > PathFinderModule.MIN_FRUIT_VALUE) then + self.fruitToCheck = fruitTypeIndex + end + local wasRestricted = cell.isRestricted + cell.isRestricted = cell.isRestricted or (fruitValue > PathFinderModule.MIN_FRUIT_VALUE) + + cell.hasFruit = (fruitValue > PathFinderModule.MIN_FRUIT_VALUE) + cell.fruitValue = fruitValue + + if cell.hasFruit then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkForFruitTypeInArea cell.hasFruit xz %d %d fruitValue %s direction %s", + math.floor(cell.x), + math.floor(cell.z), + tostring(fruitValue), + tostring(self.direction_to_text[cell.direction+1]) + ) + ) + end + end + + --Allow fruit in the last few grid cells + if (self:cellDistance(cell) <= 3 and self.goingToPipe) then + cell.isRestricted = false or wasRestricted + end +end + +function PathFinderModule:drawDebugForPF() + local AutoDriveDM = ADDrawingManager + local pointTarget = self:gridLocationToWorldLocation(self.targetCell) + local pointTargetUp = self:gridLocationToWorldLocation(self.targetCell) + pointTarget.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTarget.x, 1, pointTarget.z) + 3 + pointTargetUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetUp.x, 1, pointTargetUp.z) + 20 + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) + local pointStart = {x = self.startX, z = self.startZ} + pointStart.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointStart.x, 1, pointStart.z) + 3 + AutoDriveDM:addLineTask(pointStart.x, pointStart.y, pointStart.z, pointStart.x, pointStart.y + 20, pointStart.z, 1, 0, 1, 0) + + local color_red = 0.1 + local color_green = 0.1 + local color_blue = 0.1 + local color_count = 0 + local index = 0 + for _, cell in pairs(self.grid) do + index = index + 1 + + color_red = math.min(color_red + 0.25, 1) + if color_red > 0.9 then + color_green = math.min(color_green + 0.25, 1) + end + if color_green > 0.9 then + color_blue = math.min(color_blue + 0.25, 1) + end + color_count = color_count + 1 + + local worldPos = self:gridLocationToWorldLocation(cell) + + -- local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallbackIgnore", nil, self.mask, true, true, true) + local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height + local corners = self:getCornersFromShapeDefinition(shapeDefinition) + local baseY = shapeDefinition.y + 3 + + -- corners of shape + -- DebugUtil.drawOverlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, color_red, color_green, color_blue) + for _, corner in pairs(corners) do + local point_y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corner.x, 1, corner.z) + local pointUp_y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corner.x, 1, corner.z) + 3 + -- AutoDriveDM:addLineTask(corner.x, point_y, corner.z, corner.x, pointUp_y, corner.z, 1, color_red, color_green, color_blue) + end + + -- restriction, collision line up + local pointCenter = self:gridLocationToWorldLocation(cell) + local pointCenterUp = {x = pointCenter.x, z = pointCenter.z} + pointCenter.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenter.x, 1, pointCenter.z) + 3 + pointCenterUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenterUp.x, 1, pointCenterUp.z) + 6 + + local pointA = corners[1] + local pointB = corners[2] + local pointC = corners[3] + local pointD = corners[4] + index = 0 + pointA.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointA.x, 1, pointA.z) + 1 + (0.1 * index) + pointB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointB.x, 1, pointB.z) + 1 + (0.1 * index) + pointC.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointC.x, 1, pointC.z) + 1 + (0.1 * index) + pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 1 + (0.1 * index) + + -- cell xz text + local cellText = tostring(cell.x) .. " " .. tostring(cell.z) + -- Utils.renderTextAtWorldPosition(pointCenter.x, pointCenter.y, pointCenter.z, cellText, getCorrectTextSize(0.013), 0) + +--[[ + if cell.isRestricted then + -- red + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 0) + else + if cell.isOnField then + -- blue + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) + else + -- green + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 1, 0) + end + end + local cellIndex = string.format("%d , %d", cell.x, cell.z) + Utils.renderTextAtWorldPosition(pointCenter.x, pointCenter.y - 2, pointCenter.z, cellIndex, getCorrectTextSize(0.013), 0) + + if cell.angle then + local value = string.format("%.1f", math.deg(cell.angle)) + if (not cell.hasCollision) and (not cell.isRestricted) then + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) + end + Utils.renderTextAtWorldPosition(pointCenter.x + (cell.x % 10), pointCenter.y - 1 + (cell.steps % 10 / 5) + (cell.z % 10 / 5), pointCenter.z, value, getCorrectTextSize(0.013), 0) + end + + if cell.incoming then + local cellIncommingIndex = string.format("%d -> %d , %d", cell.steps, cell.incoming.x, cell.incoming.z) + Utils.renderTextAtWorldPosition(pointCenter.x + (cell.x % 10), pointCenter.y - 0 + (cell.steps % 10 / 5) + (cell.z % 10 / 5), pointCenter.z, cellIncommingIndex, getCorrectTextSize(0.013), 0) + end +]] + if cell.isRestricted == true then + -- any restriction + if cell.hasFruit == true then + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 1) + Utils.renderTextAtWorldPosition(pointCenter.x, pointB.y + 0.4, pointCenter.z, tostring(cell.fruitValue), getCorrectTextSize(0.013), 0) + + else + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) + end + else + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) + end + + if cell.hasCollision == true then + -- ground collision, slope, water + if cell.hasVehicleCollision then -- TODO: hasVehicleCollision ??? + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) + else + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) + end + else + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) + end + + for i = 0, 10, 1 do + pointCenter.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenter.x, 1, pointCenter.z) + 1 + (i * 0.5) + -- AutoDriveDM:addLineTask(pointA.x, pointCenter.y, pointA.z, pointA.x + 0.3, pointCenter.y, pointA.z + 0.3, 1, 1, 1, 1) + end + +--[[ + -- cross marker with restriction, collision + local size = 0.3 + local pointA = self:gridLocationToWorldLocation(cell) + pointA.x = pointA.x + self.vectorX.x * size + self.vectorZ.x * size + pointA.z = pointA.z + self.vectorX.z * size + self.vectorZ.z * size + pointA.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointA.x, 1, pointA.z) + 3 + local pointB = self:gridLocationToWorldLocation(cell) -- oposide position to pointA !!! + pointB.x = pointB.x - self.vectorX.x * size - self.vectorZ.x * size + pointB.z = pointB.z - self.vectorX.z * size - self.vectorZ.z * size + pointB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointB.x, 1, pointB.z) + 3 + local pointC = self:gridLocationToWorldLocation(cell) + pointC.x = pointC.x + self.vectorX.x * size - self.vectorZ.x * size + pointC.z = pointC.z + self.vectorX.z * size - self.vectorZ.z * size + pointC.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointC.x, 1, pointC.z) + 3 + local pointD = self:gridLocationToWorldLocation(cell) + pointD.x = pointD.x - self.vectorX.x * size + self.vectorZ.x * size + pointD.z = pointD.z - self.vectorX.z * size + self.vectorZ.z * size + pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 3 + + -- restriction, collision line up + local pointCenter = self:gridLocationToWorldLocation(cell) + local pointCenterUp = pointCenter + pointCenterUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenterUp.x, 1, pointCenterUp.z) + 3 + + if cell.isRestricted == true then + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) + if cell.hasCollision == true then + if cell.hasVehicleCollision then -- TODO: hasVehicleCollision ??? + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) + else + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) + end + else + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) + end + else + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) + if cell.hasCollision == true then + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) + else + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) + end + end +]] + + if cell.bordercells > 0 then + local pointTarget = self:gridLocationToWorldLocation(cell) + local pointTargetUp = self:gridLocationToWorldLocation(cell) + pointTarget.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTarget.x, 1, pointTarget.z) + 6 + pointTargetUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetUp.x, 1, pointTargetUp.z) + 10 + if cell.bordercells == 1 then + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 1, 0, 0) + elseif cell.bordercells == 2 then + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 1, 0) + elseif cell.bordercells > 2 then + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) + end + end + --[[ + local gridFactor = PathFinderModule.GRID_SIZE_FACTOR + if self.isSecondChasingVehicle then + gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER + end + local corners = self:getCorners(cell, {x=self.vectorX.x * gridFactor, z=self.vectorX.z * gridFactor}, {x=self.vectorZ.x * gridFactor,z=self.vectorZ.z * gridFactor}) + local heightOffset = 1 + AutoDriveDM:addLineTask(corners[1].x, pointA.y+heightOffset, corners[1].z, corners[2].x, pointA.y+heightOffset, corners[2].z, 1, 0, 1, 0) + AutoDriveDM:addLineTask(corners[2].x, pointA.y+heightOffset, corners[2].z, corners[3].x, pointA.y+heightOffset, corners[3].z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(corners[3].x, pointA.y+heightOffset, corners[3].z, corners[4].x, pointA.y+heightOffset, corners[4].z, 1, 0, 0, 1) + AutoDriveDM:addLineTask(corners[4].x, pointA.y+heightOffset, corners[4].z, corners[1].x, pointA.y+heightOffset, corners[1].z, 1, 1, 0, 1) + + local shapeDefinition = self:getShapeDefByDirectionType(cell) + local red = 0 + if cell.hasCollision then + red = 1 + end + DebugUtil.drawOverlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, red, 0, 0) + --]] + end + + -- target cell marker + local size = 0.3 + local pointA = self:gridLocationToWorldLocation(self.targetCell) + pointA.x = pointA.x + self.vectorX.x * size + self.vectorZ.x * size + pointA.z = pointA.z + self.vectorX.z * size + self.vectorZ.z * size + pointA.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointA.x, 1, pointA.z) + 3 + local pointB = self:gridLocationToWorldLocation(self.targetCell) + pointB.x = pointB.x - self.vectorX.x * size - self.vectorZ.x * size + pointB.z = pointB.z - self.vectorX.z * size - self.vectorZ.z * size + pointB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointB.x, 1, pointB.z) + 3 + local pointC = self:gridLocationToWorldLocation(self.targetCell) + pointC.x = pointC.x + self.vectorX.x * size - self.vectorZ.x * size + pointC.z = pointC.z + self.vectorX.z * size - self.vectorZ.z * size + pointC.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointC.x, 1, pointC.z) + 3 + local pointD = self:gridLocationToWorldLocation(self.targetCell) + pointD.x = pointD.x - self.vectorX.x * size + self.vectorZ.x * size + pointD.z = pointD.z - self.vectorX.z * size + self.vectorZ.z * size + pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 3 + + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 1, 1) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) + + local pointAB = self:gridLocationToWorldLocation(self.targetCell) + pointAB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointAB.x, 1, pointAB.z) + 3 + + local pointTargetVector = self:gridLocationToWorldLocation(self.targetCell) + pointTargetVector.x = pointTargetVector.x + self.targetVector.x * 10 + pointTargetVector.z = pointTargetVector.z + self.targetVector.z * 10 + pointTargetVector.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetVector.x, 1, pointTargetVector.z) + 3 + AutoDriveDM:addLineTask(pointAB.x, pointAB.y, pointAB.z, pointTargetVector.x, pointTargetVector.y, pointTargetVector.z, 1, 1, 1, 1) +end + +function PathFinderModule:drawDebugForCreatedRoute() + local AutoDriveDM = ADDrawingManager + if self.chainStartToTarget ~= nil then + for _, cell in pairs(self.chainStartToTarget) do + local shape = self:getShapeDefByDirectionType(cell) + if shape.x ~= nil then + local pointA = { + x = shape.x + shape.widthX * math.cos(shape.angleRad) + shape.widthZ * math.sin(shape.angleRad), + y = shape.y, + z = shape.z + shape.widthZ * math.cos(shape.angleRad) + shape.widthX * math.sin(shape.angleRad) + } + local pointB = { + x = shape.x - shape.widthX * math.cos(shape.angleRad) - shape.widthZ * math.sin(shape.angleRad), + y = shape.y, + z = shape.z + shape.widthZ * math.cos(shape.angleRad) + shape.widthX * math.sin(shape.angleRad) + } + local pointC = { + x = shape.x - shape.widthX * math.cos(shape.angleRad) - shape.widthZ * math.sin(shape.angleRad), + y = shape.y, + z = shape.z - shape.widthZ * math.cos(shape.angleRad) - shape.widthX * math.sin(shape.angleRad) + } + local pointD = { + x = shape.x + shape.widthX * math.cos(shape.angleRad) + shape.widthZ * math.sin(shape.angleRad), + y = shape.y, + z = shape.z - shape.widthZ * math.cos(shape.angleRad) - shape.widthX * math.sin(shape.angleRad) + } + + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointC.x, pointC.y, pointC.z, 1, 1, 1, 1) + AutoDriveDM:addLineTask(pointB.x, pointB.y, pointB.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) + + if cell.incoming ~= nil then + local worldPos_cell = self:gridLocationToWorldLocation(cell) + local worldPos_incoming = self:gridLocationToWorldLocation(cell.incoming) + + local vectorX = worldPos_cell.x - worldPos_incoming.x + local vectorZ = worldPos_cell.z - worldPos_incoming.z + local angleRad = math.atan2(-vectorZ, vectorX) + angleRad = AutoDrive.normalizeAngle(angleRad) + local widthOfColBox = math.sqrt(math.pow(self.minTurnRadius, 2) + math.pow(self.minTurnRadius, 2)) + local sideLength = widthOfColBox / 2 + + local leftAngle = AutoDrive.normalizeAngle(angleRad + math.rad(-90)) + local rightAngle = AutoDrive.normalizeAngle(angleRad + math.rad(90)) + + local cornerX = worldPos_incoming.x - math.cos(leftAngle) * sideLength + local cornerZ = worldPos_incoming.z + math.sin(leftAngle) * sideLength + + local corner2X = worldPos_cell.x - math.cos(leftAngle) * sideLength + local corner2Z = worldPos_cell.z + math.sin(leftAngle) * sideLength + + local corner3X = worldPos_cell.x - math.cos(rightAngle) * sideLength + local corner3Z = worldPos_cell.z + math.sin(rightAngle) * sideLength + + local corner4X = worldPos_incoming.x - math.cos(rightAngle) * sideLength + local corner4Z = worldPos_incoming.z + math.sin(rightAngle) * sideLength + + local inY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPos_incoming.x, 1, worldPos_incoming.z) + 1 + local currentY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPos_cell.x, 1, worldPos_cell.z) + 1 + + AutoDriveDM:addLineTask(cornerX, inY, cornerZ, corner2X, currentY, corner2Z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(corner2X, currentY, corner2Z, corner3X, currentY, corner3Z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(corner3X, currentY, corner3Z, corner4X, inY, corner4Z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(corner4X, inY, corner4Z, cornerX, inY, cornerZ, 1, 1, 0, 0) + end + end + end + end + + if self.wayPoints then + for i, waypoint in pairs(self.wayPoints) do + Utils.renderTextAtWorldPosition(waypoint.x, waypoint.y + 4, waypoint.z, "Node " .. i, getCorrectTextSize(0.013), 0) + if i > 1 then + local wp = waypoint + local pfWp = self.wayPoints[i - 1] + AutoDriveDM:addLineTask(wp.x, wp.y, wp.z, pfWp.x, pfWp.y, pfWp.z, 1, 0, 1, 1) + end + end + end +end + +function PathFinderModule:getShapeDefByDirectionType(cell) + local shapeDefinition = {} + shapeDefinition.angleRad = math.atan2(-self.targetVector.z, self.targetVector.x) + shapeDefinition.angleRad = AutoDrive.normalizeAngle(shapeDefinition.angleRad) + local worldPos = self:gridLocationToWorldLocation(cell) + shapeDefinition.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPos.x, 1, worldPos.z) + shapeDefinition.height = 2.65 + + if cell.direction == self.PP_UP or cell.direction == self.PP_DOWN or cell.direction == self.PP_RIGHT or cell.direction == self.PP_LEFT or cell.direction == -1 then + --default size: + shapeDefinition.x = worldPos.x + shapeDefinition.z = worldPos.z + shapeDefinition.widthX = self.minTurnRadius / 2 + shapeDefinition.widthZ = self.minTurnRadius / 2 + elseif cell.direction == self.PP_UP_RIGHT then + local offsetX = (-self.vectorX.x) / 2 + (-self.vectorZ.x) / 4 + local offsetZ = (-self.vectorX.z) / 2 + (-self.vectorZ.z) / 4 + shapeDefinition.x = worldPos.x + offsetX + shapeDefinition.z = worldPos.z + offsetZ + shapeDefinition.widthX = (self.minTurnRadius / 2) + math.abs(offsetX) + shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) + elseif cell.direction == self.PP_UP_LEFT then + local offsetX = (-self.vectorX.x) / 2 + (self.vectorZ.x) / 4 + local offsetZ = (-self.vectorX.z) / 2 + (self.vectorZ.z) / 4 + shapeDefinition.x = worldPos.x + offsetX + shapeDefinition.z = worldPos.z + offsetZ + shapeDefinition.widthX = self.minTurnRadius / 2 + math.abs(offsetX) + shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) + elseif cell.direction == self.PP_DOWN_RIGHT then + local offsetX = (self.vectorX.x) / 2 + (-self.vectorZ.x) / 4 + local offsetZ = (self.vectorX.z) / 2 + (-self.vectorZ.z) / 4 + shapeDefinition.x = worldPos.x + offsetX + shapeDefinition.z = worldPos.z + offsetZ + shapeDefinition.widthX = self.minTurnRadius / 2 + math.abs(offsetX) + shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) + elseif cell.direction == self.PP_DOWN_LEFT then + local offsetX = (self.vectorX.x) / 2 + (self.vectorZ.x) / 4 + local offsetZ = (self.vectorX.z) / 2 + (self.vectorZ.z) / 4 + shapeDefinition.x = worldPos.x + offsetX + shapeDefinition.z = worldPos.z + offsetZ + shapeDefinition.widthX = self.minTurnRadius / 2 + math.abs(offsetX) + shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) + end + + local increaseCellFactor = 1.15 + if cell.isOnField ~= nil and cell.isOnField == true then + increaseCellFactor = 1 --0.8 + end + shapeDefinition.widthX = shapeDefinition.widthX * increaseCellFactor + shapeDefinition.widthZ = shapeDefinition.widthZ * increaseCellFactor + + local corners = self:getCornersFromShapeDefinition(shapeDefinition) + if corners ~= nil then + for _, corner in pairs(corners) do + shapeDefinition.y = math.max(shapeDefinition.y, getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corner.x, 1, corner.z)) + end + end + + return shapeDefinition +end + +function PathFinderModule:getCornersFromShapeDefinition(shapeDefinition) + local corners = {} + corners[1] = {x = shapeDefinition.x + (-shapeDefinition.widthX), z = shapeDefinition.z + (-shapeDefinition.widthZ)} + corners[2] = {x = shapeDefinition.x + (shapeDefinition.widthX), z = shapeDefinition.z + (shapeDefinition.widthZ)} + corners[3] = {x = shapeDefinition.x + (-shapeDefinition.widthX), z = shapeDefinition.z + (shapeDefinition.widthZ)} + corners[4] = {x = shapeDefinition.x + (shapeDefinition.widthX), z = shapeDefinition.z + (-shapeDefinition.widthZ)} + + return corners +end + +function PathFinderModule:getCorners(cell, vectorX, vectorZ) + local corners = {} + local centerLocation = self:gridLocationToWorldLocation(cell) + corners[1] = {x = centerLocation.x + (-vectorX.x - vectorZ.x), z = centerLocation.z + (-vectorX.z - vectorZ.z)} + corners[2] = {x = centerLocation.x + (vectorX.x - vectorZ.x), z = centerLocation.z + (vectorX.z - vectorZ.z)} + corners[3] = {x = centerLocation.x + (-vectorX.x + vectorZ.x), z = centerLocation.z + (-vectorX.z + vectorZ.z)} + corners[4] = {x = centerLocation.x + (vectorX.x + vectorZ.x), z = centerLocation.z + (vectorX.z + vectorZ.z)} + + return corners +end + +function PathFinderModule:createWayPoints() + if self.smoothStep == 0 then + local currentCell = self.targetCell + self.chainTargetToStart = {} + local index = 1 + self.chainTargetToStart[index] = currentCell + index = index + 1 + while currentCell.x ~= 0 or currentCell.z ~= 0 do + self.chainTargetToStart[index] = currentCell.incoming + currentCell = currentCell.incoming + if currentCell == nil then + break + end + index = index + 1 + end + index = index - 1 + + self.chainStartToTarget = {} + for reversedIndex = 0, index, 1 do + self.chainStartToTarget[reversedIndex + 1] = self.chainTargetToStart[index - reversedIndex] + end + + --Now build actual world coordinates as waypoints and include pre and append points + self.wayPoints = {} + for chainIndex, cell in pairs(self.chainStartToTarget) do + self.wayPoints[chainIndex] = self:gridLocationToWorldLocation(cell) + self.wayPoints[chainIndex].y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, self.wayPoints[chainIndex].x, 1, self.wayPoints[chainIndex].z) + self.wayPoints[chainIndex].direction = cell.direction + end + + -- remove zig zag line + self:smoothResultingPPPath() + end + + -- shortcut the path if possible + self:smoothResultingPPPath_Refined() + + if self.smoothStep == 2 then + -- When going to network, dont turn actual road network nodes into pathFinderPoints + if self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true + end + end + + if self.appendWayPoints ~= nil then + for i = 1, #self.appendWayPoints, 1 do + self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] + end + self.smoothStep = 3 + --PathFinderModule.debugVehicleMsg(self.vehicle, + --string.format("PFM createWayPoints appendWayPoints %s", + --tostring(#self.appendWayPoints) + --) + --) + end + + -- See comment above + if not self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true + end + end + end +end + +function PathFinderModule:smoothResultingPPPath() + local index = 1 + local filteredIndex = 1 + local filteredWPs = {} + + while index < #self.wayPoints - 1 do + local node = self.wayPoints[index] + local nodeAhead = self.wayPoints[index + 1] + local nodeTwoAhead = self.wayPoints[index + 2] + + filteredWPs[filteredIndex] = node + filteredIndex = filteredIndex + 1 + + if node.direction ~= nil and nodeAhead.direction ~= nil and nodeTwoAhead.direction ~= nil then + if node.direction == nodeTwoAhead.direction and node.direction ~= nodeAhead.direction then + index = index + 1 --skip next point because it is a zig zag line. Cut right through instead + end + end + + index = index + 1 + end + + while index <= #self.wayPoints do + local node = self.wayPoints[index] + filteredWPs[filteredIndex] = node + filteredIndex = filteredIndex + 1 + index = index + 1 + end + + self.wayPoints = filteredWPs + --PathFinderModule.debugVehicleMsg(self.vehicle, + --string.format("PFM smoothResultingPPPath self.wayPoints %s", + --tostring(#self.wayPoints) + --) + --) + +end + +function PathFinderModule:smoothResultingPPPath_Refined() + if self.smoothStep == 0 then + self.lookAheadIndex = 1 + self.smoothIndex = 1 + self.filteredIndex = 1 + self.filteredWPs = {} + self.totalEagerSteps = 0 + + --add first few without filtering + while self.smoothIndex < #self.wayPoints and self.smoothIndex < 3 do + self.filteredWPs[self.filteredIndex] = self.wayPoints[self.smoothIndex] + self.filteredIndex = self.filteredIndex + 1 + self.smoothIndex = self.smoothIndex + 1 + end + + self.smoothStep = 1 + end + + local unfilteredEndPointCount = 5 + if self.smoothStep == 1 then + local stepsThisFrame = 0 + while self.smoothIndex < #self.wayPoints - unfilteredEndPointCount and stepsThisFrame < ADScheduler:getStepsPerFrame() do + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined self.smoothIndex %d ", + self.smoothIndex + ) + ) + end + stepsThisFrame = stepsThisFrame + 1 + + local node = self.wayPoints[self.smoothIndex] + local previousNode = nil + local worldPos = self.wayPoints[self.smoothIndex] + + if self.totalEagerSteps == nil or self.totalEagerSteps == 0 then + if self.filteredWPs[self.filteredIndex-1].x ~= node.x and self.filteredWPs[self.filteredIndex-1].z ~= node.z then + self.filteredWPs[self.filteredIndex] = node + if self.filteredIndex > 1 then + previousNode = self.filteredWPs[self.filteredIndex - 1] + end + self.filteredIndex = self.filteredIndex + 1 + + self.lookAheadIndex = 1 + self.totalEagerSteps = 0 + end + end + + local widthOfColBox = self.minTurnRadius + local sideLength = widthOfColBox * PathFinderModule.GRID_SIZE_FACTOR + local y = worldPos.y + local foundCollision = false + + if stepsThisFrame > math.max(1, (ADScheduler:getStepsPerFrame() * 0.4)) then + break + end + + --local stepsOfLookAheadThisFrame = 0 + -- (foundCollision == false or self.totalEagerSteps < PathFinderModule.PP_MAX_EAGER_LOOKAHEAD_STEPS) + while (foundCollision == false) and ((self.smoothIndex + self.totalEagerSteps) < (#self.wayPoints - unfilteredEndPointCount)) and stepsThisFrame <= math.max(1, (ADScheduler:getStepsPerFrame() * 0.4)) do + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined self.smoothIndex %d self.totalEagerSteps %d", + self.smoothIndex, + self.totalEagerSteps + ) + ) + end + + local hasCollision = false + stepsThisFrame = stepsThisFrame + 1 + local nodeAhead = self.wayPoints[self.smoothIndex + self.totalEagerSteps + 1] + local nodeTwoAhead = self.wayPoints[self.smoothIndex + self.totalEagerSteps + 2] + if not hasCollision and nodeAhead and nodeTwoAhead then + local angle = AutoDrive.angleBetween({x = nodeAhead.x - node.x, z = nodeAhead.z - node.z}, {x = nodeTwoAhead.x - nodeAhead.x, z = nodeTwoAhead.z - nodeAhead.z}) + angle = math.abs(angle) + if angle > 60 then + hasCollision = true + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 1 + ) + ) + end + end + if previousNode ~= nil then + angle = AutoDrive.angleBetween({x = node.x - previousNode.x, z = node.z - previousNode.z}, {x = nodeTwoAhead.x - node.x, z = nodeTwoAhead.z - node.z}) + angle = math.abs(angle) + if angle > 60 then + hasCollision = true + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 2 + ) + ) + end + end + angle = AutoDrive.angleBetween({x = node.x - previousNode.x, z = node.z - previousNode.z}, {x = nodeAhead.x - node.x, z = nodeAhead.z - node.z}) + angle = math.abs(angle) + if angle > 60 then + hasCollision = true + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 3 + ) + ) + end + end + end + end + + if not hasCollision then + hasCollision = hasCollision or self:checkSlopeAngle(worldPos.x, worldPos.z, nodeAhead.x, nodeAhead.z) + if hasCollision then + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 4 + ) + ) + end + end + end + + local vectorX = nodeAhead.x - node.x + local vectorZ = nodeAhead.z - node.z + local angleRad = math.atan2(-vectorZ, vectorX) + angleRad = AutoDrive.normalizeAngle(angleRad) + local length = math.sqrt(math.pow(vectorX, 2) + math.pow(vectorZ, 2)) + widthOfColBox + + local leftAngle = AutoDrive.normalizeAngle(angleRad + math.rad(-90)) + local rightAngle = AutoDrive.normalizeAngle(angleRad + math.rad(90)) + + local cornerX = node.x - math.cos(leftAngle) * sideLength + local cornerZ = node.z + math.sin(leftAngle) * sideLength + + local corner2X = nodeAhead.x - math.cos(leftAngle) * sideLength + local corner2Z = nodeAhead.z + math.sin(leftAngle) * sideLength + + local corner3X = nodeAhead.x - math.cos(rightAngle) * sideLength + local corner3Z = nodeAhead.z + math.sin(rightAngle) * sideLength + + local corner4X = node.x - math.cos(rightAngle) * sideLength + local corner4Z = node.z + math.sin(rightAngle) * sideLength + + if not hasCollision then + if self.isNewPF then + self.collisionhits = 0 + local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "collisionTestCallback", self, self.mask, true, true, true) + hasCollision = hasCollision or (self.collisionhits > 0) + else + local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "Ignore", nil, self.mask, true, true, true) + hasCollision = hasCollision or (shapes > 0) + end + + if hasCollision then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 5 + ) + ) + end + end + end + + if (self.smoothIndex > 1) then + local worldPosPrevious = self.wayPoints[self.smoothIndex - 1] + length = MathUtil.vector3Length(worldPos.x - worldPosPrevious.x, worldPos.y - worldPosPrevious.y, worldPos.z - worldPosPrevious.z) + local angleBetween = math.atan(math.abs(worldPos.y - worldPosPrevious.y) / length) + + if (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then + hasCollision = true + + if hasCollision then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 6 + ) + ) + end + end + end + end + + if not hasCollision and self.avoidFruitSetting and not self.fallBackMode3 then + + local cornerWideX = node.x - math.cos(leftAngle) * sideLength * 4 + local cornerWideZ = node.z + math.sin(leftAngle) * sideLength * 4 + + local cornerWide2X = nodeAhead.x - math.cos(leftAngle) * sideLength * 4 + local cornerWide2Z = nodeAhead.z + math.sin(leftAngle) * sideLength * 4 + + local cornerWide4X = node.x - math.cos(rightAngle) * sideLength * 4 + local cornerWide4Z = node.z + math.sin(rightAngle) * sideLength * 4 + + if self.goingToNetwork then + -- check for all fruit types + for _, fruitType in pairs(g_fruitTypeManager:getFruitTypes()) do + if not (fruitType == g_fruitTypeManager:getFruitTypeByName("MEADOW")) then + local fruitTypeIndex = fruitType.index + local fruitValue = 0 + if self.isSecondChasingVehicle then + fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(fruitTypeIndex, cornerWideX, cornerWideZ, cornerWide2X, cornerWide2Z, cornerWide4X, cornerWide4Z, true, true) + else + fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(fruitTypeIndex, cornerX, cornerZ, corner2X, corner2Z, corner4X, corner4Z, true, true) + end + hasCollision = hasCollision or (fruitValue > 50) + if hasCollision then + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 7 + ) + ) + end + break + end + end + end + else + -- check only for fruit type detected on field + if self.fruitToCheck ~= nil then + local fruitValue = 0 + if self.isSecondChasingVehicle then + fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(self.fruitToCheck, cornerWideX, cornerWideZ, cornerWide2X, cornerWide2Z, cornerWide4X, cornerWide4Z, true, true) + else + fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(self.fruitToCheck, cornerX, cornerZ, corner2X, corner2Z, corner4X, corner4Z, true, true) + end + hasCollision = hasCollision or (fruitValue > 50) + + if hasCollision then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 8 + ) + ) + end + end + end + end + end + + if not hasCollision then + local cellBox = AutoDrive.boundingBoxFromCorners(cornerX, cornerZ, corner2X, corner2Z, corner3X, corner3Z, corner4X, corner4Z) + hasCollision = hasCollision or AutoDrive.checkForVehiclePathInBox(cellBox, self.minTurnRadius, self.vehicle) + + if hasCollision then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined hasCollision %d", + 9 + ) + ) + end + end + end + + foundCollision = hasCollision + + if foundCollision then + -- not used code removed + else + self.lookAheadIndex = self.totalEagerSteps + 1 + end + + self.totalEagerSteps = self.totalEagerSteps + 1 + + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined self.smoothIndex %d self.totalEagerSteps %d self.filteredIndex %d foundCollision %s", + self.smoothIndex, + self.totalEagerSteps, + self.filteredIndex, + tostring(foundCollision) + ) + ) + end + end + + if foundCollision or ((self.smoothIndex + self.totalEagerSteps) >= (#self.wayPoints - unfilteredEndPointCount)) then + self.smoothIndex = self.smoothIndex + math.max(1, (self.lookAheadIndex)) + self.totalEagerSteps = 0 + end + end + + if self.smoothIndex >= #self.wayPoints - unfilteredEndPointCount then + self.smoothStep = 2 + end + end + + if self.smoothStep == 2 then + --add remaining points without filtering + while self.smoothIndex <= #self.wayPoints do + local node = self.wayPoints[self.smoothIndex] + self.filteredWPs[self.filteredIndex] = node + self.filteredIndex = self.filteredIndex + 1 + self.smoothIndex = self.smoothIndex + 1 + end + + self.wayPoints = self.filteredWPs + + self.smoothDone = true + + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM smoothResultingPPPath_Refined self.wayPoints %s", + tostring(#self.wayPoints) + ) + ) + end +end + +function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) + local vectorFromPrevious = {x = x1 - x2, z = z1 - z2} + local worldPosMiddle = {x = x2 + vectorFromPrevious.x / 2, z = z2 + vectorFromPrevious.z / 2} + + local terrain1 = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, x1, 0, z1) + local terrain2 = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, x2, 0, z2) + local terrain3 = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPosMiddle.x, 0, worldPosMiddle.z) + local length = MathUtil.vector3Length(x1 - x2, terrain1 - terrain2, z1 - z2) + local lengthMiddle = MathUtil.vector3Length(worldPosMiddle.x - x2, terrain3 - terrain2, worldPosMiddle.z - z2) + local angleBetween = math.atan(math.abs(terrain1 - terrain2) / length) + local angleBetweenCenter = math.atan(math.abs(terrain3 - terrain2) / lengthMiddle) + + local angleLeft = 0 + local angleRight = 0 + + if self.cos90 == nil then + -- speed up the calculation + self.cos90 = math.cos(math.rad(90)) + self.sin90 = math.sin(math.rad(90)) + self.cos270 = math.cos(math.rad(270)) + self.sin270 = math.sin(math.rad(270)) + end + + local rotX = vectorFromPrevious.x * self.cos90 - vectorFromPrevious.z * self.sin90 + local rotZ = vectorFromPrevious.x * self.sin90 + vectorFromPrevious.z * self.cos90 + local vectorLeft = {x = rotX, z = rotZ} + + local rotX = vectorFromPrevious.x * self.cos270 - vectorFromPrevious.z * self.sin270 + local rotZ = vectorFromPrevious.x * self.sin270 + vectorFromPrevious.z * self.cos270 + local vectorRight = {x = rotX, z = rotZ} + + local worldPosLeft = {x = x1 + vectorLeft.x / 2, z = z1 + vectorLeft.z / 2} + local worldPosRight = {x = x1 + vectorRight.x / 2, z = z1 + vectorRight.z / 2} + local terrainLeft = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPosLeft.x, 0, worldPosLeft.z) + local terrainRight = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPosRight.x, 0, worldPosRight.z) + local lengthLeft = MathUtil.vector3Length(worldPosLeft.x - x1, terrainLeft - terrain1, worldPosLeft.z - z1) + local lengthRight = MathUtil.vector3Length(worldPosRight.x - x1, terrainRight - terrain1, worldPosRight.z - z1) + angleLeft = math.atan(math.abs(terrainLeft - terrain1) / lengthLeft) + angleRight = math.atan(math.abs(terrainRight - terrain1) / lengthRight) + + local waterY = g_currentMission.environmentAreaSystem:getWaterYAtWorldPosition(worldPosMiddle.x, terrain3, worldPosMiddle.z) or -200 + + local belowGroundLevel = terrain1 < waterY - 0.5 or terrain2 < waterY - 0.5 or terrain3 < waterY - 0.5 + + if belowGroundLevel then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkSlopeAngle belowGroundLevel x,z %d %d", + math.floor(x1), + math.floor(z1) + ) + ) + end + end + + if (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkSlopeAngle (angleBetween * 1.25) > PathFinderModule.SLOPE_DETECTION_THRESHOLD x,z %d %d", + math.floor(x1), + math.floor(z1) + ) + ) + end + end + + if (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then + if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM checkSlopeAngle (angleBetweenCenter * 1.25) > PathFinderModule.SLOPE_DETECTION_THRESHOLD x,z %d %d", + math.floor(x1), + math.floor(z1) + ) + ) + end + end + + if belowGroundLevel or (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD or (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD + or (angleLeft > PathFinderModule.SLOPE_DETECTION_THRESHOLD or angleRight > PathFinderModule.SLOPE_DETECTION_THRESHOLD) + then + return true, angleBetween + end + return false, angleBetween +end + +function PathFinderModule.debugVehicleMsg(vehicle, msg) + -- collect output for single vehicle - help to examine sequences for a single vehicle + if vehicle ~= nil and vehicle.ad ~= nil and vehicle.ad.debug ~= nil then + if AutoDrive.debugVehicleMsg ~= nil then + AutoDrive.debugVehicleMsg(vehicle, msg) + end + end +end + +function PathFinderModule:drawDebugNewPF() + if self.closedset then + for k, node in pairs(self.closedset) do + if k.text then + local point = self:gridLocationToWorldLocation({k.x, k.z}) + point.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, point.x, 1, point.z) + Utils.renderTextAtWorldPosition(point.x, point.y + 4, point.z, tostring(k.text), getCorrectTextSize(0.013), 0) + end + end + end + if self.fruitAreas then + for _, corners in ipairs(self.fruitAreas) do + local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corners[1].x, 1, corners[1].z) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) + end + end +end + +function PathFinderModule:isDriveable(cell) + cell.isRestricted = false + cell.incoming = cell.from_node + cell.hasCollision = false + + local worldPos = self:gridLocationToWorldLocation(cell) + --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped + + cell.isOnField = AutoDrive.checkIsOnField(worldPos.x, 0, worldPos.z) + + -- check the most probable restrictions on field first to prevent unneccessary checks + if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 or self.fallBackMode2) then + -- in fallBackMode1 we ignore the field restriction + cell.isRestricted = cell.isRestricted or (not cell.isOnField) + if not cell.isOnField then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable not cell.isOnField xz %d,%d " + , cell.x, cell.z + ) + end + end + + local gridFactor = PathFinderModule.GRID_SIZE_FACTOR + if self.isSecondChasingVehicle then + gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER + end + local corners = self:getCorners(cell, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) + + if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then + -- check for fruit + self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found + table.insert(self.fruitAreas, corners) + if cell.isRestricted then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.isRestricted xz %d,%d fruit found %s" + , cell.x, cell.z + , self.fruitToCheck + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + -- check for up/down is to big or below water level + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local angelToSlope, angle = self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level + cell.angle = angle + cell.hasCollision = cell.hasCollision or angelToSlope + cell.isRestricted = cell.isRestricted or cell.hasCollision + if angelToSlope then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable angelToSlope xz %d,%d" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted then + -- check for obstacles + local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height + self.collisionhits = 0 + local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallback", self, self.mask, true, true, true) + cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) + cell.isRestricted = cell.isRestricted or cell.hasCollision + if cell.hasCollision then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.hasCollision xz %d,%d collision" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local vectorX = worldPosPrevious.x - worldPos.x + local vectorZ = worldPosPrevious.z - worldPos.z + local dirVec = { x=vectorX, z = vectorZ} + + local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) + cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath + self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath + if cellUsedByVehiclePath then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cellUsedByVehiclePath xz %d,%d vehicle" + , cell.x, cell.z + ) + end + end + return not(cell.isRestricted) +end + + -- Cost of two adjacent nodes +-- current, neighbor +function PathFinderModule:get_cost(from_node, to_node) + local dx, dz = from_node.x - to_node.x, from_node.z - to_node.z + return math.sqrt(dx * dx + dz * dz) + (from_node.cost + to_node.cost) * 0.5 +end + +-- For heuristic. Estimate cost of current node to goal node +-- neighbor, goal +function PathFinderModule:estimate_cost(node, goal_node) + return self:get_cost(node, goal_node) * 1.5 + (node.cost + goal_node.cost) * 0.5 +end + +-- current = self:pop_best_node(self.openset, self.f_score) +-- return: node / nil +-- self.openset, f_score +function PathFinderModule:pop_best_node(set, score) + local best, node = math.huge, nil + + for k, v in pairs(set) do + local s = score[k] + + if s < best then + best = s or math.huge + node = k + end + end + if not node then return end + set[node] = nil + return node +end + +-- {}, self.came_from, self.nodeGoal +function PathFinderModule:unwind_path(flat_path, came_from, goal) + if came_from[goal] and (came_from[goal] ~= self.nodeGoal) then + table.insert(flat_path, 1, came_from[goal]) + return self:unwind_path(flat_path, came_from, came_from[goal]) + else + return flat_path + end +end + +-- Node must be able to check if they are the same +-- so the example cannot directly return a different table for same coord +function PathFinderModule:get_node(x, z) + local row = self.cached_nodes[z] + if not row then row = {}; self.cached_nodes[z] = row end + local node = row[x] + if not node then node = { x = x, z = z, cost = 0 }; row[x] = node end + if node.text == nil then + node.text = string.format("%d,%d", x, z) + end + return node +end + +function PathFinderModule:getDirections(fromNode, node) + if node == nil then + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node %s" + , tostring(fromNode) + , tostring(node) + ) + return + end + local directions = {} + + if fromNode then + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %d,%d fromNode.direction %s node xz %d,%d node.direction %s" + , fromNode.x, fromNode.z + , tostring(self.direction_to_text[fromNode.direction+1]) + , node.x, node.z + , tostring(self.direction_to_text[node.direction+1]) + ) + else + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %s node xz %d,%d node.direction %s" + , tostring(fromNode) + , node.x, node.z + , tostring(self.direction_to_text[node.direction+1]) + ) + end + if (fromNode == nil and node.direction == self.PP_RIGHT) or (fromNode and fromNode.x == node.x and fromNode.z < node.z) then + directions[1] = { -1, 1 } + directions[1].direction = self.PP_DOWN_RIGHT + directions[2] = { 0, 1 } + directions[2].direction = self.PP_RIGHT + directions[3] = { 1, 1 } + directions[3].direction = self.PP_UP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_LEFT) or (fromNode and fromNode.x == node.x and fromNode.z > node.z) then + directions[1] = { -1, -1 } + directions[1].direction = self.PP_DOWN_LEFT + directions[2] = { 0, -1 } + directions[2].direction = self.PP_LEFT + directions[3] = { 1, -1 } + directions[3].direction = self.PP_UP_LEFT + elseif (fromNode == nil and node.direction == self.PP_UP) or (fromNode and fromNode.x < node.x and fromNode.z == node.z) then + directions[1] = { 1, -1 } + directions[1].direction = self.PP_UP_LEFT + directions[2] = { 1, 0 } + directions[2].direction = self.PP_UP + directions[3] = { 1, 1 } + directions[3].direction = self.PP_UP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_DOWN) or (fromNode and fromNode.x > node.x and fromNode.z == node.z) then + directions[1] = { -1, -1 } + directions[1].direction = self.PP_DOWN_LEFT + directions[2] = { -1, 0 } + directions[2].direction = self.PP_DOWN + directions[3] = { -1, 1 } + directions[3].direction = self.PP_DOWN_RIGHT + elseif (fromNode == nil and node.direction == self.PP_UP_RIGHT) or (fromNode and fromNode.x < node.x and fromNode.z < node.z) then + directions[1] = { 1, 0 } + directions[1].direction = self.PP_UP + directions[2] = { 1, 1 } + directions[2].direction = self.PP_UP_RIGHT + directions[3] = { 0, 1 } + directions[3].direction = self.PP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_DOWN_LEFT) or (fromNode and fromNode.x > node.x and fromNode.z > node.z) then + directions[1] = { 0, -1 } + directions[1].direction = self.PP_LEFT + directions[2] = { -1, -1 } + directions[2].direction = self.PP_DOWN_LEFT + directions[3] = { -1, 0 } + directions[3].direction = self.PP_DOWN + elseif (fromNode == nil and node.direction == self.PP_UP_LEFT) or (fromNode and fromNode.x < node.x and fromNode.z > node.z) then + directions[1] = { 0, -1 } + directions[1].direction = self.PP_LEFT + directions[2] = { 1, -1 } + directions[2].direction = self.PP_UP_LEFT + directions[3] = { 1, 0 } + directions[3].direction = self.PP_UP + elseif (fromNode == nil and node.direction == self.PP_DOWN_RIGHT) or (fromNode and fromNode.x > node.x and fromNode.z < node.z) then + directions[1] = { -1, 0 } + directions[1].direction = self.PP_DOWN + directions[2] = { -1, 1 } + directions[2].direction = self.PP_DOWN_RIGHT + directions[3] = { 0, 1 } + directions[3].direction = self.PP_RIGHT + else + if fromNode then + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode xz %d,%d" + , fromNode.x + , fromNode.z + ) + end + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node xz %d,%d node.direction %s" + , tostring(fromNode) + , node.x, node.z + , tostring(node.direction) + ) + end + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections %d,%d direction %s %d,%d direction %s %d,%d direction %s" + , directions[1][1], directions[1][2] + , tostring(self.direction_to_text[directions[1].direction+1]) + , directions[2][1], directions[2][2] + , tostring(self.direction_to_text[directions[2].direction+1]) + , directions[3][1], directions[3][2] + , tostring(self.direction_to_text[directions[3].direction+1]) + ) + return directions +end + +-- Return all neighbor nodes. Means a target that can be moved from the current node +-- current, from_node, add_neighbor_fn +function PathFinderModule:get_neighbors(node, fromNode, add_neighbor_fn) + local x, z = node.x, node.z + local directions = self:getDirections(fromNode, node) + if directions then + for i, offset in ipairs(directions) do + local tnode = self:get_node(x + offset[1], z + offset[2]) + tnode.direction = offset.direction + tnode.from_node = fromNode + add_neighbor_fn(tnode) + end + end +end + +local all_neighbors_offset = { + { -1, -1 }, { 0, -1 }, { 1, -1 }, + { -1, 0 }, { 1, 0 }, + { -1, 1 }, { 0, 1 }, { 1, 1 } +} + +function PathFinderModule:setBlockedGoal() + local x, z = self.targetAheadCell.x, self.targetAheadCell.z + for i, offset in ipairs(all_neighbors_offset) do + if not (self.targetCell.x == (x + offset[1]) and self.targetCell.z == (z + offset[2])) then + local tnode = self:get_node(x + offset[1], z + offset[2]) + tnode.text = string.format("G %d,%d",x + offset[1], z + offset[2]) + self.closedset[tnode] = true + end + end +end + +function PathFinderModule:reachedGoal(current, goal) + if math.abs(current.x - goal.x) < 2 and math.abs(current.z - goal.z) < 2 then + return true + else + return false + end +end + +function PathFinderModule:setupNew(behindStartCell, startCell, targetCell, userdata) + PathFinderModule.debugMsg(self.vehicle, "PFM:setupNew behindStartCell %s,%s startCell %s,%s targetCell %s,%s" + , tostring(behindStartCell.x) + , tostring(behindStartCell.z) + , tostring(startCell.x) + , tostring(startCell.z) + , tostring(targetCell.x) + , tostring(targetCell.z) + ) + self.cached_nodes = {} + self.openset = {} + self.closedset = {} + self.came_from = {} + self.g_score = {} + self.h_score = {} + self.f_score = {} + + self.nodeBehindStart = self:get_node(behindStartCell.x, behindStartCell.z) + self.nodeBehindStart.text = string.format("B %d,%d",behindStartCell.x, behindStartCell.z) + self.nodeBehindStart.direction = behindStartCell.direction + + self.nodeStart = self:get_node(startCell.x, startCell.z) + self.nodeStart.text = string.format("S %d,%d",startCell.x, startCell.z) + self.nodeStart.direction = startCell.direction + + self.nodeGoal = self:get_node(targetCell.x, targetCell.z) + self.nodeGoal.text = string.format("T %d,%d",targetCell.x, targetCell.z) + + self.g_score[self.nodeBehindStart] = math.huge + self.h_score[self.nodeBehindStart] = math.huge + self.f_score[self.nodeBehindStart] = math.huge + + self.g_score[self.nodeStart] = 0 + self.h_score[self.nodeStart] = self:estimate_cost(self.nodeStart, self.nodeGoal) + self.f_score[self.nodeStart] = self.h_score[self.nodeStart] + self.came_from[self.nodeStart] = self.nodeBehindStart + + self.openset[self.nodeStart] = true + self.closedset[self.nodeBehindStart] = true + self:isDriveable(self.nodeStart) + self:setBlockedGoal() + self.initNew = true +end + +function PathFinderModule:createWayPointsNew() + if self.smoothStep == 0 then + self.wayPoints = {} + for index, cell in ipairs(self.path) do + self.wayPoints[index] = self:gridLocationToWorldLocation(cell) + self.wayPoints[index].y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, self.wayPoints[index].x, 1, self.wayPoints[index].z) + self.wayPoints[index].direction = cell.direction + end + -- remove zig zag line + self:smoothResultingPPPath() + end + -- shortcut the path if possible + self:smoothResultingPPPath_Refined() + + if self.smoothStep == 2 then + -- When going to network, dont turn actual road network nodes into pathFinderPoints + if self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true + end + end + + if self.appendWayPoints ~= nil then + for i = 1, #self.appendWayPoints, 1 do + self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] + end + self.smoothStep = 3 + end + + -- See comment above + if not self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true + end + end + end +end + +function PathFinderModule:collisionTestCallback(transformId) + if transformId ~= 0 and transformId ~= g_currentMission.terrainRootNode then + local object = g_currentMission:getNodeObject(transformId) + if (object == nil) or (object ~= nil and not (object == self.vehicle or object.rootVehicle == self.vehicle)) then + self.collisionhits = self.collisionhits + 1 + end + end +end + +function PathFinderModule.debugMsg(vehicle, debugText, ...) + if PathFinderModule.debug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, debugText, ...) + end +end diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index a0f9ea86..f9447250 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -57,6 +57,7 @@ This is inclusive of the field border cells! ]] PathFinderModule = {} +PathFinderModule.debug = false PathFinderModule.PATHFINDER_MAX_RETRIES = 3 PathFinderModule.MAX_PATHFINDER_STEPS_PER_FRAME = 2 @@ -69,27 +70,6 @@ PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE_CLOSE = 6 PathFinderModule.PATHFINDER_START_DISTANCE = 7 PathFinderModule.MAX_FIELDBORDER_CELLS = 5 -PathFinderModule.PP_UP = 0 -PathFinderModule.PP_UP_RIGHT = 1 -PathFinderModule.PP_RIGHT = 2 -PathFinderModule.PP_DOWN_RIGHT = 3 -PathFinderModule.PP_DOWN = 4 -PathFinderModule.PP_DOWN_LEFT = 5 -PathFinderModule.PP_LEFT = 6 -PathFinderModule.PP_UP_LEFT = 7 - -PathFinderModule.direction_to_text = { -"PathFinderModule.PP_UP", -"PathFinderModule.PP_UP_RIGHT", -"PathFinderModule.PP_RIGHT", -"PathFinderModule.PP_DOWN_RIGHT", -"PathFinderModule.PP_DOWN", -"PathFinderModule.PP_DOWN_LEFT", -"PathFinderModule.PP_LEFT", -"PathFinderModule.PP_UP_LEFT", -"PathFinderModule.unknown" -} - PathFinderModule.PP_MIN_DISTANCE = 20 PathFinderModule.PP_CELL_X = 9 PathFinderModule.PP_CELL_Z = 9 @@ -101,6 +81,7 @@ PathFinderModule.PP_MAX_EAGER_LOOKAHEAD_STEPS = 1 PathFinderModule.MIN_FRUIT_VALUE = 50 PathFinderModule.SLOPE_DETECTION_THRESHOLD = math.rad(20) +PathFinderModule.NEW_PF_STEP_FACTOR = 4 --[[ from Giants Engine: AITurnStrategy.SLOPE_DETECTION_THRESHOLD = 0.5235987755983 @@ -116,9 +97,14 @@ function PathFinderModule:new(vehicle) end function PathFinderModule:reset() + PathFinderModule.debugMsg(self.vehicle, "PFM:reset start") self.mask = AutoDrive.collisionMaskTerrain self.steps = 0 self.grid = {} + self.wayPoints = {} + self.initNew = false + self.path = {} + self.diffOverallNetTime = 0 self.retryCounter = 0 self.delayTime = 0 self.restrictToField = false @@ -129,6 +115,53 @@ function PathFinderModule:reset() self.fallBackMode3 = false self.isFinished = true self.smoothDone = true + self.fruitAreas = {} + + if AutoDrive.experimentalFeatures.NewPathfinder then + self.PP_UP = 0 + self.PP_UP_LEFT = 1 + self.PP_LEFT = 2 + self.PP_DOWN_LEFT = 3 + self.PP_DOWN = 4 + self.PP_DOWN_RIGHT = 5 + self.PP_RIGHT = 6 + self.PP_UP_RIGHT = 7 + self.direction_to_text = { + "PP_UP", + "PP_UP_LEFT", + "PP_LEFT", + "PP_DOWN_LEFT", + "PP_DOWN", + "PP_DOWN_RIGHT", + "PP_RIGHT", + "PP_UP_RIGHT", + "unknown" + } + self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) + self.isNewPF = true + else + self.PP_UP = 0 + self.PP_UP_RIGHT = 1 + self.PP_RIGHT = 2 + self.PP_DOWN_RIGHT = 3 + self.PP_DOWN = 4 + self.PP_DOWN_LEFT = 5 + self.PP_LEFT = 6 + self.PP_UP_LEFT = 7 + self.direction_to_text = { + "PP_UP", + "PP_UP_RIGHT", + "PP_RIGHT", + "PP_DOWN_RIGHT", + "PP_DOWN", + "PP_DOWN_LEFT", + "PP_LEFT", + "PP_UP_LEFT", + "unknown" + } + self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) * 2 / 3 + self.isNewPF = false + end end function PathFinderModule:hasFinished() @@ -316,7 +349,6 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) local angleRad = math.atan2(targetVector.z, targetVector.x) angleRad = AutoDrive.normalizeAngle(angleRad) - self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) * 2 / 3 self.vectorX = {x = math.cos(angleRad) * self.minTurnRadius, z = math.sin(angleRad) * self.minTurnRadius} self.vectorZ = {x = - math.sin(angleRad) * self.minTurnRadius, z = math.cos(angleRad) * self.minTurnRadius} @@ -347,6 +379,11 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) self.startCell.bordercells = 0 self.currentCell = nil + local vehicleBehindX, _, vehicleBehindZ = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, -self.minTurnRadius) + local vehicleBehindVector = {x = vehicleBehindX, z = vehicleBehindZ} + self.behindStartCell = self:worldLocationToGridLocation(vehicleWorldX + vehicleBehindX, vehicleWorldZ + vehicleBehindZ) + self.behindStartCell.direction = self:worldDirectionToGridDirection(vehicleBehindVector, vehicleVector) + -- table.insert(self.grid, self.startCell) local gridKey = string.format("%d|%d|%d", self.startCell.x, self.startCell.z, self.startCell.direction) self.grid[gridKey] = self.startCell @@ -357,10 +394,12 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) local targetCellZ = (((targetX - self.startX) / self.vectorX.x) * self.vectorX.z - targetZ + self.startZ) / (((self.vectorZ.x / self.vectorX.x) * self.vectorX.z) - self.vectorZ.z) local targetCellX = (targetZ - self.startZ - targetCellZ * self.vectorZ.z) / self.vectorX.z - targetCellX = AutoDrive.round(targetCellX) targetCellZ = AutoDrive.round(targetCellZ) self.targetCell = {x = targetCellX, z = targetCellZ, direction = self.PP_UP} + self.targetAhead = {x = targetX + self.vectorX.x, z = targetZ + self.vectorX.z} + self.targetAheadCell = self:worldLocationToGridLocation(self.targetAhead.x, self.targetAhead.z) + self:determineBlockedCells(self.targetCell) --self:checkGridCell(self.targetCell) @@ -371,7 +410,6 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) self.startIsOnField = AutoDrive.checkIsOnField(vehicleWorldX, vehicleWorldY, vehicleWorldZ) and self.vehicle.ad.sensors.frontSensorField:pollInfo(true) self.endIsOnField = AutoDrive.checkIsOnField(targetX, vehicleWorldY, targetZ) - self.restrictToField = AutoDrive.getSetting("restrictToField", self.vehicle) and self.startIsOnField and self.endIsOnField self.goingToPipe = false self.chasingVehicle = false @@ -406,14 +444,14 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) string.format("PFM startPathPlanningTo startCell xz %d %d direction %s", math.floor(self.startCell.x), math.floor(self.startCell.z), - tostring(PathFinderModule.direction_to_text[self.startCell.direction+1]) + tostring(self.direction_to_text[self.startCell.direction+1]) ) ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningTo start targetCell xz %d %d direction %s", math.floor(self.targetCell.x), math.floor(self.targetCell.z), - tostring(PathFinderModule.direction_to_text[self.targetCell.direction+1]) + tostring(self.direction_to_text[self.targetCell.direction+1]) ) ) PathFinderModule.debugVehicleMsg(self.vehicle, @@ -454,9 +492,13 @@ function PathFinderModule:restartAtNextWayPoint() end function PathFinderModule:autoRestart() + PathFinderModule.debugMsg(self.vehicle, "PFM:autoRestart start") self.steps = 0 self.grid = {} self.wayPoints = {} + self.initNew = false + self.path = {} + self.diffOverallNetTime = 0 self.startCell.visited = false self.startCell.out = nil self.currentCell = nil @@ -472,6 +514,7 @@ function PathFinderModule:autoRestart() end function PathFinderModule:abort() + PathFinderModule.debugMsg(self.vehicle, "PFM:abort start") self.isFinished = true self.smoothDone = true self.wayPoints = {} @@ -513,7 +556,7 @@ function PathFinderModule:getCurrentState() actualState = actualState + 1 end - return actualState, maxStates + return actualState, maxStates, self.steps, self.max_pathfinder_steps end function PathFinderModule:timedOut() --> not self:isBlocked() -- used in: ExitFieldTask, UnloadAtDestinationTask @@ -525,6 +568,7 @@ function PathFinderModule:addDelayTimer(delayTime) -- used in: ExitFieldTas end function PathFinderModule:update(dt) + --stop if called without prior 'start' method calls if self.startCell == nil then self:abort() @@ -536,19 +580,29 @@ function PathFinderModule:update(dt) end if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then - if self.isFinished and self.smoothDone and self.wayPoints ~= nil and self.chainStartToTarget ~= nil and #self.chainStartToTarget > 0 and self.vehicle.ad.stateModule:getSpeedLimit() > 40 then - self:drawDebugForCreatedRoute() + if self.isNewPF then + self:drawDebugNewPF() else - self:drawDebugForPF() + if self.isFinished and self.smoothDone and self.wayPoints ~= nil and self.chainStartToTarget ~= nil and #self.chainStartToTarget > 0 and self.vehicle.ad.stateModule:getSpeedLimit() > 40 then + self:drawDebugForCreatedRoute() + else + self:drawDebugForPF() + end end end - if self.isFinished then if not self.smoothDone then - self:createWayPoints() + if self.isNewPF then + self:createWayPointsNew() + else + self:createWayPoints() + end end if self.smoothDone then ADScheduler:removePathfinderVehicle(self.vehicle) + PathFinderModule.debugMsg(self.vehicle, "PFM:update Complete #self.path %s" + , tostring(#self.path) + ) end return end @@ -586,7 +640,7 @@ function PathFinderModule:update(dt) ) self.fallBackMode1 = true self:autoRestart() - elseif fallBackModeAllowed2 then + elseif fallBackModeAllowed2 and not self.isNewPF then AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", table.count(self.grid)) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", @@ -651,63 +705,153 @@ function PathFinderModule:update(dt) return end - --We should see some perfomance increase by localizing the sqrt/pow functions right here - local sqrt = math.sqrt - local distanceFunc = function(a, b) - return sqrt(a * a + b * b) - end - - for i = 1, ADScheduler:getStepsPerFrame(), 1 do - if self.currentCell == nil then - - self.currentCell = self:findClosestCell(self.grid, math.huge) + if self.isNewPF then + if not self.isFinished then + if not self.initNew then + self:setupNew(self.behindStartCell, self.startCell,self.targetCell) + if self.nodeBehindStart == self.nodeGoal then + self.completelyBlocked = true + return -- no valid path + end + end + local diffNetTime = netGetTime() + + local current + local add_neighbor_fn = function(neighbor, cost) + if self:isDriveable(neighbor) then + if not self.closedset[neighbor] then + if not cost then cost = self:get_cost(current, neighbor) end + local tentative_g_score = self.g_score[current] + cost + local openset_idx = self.openset[neighbor] + if not openset_idx or tentative_g_score < self.g_score[neighbor] then + self.came_from[neighbor] = current + self.g_score[neighbor] = tentative_g_score + self.h_score[neighbor] = self.h_score[neighbor] or self:estimate_cost(neighbor, self.nodeGoal) + self.f_score[neighbor] = tentative_g_score + self.h_score[neighbor] + self.openset[neighbor] = true + end + end + end + end + local count = 0 + while next(self.openset) do + count = count + 1 + if count > 10000 then + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + AutoDrive.debugMsg(self.vehicle, "PFM:find ERROR exit counter count %d self.diffOverallNetTime %d" + , count + , self.diffOverallNetTime + ) - if self.currentCell ~= nil and distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) < 1.5 then + self.completelyBlocked = true + return -- no valid path + end + current = self:pop_best_node(self.openset, self.f_score) + if current == self.nodeGoal or self:reachedGoal(current, self.nodeGoal) then + self.came_from[self.nodeGoal] = current + self.path = self:unwind_path({}, self.came_from, self.nodeGoal) + table.insert(self.path, self.nodeGoal) + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + PathFinderModule.debugMsg(self.vehicle, "PFM:update find goal reached self.steps %d diffOverallNetTime %d self.nodeGoal xz %d,%d current xz %d,%d" + , self.steps + , self.diffOverallNetTime + , self.nodeGoal.x, self.nodeGoal.z + , current.x, current.z + ) - if self.currentCell.out == nil then - self:determineNextGridCells(self.currentCell) + self.isFinished = true + return -- found path end + if current then self.closedset[current] = true end + local from_node = self.came_from[current] + self:get_neighbors(current, from_node, add_neighbor_fn) + if count > (ADScheduler:getStepsPerFrame() * PathFinderModule.NEW_PF_STEP_FACTOR) then + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + PathFinderModule.debugMsg(self.vehicle, "PFM:find steps in frame count %d diffNetTime %d self.diffOverallNetTime %d" + , count + , diffNetTime + , self.diffOverallNetTime + ) - if self:reachedTargetsNeighbor(self.currentCell.out) then - return + return -- shedule end end + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + PathFinderModule.debugMsg(self.vehicle, "PFM:find exit end count %d self.diffOverallNetTime %d" + , count + , self.diffOverallNetTime + ) + + self.completelyBlocked = true + return -- no valid path + end + else + --We should see some perfomance increase by localizing the sqrt/pow functions right here + local sqrt = math.sqrt + local distanceFunc = function(a, b) + return sqrt(a * a + b * b) + end + for i = 1, ADScheduler:getStepsPerFrame(), 1 do if self.currentCell == nil then - --Mark process stopped if we have no more cells to check - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - Mark process stopped if we have no more cells to check") - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - Mark process stopped if we have no more cells to check #self.grid %d", - table.count(self.grid) + + self.currentCell = self:findClosestCell(self.grid, math.huge) + + if self.currentCell ~= nil and distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) < 1.5 then + + if self.currentCell.out == nil then + self:determineNextGridCells(self.currentCell) + end + + if self:reachedTargetsNeighbor(self.currentCell.out) then + return + end + end + + if self.currentCell == nil then + --Mark process stopped if we have no more cells to check + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - Mark process stopped if we have no more cells to check") + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - Mark process stopped if we have no more cells to check #self.grid %d", + table.count(self.grid) + ) ) - ) - self.completelyBlocked = true - break - end - else - if self.currentCell.out == nil then - self:determineNextGridCells(self.currentCell) - end - self:testNextCells(self.currentCell) + self.completelyBlocked = true + break + end + else + if self.currentCell.out == nil then + self:determineNextGridCells(self.currentCell) + end + self:testNextCells(self.currentCell) - --Try shortcutting the process here. We dont have to go through the whole grid if one of the out points is viable and closer than the currenCell which was already closest - local currentDistance = distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) + --Try shortcutting the process here. We dont have to go through the whole grid if one of the out points is viable and closer than the currenCell which was already closest + local currentDistance = distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) - local outCells = {} - for _, outCell in pairs(self.currentCell.out) do - local gridKey = string.format("%d|%d|%d", outCell.x, outCell.z, outCell.direction) - if self.grid[gridKey] ~= nil then - table.insert(outCells, self.grid[gridKey]) + local outCells = {} + for _, outCell in pairs(self.currentCell.out) do + local gridKey = string.format("%d|%d|%d", outCell.x, outCell.z, outCell.direction) + if self.grid[gridKey] ~= nil then + table.insert(outCells, self.grid[gridKey]) + end end - end - local nextCell = self:findClosestCell(outCells, currentDistance) + local nextCell = self:findClosestCell(outCells, currentDistance) - -- Lets again check if we have reached our target already - if self:reachedTargetsNeighbor(self.currentCell.out) then - return - end + -- Lets again check if we have reached our target already + if self:reachedTargetsNeighbor(self.currentCell.out) then + return + end - self.currentCell = nextCell + self.currentCell = nextCell + end end end end @@ -764,7 +908,7 @@ function PathFinderModule:testNextCells(cell) math.floor(cell.z), tostring(cell.isRestricted), tostring(cell.hasFruit), - tostring(PathFinderModule.direction_to_text[cell.direction+1]) + tostring(self.direction_to_text[cell.direction+1]) ) ) end @@ -776,11 +920,11 @@ function PathFinderModule:testNextCells(cell) string.format("PFM testNextCells location xz %d %d direction %s", math.floor(location.x), math.floor(location.z), - tostring(PathFinderModule.direction_to_text[location.direction+1]) + tostring(self.direction_to_text[location.direction+1]) ) ) end - for i = -1, PathFinderModule.PP_UP_LEFT, 1 do -- important: do not break this loop to check for all directions! + for i = -1, self.PP_UP_LEFT, 1 do -- important: do not break this loop to check for all directions! local gridKey = string.format("%d|%d|%d", location.x, location.z, i) if self.grid[gridKey] ~= nil then -- cell is already in the grid @@ -790,7 +934,7 @@ function PathFinderModule:testNextCells(cell) tostring(gridKey), tostring(self.grid[gridKey].x), tostring(self.grid[gridKey].z), - tostring(PathFinderModule.direction_to_text[self.grid[gridKey].direction+1]) + tostring(self.direction_to_text[self.grid[gridKey].direction+1]) ) ) end @@ -822,7 +966,7 @@ function PathFinderModule:testNextCells(cell) string.format("PFM testNextCells location xz %d %d createPoint %s", math.floor(location.x), math.floor(location.z), - tostring(PathFinderModule.direction_to_text[location.direction+1]) + tostring(self.direction_to_text[location.direction+1]) ) ) end @@ -849,7 +993,7 @@ function PathFinderModule:testNextCells(cell) string.format("PFM testNextCells different direction xz %d %d createPoint %s", math.floor(location.x), math.floor(location.z), - tostring(PathFinderModule.direction_to_text[location.direction+1]) + tostring(self.direction_to_text[location.direction+1]) ) ) end @@ -983,8 +1127,9 @@ function PathFinderModule:gridLocationToWorldLocation(cell) return result end -function PathFinderModule:worldDirectionToGridDirection(vector) - local angle = AutoDrive.angleBetween(self.vectorX, vector) +function PathFinderModule:worldDirectionToGridDirection(vector, baseVector) + local baseVector = baseVector or self.vectorX + local angle = AutoDrive.angleBetween(baseVector, vector) local direction = math.floor(angle / 45) local remainder = angle % 45 @@ -1169,7 +1314,7 @@ function PathFinderModule:checkForFruitTypeInArea(cell, fruitTypeIndex, corners) math.floor(cell.x), math.floor(cell.z), tostring(fruitValue), - tostring(PathFinderModule.direction_to_text[cell.direction+1]) + tostring(self.direction_to_text[cell.direction+1]) ) ) end @@ -1490,12 +1635,14 @@ function PathFinderModule:drawDebugForCreatedRoute() end end - for i, waypoint in pairs(self.wayPoints) do - Utils.renderTextAtWorldPosition(waypoint.x, waypoint.y + 4, waypoint.z, "Node " .. i, getCorrectTextSize(0.013), 0) - if i > 1 then - local wp = waypoint - local pfWp = self.wayPoints[i - 1] - AutoDriveDM:addLineTask(wp.x, wp.y, wp.z, pfWp.x, pfWp.y, pfWp.z, 1, 0, 1, 1) + if self.wayPoints then + for i, waypoint in pairs(self.wayPoints) do + Utils.renderTextAtWorldPosition(waypoint.x, waypoint.y + 4, waypoint.z, "Node " .. i, getCorrectTextSize(0.013), 0) + if i > 1 then + local wp = waypoint + local pfWp = self.wayPoints[i - 1] + AutoDriveDM:addLineTask(wp.x, wp.y, wp.z, pfWp.x, pfWp.y, pfWp.z, 1, 0, 1, 1) + end end end end @@ -1841,8 +1988,14 @@ function PathFinderModule:smoothResultingPPPath_Refined() local corner4Z = node.z + math.sin(rightAngle) * sideLength if not hasCollision then - local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "collisionTestCallbackIgnore", nil, self.mask, true, true, true) - hasCollision = hasCollision or (shapes > 0) + if self.isNewPF then + self.collisionhits = 0 + local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "collisionTestCallback", self, self.mask, true, true, true) + hasCollision = hasCollision or (self.collisionhits > 0) + else + local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "Ignore", nil, self.mask, true, true, true) + hasCollision = hasCollision or (shapes > 0) + end if hasCollision then if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then @@ -2097,3 +2250,407 @@ function PathFinderModule.debugVehicleMsg(vehicle, msg) end end end + +function PathFinderModule:drawDebugNewPF() + if self.closedset then + for k, node in pairs(self.closedset) do + if k.text then + local point = self:gridLocationToWorldLocation({k.x, k.z}) + point.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, point.x, 1, point.z) + Utils.renderTextAtWorldPosition(point.x, point.y + 4, point.z, tostring(k.text), getCorrectTextSize(0.013), 0) + end + end + end + if self.fruitAreas then + for _, corners in ipairs(self.fruitAreas) do + local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corners[1].x, 1, corners[1].z) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) + end + end +end + +function PathFinderModule:isDriveable(cell) + cell.isRestricted = false + cell.incoming = cell.from_node + cell.hasCollision = false + + local worldPos = self:gridLocationToWorldLocation(cell) + --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped + + cell.isOnField = AutoDrive.checkIsOnField(worldPos.x, 0, worldPos.z) + + -- check the most probable restrictions on field first to prevent unneccessary checks + if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 or self.fallBackMode2) then + -- in fallBackMode1 we ignore the field restriction + cell.isRestricted = cell.isRestricted or (not cell.isOnField) + if not cell.isOnField then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable not cell.isOnField xz %d,%d " + , cell.x, cell.z + ) + end + end + + local gridFactor = PathFinderModule.GRID_SIZE_FACTOR + if self.isSecondChasingVehicle then + gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER + end + local corners = self:getCorners(cell, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) + + if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then + -- check for fruit + self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found + table.insert(self.fruitAreas, corners) + if cell.isRestricted then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.isRestricted xz %d,%d fruit found %s" + , cell.x, cell.z + , self.fruitToCheck + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + -- check for up/down is to big or below water level + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local angelToSlope, angle = self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level + cell.angle = angle + cell.hasCollision = cell.hasCollision or angelToSlope + cell.isRestricted = cell.isRestricted or cell.hasCollision + if angelToSlope then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable angelToSlope xz %d,%d" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted then + -- check for obstacles + local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height + self.collisionhits = 0 + local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallback", self, self.mask, true, true, true) + cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) + cell.isRestricted = cell.isRestricted or cell.hasCollision + if cell.hasCollision then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.hasCollision xz %d,%d collision" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local vectorX = worldPosPrevious.x - worldPos.x + local vectorZ = worldPosPrevious.z - worldPos.z + local dirVec = { x=vectorX, z = vectorZ} + + local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) + cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath + self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath + if cellUsedByVehiclePath then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cellUsedByVehiclePath xz %d,%d vehicle" + , cell.x, cell.z + ) + end + end + return not(cell.isRestricted) +end + + -- Cost of two adjacent nodes +-- current, neighbor +function PathFinderModule:get_cost(from_node, to_node) + local dx, dz = from_node.x - to_node.x, from_node.z - to_node.z + return math.sqrt(dx * dx + dz * dz) + (from_node.cost + to_node.cost) * 0.5 +end + +-- For heuristic. Estimate cost of current node to goal node +-- neighbor, goal +function PathFinderModule:estimate_cost(node, goal_node) + return self:get_cost(node, goal_node) * 1.5 + (node.cost + goal_node.cost) * 0.5 +end + +-- current = self:pop_best_node(self.openset, self.f_score) +-- return: node / nil +-- self.openset, f_score +function PathFinderModule:pop_best_node(set, score) + local best, node = math.huge, nil + + for k, v in pairs(set) do + local s = score[k] + + if s < best then + best = s or math.huge + node = k + end + end + if not node then return end + set[node] = nil + return node +end + +-- {}, self.came_from, self.nodeGoal +function PathFinderModule:unwind_path(flat_path, came_from, goal) + if came_from[goal] and (came_from[goal] ~= self.nodeGoal) then + table.insert(flat_path, 1, came_from[goal]) + return self:unwind_path(flat_path, came_from, came_from[goal]) + else + return flat_path + end +end + +-- Node must be able to check if they are the same +-- so the example cannot directly return a different table for same coord +function PathFinderModule:get_node(x, z) + local row = self.cached_nodes[z] + if not row then row = {}; self.cached_nodes[z] = row end + local node = row[x] + if not node then node = { x = x, z = z, cost = 0 }; row[x] = node end + if node.text == nil then + node.text = string.format("%d,%d", x, z) + end + return node +end + +function PathFinderModule:getDirections(fromNode, node) + if node == nil then + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node %s" + , tostring(fromNode) + , tostring(node) + ) + return + end + local directions = {} + + if fromNode then + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %d,%d fromNode.direction %s node xz %d,%d node.direction %s" + , fromNode.x, fromNode.z + , tostring(self.direction_to_text[fromNode.direction+1]) + , node.x, node.z + , tostring(self.direction_to_text[node.direction+1]) + ) + else + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %s node xz %d,%d node.direction %s" + , tostring(fromNode) + , node.x, node.z + , tostring(self.direction_to_text[node.direction+1]) + ) + end + if (fromNode == nil and node.direction == self.PP_RIGHT) or (fromNode and fromNode.x == node.x and fromNode.z < node.z) then + directions[1] = { -1, 1 } + directions[1].direction = self.PP_DOWN_RIGHT + directions[2] = { 0, 1 } + directions[2].direction = self.PP_RIGHT + directions[3] = { 1, 1 } + directions[3].direction = self.PP_UP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_LEFT) or (fromNode and fromNode.x == node.x and fromNode.z > node.z) then + directions[1] = { -1, -1 } + directions[1].direction = self.PP_DOWN_LEFT + directions[2] = { 0, -1 } + directions[2].direction = self.PP_LEFT + directions[3] = { 1, -1 } + directions[3].direction = self.PP_UP_LEFT + elseif (fromNode == nil and node.direction == self.PP_UP) or (fromNode and fromNode.x < node.x and fromNode.z == node.z) then + directions[1] = { 1, -1 } + directions[1].direction = self.PP_UP_LEFT + directions[2] = { 1, 0 } + directions[2].direction = self.PP_UP + directions[3] = { 1, 1 } + directions[3].direction = self.PP_UP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_DOWN) or (fromNode and fromNode.x > node.x and fromNode.z == node.z) then + directions[1] = { -1, -1 } + directions[1].direction = self.PP_DOWN_LEFT + directions[2] = { -1, 0 } + directions[2].direction = self.PP_DOWN + directions[3] = { -1, 1 } + directions[3].direction = self.PP_DOWN_RIGHT + elseif (fromNode == nil and node.direction == self.PP_UP_RIGHT) or (fromNode and fromNode.x < node.x and fromNode.z < node.z) then + directions[1] = { 1, 0 } + directions[1].direction = self.PP_UP + directions[2] = { 1, 1 } + directions[2].direction = self.PP_UP_RIGHT + directions[3] = { 0, 1 } + directions[3].direction = self.PP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_DOWN_LEFT) or (fromNode and fromNode.x > node.x and fromNode.z > node.z) then + directions[1] = { 0, -1 } + directions[1].direction = self.PP_LEFT + directions[2] = { -1, -1 } + directions[2].direction = self.PP_DOWN_LEFT + directions[3] = { -1, 0 } + directions[3].direction = self.PP_DOWN + elseif (fromNode == nil and node.direction == self.PP_UP_LEFT) or (fromNode and fromNode.x < node.x and fromNode.z > node.z) then + directions[1] = { 0, -1 } + directions[1].direction = self.PP_LEFT + directions[2] = { 1, -1 } + directions[2].direction = self.PP_UP_LEFT + directions[3] = { 1, 0 } + directions[3].direction = self.PP_UP + elseif (fromNode == nil and node.direction == self.PP_DOWN_RIGHT) or (fromNode and fromNode.x > node.x and fromNode.z < node.z) then + directions[1] = { -1, 0 } + directions[1].direction = self.PP_DOWN + directions[2] = { -1, 1 } + directions[2].direction = self.PP_DOWN_RIGHT + directions[3] = { 0, 1 } + directions[3].direction = self.PP_RIGHT + else + if fromNode then + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode xz %d,%d" + , fromNode.x + , fromNode.z + ) + end + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node xz %d,%d node.direction %s" + , tostring(fromNode) + , node.x, node.z + , tostring(node.direction) + ) + end + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections %d,%d direction %s %d,%d direction %s %d,%d direction %s" + , directions[1][1], directions[1][2] + , tostring(self.direction_to_text[directions[1].direction+1]) + , directions[2][1], directions[2][2] + , tostring(self.direction_to_text[directions[2].direction+1]) + , directions[3][1], directions[3][2] + , tostring(self.direction_to_text[directions[3].direction+1]) + ) + return directions +end + +-- Return all neighbor nodes. Means a target that can be moved from the current node +-- current, from_node, add_neighbor_fn +function PathFinderModule:get_neighbors(node, fromNode, add_neighbor_fn) + local x, z = node.x, node.z + local directions = self:getDirections(fromNode, node) + if directions then + for i, offset in ipairs(directions) do + local tnode = self:get_node(x + offset[1], z + offset[2]) + tnode.direction = offset.direction + tnode.from_node = fromNode + add_neighbor_fn(tnode) + end + end +end + +local all_neighbors_offset = { + { -1, -1 }, { 0, -1 }, { 1, -1 }, + { -1, 0 }, { 1, 0 }, + { -1, 1 }, { 0, 1 }, { 1, 1 } +} + +function PathFinderModule:setBlockedGoal() + local x, z = self.targetAheadCell.x, self.targetAheadCell.z + for i, offset in ipairs(all_neighbors_offset) do + if not (self.targetCell.x == (x + offset[1]) and self.targetCell.z == (z + offset[2])) then + local tnode = self:get_node(x + offset[1], z + offset[2]) + tnode.text = string.format("G %d,%d",x + offset[1], z + offset[2]) + self.closedset[tnode] = true + end + end +end + +function PathFinderModule:reachedGoal(current, goal) + if math.abs(current.x - goal.x) < 2 and math.abs(current.z - goal.z) < 2 then + return true + else + return false + end +end + +function PathFinderModule:setupNew(behindStartCell, startCell, targetCell, userdata) + PathFinderModule.debugMsg(self.vehicle, "PFM:setupNew behindStartCell %s,%s startCell %s,%s targetCell %s,%s" + , tostring(behindStartCell.x) + , tostring(behindStartCell.z) + , tostring(startCell.x) + , tostring(startCell.z) + , tostring(targetCell.x) + , tostring(targetCell.z) + ) + self.cached_nodes = {} + self.openset = {} + self.closedset = {} + self.came_from = {} + self.g_score = {} + self.h_score = {} + self.f_score = {} + + self.nodeBehindStart = self:get_node(behindStartCell.x, behindStartCell.z) + self.nodeBehindStart.text = string.format("B %d,%d",behindStartCell.x, behindStartCell.z) + self.nodeBehindStart.direction = behindStartCell.direction + + self.nodeStart = self:get_node(startCell.x, startCell.z) + self.nodeStart.text = string.format("S %d,%d",startCell.x, startCell.z) + self.nodeStart.direction = startCell.direction + + self.nodeGoal = self:get_node(targetCell.x, targetCell.z) + self.nodeGoal.text = string.format("T %d,%d",targetCell.x, targetCell.z) + + self.g_score[self.nodeBehindStart] = math.huge + self.h_score[self.nodeBehindStart] = math.huge + self.f_score[self.nodeBehindStart] = math.huge + + self.g_score[self.nodeStart] = 0 + self.h_score[self.nodeStart] = self:estimate_cost(self.nodeStart, self.nodeGoal) + self.f_score[self.nodeStart] = self.h_score[self.nodeStart] + self.came_from[self.nodeStart] = self.nodeBehindStart + + self.openset[self.nodeStart] = true + self.closedset[self.nodeBehindStart] = true + self:isDriveable(self.nodeStart) + self:setBlockedGoal() + self.initNew = true +end + +function PathFinderModule:createWayPointsNew() + if self.smoothStep == 0 then + self.wayPoints = {} + for index, cell in ipairs(self.path) do + self.wayPoints[index] = self:gridLocationToWorldLocation(cell) + self.wayPoints[index].y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, self.wayPoints[index].x, 1, self.wayPoints[index].z) + self.wayPoints[index].direction = cell.direction + end + -- remove zig zag line + self:smoothResultingPPPath() + end + -- shortcut the path if possible + self:smoothResultingPPPath_Refined() + + if self.smoothStep == 2 then + -- When going to network, dont turn actual road network nodes into pathFinderPoints + if self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true + end + end + + if self.appendWayPoints ~= nil then + for i = 1, #self.appendWayPoints, 1 do + self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] + end + self.smoothStep = 3 + end + + -- See comment above + if not self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true + end + end + end +end + +function PathFinderModule:collisionTestCallback(transformId) + if transformId ~= 0 and transformId ~= g_currentMission.terrainRootNode then + local object = g_currentMission:getNodeObject(transformId) + if (object == nil) or (object ~= nil and not (object == self.vehicle or object.rootVehicle == self.vehicle)) then + self.collisionhits = self.collisionhits + 1 + end + end +end + +function PathFinderModule.debugMsg(vehicle, debugText, ...) + if PathFinderModule.debug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, debugText, ...) + end +end diff --git a/scripts/Tasks/CatchCombinePipeTask.lua b/scripts/Tasks/CatchCombinePipeTask.lua index 933a2188..79247e8e 100644 --- a/scripts/Tasks/CatchCombinePipeTask.lua +++ b/scripts/Tasks/CatchCombinePipeTask.lua @@ -199,6 +199,7 @@ function CatchCombinePipeTask:startNewPathFinding() -- if self.combine:getIsBufferCombine() or (pipeChaseSide ~= AutoDrive.CHASEPOS_REAR and targetFieldId == combineFieldId and cFillRatio <= 0.85) then -- is chopper or chase not rear and harvester on correct field and filled < 85% - i.e. combine pipe not in fruit CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks good - calculate path to it...") + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) -- use false to enable pathfinder fallback self.combinesStartLocation = {} @@ -244,8 +245,8 @@ end function CatchCombinePipeTask:getI18nInfo() local text = "$l10n_AD_task_catch_up_with_combine;" if self.state == CatchCombinePipeTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) elseif self.state == CatchCombinePipeTask.STATE_DELAY_PATHPLANNING then text = text .. " - " .. "$l10n_AD_task_unload_area_in_fruit;" elseif self.state == CatchCombinePipeTask.STATE_REVERSING then diff --git a/scripts/Tasks/DriveToDestinationTask.lua b/scripts/Tasks/DriveToDestinationTask.lua index 193b7a26..cff397eb 100644 --- a/scripts/Tasks/DriveToDestinationTask.lua +++ b/scripts/Tasks/DriveToDestinationTask.lua @@ -17,6 +17,7 @@ function DriveToDestinationTask:setUp() self.vehicle.ad.trainModule:setPathTo(self.destinationID) elseif ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = DriveToDestinationTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) else self.state = DriveToDestinationTask.STATE_DRIVING @@ -63,8 +64,8 @@ end function DriveToDestinationTask:getI18nInfo() if self.state == DriveToDestinationTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_destination;" end diff --git a/scripts/Tasks/DriveToVehicleTask.lua b/scripts/Tasks/DriveToVehicleTask.lua index 0f94e6f5..275c3186 100644 --- a/scripts/Tasks/DriveToVehicleTask.lua +++ b/scripts/Tasks/DriveToVehicleTask.lua @@ -17,6 +17,7 @@ function DriveToVehicleTask:new(vehicle, targetVehicle) end function DriveToVehicleTask:setUp() + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToVehicle(self.targetVehicle, DriveToVehicleTask.TARGET_DISTANCE) self.trailers, _ = AutoDrive.getAllUnits(self.vehicle) AutoDrive.setTrailerCoverOpen(self.vehicle, self.trailers, false) @@ -92,8 +93,8 @@ end function DriveToVehicleTask:getI18nInfo() if self.state == DriveToVehicleTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_vehicle;" end diff --git a/scripts/Tasks/EmptyHarvesterTask.lua b/scripts/Tasks/EmptyHarvesterTask.lua index b85f4c31..9dd32d06 100644 --- a/scripts/Tasks/EmptyHarvesterTask.lua +++ b/scripts/Tasks/EmptyHarvesterTask.lua @@ -30,6 +30,7 @@ end function EmptyHarvesterTask:setUp() EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:setUp") + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) self.trailers, self.trailerCount = AutoDrive.getAllUnits(self.vehicle) self.tractorTrainLength = AutoDrive.getTractorTrainLength(self.vehicle, true, false) @@ -59,6 +60,7 @@ function EmptyHarvesterTask:update(dt) return else self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) self.vehicle.ad.pathFinderModule:addDelayTimer(10000) end @@ -222,8 +224,8 @@ end function EmptyHarvesterTask:getI18nInfo() local text = "$l10n_AD_task_unloading_combine;" if self.state == EmptyHarvesterTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) elseif self.state == EmptyHarvesterTask.STATE_DRIVING then text = text .. " - " .. "$l10n_AD_task_drive_to_combine_pipe;" elseif self.state == EmptyHarvesterTask.STATE_UNLOADING then diff --git a/scripts/Tasks/ExitFieldTask.lua b/scripts/Tasks/ExitFieldTask.lua index d582b5db..2225b9dc 100644 --- a/scripts/Tasks/ExitFieldTask.lua +++ b/scripts/Tasks/ExitFieldTask.lua @@ -78,6 +78,7 @@ function ExitFieldTask:startPathPlanning() if closestDistance > AutoDrive.getDriverRadius(self.vehicle) then -- initiate pathFinder only if distance to closest wayPoint is enought to find a path local vecToNextPoint = {x = wayPoints[2].x - closestNode.x, z = wayPoints[2].z - closestNode.z} + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningTo(closestNode, vecToNextPoint) else -- close to network, set task finished @@ -97,6 +98,7 @@ function ExitFieldTask:startPathPlanning() targetNode = wayPoints[5] vecToNextPoint = {x = wayPoints[6].x - targetNode.x, z = wayPoints[6].z - targetNode.z} end + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningTo(targetNode, vecToNextPoint) else AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.WARN, "$l10n_AD_Driver_of; %s $l10n_AD_cannot_find_path;", 5000, self.vehicle.ad.stateModule:getName()) @@ -124,8 +126,8 @@ end function ExitFieldTask:getI18nInfo() if self.state == ExitFieldTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_exiting_field;" end diff --git a/scripts/Tasks/FollowCombineTask.lua b/scripts/Tasks/FollowCombineTask.lua index fb2cd577..88ef36be 100644 --- a/scripts/Tasks/FollowCombineTask.lua +++ b/scripts/Tasks/FollowCombineTask.lua @@ -116,8 +116,8 @@ function FollowCombineTask:update(dt) return end - if self.combine.ad.isHarvester and self.combineFillPercent > 90 - and AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE -- if to close -> reverse + if self.combine.ad.isHarvester and ((self.combineFillPercent > 90 and AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE) -- if to close -> reverse + or AutoDrive:getIsCPTurning(self.combine)) then -- Stop chasing and wait for a normal unload call while standing FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - to close to harvester -> reverse") @@ -402,6 +402,7 @@ function FollowCombineTask:startPathPlanningForCircling() local targetPos = AutoDrive.createWayPointRelativeToVehicle(self.vehicle, sideOffset, 0) local directionX, directionY, directionZ = localToWorld(self.vehicle.components[1].node, 0, 0, 0) local direction = {x = directionX - targetPos.x, z = directionZ - targetPos.z} + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningTo(targetPos, direction) end diff --git a/scripts/Tasks/LoadAtDestinationTask.lua b/scripts/Tasks/LoadAtDestinationTask.lua index 6cbb1f27..38056175 100644 --- a/scripts/Tasks/LoadAtDestinationTask.lua +++ b/scripts/Tasks/LoadAtDestinationTask.lua @@ -29,6 +29,7 @@ function LoadAtDestinationTask:setUp() --self.vehicle.ad.pathFinderModule:startPathPlanningToWayPoint(self.vehicle.ad.stateModule:getSecondWayPoint(), self.destinationID) --end --else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) --end else @@ -173,8 +174,8 @@ end function LoadAtDestinationTask:getI18nInfo() if self.state == LoadAtDestinationTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_load_point;" end diff --git a/scripts/Tasks/ParkTask.lua b/scripts/Tasks/ParkTask.lua index 640d5272..13e3ac21 100644 --- a/scripts/Tasks/ParkTask.lua +++ b/scripts/Tasks/ParkTask.lua @@ -38,6 +38,7 @@ function ParkTask:setUp() self.vehicle.ad.trainModule:setPathTo(targetParkParkDestination) elseif ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = ParkTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(targetParkParkDestination) else self.state = ParkTask.STATE_DRIVING @@ -93,8 +94,8 @@ end function ParkTask:getI18nInfo() if self.state == ParkTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) elseif self.vehicle.ad.onRouteToPark == true then return "$l10n_AD_task_drive_to_park;" else diff --git a/scripts/Tasks/RefuelTask.lua b/scripts/Tasks/RefuelTask.lua index 320265c2..fe510f46 100644 --- a/scripts/Tasks/RefuelTask.lua +++ b/scripts/Tasks/RefuelTask.lua @@ -17,6 +17,7 @@ function RefuelTask:setUp() self.matchingFillTypes = {} if ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = RefuelTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) else self.state = RefuelTask.STATE_DRIVING @@ -193,8 +194,8 @@ end function RefuelTask:getI18nInfo() if self.state == RefuelTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_refuel_point;" end diff --git a/scripts/Tasks/RepairTask.lua b/scripts/Tasks/RepairTask.lua index 3133c27e..ca639088 100644 --- a/scripts/Tasks/RepairTask.lua +++ b/scripts/Tasks/RepairTask.lua @@ -17,6 +17,7 @@ function RepairTask:setUp() self.repairTrigger = nil if ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = RepairTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) else self.state = RepairTask.STATE_DRIVING @@ -103,8 +104,8 @@ end function RepairTask:getI18nInfo() if self.state == RepairTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_repair_point;" end diff --git a/scripts/Tasks/UnloadAtDestinationTask.lua b/scripts/Tasks/UnloadAtDestinationTask.lua index 72695a4b..d7da9104 100644 --- a/scripts/Tasks/UnloadAtDestinationTask.lua +++ b/scripts/Tasks/UnloadAtDestinationTask.lua @@ -28,11 +28,14 @@ function UnloadAtDestinationTask:setUp() self.state = UnloadAtDestinationTask.STATE_PATHPLANNING if self.vehicle.ad.restartCP == true then if self.vehicle.ad.stateModule:getMode() == AutoDrive.MODE_PICKUPANDDELIVER then + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToWayPoint(self.vehicle.ad.stateModule:getFirstWayPoint(), self.destinationID) else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToWayPoint(self.vehicle.ad.stateModule:getSecondWayPoint(), self.destinationID) end else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) end else @@ -62,14 +65,17 @@ function UnloadAtDestinationTask:update(dt) self.wayPoints = self.vehicle.ad.pathFinderModule:getPath() if self.wayPoints == nil or #self.wayPoints == 0 then if self.vehicle.ad.pathFinderModule:isTargetBlocked() then - -- If the selected field exit isn't reachable, try the closest one + -- If the selected field exit isn't reachable, try the closest one + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.vehicle.ad.stateModule:getSecondWayPoint()) elseif self.vehicle.ad.pathFinderModule:timedOut() or self.vehicle.ad.pathFinderModule:isBlocked() then -- Add some delay to give the situation some room to clear itself self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.vehicle.ad.stateModule:getSecondWayPoint()) self.vehicle.ad.pathFinderModule:addDelayTimer(10000) else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.vehicle.ad.stateModule:getSecondWayPoint()) end @@ -247,8 +253,8 @@ end function UnloadAtDestinationTask:getI18nInfo() if self.state == UnloadAtDestinationTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_unload_point;" end From c705fa08b902b39a5dd5d1711fd96b83a906f1cb Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Wed, 17 Jul 2024 21:44:48 +0200 Subject: [PATCH 18/44] small fix --- scripts/Modules/PathFinderModule.lua | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index f9447250..706683e4 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -2253,9 +2253,9 @@ end function PathFinderModule:drawDebugNewPF() if self.closedset then - for k, node in pairs(self.closedset) do + for k, _ in pairs(self.closedset) do if k.text then - local point = self:gridLocationToWorldLocation({k.x, k.z}) + local point = self:gridLocationToWorldLocation({x = k.x, z = k.z}) point.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, point.x, 1, point.z) Utils.renderTextAtWorldPosition(point.x, point.y + 4, point.z, tostring(k.text), getCorrectTextSize(0.013), 0) end From a715b92a5e103b9bab1178e054bc8f4ac8bd4437 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Wed, 17 Jul 2024 22:03:13 +0200 Subject: [PATCH 19/44] add printCallstack in nil check --- scripts/Utils/CollisionDetectionUtils.lua | 3 +++ 1 file changed, 3 insertions(+) diff --git a/scripts/Utils/CollisionDetectionUtils.lua b/scripts/Utils/CollisionDetectionUtils.lua index 6117d34c..4cf439d0 100644 --- a/scripts/Utils/CollisionDetectionUtils.lua +++ b/scripts/Utils/CollisionDetectionUtils.lua @@ -150,6 +150,9 @@ function AutoDrive.getBoundingBoxForVehicle(vehicle, force) end function AutoDrive.getDistanceBetween(vehicleOne, vehicleTwo) + if vehicleOne == nil or vehicleTwo == nil then + printCallstack() + end local x1, _, z1 = getWorldTranslation(vehicleOne.components[1].node) local x2, _, z2 = getWorldTranslation(vehicleTwo.components[1].node) From 6c07e864eb433fedf491f3772f27d8836036f695 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Thu, 18 Jul 2024 22:39:51 +0200 Subject: [PATCH 20/44] add collision debug --- scripts/Modules/PathFinderModule.lua | 33 ++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 706683e4..504c0aee 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -333,6 +333,10 @@ function PathFinderModule:startPathPlanningToVehicle(targetVehicle, targetDistan end function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo self.minTurnRadius %.1f" + , self.minTurnRadius + ) + PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningTo targetPoint x,z %d %d", math.floor(targetPoint.x), @@ -2640,10 +2644,35 @@ end function PathFinderModule:collisionTestCallback(transformId) if transformId ~= 0 and transformId ~= g_currentMission.terrainRootNode then - local object = g_currentMission:getNodeObject(transformId) - if (object == nil) or (object ~= nil and not (object == self.vehicle or object.rootVehicle == self.vehicle)) then + local collisionObject = g_currentMission:getNodeObject(transformId) + if (collisionObject == nil) or (collisionObject ~= nil and not (collisionObject.rootVehicle == self.vehicle)) then self.collisionhits = self.collisionhits + 1 end + if PathFinderModule.debug == true then + local currentCollMask = getCollisionMask(transformId) + if currentCollMask then + local x, _, z = getWorldTranslation(self.vehicle.components[1].node) + x = x + g_currentMission.mapWidth/2 + z = z + g_currentMission.mapHeight/2 + + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback transformId ->%s<- collisionObject ->%s<- getRigidBodyType->transformId %s getName->transformId %s getNodePath %s" + , tostring(transformId) + , tostring(collisionObject) + , tostring(getRigidBodyType(transformId)) + , tostring(getName(transformId)) + , tostring(I3DUtil.getNodePath(transformId)) + ) + if collisionObject then + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback collisionObject type ->%s<-" + , tostring(type(collisionObject)) + ) + end + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:collisionTestCallback xz %.0f %.0f currentCollMask %s" + , x, z + , MathUtil.numberToSetBitsStr(currentCollMask) + ) + end + end end end From cdade62a78563c219888dc86e7318b1b593a73aa Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Fri, 19 Jul 2024 21:32:36 +0200 Subject: [PATCH 21/44] fix lua error --- scripts/Tasks/ClearCropTask.lua | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/scripts/Tasks/ClearCropTask.lua b/scripts/Tasks/ClearCropTask.lua index 496dc0ec..2bc91b70 100644 --- a/scripts/Tasks/ClearCropTask.lua +++ b/scripts/Tasks/ClearCropTask.lua @@ -51,11 +51,13 @@ function ClearCropTask:setUp() self.wayPoints = {} - local distance = AutoDrive.getDistanceBetween(self.vehicle, self.harvester) - ClearCropTask.debugMsg(self.harvester, "ClearCropTask:setUp distance %.0f" - , distance - ) - if self.harvester and distance < ClearCropTask.MAX_HARVESTER_DISTANCE then + if self.harvester then + local distance = AutoDrive.getDistanceBetween(self.vehicle, self.harvester) + ClearCropTask.debugMsg(self.harvester, "ClearCropTask:setUp distance %.0f" + , distance + ) + end + if self.harvester and AutoDrive.getDistanceBetween(self.vehicle, self.harvester) < ClearCropTask.MAX_HARVESTER_DISTANCE then table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 1)) table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 2)) table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 3)) From 1e6d1259c0877dfdd161f6338751f76b699a0a01 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 21 Jul 2024 09:58:22 +0200 Subject: [PATCH 22/44] implement #1154 --- gui/userSettingsPage.xml | 8 ++++++++ scripts/Settings.lua | 12 ++++++++++++ scripts/Specialization.lua | 3 ++- translations/translation_br.xml | 2 ++ translations/translation_cs.xml | 2 ++ translations/translation_cz.xml | 2 ++ translations/translation_de.xml | 2 ++ translations/translation_en.xml | 2 ++ translations/translation_es.xml | 2 ++ translations/translation_fr.xml | 2 ++ translations/translation_hu.xml | 2 ++ translations/translation_it.xml | 2 ++ translations/translation_nl.xml | 2 ++ translations/translation_pl.xml | 2 ++ translations/translation_pt.xml | 2 ++ translations/translation_ru.xml | 2 ++ translations/translation_tr.xml | 2 ++ 17 files changed, 50 insertions(+), 1 deletion(-) diff --git a/gui/userSettingsPage.xml b/gui/userSettingsPage.xml index 746cf513..3063fd4d 100644 --- a/gui/userSettingsPage.xml +++ b/gui/userSettingsPage.xml @@ -114,6 +114,14 @@ + + + + + + + + diff --git a/scripts/Settings.lua b/scripts/Settings.lua index 8321816a..0ddbc8b3 100644 --- a/scripts/Settings.lua +++ b/scripts/Settings.lua @@ -1047,6 +1047,18 @@ AutoDrive.settings.scaleLines = { isUserSpecific = true } +AutoDrive.settings.scaleMarkerText = { + values = {0.5, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}, + texts = {"50%", "100%", "200%", "300%", "400%", "500%", "600%", "700%", "800%", "900%", "1000%"}, + default = 2, + current = 2, + text = "gui_ad_scaleMarkerText", + tooltip = "gui_ad_scaleMarkerText_tooltip", + translate = false, + isVehicleSpecific = false, + isUserSpecific = true +} + AutoDrive.settings.remainingDriveTimeInterval = { values = {0, 1, 3, 5, 10, 20, 30, 60}, texts = {"gui_ad_off", "1s", "3s", "5s", "10s", "20s", "30s", "60s"}, diff --git a/scripts/Specialization.lua b/scripts/Specialization.lua index 7e15817e..2796be7b 100644 --- a/scripts/Specialization.lua +++ b/scripts/Specialization.lua @@ -818,7 +818,8 @@ function AutoDrive:onDrawEditorMode() for _, marker in pairs(ADGraphManager:getMapMarkers()) do local wp = ADGraphManager:getWayPointById(marker.id) if MathUtil.vector2Length(wp.x - x1, wp.z - z1) < maxDistance then - Utils.renderTextAtWorldPosition(wp.x, wp.y + 4, wp.z, marker.name, getCorrectTextSize(0.013), 0) + local scale = AutoDrive.getSetting("scaleMarkerText") or 1 + Utils.renderTextAtWorldPosition(wp.x, wp.y + 4, wp.z, marker.name, getCorrectTextSize(0.013) * scale, 0) DrawingManager:addMarkerTask(wp.x, wp.y + 0.45, wp.z) end end diff --git a/translations/translation_br.xml b/translations/translation_br.xml index 7f6ce6ed..6d3700fd 100644 --- a/translations/translation_br.xml +++ b/translations/translation_br.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_cs.xml b/translations/translation_cs.xml index 96ed7360..d8f42fce 100644 --- a/translations/translation_cs.xml +++ b/translations/translation_cs.xml @@ -224,6 +224,8 @@ + + diff --git a/translations/translation_cz.xml b/translations/translation_cz.xml index e1f48dbc..f68307f3 100644 --- a/translations/translation_cz.xml +++ b/translations/translation_cz.xml @@ -224,6 +224,8 @@ + + diff --git a/translations/translation_de.xml b/translations/translation_de.xml index 6909a75b..fbabbbea 100644 --- a/translations/translation_de.xml +++ b/translations/translation_de.xml @@ -228,6 +228,8 @@ + + diff --git a/translations/translation_en.xml b/translations/translation_en.xml index 2f3a8a95..80ca8695 100644 --- a/translations/translation_en.xml +++ b/translations/translation_en.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_es.xml b/translations/translation_es.xml index 43d1c42c..96c82a64 100644 --- a/translations/translation_es.xml +++ b/translations/translation_es.xml @@ -220,6 +220,8 @@ + + diff --git a/translations/translation_fr.xml b/translations/translation_fr.xml index 50669231..3b89b7bc 100644 --- a/translations/translation_fr.xml +++ b/translations/translation_fr.xml @@ -224,6 +224,8 @@ + + diff --git a/translations/translation_hu.xml b/translations/translation_hu.xml index 0adef702..729525ce 100644 --- a/translations/translation_hu.xml +++ b/translations/translation_hu.xml @@ -221,6 +221,8 @@ + + diff --git a/translations/translation_it.xml b/translations/translation_it.xml index 6ec061b6..9f1b903d 100644 --- a/translations/translation_it.xml +++ b/translations/translation_it.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_nl.xml b/translations/translation_nl.xml index b50a3eb4..18d2fd5d 100644 --- a/translations/translation_nl.xml +++ b/translations/translation_nl.xml @@ -224,6 +224,8 @@ + + diff --git a/translations/translation_pl.xml b/translations/translation_pl.xml index e71d3ea4..3b41f2bf 100644 --- a/translations/translation_pl.xml +++ b/translations/translation_pl.xml @@ -223,6 +223,8 @@ + + diff --git a/translations/translation_pt.xml b/translations/translation_pt.xml index cde53af4..0e2de0b3 100644 --- a/translations/translation_pt.xml +++ b/translations/translation_pt.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_ru.xml b/translations/translation_ru.xml index 4df34ae5..46b3f517 100644 --- a/translations/translation_ru.xml +++ b/translations/translation_ru.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_tr.xml b/translations/translation_tr.xml index bbb2e135..fd60b65f 100644 --- a/translations/translation_tr.xml +++ b/translations/translation_tr.xml @@ -220,6 +220,8 @@ + + From 7505e3f61be0cd6059594ed0b68facb80be3f1cc Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Mon, 22 Jul 2024 20:37:48 +0200 Subject: [PATCH 23/44] TWEAK: Pathfinder with Dubins Curves - added dubins curves for small distances before continue with Astar - first try to find a shorest dubins solution, if not successful try all 6 solutions - use also check for field, fruit, collisions etc. on dubins path --- modDesc.xml | 2 +- register.lua | 1 + scripts/AutoDrive.lua | 2 +- scripts/Modules/PathFinderModule.lua | 530 +++++++++++++++++++++++---- scripts/Utils/Dubins.lua | 428 +++++++++++++++++++++ 5 files changed, 891 insertions(+), 72 deletions(-) create mode 100644 scripts/Utils/Dubins.lua diff --git a/modDesc.xml b/modDesc.xml index 84b20976..10d79dfc 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -32,7 +32,7 @@ Différents modes d'utilisation ont été ajoutés depuis les premières version - 2.0.1.7-RC3 + 2.0.1.7-RC4 icon.dds diff --git a/register.lua b/register.lua index 02e6797d..fdef7964 100644 --- a/register.lua +++ b/register.lua @@ -64,6 +64,7 @@ source(Utils.getFilename("scripts/Utils/Buffer.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/FlaggedTable.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/CollisionDetectionUtils.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/PathFinderUtils.lua", g_currentModDirectory)) +source(Utils.getFilename("scripts/Utils/Dubins.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/AutoDriveUtilFuncs.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/SortedQueue.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/DevFuncs.lua", g_currentModDirectory)) diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index ba957160..dcccb966 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -1,5 +1,5 @@ AutoDrive = {} -AutoDrive.version = "2.0.1.7-RC3" +AutoDrive.version = "2.0.1.7-RC4" AutoDrive.directory = g_currentModDirectory diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 504c0aee..415b8b9d 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -69,6 +69,7 @@ PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE = 16 PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE_CLOSE = 6 PathFinderModule.PATHFINDER_START_DISTANCE = 7 PathFinderModule.MAX_FIELDBORDER_CELLS = 5 +PathFinderModule.PATHFINDER_MIN_DISTANCE_START_TARGET = 50 PathFinderModule.PP_MIN_DISTANCE = 20 PathFinderModule.PP_CELL_X = 9 @@ -92,6 +93,7 @@ function PathFinderModule:new(vehicle) setmetatable(o, self) self.__index = self o.vehicle = vehicle + o.dubins = ADDubins:new() PathFinderModule.reset(o) return o end @@ -138,6 +140,8 @@ function PathFinderModule:reset() "unknown" } self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) + self.dubinsDone = false + self.dubinsCount = 0 self.isNewPF = true else self.PP_UP = 0 @@ -179,6 +183,9 @@ function PathFinderModule:getPath() end function PathFinderModule:startPathPlanningToNetwork(destinationId) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToNetwork destinationId %s" + , tostring(destinationId) + ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToNetwork destinationId %s", tostring(destinationId) @@ -190,6 +197,9 @@ function PathFinderModule:startPathPlanningToNetwork(destinationId) end function PathFinderModule:startPathPlanningToWayPoint(wayPointId, destinationId) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToWayPoint destinationId %s" + , tostring(destinationId) + ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToWayPoint wayPointId %s", tostring(wayPointId) @@ -209,6 +219,9 @@ function PathFinderModule:startPathPlanningToWayPoint(wayPointId, destinationId) end function PathFinderModule:startPathPlanningToPipe(combine, chasing) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToPipe chasing %s" + , tostring(chasing) + ) AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:startPathPlanningToPipe") PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToPipe combine %s", @@ -312,6 +325,9 @@ function PathFinderModule:startPathPlanningToPipe(combine, chasing) end function PathFinderModule:startPathPlanningToVehicle(targetVehicle, targetDistance) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToVehicle targetDistance %s" + , tostring(targetDistance) + ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToVehicle targetVehicle %s", tostring(targetVehicle:getName()) @@ -372,6 +388,7 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) self.fruitToCheck = nil + self.start = {x = self.startX, z = self.startZ} self.startCell = {x = 0, z = 0} self.startCell.direction = self:worldDirectionToGridDirection(vehicleVector) self.startCell.visited = false @@ -387,6 +404,7 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) local vehicleBehindVector = {x = vehicleBehindX, z = vehicleBehindZ} self.behindStartCell = self:worldLocationToGridLocation(vehicleWorldX + vehicleBehindX, vehicleWorldZ + vehicleBehindZ) self.behindStartCell.direction = self:worldDirectionToGridDirection(vehicleBehindVector, vehicleVector) + self.behind = {x = vehicleWorldX + vehicleBehindX, z = vehicleWorldZ + vehicleBehindZ} -- table.insert(self.grid, self.startCell) local gridKey = string.format("%d|%d|%d", self.startCell.x, self.startCell.z, self.startCell.direction) @@ -434,7 +452,40 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) -- else -- self.reachedFieldBorder = true -- end + + self.q0 = { + vehicleWorldX + , -vehicleWorldZ + , AutoDrive.normalizeAngle(math.atan2(vehicleRx, vehicleRz) + math.pi + math.pi / 2) + } + + self.q1 = { + targetX + , -targetZ + , AutoDrive.normalizeAngle(math.atan2(targetVector.x, targetVector.z) + math.pi + math.pi / 2) + } + + self:setupNew(self.behindStartCell, self.startCell,self.targetCell) + self.chainStartToTarget = {} + + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo vehicleWorldX xz %.1f,%.1f" + , vehicleWorldX, vehicleWorldZ + ) + + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo targetX xz %.1f,%.1f" + , targetX, targetZ + ) + + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo self.behind xz %.1f,%.1f" + , self.behind.x, self.behind.z + ) + + local location = self:gridLocationToWorldLocation(self.behindStartCell) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo behindStartCell location xz %.1f,%.1f" + , location.x, location.z + ) + PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningTo self.minTurnRadius %s vectorX.x,vectorX.z %s %s vectorZ.x,vectorZ.z %s %s", tostring(self.minTurnRadius), @@ -604,9 +655,9 @@ function PathFinderModule:update(dt) end if self.smoothDone then ADScheduler:removePathfinderVehicle(self.vehicle) - PathFinderModule.debugMsg(self.vehicle, "PFM:update Complete #self.path %s" - , tostring(#self.path) - ) + -- PathFinderModule.debugMsg(self.vehicle, "PFM:update Complete #self.path %s" + -- , tostring(#self.path) + -- ) end return end @@ -711,6 +762,40 @@ function PathFinderModule:update(dt) if self.isNewPF then if not self.isFinished then + if MathUtil.vector2Length(self.start.x - self.target.x, self.start.z - self.target.z) < PathFinderModule.PATHFINDER_MIN_DISTANCE_START_TARGET then + -- try dubins first before full pathfinder if close to target + if not self.dubinsDone then + self.dubinsCount = self.dubinsCount + 1 + local dubinsPath = self:getDubinsPath() + PathFinderModule.debugMsg(self.vehicle, "PFM:update getDubinsPath dubinsPath %s" + , tostring(dubinsPath) + ) + if dubinsPath then + self.dubinsDone = true + self.wayPoints = dubinsPath + self.isFinished = true + self.smoothDone = true + return -- found path + else + -- self.completelyBlocked = true + -- no valid path + -- PathFinderModule.debugMsg(self.vehicle, "PFM:update getDubinsPath self.completelyBlocked %s" + -- , tostring(self.completelyBlocked) + -- ) + end + PathFinderModule.debugMsg(self.vehicle, "PFM:update getDubinsPath self.fallBackMode3 %s" + , tostring(self.fallBackMode3) + ) + if self.fallBackMode3 or self.dubinsCount > 4 then + self.dubinsDone = true + -- self.completelyBlocked = false + -- self.fallBackMode1 = false -- disable restrict to field + -- self.fallBackMode2 = false -- disable restrict to field border + -- self.fallBackMode3 = false -- disable avoid fruit + end + -- return + end + end if not self.initNew then self:setupNew(self.behindStartCell, self.startCell,self.targetCell) if self.nodeBehindStart == self.nodeGoal then @@ -722,7 +807,7 @@ function PathFinderModule:update(dt) local current local add_neighbor_fn = function(neighbor, cost) - if self:isDriveable(neighbor) then + if self:isDriveableAstar(neighbor) then if not self.closedset[neighbor] then if not cost then cost = self:get_cost(current, neighbor) end local tentative_g_score = self.g_score[current] + cost @@ -1426,28 +1511,28 @@ function PathFinderModule:drawDebugForPF() if cell.isRestricted == true then -- any restriction if cell.hasFruit == true then - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 1) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 1) -- cyan Utils.renderTextAtWorldPosition(pointCenter.x, pointB.y + 0.4, pointCenter.z, tostring(cell.fruitValue), getCorrectTextSize(0.013), 0) else - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) -- red end else - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) -- green end if cell.hasCollision == true then -- ground collision, slope, water if cell.hasVehicleCollision then -- TODO: hasVehicleCollision ??? - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) -- blue + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) -- blue else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) -- yellow + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) -- yellow end else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) -- magenta + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) -- magenta end for i = 0, 10, 1 do @@ -1511,11 +1596,11 @@ function PathFinderModule:drawDebugForPF() pointTarget.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTarget.x, 1, pointTarget.z) + 6 pointTargetUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetUp.x, 1, pointTargetUp.z) + 10 if cell.bordercells == 1 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 1, 0, 0) -- red elseif cell.bordercells == 2 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 1, 0) + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 1, 0) -- green elseif cell.bordercells > 2 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) -- blue end end --[[ @@ -1558,8 +1643,8 @@ function PathFinderModule:drawDebugForPF() pointD.z = pointD.z - self.vectorX.z * size + self.vectorZ.z * size pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 3 - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 1, 1) - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 1, 1) -- white + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) -- white local pointAB = self:gridLocationToWorldLocation(self.targetCell) pointAB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointAB.x, 1, pointAB.z) + 3 @@ -1568,7 +1653,7 @@ function PathFinderModule:drawDebugForPF() pointTargetVector.x = pointTargetVector.x + self.targetVector.x * 10 pointTargetVector.z = pointTargetVector.z + self.targetVector.z * 10 pointTargetVector.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetVector.x, 1, pointTargetVector.z) + 3 - AutoDriveDM:addLineTask(pointAB.x, pointAB.y, pointAB.z, pointTargetVector.x, pointTargetVector.y, pointTargetVector.z, 1, 1, 1, 1) + AutoDriveDM:addLineTask(pointAB.x, pointAB.y, pointAB.z, pointTargetVector.x, pointTargetVector.y, pointTargetVector.z, 1, 1, 1, 1) -- white end function PathFinderModule:drawDebugForCreatedRoute() @@ -2256,27 +2341,148 @@ function PathFinderModule.debugVehicleMsg(vehicle, msg) end function PathFinderModule:drawDebugNewPF() - if self.closedset then - for k, _ in pairs(self.closedset) do - if k.text then - local point = self:gridLocationToWorldLocation({x = k.x, z = k.z}) - point.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, point.x, 1, point.z) - Utils.renderTextAtWorldPosition(point.x, point.y + 4, point.z, tostring(k.text), getCorrectTextSize(0.013), 0) + if self.cachedNodes and #self.cachedNodes > 0 then + for z, row in pairs(self.cachedNodes) do + for x, node in pairs(row) do + -- cell outline + local gridFactor = PathFinderModule.GRID_SIZE_FACTOR + if self.isSecondChasingVehicle then + gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER + end + local corners = self:getCorners(node, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) + local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corners[1].x, 1, corners[1].z) + if node.isOnField then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 0, 1, 0) -- green + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) -- red + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) + end + if node.isRestricted then + if node.hasFruit then + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 1) -- cyan + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) -- red + end + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) -- green + end + if node.hasCollision then + if node.hasVehicleCollision then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 1) -- blue + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 1, 0) -- yellow + end + end + + -- cell text + if node.text == nil then + node.text = string.format("%d,%d", x, z) + end + local point = self:gridLocationToWorldLocation({x = node.x, z = node.z}) + Utils.renderTextAtWorldPosition(point.x, tempY + 3, point.z, node.text, getCorrectTextSize(0.013), 0) + + -- behind point + if node.isBehind then + local text = string.format("B %d,%d", x, z) + Utils.renderTextAtWorldPosition(point.x, tempY + 4, point.z, text, getCorrectTextSize(0.013), 0) + ADDrawingManager:addSphereTask(self.behind.x, tempY + 3, self.behind.z, 6, 0, 0, 1, 0) -- blue + end + + -- start point + if node.isStart then + local text = string.format("S %d,%d", x, z) + Utils.renderTextAtWorldPosition(point.x, tempY + 5, point.z, text, getCorrectTextSize(0.013), 0) + ADDrawingManager:addSphereTask(self.startX, tempY + 3, self.startZ, 6, 0, 1, 0, 0) -- green + end + + -- goal point + if node.isGoal then + local text = string.format("T %d,%d", x, z) + Utils.renderTextAtWorldPosition(point.x, tempY + 6, point.z, text, getCorrectTextSize(0.013), 0) + ADDrawingManager:addSphereTask(self.target.x, tempY + 3, self.target.z, 6, 1, 0, 0, 0) -- red + end end end end - if self.fruitAreas then - for _, corners in ipairs(self.fruitAreas) do - local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corners[1].x, 1, corners[1].z) - ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) - ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) - ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) - ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) + + if self.dubinsPath and #self.dubinsPath > 0 then + + local lastPoint = nil + for index, point in ipairs(self.dubinsPath) do + if lastPoint ~= nil then + ADDrawingManager:addLineTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + + if AutoDrive.getSettingState("lineHeight") == 1 then + local gy = point.y - AutoDrive.drawHeight + 4 + local ty = lastPoint.y - AutoDrive.drawHeight + 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + else + local gy = point.y - AutoDrive.drawHeight - 4 + local ty = lastPoint.y - AutoDrive.drawHeight - 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + end + end + lastPoint = point + end + end + if self.dubinsNodes then + local i = 0 + for z, row in pairs(self.dubinsNodes) do + for x, node in pairs(row) do + local corners = node.corners + i = i + 1 + local text = string.format("%d",i) + -- Utils.renderTextAtWorldPosition(x, node.worldPos.y + 3, z, text, getCorrectTextSize(0.013), 0) + local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, node.corners[1].x, 1, node.corners[1].z) + if node.isOnField then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 0, 1, 0) -- green + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) -- red + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) + end + if node.isRestricted then + if node.hasFruit then + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 1) -- cyan + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) -- red + end + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) -- green + end + if node.hasCollision then + if node.hasVehicleCollision then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 1) -- blue + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 1, 0) -- yellow + end + end + if node.fruitValue and node.fruitValue > 0 then + local text = string.format("%d",node.fruitValue) + Utils.renderTextAtWorldPosition(x, node.worldPos.y + 3, z, text, getCorrectTextSize(0.013), 0) + end + end end end end -function PathFinderModule:isDriveable(cell) +function PathFinderModule:isDriveableAstar(cell) cell.isRestricted = false cell.incoming = cell.from_node cell.hasCollision = false @@ -2291,7 +2497,7 @@ function PathFinderModule:isDriveable(cell) -- in fallBackMode1 we ignore the field restriction cell.isRestricted = cell.isRestricted or (not cell.isOnField) if not cell.isOnField then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable not cell.isOnField xz %d,%d " + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar not cell.isOnField xz %d,%d " , cell.x, cell.z ) end @@ -2308,7 +2514,7 @@ function PathFinderModule:isDriveable(cell) self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found table.insert(self.fruitAreas, corners) if cell.isRestricted then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.isRestricted xz %d,%d fruit found %s" + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar cell.isRestricted xz %d,%d fruit found %s" , cell.x, cell.z , self.fruitToCheck ) @@ -2323,7 +2529,7 @@ function PathFinderModule:isDriveable(cell) cell.hasCollision = cell.hasCollision or angelToSlope cell.isRestricted = cell.isRestricted or cell.hasCollision if angelToSlope then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable angelToSlope xz %d,%d" + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar angelToSlope xz %d,%d" , cell.x, cell.z ) end @@ -2337,7 +2543,7 @@ function PathFinderModule:isDriveable(cell) cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) cell.isRestricted = cell.isRestricted or cell.hasCollision if cell.hasCollision then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.hasCollision xz %d,%d collision" + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar cell.hasCollision xz %d,%d collision" , cell.x, cell.z ) end @@ -2353,7 +2559,7 @@ function PathFinderModule:isDriveable(cell) cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath if cellUsedByVehiclePath then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cellUsedByVehiclePath xz %d,%d vehicle" + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar cellUsedByVehiclePath xz %d,%d vehicle" , cell.x, cell.z ) end @@ -2406,13 +2612,10 @@ end -- Node must be able to check if they are the same -- so the example cannot directly return a different table for same coord function PathFinderModule:get_node(x, z) - local row = self.cached_nodes[z] - if not row then row = {}; self.cached_nodes[z] = row end + local row = self.cachedNodes[z] + if not row then row = {}; self.cachedNodes[z] = row end local node = row[x] if not node then node = { x = x, z = z, cost = 0 }; row[x] = node end - if node.text == nil then - node.text = string.format("%d,%d", x, z) - end return node end @@ -2547,6 +2750,7 @@ function PathFinderModule:setBlockedGoal() if not (self.targetCell.x == (x + offset[1]) and self.targetCell.z == (z + offset[2])) then local tnode = self:get_node(x + offset[1], z + offset[2]) tnode.text = string.format("G %d,%d",x + offset[1], z + offset[2]) + tnode.isBlockedGoal = true self.closedset[tnode] = true end end @@ -2569,7 +2773,7 @@ function PathFinderModule:setupNew(behindStartCell, startCell, targetCell, userd , tostring(targetCell.x) , tostring(targetCell.z) ) - self.cached_nodes = {} + self.cachedNodes = {} self.openset = {} self.closedset = {} self.came_from = {} @@ -2578,15 +2782,16 @@ function PathFinderModule:setupNew(behindStartCell, startCell, targetCell, userd self.f_score = {} self.nodeBehindStart = self:get_node(behindStartCell.x, behindStartCell.z) - self.nodeBehindStart.text = string.format("B %d,%d",behindStartCell.x, behindStartCell.z) + self.nodeBehindStart.isBehind = true self.nodeBehindStart.direction = behindStartCell.direction self.nodeStart = self:get_node(startCell.x, startCell.z) - self.nodeStart.text = string.format("S %d,%d",startCell.x, startCell.z) + self.nodeStart.isStart = true self.nodeStart.direction = startCell.direction self.nodeGoal = self:get_node(targetCell.x, targetCell.z) - self.nodeGoal.text = string.format("T %d,%d",targetCell.x, targetCell.z) + self.nodeGoal.isGoal = true + self.nodeGoal.direction = targetCell.direction self.g_score[self.nodeBehindStart] = math.huge self.h_score[self.nodeBehindStart] = math.huge @@ -2599,7 +2804,7 @@ function PathFinderModule:setupNew(behindStartCell, startCell, targetCell, userd self.openset[self.nodeStart] = true self.closedset[self.nodeBehindStart] = true - self:isDriveable(self.nodeStart) + self:isDriveableAstar(self.nodeStart) self:setBlockedGoal() self.initNew = true end @@ -2642,35 +2847,220 @@ function PathFinderModule:createWayPointsNew() end end +function PathFinderModule:isDriveableDubins(cell) + cell.isRestricted = false + cell.hasCollision = false + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins start xz %d,%d " + , cell.x, cell.z + ) + + --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped + + cell.isOnField = AutoDrive.checkIsOnField(cell.worldPos.x, 0, cell.worldPos.z) + + -- check the most probable restrictions on field first to prevent unneccessary checks + if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 or self.fallBackMode2) then + cell.isRestricted = cell.isRestricted or (not cell.isOnField) + if not cell.isOnField then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins not cell.isOnField xz %d,%d " + , cell.x, cell.z + ) + end + end + + local angleRad = AutoDrive.normalizeAngle(cell.t) + local sizeMax = self.vehicle.size.width / 2 + local vectorX = {x = math.cos(angleRad) * sizeMax, z = math.sin(angleRad) * sizeMax} + local vectorZ = {x = - math.sin(angleRad) * sizeMax, z = math.cos(angleRad) * sizeMax} + + local corners = {} + local centerLocation = cell.worldPos + corners[1] = {x = centerLocation.x + (-vectorX.x - vectorZ.x), z = centerLocation.z + (-vectorX.z - vectorZ.z)} + corners[2] = {x = centerLocation.x + (vectorX.x - vectorZ.x), z = centerLocation.z + (vectorX.z - vectorZ.z)} + corners[3] = {x = centerLocation.x + (-vectorX.x + vectorZ.x), z = centerLocation.z + (-vectorX.z + vectorZ.z)} + corners[4] = {x = centerLocation.x + (vectorX.x + vectorZ.x), z = centerLocation.z + (vectorX.z + vectorZ.z)} + cell.corners = corners + + if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then + -- check for fruit + self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found + if cell.isRestricted then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins cell.isRestricted xz %d,%d fruit found %s" + , cell.x, cell.z + , self.fruitToCheck + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + -- check for up/down is to big or below water level + local worldPosPrevious = cell.incoming.worldPos + local angelToSlope, angle = self:checkSlopeAngle(cell.worldPos.x, cell.worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level + cell.angle = angle + cell.hasCollision = cell.hasCollision or angelToSlope + cell.isRestricted = cell.isRestricted or cell.hasCollision + if angelToSlope then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins angelToSlope xz %d,%d" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted then + -- check for obstacles + self.collisionhits = 0 + local shapes = overlapBox(cell.worldPos.x, cell.worldPos.y + 3, cell.worldPos.z, 0, cell.t, 0, sizeMax, 5, sizeMax, "collisionTestCallback", self, self.mask, true, true, true) + cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) + cell.isRestricted = cell.isRestricted or cell.hasCollision + if cell.hasCollision then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins cell.hasCollision xz %d,%d collision" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + local worldPosPrevious = cell.incoming.worldPos + local vectorX = worldPosPrevious.x - cell.worldPos.x + local vectorZ = worldPosPrevious.z - cell.worldPos.z + local dirVec = { x=vectorX, z = vectorZ} + + local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) + cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath + self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath + if cellUsedByVehiclePath then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins cellUsedByVehiclePath xz %d,%d vehicle" + , cell.x, cell.z + ) + end + end + if cell.isRestricted then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins end isRestricted xz %d,%d " + , cell.x, cell.z + ) + end + return not(cell.isRestricted) +end + +function PathFinderModule:getDubinsPath() + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath start") + self.dubinsPath = nil + local result = ADDubins.EDUBNOPATH + self.dubinsNodes = {} + local diffNetTime = netGetTime() + + local function get_node(x, z) + local row = self.dubinsNodes[z] + if not row then row = {}; self.dubinsNodes[z] = row end + local node = row[x] + if not node then node = { x = x, z = z, cost = 0 }; row[x] = node end + return node + end + + local function checkPath() + local result = self.dubins:dubins_path_sample_many(ADDubins.DubinsPath, 1, self.dubins.createWayPoints) + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_path_sample_many result %d" + , result + ) + if result == ADDubins.EDUBOK then + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_path_sample_many #self.dubins.outPath %d" + , #self.dubins.outPath + ) + if self.dubins.outPath and #self.dubins.outPath > 0 then + local fromCell = nil + for i, wayPoint in ipairs(self.dubins.outPath) do + local cell = get_node(wayPoint.x, wayPoint.z) + cell.worldPos = {x = wayPoint.x, y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, wayPoint.x, 1, wayPoint.z), z = wayPoint.z} + cell.t = wayPoint.t + cell.incomming = fromCell + fromCell = cell + if not self:isDriveableDubins(cell) then + result = ADDubins.EDUBNOPATH + break + end + end + else + result = ADDubins.EDUBNOPATH + end + end + return result + end + + self.dubins.outPath = {} + result = self.dubins:dubins_shortest_path(ADDubins.DubinsPath, self.q0, self.q1, self.minTurnRadius) + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_shortest_path result %d" + , result + ) + if result == ADDubins.EDUBOK then + result = checkPath() + if result == ADDubins.EDUBOK then + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath found shortest path #self.dubins.outPath %d result %d" + , #self.dubins.outPath + , result + ) + self.dubinsPath = self.dubins.outPath + return self.dubinsPath + end + end + + for i = 1, 6, 1 do + self.dubins.outPath = {} + result = self.dubins:dubins_path(ADDubins.DubinsPath, self.q0, self.q1, self.minTurnRadius, i) + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_path i %d result %d" + , i + , result + ) + if result == ADDubins.EDUBOK then + result = checkPath() + end + if result == ADDubins.EDUBOK then + break + end + end + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath #self.dubins.outPath %d result %d" + , #self.dubins.outPath + , result + ) + if result == ADDubins.EDUBOK then + self.dubinsPath = self.dubins.outPath + end + + diffNetTime = netGetTime() - diffNetTime + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath end diffNetTime %d" + , diffNetTime + ) + return self.dubinsPath +end + function PathFinderModule:collisionTestCallback(transformId) if transformId ~= 0 and transformId ~= g_currentMission.terrainRootNode then local collisionObject = g_currentMission:getNodeObject(transformId) if (collisionObject == nil) or (collisionObject ~= nil and not (collisionObject.rootVehicle == self.vehicle)) then self.collisionhits = self.collisionhits + 1 - end - if PathFinderModule.debug == true then - local currentCollMask = getCollisionMask(transformId) - if currentCollMask then - local x, _, z = getWorldTranslation(self.vehicle.components[1].node) - x = x + g_currentMission.mapWidth/2 - z = z + g_currentMission.mapHeight/2 - - PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback transformId ->%s<- collisionObject ->%s<- getRigidBodyType->transformId %s getName->transformId %s getNodePath %s" - , tostring(transformId) - , tostring(collisionObject) - , tostring(getRigidBodyType(transformId)) - , tostring(getName(transformId)) - , tostring(I3DUtil.getNodePath(transformId)) - ) - if collisionObject then - PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback collisionObject type ->%s<-" - , tostring(type(collisionObject)) + if PathFinderModule.debug == true then + local currentCollMask = getCollisionMask(transformId) + if currentCollMask then + local x, _, z = getWorldTranslation(self.vehicle.components[1].node) + x = x + g_currentMission.mapWidth/2 + z = z + g_currentMission.mapHeight/2 + + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback transformId ->%s<- collisionObject ->%s<- getRigidBodyType->transformId %s getName->transformId %s getNodePath %s" + , tostring(transformId) + , tostring(collisionObject) + , tostring(getRigidBodyType(transformId)) + , tostring(getName(transformId)) + , tostring(I3DUtil.getNodePath(transformId)) + ) + if collisionObject then + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback collisionObject type ->%s<-" + , tostring(type(collisionObject)) + ) + end + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:collisionTestCallback xz %.0f %.0f currentCollMask %s" + , x, z + , MathUtil.numberToSetBitsStr(currentCollMask) ) end - PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:collisionTestCallback xz %.0f %.0f currentCollMask %s" - , x, z - , MathUtil.numberToSetBitsStr(currentCollMask) - ) end end end diff --git a/scripts/Utils/Dubins.lua b/scripts/Utils/Dubins.lua new file mode 100644 index 00000000..3fbf020d --- /dev/null +++ b/scripts/Utils/Dubins.lua @@ -0,0 +1,428 @@ +--[[ +Dubins curves adapted for use with AutoDrive + +Copyright (c) 2008-2018, Andrew Walker + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + + +@Misc{DubinsCurves, + author = {Andrew Walker}, + title = {Dubins-Curves: an open implementation of shortest paths for the forward only car}, + year = {2008--}, + url = "https://github.com/AndrewWalker/Dubins-Curves" +} + +]] + +ADDubins = {} +function ADDubins:new() + local o = {} + setmetatable(o, self) + self.__index = self + self.outPath = {} + return o +end + +ADDubins.DubinsPathType = { + LSL = 1, + LSR = 2, + RSL = 3, + RSR = 4, + RLR = 5, + LRL = 6 +} + +ADDubins.DubinsPath = { + -- /* the initial configuration */ + qi = {}, + -- /* the lengths of the three segments */ + param = {}, + -- /* model forward velocity / model angular velocity */ + rho = 0, + -- /* the path type described */ + type = 0 +} + +ADDubins.EDUBOK = 0 +ADDubins.EDUBCOCONFIGS = 1 +ADDubins.EDUBPARAM = 2 +ADDubins.EDUBBADRHO = 3 +ADDubins.EDUBNOPATH = 4 +ADDubins.EPSILON = (10e-10) + +ADDubins.SegmentType = { + L_SEG = 0, + S_SEG = 1, + R_SEG = 2 +} + +ADDubins.DIRDATA = { + { 0, 1, 0 }, + { 0, 1, 2 }, + { 2, 1, 0 }, + { 2, 1, 2 }, + { 2, 0, 2 }, + { 0, 2, 0 } +} + +ADDubins.DubinsIntermediateResults = { + alpha = 0, + beta = 0, + d = 0, + sa = 0, + sb = 0, + ca = 0, + cb = 0, + c_ab = 0, + d_sq = 0 +} + +function ADDubins:fmodr(x, y) + return (x - y * math.floor(x/y)) +end + +function ADDubins:mod2pi( theta ) + return (self:fmodr( theta, 2 * math.pi )) +end + +function ADDubins:dubins_shortest_path(path, q0, q1, rho) + local inTemp = {} + local params = {} + local cost + local best_cost = math.huge + local best_word = -1 + local errcode = self:dubins_intermediate_results(inTemp, q0, q1, rho) + if errcode ~= ADDubins.EDUBOK then + return errcode + end + + path.qi[1] = q0[1] + path.qi[2] = q0[2] + path.qi[3] = q0[3] + path.rho = rho + + for i = 1, 6, 1 do + errcode = ADDubins:dubins_word(inTemp, i, params) + if errcode == ADDubins.EDUBOK then + cost = params[1] + params[2] + params[3] + if(cost < best_cost) then + best_word = i + best_cost = cost + path.param[1] = params[1] + path.param[2] = params[2] + path.param[3] = params[3] + path.type = i + end + end + end + if(best_word == -1) then + return ADDubins.EDUBNOPATH + end + return ADDubins.EDUBOK +end + +function ADDubins:dubins_path(path, q0, q1, rho, pathType) + local inTemp = {} + local errcode = ADDubins:dubins_intermediate_results(inTemp, q0, q1, rho) + if(errcode == ADDubins.EDUBOK) then + local params = {} + errcode = ADDubins:dubins_word(inTemp, pathType, params) + if(errcode == ADDubins.EDUBOK) then + path.param[1] = params[1] + path.param[2] = params[2] + path.param[3] = params[3] + path.qi[1] = q0[1] + path.qi[2] = q0[2] + path.qi[3] = q0[3] + path.rho = rho + path.type = pathType + end + end + return errcode +end + +function ADDubins:dubins_path_length( path ) + local length = 0 + length = length + path.param[1] + length = length + path.param[2] + length = length + path.param[3] + length = length * path.rho; + return length +end + +function ADDubins:dubins_segment_length( path, i ) + if( (i < 1) or (i > 3) ) then + return math.huge + end + return path.param[i] * path.rho; +end + +function ADDubins:dubins_segment_length_normalized( path, i ) + if( (i < 1) or (i > 3) ) then + return math.huge + end + return path.param[i] +end + +function ADDubins:dubins_path_type( path ) + return path.type +end + +function ADDubins:dubins_segment( t, qi, qt, type) + local st = math.sin(qi[3]) + local ct = math.cos(qi[3]) + if( type == ADDubins.SegmentType.L_SEG ) then + qt[1] = math.sin(qi[3]+t) - st + qt[2] = - math.cos(qi[3]+t) + ct + qt[3] = t + elseif( type == ADDubins.SegmentType.R_SEG ) then + qt[1] = - math.sin(qi[3]-t) + st + qt[2] = math.cos(qi[3]-t) - ct + qt[3] = -t + elseif( type == ADDubins.SegmentType.S_SEG ) then + qt[1] = ct * t + qt[2] = st * t + qt[3] = 0 + end + qt[1] = qt[1] + qi[1] + qt[2] = qt[2] + qi[2] + qt[3] = qt[3] + qi[3] +end + +function ADDubins:dubins_path_sample( path, t, q ) + -- /* tprime is the normalised variant of the parameter t */ + local tprime = t / path.rho; + local qi = {} --/* The translated initial configuration */ + local q1 = {} --/* end-of segment 1 */ + local q2 = {} --/* end-of segment 2 */ + local types = ADDubins.DIRDATA[path.type] + local p1, p2 + + if( t < 0 or t > self:dubins_path_length(path) ) then + return ADDubins.EDUBPARAM + end + + -- /* initial configuration */ + qi[1] = 0 + qi[2] = 0 + qi[3] = path.qi[3] + + -- /* generate the target configuration */ + p1 = path.param[1] + p2 = path.param[2] + self:dubins_segment( p1, qi, q1, types[1] ) + self:dubins_segment( p2, q1, q2, types[2] ) + if( tprime < p1 ) then + self:dubins_segment( tprime, qi, q, types[1] ) + elseif( tprime < (p1+p2) ) then + self:dubins_segment( tprime-p1, q1, q, types[2] ) + else + self:dubins_segment( tprime-p1-p2, q2, q, types[3] ) + end + + -- /* scale the target configuration, translate back to the original starting point */ + q[1] = q[1] * path.rho + path.qi[1] + q[2] = q[2] * path.rho + path.qi[2] + q[3] = self:mod2pi(q[3]) + + return ADDubins.EDUBOK +end + +function ADDubins.createWayPoints(q, x, outPath) + local y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, q[1], 1, -q[2]) + -- print(string.format("printConfiguration xyz %.1f %.1f %.1f", q[1], y, q[2])) + table.insert(outPath, {x = q[1], y = y, z = -q[2], t = q[3]}) + return 0 +end + +function ADDubins:dubins_path_sample_many(path, stepSize, cb) + local retcode + local q = {} + local x = 0 + local length = self:dubins_path_length(path) + while( x < length ) do + self:dubins_path_sample( path, x, q ) + retcode = cb(q, x, self.outPath) + if( retcode ~= 0 ) then + return retcode + end + x = x + stepSize + end + return 0 +end + +function ADDubins:dubins_path_endpoint( path, q ) + return self:dubins_path_sample( path, self:dubins_path_length(path) - ADDubins.EPSILON, q ) +end + +function ADDubins:dubins_extract_subpath( path, t, newpath ) + -- /* calculate the true parameter */ + local tprime = t / path.rho + + if ((t < 0) or (t > self:dubins_path_length(path))) then + return ADDubins.EDUBPARAM + end + + -- /* copy most of the data */ + newpath.qi[1] = path.qi[1] + newpath.qi[2] = path.qi[2] + newpath.qi[3] = path.qi[3] + newpath.rho = path.rho + newpath.type = path.type + + -- /* fix the parameters */ + newpath.param[1] = math.min( path.param[1], tprime ) + newpath.param[2] = math.min( path.param[2], tprime - newpath.param[1]) + newpath.param[3] = math.min( path.param[3], tprime - newpath.param[1] - newpath.param[2]) + return 0 +end + +function ADDubins:dubins_intermediate_results(inTemp, q0, q1, rho) + local dx, dy, D, d, theta, alpha, beta + if( rho <= 0 ) then + return ADDubins.EDUBBADRHO + end + + dx = q1[1] - q0[1] + dy = q1[2] - q0[2] + D = math.sqrt( dx * dx + dy * dy ) + d = D / rho + theta = 0 + + -- /* test required to prevent domain errors if dx=0 and dy=0 */ + if(d > 0) then + theta = self:mod2pi(math.atan2( dy, dx )) + end + alpha = self:mod2pi(q0[3] - theta) + beta = self:mod2pi(q1[3] - theta) + + inTemp.alpha = alpha + inTemp.beta = beta + inTemp.d = d + inTemp.sa = math.sin(alpha) + inTemp.sb = math.sin(beta) + inTemp.ca = math.cos(alpha) + inTemp.cb = math.cos(beta) + inTemp.c_ab = math.cos(alpha - beta) + inTemp.d_sq = d * d + + return ADDubins.EDUBOK +end + +function ADDubins:dubins_LSL( inTemp, out) + local tmp0, tmp1, p_sq + tmp0 = inTemp.d + inTemp.sa - inTemp.sb + p_sq = 2 + inTemp.d_sq - (2*inTemp.c_ab) + (2 * inTemp.d * (inTemp.sa - inTemp.sb)) + + if(p_sq >= 0) then + tmp1 = math.atan2( (inTemp.cb - inTemp.ca), tmp0 ) + out[1] = self:mod2pi(tmp1 - inTemp.alpha) + out[2] = math.sqrt(p_sq) + out[3] = self:mod2pi(inTemp.beta - tmp1) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_RSR(inTemp, out) + local tmp0 = inTemp.d - inTemp.sa + inTemp.sb + local p_sq = 2 + inTemp.d_sq - (2 * inTemp.c_ab) + (2 * inTemp.d * (inTemp.sb - inTemp.sa)) + if( p_sq >= 0 ) then + local tmp1 = math.atan2( (inTemp.ca - inTemp.cb), tmp0 ) + out[1] = self:mod2pi(inTemp.alpha - tmp1) + out[2] = math.sqrt(p_sq) + out[3] = self:mod2pi(tmp1 -inTemp.beta) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_LSR(inTemp, out) + local p_sq = -2 + (inTemp.d_sq) + (2 * inTemp.c_ab) + (2 * inTemp.d * (inTemp.sa + inTemp.sb)) + if( p_sq >= 0 ) then + local p = math.sqrt(p_sq) + local tmp0 = math.atan2( (-inTemp.ca - inTemp.cb), (inTemp.d + inTemp.sa + inTemp.sb) ) - math.atan2(-2.0, p) + out[1] = self:mod2pi(tmp0 - inTemp.alpha) + out[2] = p + out[3] = self:mod2pi(tmp0 - self:mod2pi(inTemp.beta)) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_RSL(inTemp, out) + local p_sq = -2 + inTemp.d_sq + (2 * inTemp.c_ab) - (2 * inTemp.d * (inTemp.sa + inTemp.sb)) + if( p_sq >= 0 ) then + local p = math.sqrt(p_sq); + local tmp0 = math.atan2( (inTemp.ca + inTemp.cb), (inTemp.d - inTemp.sa - inTemp.sb) ) - math.atan2(2.0, p) + out[1] = self:mod2pi(inTemp.alpha - tmp0) + out[2] = p + out[3] = self:mod2pi(inTemp.beta - tmp0) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_RLR(inTemp, out) + local tmp0 = (6. - inTemp.d_sq + 2*inTemp.c_ab + 2*inTemp.d*(inTemp.sa - inTemp.sb)) / 8 + local phi = math.atan2( inTemp.ca - inTemp.cb, inTemp.d - inTemp.sa + inTemp.sb ) + if( math.abs(tmp0) <= 1) then + local p = self:mod2pi((2 * math.pi) - math.acos(tmp0) ) + local t = self:mod2pi(inTemp.alpha - phi + self:mod2pi(p/2)) + out[1] = t + out[2] = p + out[3] = self:mod2pi(inTemp.alpha - inTemp.beta - t + self:mod2pi(p)) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_LRL(inTemp, out) + local tmp0 = (6. - inTemp.d_sq + 2*inTemp.c_ab + 2*inTemp.d*(inTemp.sb - inTemp.sa)) / 8 + local phi = math.atan2( inTemp.ca - inTemp.cb, inTemp.d + inTemp.sa - inTemp.sb ) + if( math.abs(tmp0) <= 1) then + local p = self:mod2pi( 2 * math.pi - math.acos( tmp0) ) + local t = self:mod2pi(-inTemp.alpha - phi + p/2) + out[1] = t + out[2] = p + out[3] = self:mod2pi(self:mod2pi(inTemp.beta) - inTemp.alpha -t + self:mod2pi(p)) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_word(inTemp, pathType, out) + local result + if pathType == ADDubins.DubinsPathType.LSL then + result = self:dubins_LSL(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.RSL then + result = self:dubins_RSL(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.LSR then + result = self:dubins_LSR(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.RSR then + result = self:dubins_RSR(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.LRL then + result = self:dubins_LRL(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.RLR then + result = self:dubins_RLR(inTemp, out) + end + + return result +end From 71da45906d382cd2fcaeb33b2f3f31be26259b01 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Tue, 23 Jul 2024 00:34:27 +0200 Subject: [PATCH 24/44] smal fix added missed appended waypoints --- scripts/Modules/PathFinderModule.lua | 35 ++++++++++++++++------------ 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 415b8b9d..67d631ad 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -773,6 +773,7 @@ function PathFinderModule:update(dt) if dubinsPath then self.dubinsDone = true self.wayPoints = dubinsPath + self:appendWayPointsNew() self.isFinished = true self.smoothDone = true return -- found path @@ -2824,25 +2825,29 @@ function PathFinderModule:createWayPointsNew() self:smoothResultingPPPath_Refined() if self.smoothStep == 2 then - -- When going to network, dont turn actual road network nodes into pathFinderPoints - if self.goingToNetwork then - for i = 1, #self.wayPoints, 1 do - self.wayPoints[i].isPathFinderPoint = true - end + self:appendWayPointsNew() + end +end + +function PathFinderModule:appendWayPointsNew() + -- When going to network, dont turn actual road network nodes into pathFinderPoints + if self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true end + end - if self.appendWayPoints ~= nil then - for i = 1, #self.appendWayPoints, 1 do - self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] - end - self.smoothStep = 3 + if self.appendWayPoints ~= nil then + for i = 1, #self.appendWayPoints, 1 do + self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] end + self.smoothStep = 3 + end - -- See comment above - if not self.goingToNetwork then - for i = 1, #self.wayPoints, 1 do - self.wayPoints[i].isPathFinderPoint = true - end + -- See comment above + if not self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true end end end From 74283744cf668ace0d4168342189fba2dee63cb2 Mon Sep 17 00:00:00 2001 From: TaidyBlue Date: Tue, 30 Jul 2024 22:18:20 +0800 Subject: [PATCH 25/44] Add Chinese Traditional language translations\translation_ct.xml --- translations/translation_ct.xml | 366 ++++++++++++++++++++++++++++++++ 1 file changed, 366 insertions(+) create mode 100644 translations/translation_ct.xml diff --git a/translations/translation_ct.xml b/translations/translation_ct.xml new file mode 100644 index 00000000..8cbac5d5 --- /dev/null +++ b/translations/translation_ct.xml @@ -0,0 +1,366 @@ + + + 泰迪藍 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From cc2a3e562b085fed736a657ba4d90f8e560b42ea Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Mon, 12 Aug 2024 21:41:15 +0200 Subject: [PATCH 26/44] move planned rear target closer to chopper --- scripts/Modes/CombineUnloaderMode.lua | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/scripts/Modes/CombineUnloaderMode.lua b/scripts/Modes/CombineUnloaderMode.lua index ad312cb2..59f79e3e 100644 --- a/scripts/Modes/CombineUnloaderMode.lua +++ b/scripts/Modes/CombineUnloaderMode.lua @@ -659,11 +659,13 @@ function CombineUnloaderMode:getRearChaseOffsetX(leftBlocked, rightBlocked) end end -function CombineUnloaderMode:getRearChaseOffsetZ() +function CombineUnloaderMode:getRearChaseOffsetZ(planningPhase) local followDistance = AutoDrive.getSetting("followDistance", self.vehicle) local rearChaseOffset = -followDistance - (self.combine.size.length / 2) if self.combine.ad.isAutoAimingChopper and not self.combine.ad.isSugarcaneHarvester then - rearChaseOffset = -followDistance - (self.combine.size.length / 2) + if planningPhase then + rearChaseOffset = - (self.combine.size.length / 2) + end else -- math.sqrt(2) ensures the trailer could straighten if it was turned 90 degrees, and it makes this point further -- back than the pathfinder (straightening) target in PathFinderModule:startPathPlanningToPipe @@ -714,7 +716,7 @@ function CombineUnloaderMode:getPipeChasePosition(planningPhase) local sideChaseTermX = self:getSideChaseOffsetX() local sideChaseTermZ = self:getSideChaseOffsetZ(AutoDrive.dynamicChaseDistance or self.combine.ad.isHarvester) - local rearChaseTermZ = self:getRearChaseOffsetZ() + local rearChaseTermZ = self:getRearChaseOffsetZ(planningPhase) if self.combine.ad.isChopper then -- any chopper From f205ac282f5dba8b34bfebefd07a80a19ea8524d Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 17 Aug 2024 17:08:12 +0200 Subject: [PATCH 27/44] increase angle to switch to next WP from >90 to >135 this result in better approach a harvester by unloader in sharp turns --- scripts/Modules/DrivePathModule.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/Modules/DrivePathModule.lua b/scripts/Modules/DrivePathModule.lua index de5cd0b2..150d175a 100644 --- a/scripts/Modules/DrivePathModule.lua +++ b/scripts/Modules/DrivePathModule.lua @@ -215,7 +215,7 @@ function ADDrivePathModule:isCloseToWaypoint() angle = math.abs(angle) local isReverseStart = wp_ahead.incoming ~= nil and (not table.contains(wp_ahead.incoming, wp_current.id)) - if angle >= 90 and not isReverseStart then + if angle >= 135 and not isReverseStart then return true end end From c00c5922929d17c2c47df2fcb2c1c9344102667d Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 17 Aug 2024 17:10:49 +0200 Subject: [PATCH 28/44] TWAEK: collision detect only other vehicles <10 above each other avoid detection of vehicles above / under bridges etc. --- scripts/Modules/CollisionDetectionModule.lua | 1 + scripts/Utils/CollisionDetectionUtils.lua | 6 ++++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/scripts/Modules/CollisionDetectionModule.lua b/scripts/Modules/CollisionDetectionModule.lua index 7439dfeb..4d2232a3 100644 --- a/scripts/Modules/CollisionDetectionModule.lua +++ b/scripts/Modules/CollisionDetectionModule.lua @@ -56,6 +56,7 @@ function ADCollisionDetectionModule:detectObstacle() boundingBox[2] = box.topRight boundingBox[3] = box.downRight boundingBox[4] = box.downLeft + boundingBox[1].y = box.y self.detectedCollision = AutoDrive:checkForVehicleCollision(self.vehicle, boundingBox, excludedList) end diff --git a/scripts/Utils/CollisionDetectionUtils.lua b/scripts/Utils/CollisionDetectionUtils.lua index 4cf439d0..37b684e7 100644 --- a/scripts/Utils/CollisionDetectionUtils.lua +++ b/scripts/Utils/CollisionDetectionUtils.lua @@ -22,11 +22,13 @@ function AutoDrive.checkForVehiclesInBox(boundingBox, excludedVehicles) isExcluded = true end if (not isExcluded) and otherVehicle ~= nil and otherVehicle.components ~= nil and otherVehicle.size.width ~= nil and otherVehicle.size.length ~= nil and otherVehicle.rootNode ~= nil then - local x, _, z = getWorldTranslation(otherVehicle.components[1].node) + local x, y, z = getWorldTranslation(otherVehicle.components[1].node) local distance = MathUtil.vector2Length(boundingBox[1].x - x, boundingBox[1].z - z) if distance < 50 then if AutoDrive.boxesIntersect(boundingBox, AutoDrive.getBoundingBoxForVehicle(otherVehicle)) == true then - return true + if math.abs(boundingBox[1].y - y) < 10 then + return true + end end end end From 7b757c9dc11b9685da9aedb804d02cae6159373d Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sat, 17 Aug 2024 17:15:48 +0200 Subject: [PATCH 29/44] add some more debug infos --- scripts/Modules/PathFinderModule.lua | 60 +++++++++++++++++++++++++--- 1 file changed, 55 insertions(+), 5 deletions(-) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 67d631ad..51411029 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -2300,6 +2300,15 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) ) ) end + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle belowGroundLevel xz %d,%d terrain123 %.1f %.1f %.1f getWaterYAtWorldPosition %s waterY %s " + , math.floor(x1) + , math.floor(z1) + , terrain1 + , terrain2 + , terrain3 + , tostring(g_currentMission.environmentAreaSystem:getWaterYAtWorldPosition(worldPosMiddle.x, terrain3, worldPosMiddle.z)) + , tostring(waterY) + ) end if (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then @@ -2311,6 +2320,14 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) ) ) end + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleBetween xz %d,%d angleBetween %.1f terrain12 %.1f %.1f length %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleBetween) + , terrain1 + , terrain2 + , length + ) end if (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then @@ -2322,6 +2339,36 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) ) ) end + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleBetweenCenter xz %d,%d angleBetweenCenter %.1f terrain32 %.1f %.1f lengthMiddle %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleBetweenCenter) + , terrain3 + , terrain2 + , lengthMiddle + ) + end + + if (angleLeft) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleLeft xz %d,%d angleLeft %.1f terrainLeft %.1f terrain1 %.1f lengthLeft %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleLeft) + , terrainLeft + , terrain1 + , lengthLeft + ) + end + + if (angleRight) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleRight xz %d,%d angleRight %.1f terrainRight %.1f terrain1 %.1f lengthRight %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleRight) + , terrainRight + , terrain1 + , lengthRight + ) end if belowGroundLevel or (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD or (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD @@ -2630,7 +2677,7 @@ function PathFinderModule:getDirections(fromNode, node) end local directions = {} - if fromNode then +--[[ if fromNode then PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %d,%d fromNode.direction %s node xz %d,%d node.direction %s" , fromNode.x, fromNode.z , tostring(self.direction_to_text[fromNode.direction+1]) @@ -2644,6 +2691,7 @@ function PathFinderModule:getDirections(fromNode, node) , tostring(self.direction_to_text[node.direction+1]) ) end + ]] if (fromNode == nil and node.direction == self.PP_RIGHT) or (fromNode and fromNode.x == node.x and fromNode.z < node.z) then directions[1] = { -1, 1 } directions[1].direction = self.PP_DOWN_RIGHT @@ -2713,6 +2761,7 @@ function PathFinderModule:getDirections(fromNode, node) , tostring(node.direction) ) end +--[[ PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections %d,%d direction %s %d,%d direction %s %d,%d direction %s" , directions[1][1], directions[1][2] , tostring(self.direction_to_text[directions[1].direction+1]) @@ -2721,6 +2770,7 @@ function PathFinderModule:getDirections(fromNode, node) , directions[3][1], directions[3][2] , tostring(self.direction_to_text[directions[3].direction+1]) ) + ]] return directions end @@ -3045,7 +3095,7 @@ function PathFinderModule:collisionTestCallback(transformId) if PathFinderModule.debug == true then local currentCollMask = getCollisionMask(transformId) if currentCollMask then - local x, _, z = getWorldTranslation(self.vehicle.components[1].node) + local x, _, z = getWorldTranslation(transformId) x = x + g_currentMission.mapWidth/2 z = z + g_currentMission.mapHeight/2 @@ -3057,11 +3107,11 @@ function PathFinderModule:collisionTestCallback(transformId) , tostring(I3DUtil.getNodePath(transformId)) ) if collisionObject then - PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback collisionObject type ->%s<-" - , tostring(type(collisionObject)) + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback xmlFilename ->%s<-" + , tostring(collisionObject.xmlFilename) ) end - PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:collisionTestCallback xz %.0f %.0f currentCollMask %s" + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback xz %.0f %.0f currentCollMask %s" , x, z , MathUtil.numberToSetBitsStr(currentCollMask) ) From f547a90bd4b82f7f0129b9a77c5c4ad0b6b4c581 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 18 Aug 2024 13:31:03 +0200 Subject: [PATCH 30/44] avoid NAN --- scripts/Modules/PathFinderModule.lua | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 51411029..53f8ae29 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -360,6 +360,12 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) ) ) ADScheduler:addPathfinderVehicle(self.vehicle) + if math.abs(targetVector.x) < 0.001 then + targetVector.x = 0.001 + end + if math.abs(targetVector.z) < 0.001 then + targetVector.z = 0.001 + end self.targetVector = targetVector local vehicleWorldX, vehicleWorldY, vehicleWorldZ = getWorldTranslation(self.vehicle.components[1].node) local vehicleRx, _, vehicleRz = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, 1) From e46080237e9c0d25baf8dcf632dd14e5cdabf976 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 18 Aug 2024 13:35:15 +0200 Subject: [PATCH 31/44] adjust collision detection --- scripts/Modules/PathFinderModule.lua | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 53f8ae29..1b4071ba 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -2395,6 +2395,7 @@ function PathFinderModule.debugVehicleMsg(vehicle, msg) end function PathFinderModule:drawDebugNewPF() + -- AStar if self.cachedNodes and #self.cachedNodes > 0 then for z, row in pairs(self.cachedNodes) do for x, node in pairs(row) do @@ -2464,6 +2465,7 @@ function PathFinderModule:drawDebugNewPF() end end + -- Dubins Path if self.dubinsPath and #self.dubinsPath > 0 then local lastPoint = nil @@ -2593,7 +2595,7 @@ function PathFinderModule:isDriveableAstar(cell) -- check for obstacles local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height self.collisionhits = 0 - local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallback", self, self.mask, true, true, true) + local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, 2.65, shapeDefinition.widthZ, "collisionTestCallback", self, self.mask, true, true, true) cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) cell.isRestricted = cell.isRestricted or cell.hasCollision if cell.hasCollision then @@ -2970,7 +2972,7 @@ function PathFinderModule:isDriveableDubins(cell) if not cell.isRestricted then -- check for obstacles self.collisionhits = 0 - local shapes = overlapBox(cell.worldPos.x, cell.worldPos.y + 3, cell.worldPos.z, 0, cell.t, 0, sizeMax, 5, sizeMax, "collisionTestCallback", self, self.mask, true, true, true) + local shapes = overlapBox(cell.worldPos.x, cell.worldPos.y + 3, cell.worldPos.z, 0, cell.t, 0, sizeMax, 2.65, sizeMax, "collisionTestCallback", self, self.mask, true, true, true) cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) cell.isRestricted = cell.isRestricted or cell.hasCollision if cell.hasCollision then From 489f4359137f9ca5546e2c8c9f13e9ea2f4ce9ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20M=C3=A9ndez?= <46683179+KCHARRO@users.noreply.github.com> Date: Sun, 25 Aug 2024 20:52:03 -0400 Subject: [PATCH 32/44] Update translation_es.xml --- translations/translation_es.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/translations/translation_es.xml b/translations/translation_es.xml index 96c82a64..a95a6c52 100644 --- a/translations/translation_es.xml +++ b/translations/translation_es.xml @@ -218,10 +218,10 @@ - - - - + + + + From 62a4b80825de708b68aa7768b1a5fd0d5d5b2b95 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Fri, 30 Aug 2024 21:05:53 +0200 Subject: [PATCH 33/44] cosmetic --- scripts/Manager/HarvestManager.lua | 2 +- scripts/Modules/PathFinderModule.lua | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/scripts/Manager/HarvestManager.lua b/scripts/Manager/HarvestManager.lua index cd90f573..a655a3a8 100644 --- a/scripts/Manager/HarvestManager.lua +++ b/scripts/Manager/HarvestManager.lua @@ -137,7 +137,7 @@ function ADHarvestManager:update(dt) else if AutoDrive.getSetting("callSecondUnloader", harvester) then local unloader = self:getAssignedUnloader(harvester) - if unloader.ad.modes[AutoDrive.MODE_UNLOAD]:getFollowingUnloader() == nil then + if unloader and unloader.ad and unloader.ad.modes and unloader.ad.modes[AutoDrive.MODE_UNLOAD]:getFollowingUnloader() == nil then local trailers, _ = AutoDrive.getAllUnits(unloader) local fillLevel, _, _, fillFreeCapacity = AutoDrive.getAllFillLevels(trailers) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 1b4071ba..1758c1fb 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -851,13 +851,14 @@ function PathFinderModule:update(dt) table.insert(self.path, self.nodeGoal) diffNetTime = netGetTime() - diffNetTime self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime - - PathFinderModule.debugMsg(self.vehicle, "PFM:update find goal reached self.steps %d diffOverallNetTime %d self.nodeGoal xz %d,%d current xz %d,%d" - , self.steps - , self.diffOverallNetTime - , self.nodeGoal.x, self.nodeGoal.z - , current.x, current.z - ) + if current then + PathFinderModule.debugMsg(self.vehicle, "PFM:update find goal reached self.steps %d diffOverallNetTime %d self.nodeGoal xz %d,%d current xz %d,%d" + , self.steps + , self.diffOverallNetTime + , self.nodeGoal.x, self.nodeGoal.z + , current.x, current.z + ) + end self.isFinished = true return -- found path From 7a1e6a2ec2d047ced5fe49474c06a4eb1fa7f58a Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Fri, 30 Aug 2024 21:09:17 +0200 Subject: [PATCH 34/44] handle stuck --- scripts/Tasks/CatchCombinePipeTask.lua | 57 +++++++++++++------- scripts/Tasks/ClearCropTask.lua | 18 +++---- scripts/Tasks/EmptyHarvesterTask.lua | 41 +++++++++++++-- scripts/Tasks/FollowCombineTask.lua | 73 +++++++++++++++++--------- 4 files changed, 130 insertions(+), 59 deletions(-) diff --git a/scripts/Tasks/CatchCombinePipeTask.lua b/scripts/Tasks/CatchCombinePipeTask.lua index 79247e8e..5c5f37eb 100644 --- a/scripts/Tasks/CatchCombinePipeTask.lua +++ b/scripts/Tasks/CatchCombinePipeTask.lua @@ -55,6 +55,24 @@ function CatchCombinePipeTask:update(dt) if self.lastState ~= self.state then CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) self.lastState = self.state + self:resetAllTimers() + end + + local checkForStuck = (self.vehicle.lastSpeedReal <= 0.0002) and ( + self.state == CatchCombinePipeTask.STATE_DRIVING + ) + + self.stuckTimer:timer(checkForStuck, self.MAX_STUCK_TIME, dt) + if self.stuckTimer:done() + -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE + then + -- got stuck or to close to combine -> reverse + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING stuck") + self.stuckTimer:timer(false) + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + self.state = CatchCombinePipeTask.STATE_REVERSING + return end if self.state == CatchCombinePipeTask.STATE_PATHPLANNING then @@ -71,52 +89,43 @@ function CatchCombinePipeTask:update(dt) return else self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) - self.stuckTimer:timer(false) self.state = CatchCombinePipeTask.STATE_DRIVING + return end else self.vehicle.ad.pathFinderModule:update(dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end elseif self.state == CatchCombinePipeTask.STATE_DELAY_PATHPLANNING then CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DELAY_PATHPLANNING") if self.waitForCheckTimer:timer(true, 1000, dt) then - if self:startNewPathFinding() then - self.vehicle.ad.pathFinderModule:addDelayTimer(6000) - self.state = CatchCombinePipeTask.STATE_PATHPLANNING - end if self.newPathFindingCounter > self.MAX_COUNT_NEW_PATHFINDING then -- prevent deadlock self.state = CatchCombinePipeTask.STATE_FINISHED return end + if self:startNewPathFinding() then + self.vehicle.ad.pathFinderModule:addDelayTimer(6000) + self.state = CatchCombinePipeTask.STATE_PATHPLANNING + return + end end self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return elseif self.state == CatchCombinePipeTask.STATE_DRIVING then -- check if this is still a clever path to follow -- do this by distance of the combine to the last location pathfinder started at local x, y, z = getWorldTranslation(self.combine.components[1].node) local combineTravelDistance = MathUtil.vector2Length(x - self.combinesStartLocation.x, z - self.combinesStartLocation.z) - self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, self.MAX_STUCK_TIME, dt) - if self.stuckTimer:done() - -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE - then - -- got stuck or to close to combine -> reverse - CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING stuck") - self.stuckTimer:timer(false) - local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.reverseStartLocation = {x = x, y = y, z = z} - self.state = CatchCombinePipeTask.STATE_REVERSING - return - end - if combineTravelDistance > 85 then CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - combine travelled - recalculate path") self.waitForCheckTimer.elapsedTime = 4000 self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING + return else if self.vehicle.ad.drivePathModule:isTargetReached() then -- check if we have actually reached the target or not @@ -132,6 +141,7 @@ function CatchCombinePipeTask:update(dt) else CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - angle or distance to combine too high - recalculate path now") self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING + return end else self.vehicle.ad.drivePathModule:update(dt) @@ -142,8 +152,8 @@ function CatchCombinePipeTask:update(dt) local distanceToReverseStart = MathUtil.vector2Length(x - self.reverseStartLocation.x, z - self.reverseStartLocation.z) self.reverseTimer:timer(true, self.MAX_REVERSE_TIME, dt) if distanceToReverseStart > self.MAX_REVERSE_DISTANCE or self.reverseTimer:done() then - self.reverseTimer:timer(false) self.state = CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH + return else self.vehicle.ad.specialDrivingModule:driveReverse(dt, 15, 1, self.vehicle.ad.trailerModule:canBeHandledInReverse()) end @@ -151,12 +161,12 @@ function CatchCombinePipeTask:update(dt) CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_WAIT_BEFORE_FINISH") self.waitTimer:timer(true, self.MAX_REVERSE_TIME, dt) if self.waitTimer:done() then - self.waitTimer:timer(false) self.state = CatchCombinePipeTask.STATE_FINISHED return else self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end elseif self.state == CatchCombinePipeTask.STATE_FINISHED then CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_FINISHED") @@ -242,6 +252,13 @@ function CatchCombinePipeTask:getStateName(state) return self.statesToNames[requestedState] or "" end +function CatchCombinePipeTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.reverseTimer:timer(false) + self.waitTimer:timer(false) + self.waitForCheckTimer:timer(false) +end + function CatchCombinePipeTask:getI18nInfo() local text = "$l10n_AD_task_catch_up_with_combine;" if self.state == CatchCombinePipeTask.STATE_PATHPLANNING then diff --git a/scripts/Tasks/ClearCropTask.lua b/scripts/Tasks/ClearCropTask.lua index 2bc91b70..c59401d3 100644 --- a/scripts/Tasks/ClearCropTask.lua +++ b/scripts/Tasks/ClearCropTask.lua @@ -94,9 +94,7 @@ function ClearCropTask:update(dt) self.waitTimer:timer(true, ClearCropTask.WAIT_TIME, dt) if self.waitTimer:done() then ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update STATE_WAITING - done waiting - clear now...") - self.waitTimer:timer(false) - self.driveTimer:timer(false) - self.stuckTimer:timer(false) + self:resetAllTimers() self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) self.state = ClearCropTask.STATE_CLEARING_FIRST return @@ -109,9 +107,7 @@ function ClearCropTask:update(dt) return elseif self.driveTimer:done() then ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update 1 driveTimer:done") - self.waitTimer:timer(false) - self.driveTimer:timer(false) - self.stuckTimer:timer(false) + self:resetAllTimers() local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = ClearCropTask.STATE_REVERSING @@ -129,9 +125,7 @@ function ClearCropTask:update(dt) end if distanceToReversStart > 20 then ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update distanceToReversStart > 20") - self.waitTimer:timer(false) - self.driveTimer:timer(false) - self.stuckTimer:timer(false) + self:resetAllTimers() self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) self.state = ClearCropTask.STATE_CLEARING_SECOND return @@ -180,6 +174,12 @@ function ClearCropTask:getStateName(state) return self.statesToNames[requestedState] or "" end +function ClearCropTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.waitTimer:timer(false) + self.driveTimer:timer(false) +end + function ClearCropTask:getI18nInfo() local text = "$l10n_AD_task_clearcrop;" if self.state == ClearCropTask.STATE_CLEARING_FIRST then diff --git a/scripts/Tasks/EmptyHarvesterTask.lua b/scripts/Tasks/EmptyHarvesterTask.lua index 9dd32d06..8f747744 100644 --- a/scripts/Tasks/EmptyHarvesterTask.lua +++ b/scripts/Tasks/EmptyHarvesterTask.lua @@ -9,6 +9,7 @@ EmptyHarvesterTask.STATE_WAITING = {} EmptyHarvesterTask.STATE_UNLOADING_FINISHED = {} EmptyHarvesterTask.REVERSE_TIME = 30000 +EmptyHarvesterTask.MAX_STUCK_TIME = 60000 EmptyHarvesterTask.WAITING_TIME = 7000 function EmptyHarvesterTask:new(vehicle, combine) @@ -18,6 +19,7 @@ function EmptyHarvesterTask:new(vehicle, combine) o.state = EmptyHarvesterTask.STATE_PATHPLANNING o.wayPoints = nil o.reverseStartLocation = nil + o.stuckTimer = AutoDriveTON:new() o.reverseTimer = AutoDriveTON:new() o.waitTimer = AutoDriveTON:new() o.holdCPCombineTimer = AutoDriveTON:new() @@ -45,6 +47,23 @@ function EmptyHarvesterTask:update(dt) if self.lastState ~= self.state then EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) self.lastState = self.state + self:resetAllTimers() + end + + local checkForStuck = (self.vehicle.lastSpeedReal <= 0.0002) and ( + self.state == EmptyHarvesterTask.STATE_DRIVING + or self.state == EmptyHarvesterTask.STATE_UNLOADING + ) + + self.stuckTimer:timer(checkForStuck, self.MAX_STUCK_TIME, dt) + if self.stuckTimer:done() + -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE + then + -- got stuck or to close to combine -> reverse + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update - STATE_DRIVING stuck") + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + self.state = EmptyHarvesterTask.STATE_REVERSING end if self.state == EmptyHarvesterTask.STATE_PATHPLANNING then @@ -66,15 +85,18 @@ function EmptyHarvesterTask:update(dt) end else self.state = EmptyHarvesterTask.STATE_DRIVING + return end else self.vehicle.ad.pathFinderModule:update(dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end elseif self.state == EmptyHarvesterTask.STATE_DRIVING then if self.vehicle.ad.drivePathModule:isTargetReached() then self.state = EmptyHarvesterTask.STATE_UNLOADING + return else self.vehicle.ad.drivePathModule:update(dt) end @@ -110,9 +132,8 @@ function EmptyHarvesterTask:update(dt) local distanceToCombine = AutoDrive.getDistanceBetween(self.vehicle, self.combine) if combineFillLevel <= 0.1 or filledToUnload then - local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.reverseStartLocation = {x = x, y = y, z = z} self.state = EmptyHarvesterTask.STATE_UNLOADING_FINISHED + return else -- Drive forward with collision checks active and only for a limited distance if distanceToCombine > 30 then @@ -129,9 +150,13 @@ function EmptyHarvesterTask:update(dt) if AutoDrive:getIsCPCombineInPocket(self.combine) or AutoDrive.combineIsTurning(self.combine) then -- reverse if CP unload in a pocket or pullback position -- reverse if combine is turning + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} self.state = EmptyHarvesterTask.STATE_REVERSING + return else self.state = EmptyHarvesterTask.STATE_WAITING + return end elseif self.state == EmptyHarvesterTask.STATE_REVERSING then self.vehicle.ad.specialDrivingModule.motorShouldNotBeStopped = false @@ -158,9 +183,8 @@ function EmptyHarvesterTask:update(dt) end self.reverseTimer:timer(true, EmptyHarvesterTask.REVERSE_TIME, dt) if (distanceToReversStart > overallLength) or self.reverseTimer:done() then - self.holdCPCombineTimer:timer(false) - self.reverseTimer:timer(false) self.state = EmptyHarvesterTask.STATE_WAITING + return else self.vehicle.ad.specialDrivingModule:driveReverse(dt, 10, 1, self.vehicle.ad.trailerModule:canBeHandledInReverse()) end @@ -172,12 +196,12 @@ function EmptyHarvesterTask:update(dt) end self.waitTimer:timer(true, waitTime, dt) if self.waitTimer:done() then - self.waitTimer:timer(false) self:finished() return else self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end end end @@ -221,6 +245,13 @@ function EmptyHarvesterTask:getStateName(state) return self.statesToNames[requestedState] or "" end +function EmptyHarvesterTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.reverseTimer:timer(false) + self.waitTimer:timer(false) + self.holdCPCombineTimer:timer(false) +end + function EmptyHarvesterTask:getI18nInfo() local text = "$l10n_AD_task_unloading_combine;" if self.state == EmptyHarvesterTask.STATE_PATHPLANNING then diff --git a/scripts/Tasks/FollowCombineTask.lua b/scripts/Tasks/FollowCombineTask.lua index 88ef36be..b6105a4f 100644 --- a/scripts/Tasks/FollowCombineTask.lua +++ b/scripts/Tasks/FollowCombineTask.lua @@ -6,8 +6,6 @@ FollowCombineTask.STATE_WAIT_FOR_TURN = {} FollowCombineTask.STATE_REVERSING = {} FollowCombineTask.STATE_REVERSING_FROM_CHOPPER = {} FollowCombineTask.STATE_WAIT_FOR_PASS_BY = {} -FollowCombineTask.STATE_CIRCLING_PATHPLANNING = {} -FollowCombineTask.STATE_CIRCLING = {} FollowCombineTask.STATE_FINISHED = {} FollowCombineTask.STATE_WAIT_BEFORE_FINISH = {} FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY = {} @@ -66,11 +64,34 @@ function FollowCombineTask:update(dt) if self.lastState ~= self.state then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) self.lastState = self.state + self:resetAllTimers() + end + + local checkForStuck = (self.vehicle.lastSpeedReal <= 0.0002) and ( + self.state == FollowCombineTask.STATE_CHASING + or self.state == FollowCombineTask.STATE_WAIT_FOR_TURN + or self.state == FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY + or self.state == FollowCombineTask.STATE_GENERATE_UTURN_PATH + or self.state == FollowCombineTask.STATE_DRIVE_UTURN_PATH + ) + + self.stuckTimer:timer(checkForStuck, self.MAX_STUCK_TIME, dt) + if self.stuckTimer:done() + -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE + then + -- got stuck or to close to combine -> reverse + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update - STATE_DRIVING stuck") + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + if self.combine.ad.isChopper then + self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER + else + self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester + end end if self.state == FollowCombineTask.STATE_CHASING then self.chaseTimer:timer(true, 4000, dt) - self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, self.MAX_STUCK_TIME, dt) if self.combine.ad.isChopper then if self.filled and self.chaseSide ~= nil and self.chaseSide ~= AutoDrive.CHASEPOS_REAR then @@ -91,23 +112,23 @@ function FollowCombineTask:update(dt) return end - if self.stuckTimer:done() or self.angleWrongTimer.elapsedTime > 15000 then + if self.angleWrongTimer.elapsedTime > 15000 then -- if stuck with harvester - try reverse if (g_updateLoopIndex % 60 == 0) or self.loop5 == nil then self.loop5 = true FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING stuckTimer:done -> STATE_REVERSING") end - self.stuckTimer:timer(false) FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - stuck -> stuckTimer:%s angleWrongTimer:%s" , tostring(self.stuckTimer:done()), tostring(self.angleWrongTimer.elapsedTime > 15000)) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} if self.combine.ad.isChopper then self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER + return else self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester + return end - return end if not self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then @@ -131,6 +152,7 @@ function FollowCombineTask:update(dt) if self.lastChaseSide ~= CombineUnloaderMode.CHASEPOS_REAR then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - switching chase side from side to elsewhere - let's wait for passby next") self.state = FollowCombineTask.STATE_WAIT_FOR_PASS_BY + return end end @@ -150,7 +172,6 @@ function FollowCombineTask:update(dt) self.waitForTurnTimer:timer(true, self.MAX_TURN_TIME, dt) if self.waitForTurnTimer:done() then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn took to long - set finished now") - self.waitForTurnTimer:timer(false) self.state = FollowCombineTask.STATE_FINISHED return end @@ -166,15 +187,13 @@ function FollowCombineTask:update(dt) if self.combine.ad.isAutoAimingChopper then local isdrivingReverse = ((self.combine.lastSpeedReal * self.combine.movingDirection) <= -0.00051) local combineIsDriving = (self.combine.lastSpeedReal > 0.001) - self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, 15000, dt) if isdrivingReverse then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> self:reverse") self:reverse(dt) - elseif self.stuckTimer:done() or (not combineIsDriving and (self:getAngleToCobine() > 45)) then + elseif (not combineIsDriving and (self:getAngleToCobine() > 45)) then -- if stuck with harvester - try reverse FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - stuck / getAngleToCobine() > 45 -> STATE_REVERSING_FROM_CHOPPER combineIsDriving %s", tostring(combineIsDriving)) - self.stuckTimer:timer(false) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER -- reverse to get room from harvester @@ -205,8 +224,6 @@ function FollowCombineTask:update(dt) self.dischargeTimer:timer(dischargeState ~= Dischargeable.DISCHARGE_STATE_OBJECT , 500, dt) if self.dischargeTimer:done() and self.fillingTimer:done() and self.combine.ad.noMovementTimer.elapsedTime < 5000 then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - dischargeTimer:done") - self.fillingTimer:timer(false) - self.dischargeTimer:timer(false) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER -- reverse to get room from harvester @@ -235,17 +252,13 @@ function FollowCombineTask:update(dt) ) then if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide(self.chaseSide) then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks good - start chasing again") - self.waitForTurnTimer:timer(false) - self.chaseTimer:timer(false) self.state = FollowCombineTask.STATE_CHASING return elseif self.angleToCombineHeading > 150 and self.angleToCombineHeading < 210 and self.distanceToCombine < 80 and AutoDrive.experimentalFeatures.UTurn == true and self.combine.ad.isHarvester then -- Instead of directly trying a long way around to get behind the harvester, let's wait for him to pass us by and then U-turn FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading inverted - wait for passby, then U-turn") self.state = FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY - self.waitForTurnTimer:timer(false) - self.chaseTimer:timer(false) - self.waitForPassByTimer:timer(false) + return else FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks bad - stop to be able to start pathfinder") self.stayOnField = true @@ -259,8 +272,6 @@ function FollowCombineTask:update(dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) if self.waitForPassByTimer:done() then - self.waitForPassByTimer:timer(false) - self.chaseTimer:timer(false) if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks good - chasing again") self.state = FollowCombineTask.STATE_CHASING @@ -280,7 +291,6 @@ function FollowCombineTask:update(dt) local doneReversing = distanceToReverseStart > self.MAX_REVERSE_DISTANCE or (not self.startedChasing) if doneReversing or self.reverseTimer:done() then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING - done reversing - set finished") - self.reverseTimer:timer(false) self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return else @@ -296,14 +306,14 @@ function FollowCombineTask:update(dt) local doneReversing = distanceToReverseStart > self.MAX_REVERSE_DISTANCE or distanceToCombine > self.MAX_REVERSE_DISTANCE if doneReversing or self.reverseTimer:done() then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - done reversing - set finished") - self.reverseTimer:timer(false) if self.combine.ad.isFixedPipeChopper and AutoDrive:getIsCPActive(self.combine) then -- wait for CP to finish a turn maneuver before invoke pathfinder self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH + return else self.state = FollowCombineTask.STATE_FINISHED + return end - return else if self.combine.ad.isFixedPipeChopper then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - not AutoAimingChopper -> holdCPCombine") @@ -323,7 +333,6 @@ function FollowCombineTask:update(dt) local combineIsDriving = self.combine.ad.isFixedPipeChopper and AutoDrive:getIsCPActive(self.combine) and (self.combine.lastSpeedReal > 0.001) self.waitTimer:timer(not combineIsDriving, self.WAIT_BEFORE_FINISH_TIME, dt) if self.waitTimer:done() then - self.waitTimer:timer(false) self.state = FollowCombineTask.STATE_FINISHED return else @@ -345,11 +354,11 @@ function FollowCombineTask:update(dt) local _, _, offsetZ = worldToLocal(self.vehicle.components[1].node, cx, cy, cz) if offsetZ <= -10 then FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - combine passed us. Calculate U-turn now") - self.state = FollowCombineTask.STATE_GENERATE_UTURN_PATH local cx, cy, cz = getWorldTranslation(self.combine.components[1].node) local offsetX, _, _ = worldToLocal(self.vehicle.components[1].node, cx, cy, cz) self.vehicle:generateUTurn(offsetX > 0) - self.waitForPassByTimer:timer(false) + self.state = FollowCombineTask.STATE_GENERATE_UTURN_PATH + return end end elseif self.state == FollowCombineTask.STATE_GENERATE_UTURN_PATH then @@ -361,10 +370,12 @@ function FollowCombineTask:update(dt) FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation failed due to collision - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH + return else FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation finished - passing points to drivePathModule now") self.vehicle.ad.drivePathModule:setWayPoints(self.vehicle.ad.uTurn.points) self.state = FollowCombineTask.STATE_DRIVE_UTURN_PATH + return end end elseif self.state == FollowCombineTask.STATE_DRIVE_UTURN_PATH then @@ -549,6 +560,18 @@ function FollowCombineTask:getStateName(state) return self.statesToNames[requestedState] or "" end +function FollowCombineTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.angleWrongTimer:timer(false) + self.waitForTurnTimer:timer(false) + self.dischargeTimer:timer(false) + self.fillingTimer:timer(false) + self.waitForPassByTimer:timer(false) + self.chaseTimer:timer(false) + self.reverseTimer:timer(false) + self.waitTimer:timer(false) +end + function FollowCombineTask:getI18nInfo() local text = "$l10n_AD_task_chasing_combine;" From a67b885969ce2c03308c64ae415e1b9065a7a53d Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Fri, 30 Aug 2024 21:10:03 +0200 Subject: [PATCH 35/44] wrongly commited - deleted --- .../Modules/PathFinderModule - Kopie (15).lua | 2656 ----------------- 1 file changed, 2656 deletions(-) delete mode 100644 scripts/Modules/PathFinderModule - Kopie (15).lua diff --git a/scripts/Modules/PathFinderModule - Kopie (15).lua b/scripts/Modules/PathFinderModule - Kopie (15).lua deleted file mode 100644 index f9447250..00000000 --- a/scripts/Modules/PathFinderModule - Kopie (15).lua +++ /dev/null @@ -1,2656 +0,0 @@ ---[[ -New to Pathfinder: -- 3 Settings are considered: restrictToField, avoidFruit, pathFinderTime - -1. restrictToField -The Pathfinder tries to find a path to target in the limit of the current field borders. -This is only possible if the vehicle is located inside a field. -If disabled, only setting avoidFruit limits or not, the shortest path to target will be calculated! - -2. avoidFruit -The Pathfinder tries to find a path to target without passing through fruit. -This is effective if vehicle is inside a field. -NEW: Working also outside of a field, i.e. if possible a path around a field to network will be searched for! -If disabled, the shortest path to target will be calculated! - -3. pathFinderTime -Is more a factor for the range the Pathfinder will search for path to target, default is 1. -In case fruit avoid on large fields with sufficient headland is not working, you may try to increase. -But be aware the calculation time will increase as well! - -- 3 fallback scenario will automatic take effect: -If setting restrictToField is enabled, fallback 1 and 2 are possible: -fallback 1: -The first fallback will extend the field border by 30+ meters around the field pathfinding was started. -With this the vehicle will search for a path also around the field border! -30+ meters depend on the vehicle + trailer turn radius, if greater then extended wider. -fallback 2: -Second fallback will deactivate all field border restrictions, means setting avoidFruit will limit or not the search range for path. - -fallback 3: -Third fallback will take effect only if setting avoidFruit is enabled. -It will disable fruit avoid automatically if no path was found. - -Inside informations: -This is a calculation with the worst assumption of all cells to be checked: - -Number of cells: -#cells = MAX_PATHFINDER_STEPS_PER_FRAME / 2 * MAX_PATHFINDER_STEPS_TOTAL * 3 (next directions - see determineNextGridCells) - -PathFinderModule.MAX_PATHFINDER_STEPS_PER_FRAME = 10 -PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL = 400 -#cells = 6000 - -with minTurnRadius = 7m calculated area: - -cellsize = 7m * 7m = 49m^2 -overall area = #cells * cellsize * pathFinderTime - -with pathFinderTime = 1: -overall area = 6000 * 49 * 1 = 294000 m^2 -for quadrat field layout: side length ~ 540m - -with pathFinderTime = 2: side length ~ 760m -with pathFinderTime = 3: side length ~ 940m - -This is inclusive of the field border cells! -]] - -PathFinderModule = {} -PathFinderModule.debug = false - -PathFinderModule.PATHFINDER_MAX_RETRIES = 3 -PathFinderModule.MAX_PATHFINDER_STEPS_PER_FRAME = 2 -PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL = 400 -PathFinderModule.MAX_PATHFINDER_STEPS_COMBINE_TURN = 100 -PathFinderModule.PATHFINDER_FOLLOW_DISTANCE = 45 -PathFinderModule.PATHFINDER_TARGET_DISTANCE = 7 -PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE = 16 -PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE_CLOSE = 6 -PathFinderModule.PATHFINDER_START_DISTANCE = 7 -PathFinderModule.MAX_FIELDBORDER_CELLS = 5 - -PathFinderModule.PP_MIN_DISTANCE = 20 -PathFinderModule.PP_CELL_X = 9 -PathFinderModule.PP_CELL_Z = 9 - -PathFinderModule.GRID_SIZE_FACTOR = 0.5 -PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER = 1.1 - -PathFinderModule.PP_MAX_EAGER_LOOKAHEAD_STEPS = 1 - -PathFinderModule.MIN_FRUIT_VALUE = 50 -PathFinderModule.SLOPE_DETECTION_THRESHOLD = math.rad(20) -PathFinderModule.NEW_PF_STEP_FACTOR = 4 ---[[ -from Giants Engine: -AITurnStrategy.SLOPE_DETECTION_THRESHOLD = 0.5235987755983 -]] - -function PathFinderModule:new(vehicle) - local o = {} - setmetatable(o, self) - self.__index = self - o.vehicle = vehicle - PathFinderModule.reset(o) - return o -end - -function PathFinderModule:reset() - PathFinderModule.debugMsg(self.vehicle, "PFM:reset start") - self.mask = AutoDrive.collisionMaskTerrain - self.steps = 0 - self.grid = {} - self.wayPoints = {} - self.initNew = false - self.path = {} - self.diffOverallNetTime = 0 - self.retryCounter = 0 - self.delayTime = 0 - self.restrictToField = false - self.avoidFruitSetting = false - self.destinationId = 0 - self.fallBackMode1 = false - self.fallBackMode2 = false - self.fallBackMode3 = false - self.isFinished = true - self.smoothDone = true - self.fruitAreas = {} - - if AutoDrive.experimentalFeatures.NewPathfinder then - self.PP_UP = 0 - self.PP_UP_LEFT = 1 - self.PP_LEFT = 2 - self.PP_DOWN_LEFT = 3 - self.PP_DOWN = 4 - self.PP_DOWN_RIGHT = 5 - self.PP_RIGHT = 6 - self.PP_UP_RIGHT = 7 - self.direction_to_text = { - "PP_UP", - "PP_UP_LEFT", - "PP_LEFT", - "PP_DOWN_LEFT", - "PP_DOWN", - "PP_DOWN_RIGHT", - "PP_RIGHT", - "PP_UP_RIGHT", - "unknown" - } - self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) - self.isNewPF = true - else - self.PP_UP = 0 - self.PP_UP_RIGHT = 1 - self.PP_RIGHT = 2 - self.PP_DOWN_RIGHT = 3 - self.PP_DOWN = 4 - self.PP_DOWN_LEFT = 5 - self.PP_LEFT = 6 - self.PP_UP_LEFT = 7 - self.direction_to_text = { - "PP_UP", - "PP_UP_RIGHT", - "PP_RIGHT", - "PP_DOWN_RIGHT", - "PP_DOWN", - "PP_DOWN_LEFT", - "PP_LEFT", - "PP_UP_LEFT", - "unknown" - } - self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) * 2 / 3 - self.isNewPF = false - end -end - -function PathFinderModule:hasFinished() - if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then - return false - end - if self.isFinished and self.smoothDone == true then - return true - end - return false -end - -function PathFinderModule:getPath() - return self.wayPoints -end - -function PathFinderModule:startPathPlanningToNetwork(destinationId) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningToNetwork destinationId %s", - tostring(destinationId) - ) - ) - local closest = self.vehicle:getClosestWayPoint() - self:startPathPlanningToWayPoint(closest, destinationId) - self.goingToNetwork = true -end - -function PathFinderModule:startPathPlanningToWayPoint(wayPointId, destinationId) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningToWayPoint wayPointId %s", - tostring(wayPointId) - ) - ) - local targetNode = ADGraphManager:getWayPointById(wayPointId) - local wayPoints = ADGraphManager:pathFromTo(wayPointId, destinationId) - if wayPoints ~= nil and #wayPoints > 1 then - local vecToNextPoint = {x = wayPoints[2].x - targetNode.x, z = wayPoints[2].z - targetNode.z} - self:startPathPlanningTo(targetNode, vecToNextPoint) - self.goingToNetwork = true - self.destinationId = destinationId - self.targetWayPointId = wayPointId - self.appendWayPoints = wayPoints - end - return -end - -function PathFinderModule:startPathPlanningToPipe(combine, chasing) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:startPathPlanningToPipe") - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningToPipe combine %s", - tostring(combine:getName()) - ) - ) - local _, worldY, _ = getWorldTranslation(combine.components[1].node) - local rx, _, rz = localDirectionToWorld(combine.components[1].node, 0, 0, 1) - if combine.components[2] ~= nil and combine.components[2].node ~= nil then - rx, _, rz = localDirectionToWorld(combine.components[2].node, 0, 0, 1) - end - local combineVector = {x = rx, z = rz} - - local pipeChasePos, pipeChaseSide = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getPipeChasePosition(true) - -- We use the follow distance as a proxy measure for "what works" for the size of the - -- field being worked. - -- local followDistance = AutoDrive.getSetting("followDistance", self.vehicle) - -- Use the length of the tractor-trailer combo to determine how far to drive to straighten - -- the trailer. - -- 2*math.sin(math.pi/8)) is the third side of a 45-67.5-67.5 isosceles triangle with the - -- equal sides being the length of the tractor train - local lengthOffset = combine.size.length / 2 + - AutoDrive.getTractorTrainLength(self.vehicle, true, false) * (2 * math.sin(math.pi / 8)) - -- A bit of a sanity check, in case the vehicle is absurdly long. - --if lengthOffset > self.PATHFINDER_FOLLOW_DISTANCE then - -- lengthOffset = self.PATHFINDER_FOLLOW_DISTANCE - --elseif - if lengthOffset <= self.PATHFINDER_TARGET_DISTANCE then - lengthOffset = self.PATHFINDER_TARGET_DISTANCE - end - - --local target = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} - -- The sugarcane harvester needs extra room or it collides - --if pipeChaseSide ~= CombineUnloaderMode.CHASEPOS_REAR or CombineUnloaderMode:isSugarcaneHarvester(combine) then - -- AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:startPathPlanningToPipe?lengthOffset " .. lengthOffset) - -- local straightenNode = {x = pipeChasePos.x - lengthOffset * rx, y = worldY, z = pipeChasePos.z - lengthOffset * rz} - -- self:startPathPlanningTo(straightenNode, combineVector) - -- table.insert(self.appendWayPoints, target) - --else - -- self:startPathPlanningTo(target, combineVector) - --end - if combine.ad.isAutoAimingChopper then - -- local pathFinderTarget = {x = pipeChasePos.x - (self.PATHFINDER_TARGET_DISTANCE) * rx, y = worldY, z = pipeChasePos.z - (self.PATHFINDER_TARGET_DISTANCE) * rz} - local pathFinderTarget = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} - self:startPathPlanningTo(pathFinderTarget, combineVector) - - elseif combine.ad.isFixedPipeChopper then - local pathFinderTarget = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} - -- only append target points / try to straighten the driver/trailer combination if we are driving up to the pipe not the rear end - if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then - pathFinderTarget = {x = pipeChasePos.x - (combine.size.length) * rx, y = worldY, z = pipeChasePos.z - (combine.size.length) * rz} - end - local appendedNode = {x = pipeChasePos.x - (combine.size.length / 2 * rx), y = worldY, z = pipeChasePos.z - (combine.size.length / 2 * rz)} - - self:startPathPlanningTo(pathFinderTarget, combineVector) - - if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then - table.insert(self.appendWayPoints, appendedNode) - end - else - -- combine.ad.isHarvester - local pathFinderTarget = {x = pipeChasePos.x, y = worldY, z = pipeChasePos.z} - -- only append target points / try to straighten the driver/trailer combination if we are driving up to the pipe not the rear end - if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then - pathFinderTarget = {x = pipeChasePos.x - (lengthOffset) * rx, y = worldY, z = pipeChasePos.z - (lengthOffset) * rz} - end - local appendedNode = {x = pipeChasePos.x - (combine.size.length / 2 * rx), y = worldY, z = pipeChasePos.z - (combine.size.length / 2 * rz)} - - self:startPathPlanningTo(pathFinderTarget, combineVector) - - if pipeChaseSide ~= AutoDrive.CHASEPOS_REAR then - table.insert(self.appendWayPoints, appendedNode) - table.insert(self.appendWayPoints, pipeChasePos) - end - end - - if combine.spec_combine ~= nil and combine.ad.isHarvester then - if combine.spec_combine.fillUnitIndex ~= nil and combine.spec_combine.fillUnitIndex ~= 0 then - local fillType = g_fruitTypeManager:getFruitTypeIndexByFillTypeIndex(combine:getFillUnitFillType(combine.spec_combine.fillUnitIndex)) - if fillType ~= nil then - self.fruitToCheck = fillType - local fruitType = g_fruitTypeManager:getFruitTypeByIndex(fillType) - - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningToPipe self.fruitToCheck %s Fruit name %s title %s", - tostring(self.fruitToCheck), - tostring(fruitType.fillType.name), - tostring(fruitType.fillType.title) - ) - ) - end - end - end - - self.goingToPipe = true - if AutoDrive.getDistanceBetween(self.vehicle, combine) < 50 then - -- shorten path calculation for close combine - self.max_pathfinder_steps = PathFinderModule.MAX_PATHFINDER_STEPS_COMBINE_TURN - end - self.chasingVehicle = chasing -end - -function PathFinderModule:startPathPlanningToVehicle(targetVehicle, targetDistance) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningToVehicle targetVehicle %s", - tostring(targetVehicle:getName()) - ) - ) - local worldX, worldY, worldZ = getWorldTranslation(targetVehicle.components[1].node) - local rx, _, rz = localDirectionToWorld(targetVehicle.components[1].node, 0, 0, 1) - local targetVector = {x = rx, z = rz} - - local wpBehind = {x = worldX - targetDistance * rx, y = worldY, z = worldZ - targetDistance * rz} - self:startPathPlanningTo(wpBehind, targetVector) - - self.goingToPipe = false - self.chasingVehicle = true - self.isSecondChasingVehicle = true - if targetVehicle.ad ~= nil and targetVehicle.ad.pathFinderModule ~= nil and targetVehicle.ad.pathFinderModule.fruitToCheck ~= nil then - self.fruitToCheck = targetVehicle.ad.pathFinderModule.fruitToCheck - end -end - -function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningTo targetPoint x,z %d %d", - math.floor(targetPoint.x), - math.floor(targetPoint.z) - ) - ) - ADScheduler:addPathfinderVehicle(self.vehicle) - self.targetVector = targetVector - local vehicleWorldX, vehicleWorldY, vehicleWorldZ = getWorldTranslation(self.vehicle.components[1].node) - local vehicleRx, _, vehicleRz = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, 1) - local vehicleVector = {x = vehicleRx, z = vehicleRz} - self.startX = vehicleWorldX + self.PATHFINDER_START_DISTANCE * vehicleRx - self.startZ = vehicleWorldZ + self.PATHFINDER_START_DISTANCE * vehicleRz - - local angleRad = math.atan2(targetVector.z, targetVector.x) - angleRad = AutoDrive.normalizeAngle(angleRad) - - self.vectorX = {x = math.cos(angleRad) * self.minTurnRadius, z = math.sin(angleRad) * self.minTurnRadius} - self.vectorZ = {x = - math.sin(angleRad) * self.minTurnRadius, z = math.cos(angleRad) * self.minTurnRadius} - - --Make the target a few meters ahead of the road to the start point - local targetX = targetPoint.x - math.cos(angleRad) * self.PATHFINDER_TARGET_DISTANCE - local targetZ = targetPoint.z - math.sin(angleRad) * self.PATHFINDER_TARGET_DISTANCE - - self.grid = {} - self.steps = 0 - self.retryCounter = 0 - self.isFinished = false - self.fallBackMode1 = false -- disable restrict to field - self.fallBackMode2 = false -- disable restrict to field border - self.fallBackMode3 = false -- disable avoid fruit - self.max_pathfinder_steps = PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL * AutoDrive.getSetting("pathFinderTime") - - self.fruitToCheck = nil - - self.startCell = {x = 0, z = 0} - self.startCell.direction = self:worldDirectionToGridDirection(vehicleVector) - self.startCell.visited = false - self.startCell.out = nil - self.startCell.isRestricted = false - self.startCell.hasCollision = false - self.startCell.hasFruit = false - self.startCell.steps = 0 - self.startCell.bordercells = 0 - self.currentCell = nil - - local vehicleBehindX, _, vehicleBehindZ = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, -self.minTurnRadius) - local vehicleBehindVector = {x = vehicleBehindX, z = vehicleBehindZ} - self.behindStartCell = self:worldLocationToGridLocation(vehicleWorldX + vehicleBehindX, vehicleWorldZ + vehicleBehindZ) - self.behindStartCell.direction = self:worldDirectionToGridDirection(vehicleBehindVector, vehicleVector) - - -- table.insert(self.grid, self.startCell) - local gridKey = string.format("%d|%d|%d", self.startCell.x, self.startCell.z, self.startCell.direction) - self.grid[gridKey] = self.startCell - - self.smoothStep = 0 - self.smoothDone = false - self.target = {x = targetX, z = targetZ} - - local targetCellZ = (((targetX - self.startX) / self.vectorX.x) * self.vectorX.z - targetZ + self.startZ) / (((self.vectorZ.x / self.vectorX.x) * self.vectorX.z) - self.vectorZ.z) - local targetCellX = (targetZ - self.startZ - targetCellZ * self.vectorZ.z) / self.vectorX.z - targetCellX = AutoDrive.round(targetCellX) - targetCellZ = AutoDrive.round(targetCellZ) - self.targetCell = {x = targetCellX, z = targetCellZ, direction = self.PP_UP} - self.targetAhead = {x = targetX + self.vectorX.x, z = targetZ + self.vectorX.z} - self.targetAheadCell = self:worldLocationToGridLocation(self.targetAhead.x, self.targetAhead.z) - - self:determineBlockedCells(self.targetCell) - --self:checkGridCell(self.targetCell) - - self.appendWayPoints = {} - self.appendWayPoints[1] = targetPoint - - self.goingToCombine = false - - self.startIsOnField = AutoDrive.checkIsOnField(vehicleWorldX, vehicleWorldY, vehicleWorldZ) and self.vehicle.ad.sensors.frontSensorField:pollInfo(true) - self.endIsOnField = AutoDrive.checkIsOnField(targetX, vehicleWorldY, targetZ) - self.restrictToField = AutoDrive.getSetting("restrictToField", self.vehicle) and self.startIsOnField and self.endIsOnField - self.goingToPipe = false - self.chasingVehicle = false - self.isSecondChasingVehicle = false - self.goingToNetwork = false - self.destinationId = nil - self.completelyBlocked = false - self.targetBlocked = false --self.targetCell.hasCollision or self.targetCell.isRestricted --> always false TODO: how to use this ? - self.blockedByOtherVehicle = false - self.avoidFruitSetting = AutoDrive.getSetting("avoidFruit", self.vehicle) - -- self.targetFieldId = g_farmlandManager:getFarmlandIdAtWorldPosition(targetX, targetZ) -- only has ID if vector target is onField - -- self.targetFieldId = nil - -- if self.restrictToField and self.targetFieldId ~= nil and self.targetFieldId > 0 then - -- self.reachedFieldBorder = startIsOnField - -- local targetFieldPos = {x = g_farmlandManager.farmlands[self.targetFieldId].xWorldPos, z = g_farmlandManager.farmlands[self.targetFieldId].zWorldPos} - - -- self.fieldCell = self:worldLocationToGridLocation(targetFieldPos.x, targetFieldPos.z) - -- else - -- self.reachedFieldBorder = true - -- end - self.chainStartToTarget = {} - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningTo self.minTurnRadius %s vectorX.x,vectorX.z %s %s vectorZ.x,vectorZ.z %s %s", - tostring(self.minTurnRadius), - tostring(self.vectorX.x), - tostring(self.vectorX.z), - tostring(self.vectorZ.x), - tostring(self.vectorZ.z) - ) - ) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningTo startCell xz %d %d direction %s", - math.floor(self.startCell.x), - math.floor(self.startCell.z), - tostring(self.direction_to_text[self.startCell.direction+1]) - ) - ) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningTo start targetCell xz %d %d direction %s", - math.floor(self.targetCell.x), - math.floor(self.targetCell.z), - tostring(self.direction_to_text[self.targetCell.direction+1]) - ) - ) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningTo restrictToField %s endIsOnField %s", - tostring(self.restrictToField), - tostring(self.endIsOnField) - ) - ) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM startPathPlanningTo self.fruitToCheck %s getSetting avoidFruit %s", - tostring(self.fruitToCheck), - tostring(self.avoidFruitSetting) - ) - ) -end - -function PathFinderModule:restartAtNextWayPoint() - self.targetWayPointId = self.appendWayPoints[2].id - local targetNode = ADGraphManager:getWayPointById(self.targetWayPointId) - local wayPoints = ADGraphManager:pathFromTo(self.targetWayPointId, self.destinationId) - if wayPoints ~= nil and #wayPoints > 1 then - local vecToNextPoint = {x = wayPoints[2].x - targetNode.x, z = wayPoints[2].z - targetNode.z} - local storedRetryCounter = self.retryCounter - local storedTargetWayPointId = self.targetWayPointId - local storedDestinationId = self.destinationId - self:startPathPlanningTo(targetNode, vecToNextPoint) - self.retryCounter = storedRetryCounter - self.destinationId = storedDestinationId - self.fallBackMode1 = false -- disable restrict to field - self.fallBackMode2 = false -- disable restrict to field border - self.fallBackMode3 = false -- disable avoid fruit - self.targetWayPointId = storedTargetWayPointId - if self.targetWayPointId ~= nil then - self.appendWayPoints = ADGraphManager:pathFromTo(self.targetWayPointId, self.destinationId) - end - end - self:autoRestart() -end - -function PathFinderModule:autoRestart() - PathFinderModule.debugMsg(self.vehicle, "PFM:autoRestart start") - self.steps = 0 - self.grid = {} - self.wayPoints = {} - self.initNew = false - self.path = {} - self.diffOverallNetTime = 0 - self.startCell.visited = false - self.startCell.out = nil - self.currentCell = nil - - local gridKey = string.format("%d|%d|%d", self.startCell.x, self.startCell.z, self.startCell.direction) - self.grid[gridKey] = self.startCell - - self:determineBlockedCells(self.targetCell) - self.smoothStep = 0 - self.smoothDone = false - self.completelyBlocked = false - -- self.targetBlocked = false --> always false TODO: how to use this ? -end - -function PathFinderModule:abort() - PathFinderModule.debugMsg(self.vehicle, "PFM:abort start") - self.isFinished = true - self.smoothDone = true - self.wayPoints = {} - ADScheduler:removePathfinderVehicle(self.vehicle) -end - -function PathFinderModule:isBlocked() --> true if no path in grid found -- used in: ExitFieldTask, UnloadAtDestinationTask - return self.completelyBlocked -- or self.targetBlocked --> always false TODO: how to use this ? -end - -function PathFinderModule:isTargetBlocked() --> always false TODO: how to use this ? -- used in: ExitFieldTask, UnloadAtDestinationTask, EmptyHarvesterTask - return self.targetBlocked -end - --- return the actual and max number of iterations the pathfinder will perform by itself, could be used to show info in HUD -function PathFinderModule:getCurrentState() - local maxStates = 1 - local actualState = 1 - if self.restrictToField then - maxStates = maxStates + 2 - end - if self.avoidFruitSetting then - maxStates = maxStates + 1 - end - if self.destinationId ~= nil then - maxStates = maxStates + 3 - end - - if self.fallBackMode1 then - actualState = actualState + 1 - end - if self.fallBackMode2 then - actualState = actualState + 1 - end - if self.fallBackMode3 then - actualState = actualState + 1 - end - if self.destinationId ~= nil and self.retryCounter > 0 then - actualState = actualState + 1 - end - - return actualState, maxStates, self.steps, self.max_pathfinder_steps -end - -function PathFinderModule:timedOut() --> not self:isBlocked() -- used in: ExitFieldTask, UnloadAtDestinationTask - return not self:isBlocked() -end - -function PathFinderModule:addDelayTimer(delayTime) -- used in: ExitFieldTask, UnloadAtDestinationTask, CatchCombinePipeTask, EmptyHarvesterTask - self.delayTime = delayTime -end - -function PathFinderModule:update(dt) - - --stop if called without prior 'start' method calls - if self.startCell == nil then - self:abort() - end - - self.delayTime = math.max(0, self.delayTime - dt) - if self.delayTime > 0 then - return - end - - if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then - if self.isNewPF then - self:drawDebugNewPF() - else - if self.isFinished and self.smoothDone and self.wayPoints ~= nil and self.chainStartToTarget ~= nil and #self.chainStartToTarget > 0 and self.vehicle.ad.stateModule:getSpeedLimit() > 40 then - self:drawDebugForCreatedRoute() - else - self:drawDebugForPF() - end - end - end - if self.isFinished then - if not self.smoothDone then - if self.isNewPF then - self:createWayPointsNew() - else - self:createWayPoints() - end - end - if self.smoothDone then - ADScheduler:removePathfinderVehicle(self.vehicle) - PathFinderModule.debugMsg(self.vehicle, "PFM:update Complete #self.path %s" - , tostring(#self.path) - ) - end - return - end - - self.steps = self.steps + 1 - if (self.steps % 100) == 0 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - self.steps %d #self.grid %d", self.steps, table.count(self.grid)) - end - - if self.completelyBlocked or self.targetBlocked or self.steps > (self.max_pathfinder_steps) then - --[[ We need some better logic here. - Some situations might be solved by the module itself by either - a) 'fallBackMode (ignore fruit and field restrictions)' - b) 'try next wayPoint' - while others should be handled by the calling task, to properly assess the current situation - c) 'retry same location - with or without prior pausing' - d) 'update target location and reinvoke pathfinder if target has moved' - e) 'try different field exit strategy' - --]] - -- Only allow fallback if we are not heading for a moving vehicle - local fallBackModeAllowed1 = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.restrictToField) and (not self.fallBackMode1) -- disable restrict to field - local fallBackModeAllowed2 = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.restrictToField) and (not self.fallBackMode2) -- disable restrict to field border - local fallBackModeAllowed3 = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.avoidFruitSetting) and (not self.fallBackMode3) -- disable avoid fruit - local increaseStepsAllowed = (not self.chasingVehicle) and (not self.isSecondChasingVehicle) and (self.max_pathfinder_steps < PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL * AutoDrive.getSetting("pathFinderTime")) -- increase number of steps if possible - - -- Only allow auto restart when planning path to network and we can adjust target wayPoint - local retryAllowed = self.destinationId ~= nil and self.retryCounter < self.PATHFINDER_MAX_RETRIES - - if fallBackModeAllowed1 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed1: yes -> going fallback now -> disable restrict to field #self.grid %d", table.count(self.grid)) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed1: yes -> going fallback now -> disable restrict to field #self.grid %d", - table.count(self.grid) - ) - ) - self.fallBackMode1 = true - self:autoRestart() - elseif fallBackModeAllowed2 and not self.isNewPF then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", table.count(self.grid)) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", - table.count(self.grid) - ) - ) - self.fallBackMode2 = true - self:autoRestart() - elseif fallBackModeAllowed3 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed3: yes -> going fallback now -> disable avoid fruit #self.grid %d", table.count(self.grid)) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed3: yes -> going fallback now -> disable avoid fruit #self.grid %d", - table.count(self.grid) - ) - ) - self.fallBackMode3 = true - self:autoRestart() - elseif increaseStepsAllowed then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> increaseStepsAllowed: yes -> restart #self.grid %d self.max_pathfinder_steps %d", table.count(self.grid), self.max_pathfinder_steps) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - error - retryAllowed: no -> increaseStepsAllowed: yes -> restart -> disable avoid fruit #self.grid %d self.max_pathfinder_steps %d", - table.count(self.grid), - self.max_pathfinder_steps - ) - ) - self.max_pathfinder_steps = self.max_pathfinder_steps + PathFinderModule.MAX_PATHFINDER_STEPS_COMBINE_TURN - self.max_pathfinder_steps = math.min(self.max_pathfinder_steps, PathFinderModule.MAX_PATHFINDER_STEPS_TOTAL * AutoDrive.getSetting("pathFinderTime")) - self.fallBackMode1 = false - self.fallBackMode2 = false - self.fallBackMode3 = false - self:autoRestart() - elseif retryAllowed then - self.retryCounter = self.retryCounter + 1 - --if we are going to the network and can't find a path. Just select the next waypoint for now - if self.appendWayPoints ~= nil and #self.appendWayPoints > 2 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: yes -> retry now retryCounter %d", self.retryCounter) - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - error - retryAllowed: yes -> retry now retryCounter %d", - self.retryCounter - ) - ) - self:restartAtNextWayPoint() - else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: yes -> but no appendWayPoints") - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - error - retryAllowed: yes -> but no appendWayPoints" - ) - ) - self:abort() - end - else - if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then - return - end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed: no -> aborting now") - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed: no -> aborting now" - ) - ) - self:abort() - end - return - end - - if self.isNewPF then - if not self.isFinished then - if not self.initNew then - self:setupNew(self.behindStartCell, self.startCell,self.targetCell) - if self.nodeBehindStart == self.nodeGoal then - self.completelyBlocked = true - return -- no valid path - end - end - local diffNetTime = netGetTime() - - local current - local add_neighbor_fn = function(neighbor, cost) - if self:isDriveable(neighbor) then - if not self.closedset[neighbor] then - if not cost then cost = self:get_cost(current, neighbor) end - local tentative_g_score = self.g_score[current] + cost - local openset_idx = self.openset[neighbor] - if not openset_idx or tentative_g_score < self.g_score[neighbor] then - self.came_from[neighbor] = current - self.g_score[neighbor] = tentative_g_score - self.h_score[neighbor] = self.h_score[neighbor] or self:estimate_cost(neighbor, self.nodeGoal) - self.f_score[neighbor] = tentative_g_score + self.h_score[neighbor] - self.openset[neighbor] = true - end - end - end - end - local count = 0 - while next(self.openset) do - count = count + 1 - if count > 10000 then - diffNetTime = netGetTime() - diffNetTime - self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime - - AutoDrive.debugMsg(self.vehicle, "PFM:find ERROR exit counter count %d self.diffOverallNetTime %d" - , count - , self.diffOverallNetTime - ) - - self.completelyBlocked = true - return -- no valid path - end - current = self:pop_best_node(self.openset, self.f_score) - if current == self.nodeGoal or self:reachedGoal(current, self.nodeGoal) then - self.came_from[self.nodeGoal] = current - self.path = self:unwind_path({}, self.came_from, self.nodeGoal) - table.insert(self.path, self.nodeGoal) - diffNetTime = netGetTime() - diffNetTime - self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime - - PathFinderModule.debugMsg(self.vehicle, "PFM:update find goal reached self.steps %d diffOverallNetTime %d self.nodeGoal xz %d,%d current xz %d,%d" - , self.steps - , self.diffOverallNetTime - , self.nodeGoal.x, self.nodeGoal.z - , current.x, current.z - ) - - self.isFinished = true - return -- found path - end - if current then self.closedset[current] = true end - local from_node = self.came_from[current] - self:get_neighbors(current, from_node, add_neighbor_fn) - if count > (ADScheduler:getStepsPerFrame() * PathFinderModule.NEW_PF_STEP_FACTOR) then - diffNetTime = netGetTime() - diffNetTime - self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime - - PathFinderModule.debugMsg(self.vehicle, "PFM:find steps in frame count %d diffNetTime %d self.diffOverallNetTime %d" - , count - , diffNetTime - , self.diffOverallNetTime - ) - - return -- shedule - end - end - diffNetTime = netGetTime() - diffNetTime - self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime - - PathFinderModule.debugMsg(self.vehicle, "PFM:find exit end count %d self.diffOverallNetTime %d" - , count - , self.diffOverallNetTime - ) - - self.completelyBlocked = true - return -- no valid path - end - else - --We should see some perfomance increase by localizing the sqrt/pow functions right here - local sqrt = math.sqrt - local distanceFunc = function(a, b) - return sqrt(a * a + b * b) - end - - for i = 1, ADScheduler:getStepsPerFrame(), 1 do - if self.currentCell == nil then - - self.currentCell = self:findClosestCell(self.grid, math.huge) - - if self.currentCell ~= nil and distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) < 1.5 then - - if self.currentCell.out == nil then - self:determineNextGridCells(self.currentCell) - end - - if self:reachedTargetsNeighbor(self.currentCell.out) then - return - end - end - - if self.currentCell == nil then - --Mark process stopped if we have no more cells to check - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - Mark process stopped if we have no more cells to check") - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - Mark process stopped if we have no more cells to check #self.grid %d", - table.count(self.grid) - ) - ) - self.completelyBlocked = true - break - end - else - if self.currentCell.out == nil then - self:determineNextGridCells(self.currentCell) - end - self:testNextCells(self.currentCell) - - --Try shortcutting the process here. We dont have to go through the whole grid if one of the out points is viable and closer than the currenCell which was already closest - local currentDistance = distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) - - local outCells = {} - for _, outCell in pairs(self.currentCell.out) do - local gridKey = string.format("%d|%d|%d", outCell.x, outCell.z, outCell.direction) - if self.grid[gridKey] ~= nil then - table.insert(outCells, self.grid[gridKey]) - end - end - local nextCell = self:findClosestCell(outCells, currentDistance) - - -- Lets again check if we have reached our target already - if self:reachedTargetsNeighbor(self.currentCell.out) then - return - end - - self.currentCell = nextCell - end - end - end -end - -function PathFinderModule:reachedTargetsNeighbor(cells) - for _, outCell in pairs(cells) do - if outCell.x == self.targetCell.x and outCell.z == self.targetCell.z then - self.isFinished = true - self.targetCell.incoming = self.currentCell --.incoming - - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - path found") - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - path found #self.grid %d", - table.count(self.grid) - ) - ) - - return true - end - end - return false -end - -function PathFinderModule:findClosestCell(cells, startDistance) - local cellsToCheck = cells - local sqrt = math.sqrt - local distanceFunc = function(a, b) - return sqrt(a * a + b * b) - end - local minDistance = startDistance - local bestCell = nil - local bestSteps = math.huge - - for _, cell in pairs(cellsToCheck) do - if (not cell.visited) and (not cell.hasCollision) and (not cell.isRestricted) and (cell.bordercells < PathFinderModule.MAX_FIELDBORDER_CELLS) then - local distance = distanceFunc(self.targetCell.x - cell.x, self.targetCell.z - cell.z) - - if (distance < minDistance) or (distance == minDistance and cell.steps < bestSteps) then - minDistance = distance - bestCell = cell - bestSteps = cell.steps - end - end - end - - return bestCell -end - -function PathFinderModule:testNextCells(cell) - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM testNextCells start cell xz %d %d isRestricted %s hasFruit %s direction %s", - math.floor(cell.x), - math.floor(cell.z), - tostring(cell.isRestricted), - tostring(cell.hasFruit), - tostring(self.direction_to_text[cell.direction+1]) - ) - ) - end - for _, location in pairs(cell.out) do - local createPoint = true - local duplicatePointDirection = -1 - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM testNextCells location xz %d %d direction %s", - math.floor(location.x), - math.floor(location.z), - tostring(self.direction_to_text[location.direction+1]) - ) - ) - end - for i = -1, self.PP_UP_LEFT, 1 do -- important: do not break this loop to check for all directions! - local gridKey = string.format("%d|%d|%d", location.x, location.z, i) - if self.grid[gridKey] ~= nil then - -- cell is already in the grid - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM testNextCells gridKey %s cell.x,cell.z %s %s direction %s", - tostring(gridKey), - tostring(self.grid[gridKey].x), - tostring(self.grid[gridKey].z), - tostring(self.direction_to_text[self.grid[gridKey].direction+1]) - ) - ) - end - - if self.grid[gridKey].x == location.x and self.grid[gridKey].z == location.z then -- out cell is already in grid - - if self.grid[gridKey].direction == -1 then - createPoint = false - elseif self.grid[gridKey].direction == location.direction then - createPoint = false - if self.grid[gridKey].steps > (cell.steps + 1) then --found shortcut - self.grid[gridKey].incoming = cell - self.grid[gridKey].steps = cell.steps + 1 - end - --elseif self.grid[gridKey].direction ~= location.direction then - --duplicatePointDirection = self.grid[gridKey].direction -- remember the grid direction - --if self.grid[gridKey].steps > (cell.steps + 1) then --found shortcut -> not true!!! The outgoing angles would be all wrong here. This caused issues with undrivable paths being generated! - --self.grid[gridKey].incoming = cell - --self.grid[gridKey].steps = cell.steps + 1 - --end - end - end - end - end - if createPoint then - local gridKey - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM testNextCells location xz %d %d createPoint %s", - math.floor(location.x), - math.floor(location.z), - tostring(self.direction_to_text[location.direction+1]) - ) - ) - end - if duplicatePointDirection >= 0 then - -- if different direction, it is not necessary to check the cell details again, just add a new entry in grid with known required restrictions - -- Todo : Not true!! If we come from a different direction we ususally have a differently sized collision box to check. There is a difference between a 0° angle when coming from the last cell and a +/- 45° angle. - gridKey = string.format("%d|%d|%d", location.x, location.z, duplicatePointDirection) - location.isRestricted = self.grid[gridKey].isRestricted - location.hasCollision = self.grid[gridKey].hasCollision - location.bordercells = self.grid[gridKey].bordercells - location.hasFruit = self.grid[gridKey].hasFruit - location.fruitValue = self.grid[gridKey].fruitValue - - if not location.isRestricted and not location.hasCollision and location.incoming ~= nil then - -- check for up/down is to big or below water level - -- this is a required check as we come from different direction - local worldPos = self:gridLocationToWorldLocation(location) - local worldPosPrevious = self:gridLocationToWorldLocation(location.incoming) - location.hasCollision = location.hasCollision or self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down is to big or below water level - end - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM testNextCells different direction xz %d %d createPoint %s", - math.floor(location.x), - math.floor(location.z), - tostring(self.direction_to_text[location.direction+1]) - ) - ) - end - else - self:checkGridCell(location) - end - gridKey = string.format("%d|%d|%d", location.x, location.z, location.direction) - self.grid[gridKey] = location - end - end - - cell.visited = true -end - -function PathFinderModule:checkGridCell(cell) - local worldPos = self:gridLocationToWorldLocation(cell) - --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped - - cell.isOnField = AutoDrive.checkIsOnField(worldPos.x, 0, worldPos.z) - if self.restrictToField and (self.fallBackMode1 and not self.fallBackMode2) then - -- limit cells to field border only possible if started on field - cell.bordercells = cell.incoming.bordercells + 1 -- by default we assume the new cell is not on field, so increase the counter - - if cell.incoming.bordercells == 0 then - -- if incoming cell is on field we check if the new is also on field - if cell.isOnField then - -- still on field, so set the current cell counter to 0 - cell.bordercells = 0 - end - end - - if cell.bordercells > 0 then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkGridCell - xz %d %d cell.bordercells %d #self.grid %d", - math.floor(cell.x), - math.floor(cell.z), - math.floor(cell.bordercells), - table.count(self.grid) - ) - ) - end - end - end - - -- check the most probable restrictions on field first to prevent unneccessary checks - if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 and self.fallBackMode2) then - -- in fallBackMode1 we ignore the field restriction - cell.isRestricted = cell.isRestricted or (not cell.isOnField) - - if cell.isRestricted then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkGridCell isRestricted self.restrictToField %s self.fallBackMode1 %s isOnField %s x,z %d %d", - tostring(self.restrictToField), - tostring(self.fallBackMode1), - tostring(cell.isOnField), - math.floor(worldPos.x), - math.floor(worldPos.z) - ) - ) - end - end - end - - local gridFactor = PathFinderModule.GRID_SIZE_FACTOR * 1.3 --> 0.6 - if self.isSecondChasingVehicle then - gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER * 1.6 --> 1.7 - end - local corners = self:getCorners(cell, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) - - if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then - -- check for fruit - self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found - end - - if not cell.isRestricted and cell.incoming ~= nil then - -- check for up/down is to big or below water level - local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) - local angelToSlope, angle = self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level - cell.angle = angle - cell.hasCollision = cell.hasCollision or angelToSlope - end - - if not cell.isRestricted and not cell.hasCollision then - -- check for obstacles - local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height - local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallbackIgnore", nil, self.mask, true, true, true) - cell.hasCollision = cell.hasCollision or (shapes > 0) - if cell.hasCollision then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkGridCell hasCollision = (shapes > 0) %d x,z %d %d x,z %d %d ", - shapes, - math.floor(worldPos.x), - math.floor(worldPos.z), - math.floor(shapeDefinition.x), - math.floor(shapeDefinition.z) - ) - ) - end - end - end - - if not cell.isRestricted and not cell.hasCollision and cell.incoming ~= nil then - local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) - local vectorX = worldPosPrevious.x - worldPos.x - local vectorZ = worldPosPrevious.z - worldPos.z - local dirVec = { x=vectorX, z = vectorZ} - - local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) - cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath - self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkGridCell cellUsedByVehiclePath %s self.blockedByOtherVehicle %s", - tostring(cellUsedByVehiclePath), - tostring(self.blockedByOtherVehicle) - ) - ) - end - end -end - -function PathFinderModule:gridLocationToWorldLocation(cell) - local result = {x = 0, z = 0} - - result.x = self.target.x + (cell.x - self.targetCell.x) * self.vectorX.x + (cell.z - self.targetCell.z) * self.vectorZ.x - result.z = self.target.z + (cell.x - self.targetCell.x) * self.vectorX.z + (cell.z - self.targetCell.z) * self.vectorZ.z - - return result -end - -function PathFinderModule:worldDirectionToGridDirection(vector, baseVector) - local baseVector = baseVector or self.vectorX - local angle = AutoDrive.angleBetween(baseVector, vector) - - local direction = math.floor(angle / 45) - local remainder = angle % 45 - if remainder >= 22.5 then - direction = (direction + 1) - elseif remainder <= -22.5 then - direction = (direction - 1) - end - - if direction < 0 then - direction = 8 + direction - end - - return direction -end - -function PathFinderModule:worldLocationToGridLocation(worldX, worldZ) - local result = {x = 0, z = 0} - - result.z = (((worldX - self.startX) / self.vectorX.x) * self.vectorX.z - worldZ + self.startZ) / (((self.vectorZ.x / self.vectorX.x) * self.vectorX.z) - self.vectorZ.z) - result.x = (worldZ - self.startZ - result.z * self.vectorZ.z) / self.vectorX.z - - result.x = AutoDrive.round(result.x) - result.z = AutoDrive.round(result.z) - - return result -end - -function PathFinderModule:determineBlockedCells(cell) - if (math.abs(cell.x) < 2 and math.abs(cell.z) < 2) then - return - end ---[[ - table.insert(self.grid, {x = cell.x + 1, z = cell.z + 0, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_UP - table.insert(self.grid, {x = cell.x + 1, z = cell.z - 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_UP_LEFT - table.insert(self.grid, {x = cell.x + 0, z = cell.z + 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_RIGHT - table.insert(self.grid, {x = cell.x + 1, z = cell.z + 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_UP_RIGHT - table.insert(self.grid, {x = cell.x + 0, z = cell.z - 1, direction = -1, isRestricted = true, hasCollision = true, steps = 1000, bordercells = 0}) -- PP_LEFT -]] - local gridKey = "" - local direction = -1 - local x = 0 - local z = 0 - x = cell.x + 1 - z = cell.z + 0 - gridKey = string.format("%d|%d|%d", x, z, direction) - self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} - x = cell.x + 1 - z = cell.z - 1 - gridKey = string.format("%d|%d|%d", x, z, direction) - self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} - x = cell.x + 0 - z = cell.z + 1 - gridKey = string.format("%d|%d|%d", x, z, direction) - self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} - x = cell.x + 1 - z = cell.z + 1 - gridKey = string.format("%d|%d|%d", x, z, direction) - self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} - x = cell.x + 0 - z = cell.z - 1 - gridKey = string.format("%d|%d|%d", x, z, direction) - self.grid[gridKey] = {x = x, z = z, direction = direction, isRestricted = true, hasCollision = true, steps = 100000, bordercells = 0} -end - -function PathFinderModule:determineNextGridCells(cell) - if cell.out == nil then - cell.out = {} - end - if cell.direction == self.PP_UP then - cell.out[1] = {x = cell.x + 1, z = cell.z - 1} - cell.out[1].direction = self.PP_UP_LEFT - cell.out[2] = {x = cell.x + 1, z = cell.z + 0} - cell.out[2].direction = self.PP_UP - cell.out[3] = {x = cell.x + 1, z = cell.z + 1} - cell.out[3].direction = self.PP_UP_RIGHT - elseif cell.direction == self.PP_UP_RIGHT then - cell.out[1] = {x = cell.x + 1, z = cell.z + 0} - cell.out[1].direction = self.PP_UP - cell.out[2] = {x = cell.x + 1, z = cell.z + 1} - cell.out[2].direction = self.PP_UP_RIGHT - cell.out[3] = {x = cell.x + 0, z = cell.z + 1} - cell.out[3].direction = self.PP_RIGHT - elseif cell.direction == self.PP_RIGHT then - cell.out[1] = {x = cell.x + 1, z = cell.z + 1} - cell.out[1].direction = self.PP_UP_RIGHT - cell.out[2] = {x = cell.x + 0, z = cell.z + 1} - cell.out[2].direction = self.PP_RIGHT - cell.out[3] = {x = cell.x - 1, z = cell.z + 1} - cell.out[3].direction = self.PP_DOWN_RIGHT - elseif cell.direction == self.PP_DOWN_RIGHT then - cell.out[1] = {x = cell.x + 0, z = cell.z + 1} - cell.out[1].direction = self.PP_RIGHT - cell.out[2] = {x = cell.x - 1, z = cell.z + 1} - cell.out[2].direction = self.PP_DOWN_RIGHT - cell.out[3] = {x = cell.x - 1, z = cell.z + 0} - cell.out[3].direction = self.PP_DOWN - elseif cell.direction == self.PP_DOWN then - cell.out[1] = {x = cell.x - 1, z = cell.z + 1} - cell.out[1].direction = self.PP_DOWN_RIGHT - cell.out[2] = {x = cell.x - 1, z = cell.z + 0} - cell.out[2].direction = self.PP_DOWN - cell.out[3] = {x = cell.x - 1, z = cell.z - 1} - cell.out[3].direction = self.PP_DOWN_LEFT - elseif cell.direction == self.PP_DOWN_LEFT then - cell.out[1] = {x = cell.x - 1, z = cell.z - 0} - cell.out[1].direction = self.PP_DOWN - cell.out[2] = {x = cell.x - 1, z = cell.z - 1} - cell.out[2].direction = self.PP_DOWN_LEFT - cell.out[3] = {x = cell.x - 0, z = cell.z - 1} - cell.out[3].direction = self.PP_LEFT - elseif cell.direction == self.PP_LEFT then - cell.out[1] = {x = cell.x - 1, z = cell.z - 1} - cell.out[1].direction = self.PP_DOWN_LEFT - cell.out[2] = {x = cell.x - 0, z = cell.z - 1} - cell.out[2].direction = self.PP_LEFT - cell.out[3] = {x = cell.x + 1, z = cell.z - 1} - cell.out[3].direction = self.PP_UP_LEFT - elseif cell.direction == self.PP_UP_LEFT then - cell.out[1] = {x = cell.x - 0, z = cell.z - 1} - cell.out[1].direction = self.PP_LEFT - cell.out[2] = {x = cell.x + 1, z = cell.z - 1} - cell.out[2].direction = self.PP_UP_LEFT - cell.out[3] = {x = cell.x + 1, z = cell.z + 0} - cell.out[3].direction = self.PP_UP - end - - for _, outGoing in pairs(cell.out) do - outGoing.visited = false - outGoing.isRestricted = false - outGoing.hasCollision = false - outGoing.hasFruit = false - outGoing.incoming = cell - outGoing.steps = cell.steps + 1 - outGoing.bordercells = cell.bordercells - end -end - -function PathFinderModule:cellDistance(cell) - return MathUtil.vector2Length(self.targetCell.x - cell.x, self.targetCell.z - cell.z) -end - -function PathFinderModule:checkForFruitInArea(cell, corners) - - if self.goingToNetwork then - -- on the way to network, check all fruit types - self.fruitToCheck = nil - end - if self.fruitToCheck == nil then - for _, fruitType in pairs(g_fruitTypeManager:getFruitTypes()) do - if not (fruitType == g_fruitTypeManager:getFruitTypeByName("MEADOW")) then - local fruitTypeIndex = fruitType.index - self:checkForFruitTypeInArea(cell, fruitTypeIndex, corners) - end - --stop if cell is already restricted and/or fruit type is now known - if cell.isRestricted ~= false or self.fruitToCheck ~= nil then - break - end - end - else - self:checkForFruitTypeInArea(cell, self.fruitToCheck, corners) - end -end - -function PathFinderModule:checkForFruitTypeInArea(cell, fruitTypeIndex, corners) - local fruitValue = 0 - fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(fruitTypeIndex, corners[1].x, corners[1].z, corners[2].x, corners[2].z, corners[3].x, corners[3].z, true, true) - - if (self.fruitToCheck == nil or self.fruitToCheck < 1) and (fruitValue > PathFinderModule.MIN_FRUIT_VALUE) then - self.fruitToCheck = fruitTypeIndex - end - local wasRestricted = cell.isRestricted - cell.isRestricted = cell.isRestricted or (fruitValue > PathFinderModule.MIN_FRUIT_VALUE) - - cell.hasFruit = (fruitValue > PathFinderModule.MIN_FRUIT_VALUE) - cell.fruitValue = fruitValue - - if cell.hasFruit then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkForFruitTypeInArea cell.hasFruit xz %d %d fruitValue %s direction %s", - math.floor(cell.x), - math.floor(cell.z), - tostring(fruitValue), - tostring(self.direction_to_text[cell.direction+1]) - ) - ) - end - end - - --Allow fruit in the last few grid cells - if (self:cellDistance(cell) <= 3 and self.goingToPipe) then - cell.isRestricted = false or wasRestricted - end -end - -function PathFinderModule:drawDebugForPF() - local AutoDriveDM = ADDrawingManager - local pointTarget = self:gridLocationToWorldLocation(self.targetCell) - local pointTargetUp = self:gridLocationToWorldLocation(self.targetCell) - pointTarget.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTarget.x, 1, pointTarget.z) + 3 - pointTargetUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetUp.x, 1, pointTargetUp.z) + 20 - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) - local pointStart = {x = self.startX, z = self.startZ} - pointStart.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointStart.x, 1, pointStart.z) + 3 - AutoDriveDM:addLineTask(pointStart.x, pointStart.y, pointStart.z, pointStart.x, pointStart.y + 20, pointStart.z, 1, 0, 1, 0) - - local color_red = 0.1 - local color_green = 0.1 - local color_blue = 0.1 - local color_count = 0 - local index = 0 - for _, cell in pairs(self.grid) do - index = index + 1 - - color_red = math.min(color_red + 0.25, 1) - if color_red > 0.9 then - color_green = math.min(color_green + 0.25, 1) - end - if color_green > 0.9 then - color_blue = math.min(color_blue + 0.25, 1) - end - color_count = color_count + 1 - - local worldPos = self:gridLocationToWorldLocation(cell) - - -- local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallbackIgnore", nil, self.mask, true, true, true) - local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height - local corners = self:getCornersFromShapeDefinition(shapeDefinition) - local baseY = shapeDefinition.y + 3 - - -- corners of shape - -- DebugUtil.drawOverlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, color_red, color_green, color_blue) - for _, corner in pairs(corners) do - local point_y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corner.x, 1, corner.z) - local pointUp_y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corner.x, 1, corner.z) + 3 - -- AutoDriveDM:addLineTask(corner.x, point_y, corner.z, corner.x, pointUp_y, corner.z, 1, color_red, color_green, color_blue) - end - - -- restriction, collision line up - local pointCenter = self:gridLocationToWorldLocation(cell) - local pointCenterUp = {x = pointCenter.x, z = pointCenter.z} - pointCenter.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenter.x, 1, pointCenter.z) + 3 - pointCenterUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenterUp.x, 1, pointCenterUp.z) + 6 - - local pointA = corners[1] - local pointB = corners[2] - local pointC = corners[3] - local pointD = corners[4] - index = 0 - pointA.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointA.x, 1, pointA.z) + 1 + (0.1 * index) - pointB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointB.x, 1, pointB.z) + 1 + (0.1 * index) - pointC.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointC.x, 1, pointC.z) + 1 + (0.1 * index) - pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 1 + (0.1 * index) - - -- cell xz text - local cellText = tostring(cell.x) .. " " .. tostring(cell.z) - -- Utils.renderTextAtWorldPosition(pointCenter.x, pointCenter.y, pointCenter.z, cellText, getCorrectTextSize(0.013), 0) - ---[[ - if cell.isRestricted then - -- red - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 0) - else - if cell.isOnField then - -- blue - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) - else - -- green - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 1, 0) - end - end - local cellIndex = string.format("%d , %d", cell.x, cell.z) - Utils.renderTextAtWorldPosition(pointCenter.x, pointCenter.y - 2, pointCenter.z, cellIndex, getCorrectTextSize(0.013), 0) - - if cell.angle then - local value = string.format("%.1f", math.deg(cell.angle)) - if (not cell.hasCollision) and (not cell.isRestricted) then - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) - end - Utils.renderTextAtWorldPosition(pointCenter.x + (cell.x % 10), pointCenter.y - 1 + (cell.steps % 10 / 5) + (cell.z % 10 / 5), pointCenter.z, value, getCorrectTextSize(0.013), 0) - end - - if cell.incoming then - local cellIncommingIndex = string.format("%d -> %d , %d", cell.steps, cell.incoming.x, cell.incoming.z) - Utils.renderTextAtWorldPosition(pointCenter.x + (cell.x % 10), pointCenter.y - 0 + (cell.steps % 10 / 5) + (cell.z % 10 / 5), pointCenter.z, cellIncommingIndex, getCorrectTextSize(0.013), 0) - end -]] - if cell.isRestricted == true then - -- any restriction - if cell.hasFruit == true then - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 1) - Utils.renderTextAtWorldPosition(pointCenter.x, pointB.y + 0.4, pointCenter.z, tostring(cell.fruitValue), getCorrectTextSize(0.013), 0) - - else - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) - end - else - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) - end - - if cell.hasCollision == true then - -- ground collision, slope, water - if cell.hasVehicleCollision then -- TODO: hasVehicleCollision ??? - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) - else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) - end - else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) - end - - for i = 0, 10, 1 do - pointCenter.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenter.x, 1, pointCenter.z) + 1 + (i * 0.5) - -- AutoDriveDM:addLineTask(pointA.x, pointCenter.y, pointA.z, pointA.x + 0.3, pointCenter.y, pointA.z + 0.3, 1, 1, 1, 1) - end - ---[[ - -- cross marker with restriction, collision - local size = 0.3 - local pointA = self:gridLocationToWorldLocation(cell) - pointA.x = pointA.x + self.vectorX.x * size + self.vectorZ.x * size - pointA.z = pointA.z + self.vectorX.z * size + self.vectorZ.z * size - pointA.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointA.x, 1, pointA.z) + 3 - local pointB = self:gridLocationToWorldLocation(cell) -- oposide position to pointA !!! - pointB.x = pointB.x - self.vectorX.x * size - self.vectorZ.x * size - pointB.z = pointB.z - self.vectorX.z * size - self.vectorZ.z * size - pointB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointB.x, 1, pointB.z) + 3 - local pointC = self:gridLocationToWorldLocation(cell) - pointC.x = pointC.x + self.vectorX.x * size - self.vectorZ.x * size - pointC.z = pointC.z + self.vectorX.z * size - self.vectorZ.z * size - pointC.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointC.x, 1, pointC.z) + 3 - local pointD = self:gridLocationToWorldLocation(cell) - pointD.x = pointD.x - self.vectorX.x * size + self.vectorZ.x * size - pointD.z = pointD.z - self.vectorX.z * size + self.vectorZ.z * size - pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 3 - - -- restriction, collision line up - local pointCenter = self:gridLocationToWorldLocation(cell) - local pointCenterUp = pointCenter - pointCenterUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointCenterUp.x, 1, pointCenterUp.z) + 3 - - if cell.isRestricted == true then - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) - if cell.hasCollision == true then - if cell.hasVehicleCollision then -- TODO: hasVehicleCollision ??? - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) - else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) - end - else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) - end - else - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) - if cell.hasCollision == true then - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) - else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) - end - end -]] - - if cell.bordercells > 0 then - local pointTarget = self:gridLocationToWorldLocation(cell) - local pointTargetUp = self:gridLocationToWorldLocation(cell) - pointTarget.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTarget.x, 1, pointTarget.z) + 6 - pointTargetUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetUp.x, 1, pointTargetUp.z) + 10 - if cell.bordercells == 1 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 1, 0, 0) - elseif cell.bordercells == 2 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 1, 0) - elseif cell.bordercells > 2 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) - end - end - --[[ - local gridFactor = PathFinderModule.GRID_SIZE_FACTOR - if self.isSecondChasingVehicle then - gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER - end - local corners = self:getCorners(cell, {x=self.vectorX.x * gridFactor, z=self.vectorX.z * gridFactor}, {x=self.vectorZ.x * gridFactor,z=self.vectorZ.z * gridFactor}) - local heightOffset = 1 - AutoDriveDM:addLineTask(corners[1].x, pointA.y+heightOffset, corners[1].z, corners[2].x, pointA.y+heightOffset, corners[2].z, 1, 0, 1, 0) - AutoDriveDM:addLineTask(corners[2].x, pointA.y+heightOffset, corners[2].z, corners[3].x, pointA.y+heightOffset, corners[3].z, 1, 1, 0, 0) - AutoDriveDM:addLineTask(corners[3].x, pointA.y+heightOffset, corners[3].z, corners[4].x, pointA.y+heightOffset, corners[4].z, 1, 0, 0, 1) - AutoDriveDM:addLineTask(corners[4].x, pointA.y+heightOffset, corners[4].z, corners[1].x, pointA.y+heightOffset, corners[1].z, 1, 1, 0, 1) - - local shapeDefinition = self:getShapeDefByDirectionType(cell) - local red = 0 - if cell.hasCollision then - red = 1 - end - DebugUtil.drawOverlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, red, 0, 0) - --]] - end - - -- target cell marker - local size = 0.3 - local pointA = self:gridLocationToWorldLocation(self.targetCell) - pointA.x = pointA.x + self.vectorX.x * size + self.vectorZ.x * size - pointA.z = pointA.z + self.vectorX.z * size + self.vectorZ.z * size - pointA.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointA.x, 1, pointA.z) + 3 - local pointB = self:gridLocationToWorldLocation(self.targetCell) - pointB.x = pointB.x - self.vectorX.x * size - self.vectorZ.x * size - pointB.z = pointB.z - self.vectorX.z * size - self.vectorZ.z * size - pointB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointB.x, 1, pointB.z) + 3 - local pointC = self:gridLocationToWorldLocation(self.targetCell) - pointC.x = pointC.x + self.vectorX.x * size - self.vectorZ.x * size - pointC.z = pointC.z + self.vectorX.z * size - self.vectorZ.z * size - pointC.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointC.x, 1, pointC.z) + 3 - local pointD = self:gridLocationToWorldLocation(self.targetCell) - pointD.x = pointD.x - self.vectorX.x * size + self.vectorZ.x * size - pointD.z = pointD.z - self.vectorX.z * size + self.vectorZ.z * size - pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 3 - - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 1, 1) - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) - - local pointAB = self:gridLocationToWorldLocation(self.targetCell) - pointAB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointAB.x, 1, pointAB.z) + 3 - - local pointTargetVector = self:gridLocationToWorldLocation(self.targetCell) - pointTargetVector.x = pointTargetVector.x + self.targetVector.x * 10 - pointTargetVector.z = pointTargetVector.z + self.targetVector.z * 10 - pointTargetVector.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetVector.x, 1, pointTargetVector.z) + 3 - AutoDriveDM:addLineTask(pointAB.x, pointAB.y, pointAB.z, pointTargetVector.x, pointTargetVector.y, pointTargetVector.z, 1, 1, 1, 1) -end - -function PathFinderModule:drawDebugForCreatedRoute() - local AutoDriveDM = ADDrawingManager - if self.chainStartToTarget ~= nil then - for _, cell in pairs(self.chainStartToTarget) do - local shape = self:getShapeDefByDirectionType(cell) - if shape.x ~= nil then - local pointA = { - x = shape.x + shape.widthX * math.cos(shape.angleRad) + shape.widthZ * math.sin(shape.angleRad), - y = shape.y, - z = shape.z + shape.widthZ * math.cos(shape.angleRad) + shape.widthX * math.sin(shape.angleRad) - } - local pointB = { - x = shape.x - shape.widthX * math.cos(shape.angleRad) - shape.widthZ * math.sin(shape.angleRad), - y = shape.y, - z = shape.z + shape.widthZ * math.cos(shape.angleRad) + shape.widthX * math.sin(shape.angleRad) - } - local pointC = { - x = shape.x - shape.widthX * math.cos(shape.angleRad) - shape.widthZ * math.sin(shape.angleRad), - y = shape.y, - z = shape.z - shape.widthZ * math.cos(shape.angleRad) - shape.widthX * math.sin(shape.angleRad) - } - local pointD = { - x = shape.x + shape.widthX * math.cos(shape.angleRad) + shape.widthZ * math.sin(shape.angleRad), - y = shape.y, - z = shape.z - shape.widthZ * math.cos(shape.angleRad) - shape.widthX * math.sin(shape.angleRad) - } - - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointC.x, pointC.y, pointC.z, 1, 1, 1, 1) - AutoDriveDM:addLineTask(pointB.x, pointB.y, pointB.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) - - if cell.incoming ~= nil then - local worldPos_cell = self:gridLocationToWorldLocation(cell) - local worldPos_incoming = self:gridLocationToWorldLocation(cell.incoming) - - local vectorX = worldPos_cell.x - worldPos_incoming.x - local vectorZ = worldPos_cell.z - worldPos_incoming.z - local angleRad = math.atan2(-vectorZ, vectorX) - angleRad = AutoDrive.normalizeAngle(angleRad) - local widthOfColBox = math.sqrt(math.pow(self.minTurnRadius, 2) + math.pow(self.minTurnRadius, 2)) - local sideLength = widthOfColBox / 2 - - local leftAngle = AutoDrive.normalizeAngle(angleRad + math.rad(-90)) - local rightAngle = AutoDrive.normalizeAngle(angleRad + math.rad(90)) - - local cornerX = worldPos_incoming.x - math.cos(leftAngle) * sideLength - local cornerZ = worldPos_incoming.z + math.sin(leftAngle) * sideLength - - local corner2X = worldPos_cell.x - math.cos(leftAngle) * sideLength - local corner2Z = worldPos_cell.z + math.sin(leftAngle) * sideLength - - local corner3X = worldPos_cell.x - math.cos(rightAngle) * sideLength - local corner3Z = worldPos_cell.z + math.sin(rightAngle) * sideLength - - local corner4X = worldPos_incoming.x - math.cos(rightAngle) * sideLength - local corner4Z = worldPos_incoming.z + math.sin(rightAngle) * sideLength - - local inY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPos_incoming.x, 1, worldPos_incoming.z) + 1 - local currentY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPos_cell.x, 1, worldPos_cell.z) + 1 - - AutoDriveDM:addLineTask(cornerX, inY, cornerZ, corner2X, currentY, corner2Z, 1, 1, 0, 0) - AutoDriveDM:addLineTask(corner2X, currentY, corner2Z, corner3X, currentY, corner3Z, 1, 1, 0, 0) - AutoDriveDM:addLineTask(corner3X, currentY, corner3Z, corner4X, inY, corner4Z, 1, 1, 0, 0) - AutoDriveDM:addLineTask(corner4X, inY, corner4Z, cornerX, inY, cornerZ, 1, 1, 0, 0) - end - end - end - end - - if self.wayPoints then - for i, waypoint in pairs(self.wayPoints) do - Utils.renderTextAtWorldPosition(waypoint.x, waypoint.y + 4, waypoint.z, "Node " .. i, getCorrectTextSize(0.013), 0) - if i > 1 then - local wp = waypoint - local pfWp = self.wayPoints[i - 1] - AutoDriveDM:addLineTask(wp.x, wp.y, wp.z, pfWp.x, pfWp.y, pfWp.z, 1, 0, 1, 1) - end - end - end -end - -function PathFinderModule:getShapeDefByDirectionType(cell) - local shapeDefinition = {} - shapeDefinition.angleRad = math.atan2(-self.targetVector.z, self.targetVector.x) - shapeDefinition.angleRad = AutoDrive.normalizeAngle(shapeDefinition.angleRad) - local worldPos = self:gridLocationToWorldLocation(cell) - shapeDefinition.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPos.x, 1, worldPos.z) - shapeDefinition.height = 2.65 - - if cell.direction == self.PP_UP or cell.direction == self.PP_DOWN or cell.direction == self.PP_RIGHT or cell.direction == self.PP_LEFT or cell.direction == -1 then - --default size: - shapeDefinition.x = worldPos.x - shapeDefinition.z = worldPos.z - shapeDefinition.widthX = self.minTurnRadius / 2 - shapeDefinition.widthZ = self.minTurnRadius / 2 - elseif cell.direction == self.PP_UP_RIGHT then - local offsetX = (-self.vectorX.x) / 2 + (-self.vectorZ.x) / 4 - local offsetZ = (-self.vectorX.z) / 2 + (-self.vectorZ.z) / 4 - shapeDefinition.x = worldPos.x + offsetX - shapeDefinition.z = worldPos.z + offsetZ - shapeDefinition.widthX = (self.minTurnRadius / 2) + math.abs(offsetX) - shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) - elseif cell.direction == self.PP_UP_LEFT then - local offsetX = (-self.vectorX.x) / 2 + (self.vectorZ.x) / 4 - local offsetZ = (-self.vectorX.z) / 2 + (self.vectorZ.z) / 4 - shapeDefinition.x = worldPos.x + offsetX - shapeDefinition.z = worldPos.z + offsetZ - shapeDefinition.widthX = self.minTurnRadius / 2 + math.abs(offsetX) - shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) - elseif cell.direction == self.PP_DOWN_RIGHT then - local offsetX = (self.vectorX.x) / 2 + (-self.vectorZ.x) / 4 - local offsetZ = (self.vectorX.z) / 2 + (-self.vectorZ.z) / 4 - shapeDefinition.x = worldPos.x + offsetX - shapeDefinition.z = worldPos.z + offsetZ - shapeDefinition.widthX = self.minTurnRadius / 2 + math.abs(offsetX) - shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) - elseif cell.direction == self.PP_DOWN_LEFT then - local offsetX = (self.vectorX.x) / 2 + (self.vectorZ.x) / 4 - local offsetZ = (self.vectorX.z) / 2 + (self.vectorZ.z) / 4 - shapeDefinition.x = worldPos.x + offsetX - shapeDefinition.z = worldPos.z + offsetZ - shapeDefinition.widthX = self.minTurnRadius / 2 + math.abs(offsetX) - shapeDefinition.widthZ = self.minTurnRadius / 2 + math.abs(offsetZ) - end - - local increaseCellFactor = 1.15 - if cell.isOnField ~= nil and cell.isOnField == true then - increaseCellFactor = 1 --0.8 - end - shapeDefinition.widthX = shapeDefinition.widthX * increaseCellFactor - shapeDefinition.widthZ = shapeDefinition.widthZ * increaseCellFactor - - local corners = self:getCornersFromShapeDefinition(shapeDefinition) - if corners ~= nil then - for _, corner in pairs(corners) do - shapeDefinition.y = math.max(shapeDefinition.y, getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corner.x, 1, corner.z)) - end - end - - return shapeDefinition -end - -function PathFinderModule:getCornersFromShapeDefinition(shapeDefinition) - local corners = {} - corners[1] = {x = shapeDefinition.x + (-shapeDefinition.widthX), z = shapeDefinition.z + (-shapeDefinition.widthZ)} - corners[2] = {x = shapeDefinition.x + (shapeDefinition.widthX), z = shapeDefinition.z + (shapeDefinition.widthZ)} - corners[3] = {x = shapeDefinition.x + (-shapeDefinition.widthX), z = shapeDefinition.z + (shapeDefinition.widthZ)} - corners[4] = {x = shapeDefinition.x + (shapeDefinition.widthX), z = shapeDefinition.z + (-shapeDefinition.widthZ)} - - return corners -end - -function PathFinderModule:getCorners(cell, vectorX, vectorZ) - local corners = {} - local centerLocation = self:gridLocationToWorldLocation(cell) - corners[1] = {x = centerLocation.x + (-vectorX.x - vectorZ.x), z = centerLocation.z + (-vectorX.z - vectorZ.z)} - corners[2] = {x = centerLocation.x + (vectorX.x - vectorZ.x), z = centerLocation.z + (vectorX.z - vectorZ.z)} - corners[3] = {x = centerLocation.x + (-vectorX.x + vectorZ.x), z = centerLocation.z + (-vectorX.z + vectorZ.z)} - corners[4] = {x = centerLocation.x + (vectorX.x + vectorZ.x), z = centerLocation.z + (vectorX.z + vectorZ.z)} - - return corners -end - -function PathFinderModule:createWayPoints() - if self.smoothStep == 0 then - local currentCell = self.targetCell - self.chainTargetToStart = {} - local index = 1 - self.chainTargetToStart[index] = currentCell - index = index + 1 - while currentCell.x ~= 0 or currentCell.z ~= 0 do - self.chainTargetToStart[index] = currentCell.incoming - currentCell = currentCell.incoming - if currentCell == nil then - break - end - index = index + 1 - end - index = index - 1 - - self.chainStartToTarget = {} - for reversedIndex = 0, index, 1 do - self.chainStartToTarget[reversedIndex + 1] = self.chainTargetToStart[index - reversedIndex] - end - - --Now build actual world coordinates as waypoints and include pre and append points - self.wayPoints = {} - for chainIndex, cell in pairs(self.chainStartToTarget) do - self.wayPoints[chainIndex] = self:gridLocationToWorldLocation(cell) - self.wayPoints[chainIndex].y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, self.wayPoints[chainIndex].x, 1, self.wayPoints[chainIndex].z) - self.wayPoints[chainIndex].direction = cell.direction - end - - -- remove zig zag line - self:smoothResultingPPPath() - end - - -- shortcut the path if possible - self:smoothResultingPPPath_Refined() - - if self.smoothStep == 2 then - -- When going to network, dont turn actual road network nodes into pathFinderPoints - if self.goingToNetwork then - for i = 1, #self.wayPoints, 1 do - self.wayPoints[i].isPathFinderPoint = true - end - end - - if self.appendWayPoints ~= nil then - for i = 1, #self.appendWayPoints, 1 do - self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] - end - self.smoothStep = 3 - --PathFinderModule.debugVehicleMsg(self.vehicle, - --string.format("PFM createWayPoints appendWayPoints %s", - --tostring(#self.appendWayPoints) - --) - --) - end - - -- See comment above - if not self.goingToNetwork then - for i = 1, #self.wayPoints, 1 do - self.wayPoints[i].isPathFinderPoint = true - end - end - end -end - -function PathFinderModule:smoothResultingPPPath() - local index = 1 - local filteredIndex = 1 - local filteredWPs = {} - - while index < #self.wayPoints - 1 do - local node = self.wayPoints[index] - local nodeAhead = self.wayPoints[index + 1] - local nodeTwoAhead = self.wayPoints[index + 2] - - filteredWPs[filteredIndex] = node - filteredIndex = filteredIndex + 1 - - if node.direction ~= nil and nodeAhead.direction ~= nil and nodeTwoAhead.direction ~= nil then - if node.direction == nodeTwoAhead.direction and node.direction ~= nodeAhead.direction then - index = index + 1 --skip next point because it is a zig zag line. Cut right through instead - end - end - - index = index + 1 - end - - while index <= #self.wayPoints do - local node = self.wayPoints[index] - filteredWPs[filteredIndex] = node - filteredIndex = filteredIndex + 1 - index = index + 1 - end - - self.wayPoints = filteredWPs - --PathFinderModule.debugVehicleMsg(self.vehicle, - --string.format("PFM smoothResultingPPPath self.wayPoints %s", - --tostring(#self.wayPoints) - --) - --) - -end - -function PathFinderModule:smoothResultingPPPath_Refined() - if self.smoothStep == 0 then - self.lookAheadIndex = 1 - self.smoothIndex = 1 - self.filteredIndex = 1 - self.filteredWPs = {} - self.totalEagerSteps = 0 - - --add first few without filtering - while self.smoothIndex < #self.wayPoints and self.smoothIndex < 3 do - self.filteredWPs[self.filteredIndex] = self.wayPoints[self.smoothIndex] - self.filteredIndex = self.filteredIndex + 1 - self.smoothIndex = self.smoothIndex + 1 - end - - self.smoothStep = 1 - end - - local unfilteredEndPointCount = 5 - if self.smoothStep == 1 then - local stepsThisFrame = 0 - while self.smoothIndex < #self.wayPoints - unfilteredEndPointCount and stepsThisFrame < ADScheduler:getStepsPerFrame() do - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined self.smoothIndex %d ", - self.smoothIndex - ) - ) - end - stepsThisFrame = stepsThisFrame + 1 - - local node = self.wayPoints[self.smoothIndex] - local previousNode = nil - local worldPos = self.wayPoints[self.smoothIndex] - - if self.totalEagerSteps == nil or self.totalEagerSteps == 0 then - if self.filteredWPs[self.filteredIndex-1].x ~= node.x and self.filteredWPs[self.filteredIndex-1].z ~= node.z then - self.filteredWPs[self.filteredIndex] = node - if self.filteredIndex > 1 then - previousNode = self.filteredWPs[self.filteredIndex - 1] - end - self.filteredIndex = self.filteredIndex + 1 - - self.lookAheadIndex = 1 - self.totalEagerSteps = 0 - end - end - - local widthOfColBox = self.minTurnRadius - local sideLength = widthOfColBox * PathFinderModule.GRID_SIZE_FACTOR - local y = worldPos.y - local foundCollision = false - - if stepsThisFrame > math.max(1, (ADScheduler:getStepsPerFrame() * 0.4)) then - break - end - - --local stepsOfLookAheadThisFrame = 0 - -- (foundCollision == false or self.totalEagerSteps < PathFinderModule.PP_MAX_EAGER_LOOKAHEAD_STEPS) - while (foundCollision == false) and ((self.smoothIndex + self.totalEagerSteps) < (#self.wayPoints - unfilteredEndPointCount)) and stepsThisFrame <= math.max(1, (ADScheduler:getStepsPerFrame() * 0.4)) do - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined self.smoothIndex %d self.totalEagerSteps %d", - self.smoothIndex, - self.totalEagerSteps - ) - ) - end - - local hasCollision = false - stepsThisFrame = stepsThisFrame + 1 - local nodeAhead = self.wayPoints[self.smoothIndex + self.totalEagerSteps + 1] - local nodeTwoAhead = self.wayPoints[self.smoothIndex + self.totalEagerSteps + 2] - if not hasCollision and nodeAhead and nodeTwoAhead then - local angle = AutoDrive.angleBetween({x = nodeAhead.x - node.x, z = nodeAhead.z - node.z}, {x = nodeTwoAhead.x - nodeAhead.x, z = nodeTwoAhead.z - nodeAhead.z}) - angle = math.abs(angle) - if angle > 60 then - hasCollision = true - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 1 - ) - ) - end - end - if previousNode ~= nil then - angle = AutoDrive.angleBetween({x = node.x - previousNode.x, z = node.z - previousNode.z}, {x = nodeTwoAhead.x - node.x, z = nodeTwoAhead.z - node.z}) - angle = math.abs(angle) - if angle > 60 then - hasCollision = true - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 2 - ) - ) - end - end - angle = AutoDrive.angleBetween({x = node.x - previousNode.x, z = node.z - previousNode.z}, {x = nodeAhead.x - node.x, z = nodeAhead.z - node.z}) - angle = math.abs(angle) - if angle > 60 then - hasCollision = true - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 3 - ) - ) - end - end - end - end - - if not hasCollision then - hasCollision = hasCollision or self:checkSlopeAngle(worldPos.x, worldPos.z, nodeAhead.x, nodeAhead.z) - if hasCollision then - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 4 - ) - ) - end - end - end - - local vectorX = nodeAhead.x - node.x - local vectorZ = nodeAhead.z - node.z - local angleRad = math.atan2(-vectorZ, vectorX) - angleRad = AutoDrive.normalizeAngle(angleRad) - local length = math.sqrt(math.pow(vectorX, 2) + math.pow(vectorZ, 2)) + widthOfColBox - - local leftAngle = AutoDrive.normalizeAngle(angleRad + math.rad(-90)) - local rightAngle = AutoDrive.normalizeAngle(angleRad + math.rad(90)) - - local cornerX = node.x - math.cos(leftAngle) * sideLength - local cornerZ = node.z + math.sin(leftAngle) * sideLength - - local corner2X = nodeAhead.x - math.cos(leftAngle) * sideLength - local corner2Z = nodeAhead.z + math.sin(leftAngle) * sideLength - - local corner3X = nodeAhead.x - math.cos(rightAngle) * sideLength - local corner3Z = nodeAhead.z + math.sin(rightAngle) * sideLength - - local corner4X = node.x - math.cos(rightAngle) * sideLength - local corner4Z = node.z + math.sin(rightAngle) * sideLength - - if not hasCollision then - if self.isNewPF then - self.collisionhits = 0 - local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "collisionTestCallback", self, self.mask, true, true, true) - hasCollision = hasCollision or (self.collisionhits > 0) - else - local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "Ignore", nil, self.mask, true, true, true) - hasCollision = hasCollision or (shapes > 0) - end - - if hasCollision then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 5 - ) - ) - end - end - end - - if (self.smoothIndex > 1) then - local worldPosPrevious = self.wayPoints[self.smoothIndex - 1] - length = MathUtil.vector3Length(worldPos.x - worldPosPrevious.x, worldPos.y - worldPosPrevious.y, worldPos.z - worldPosPrevious.z) - local angleBetween = math.atan(math.abs(worldPos.y - worldPosPrevious.y) / length) - - if (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then - hasCollision = true - - if hasCollision then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 6 - ) - ) - end - end - end - end - - if not hasCollision and self.avoidFruitSetting and not self.fallBackMode3 then - - local cornerWideX = node.x - math.cos(leftAngle) * sideLength * 4 - local cornerWideZ = node.z + math.sin(leftAngle) * sideLength * 4 - - local cornerWide2X = nodeAhead.x - math.cos(leftAngle) * sideLength * 4 - local cornerWide2Z = nodeAhead.z + math.sin(leftAngle) * sideLength * 4 - - local cornerWide4X = node.x - math.cos(rightAngle) * sideLength * 4 - local cornerWide4Z = node.z + math.sin(rightAngle) * sideLength * 4 - - if self.goingToNetwork then - -- check for all fruit types - for _, fruitType in pairs(g_fruitTypeManager:getFruitTypes()) do - if not (fruitType == g_fruitTypeManager:getFruitTypeByName("MEADOW")) then - local fruitTypeIndex = fruitType.index - local fruitValue = 0 - if self.isSecondChasingVehicle then - fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(fruitTypeIndex, cornerWideX, cornerWideZ, cornerWide2X, cornerWide2Z, cornerWide4X, cornerWide4Z, true, true) - else - fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(fruitTypeIndex, cornerX, cornerZ, corner2X, corner2Z, corner4X, corner4Z, true, true) - end - hasCollision = hasCollision or (fruitValue > 50) - if hasCollision then - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 7 - ) - ) - end - break - end - end - end - else - -- check only for fruit type detected on field - if self.fruitToCheck ~= nil then - local fruitValue = 0 - if self.isSecondChasingVehicle then - fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(self.fruitToCheck, cornerWideX, cornerWideZ, cornerWide2X, cornerWide2Z, cornerWide4X, cornerWide4Z, true, true) - else - fruitValue, _, _, _ = FSDensityMapUtil.getFruitArea(self.fruitToCheck, cornerX, cornerZ, corner2X, corner2Z, corner4X, corner4Z, true, true) - end - hasCollision = hasCollision or (fruitValue > 50) - - if hasCollision then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 8 - ) - ) - end - end - end - end - end - - if not hasCollision then - local cellBox = AutoDrive.boundingBoxFromCorners(cornerX, cornerZ, corner2X, corner2Z, corner3X, corner3Z, corner4X, corner4Z) - hasCollision = hasCollision or AutoDrive.checkForVehiclePathInBox(cellBox, self.minTurnRadius, self.vehicle) - - if hasCollision then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined hasCollision %d", - 9 - ) - ) - end - end - end - - foundCollision = hasCollision - - if foundCollision then - -- not used code removed - else - self.lookAheadIndex = self.totalEagerSteps + 1 - end - - self.totalEagerSteps = self.totalEagerSteps + 1 - - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined self.smoothIndex %d self.totalEagerSteps %d self.filteredIndex %d foundCollision %s", - self.smoothIndex, - self.totalEagerSteps, - self.filteredIndex, - tostring(foundCollision) - ) - ) - end - end - - if foundCollision or ((self.smoothIndex + self.totalEagerSteps) >= (#self.wayPoints - unfilteredEndPointCount)) then - self.smoothIndex = self.smoothIndex + math.max(1, (self.lookAheadIndex)) - self.totalEagerSteps = 0 - end - end - - if self.smoothIndex >= #self.wayPoints - unfilteredEndPointCount then - self.smoothStep = 2 - end - end - - if self.smoothStep == 2 then - --add remaining points without filtering - while self.smoothIndex <= #self.wayPoints do - local node = self.wayPoints[self.smoothIndex] - self.filteredWPs[self.filteredIndex] = node - self.filteredIndex = self.filteredIndex + 1 - self.smoothIndex = self.smoothIndex + 1 - end - - self.wayPoints = self.filteredWPs - - self.smoothDone = true - - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM smoothResultingPPPath_Refined self.wayPoints %s", - tostring(#self.wayPoints) - ) - ) - end -end - -function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) - local vectorFromPrevious = {x = x1 - x2, z = z1 - z2} - local worldPosMiddle = {x = x2 + vectorFromPrevious.x / 2, z = z2 + vectorFromPrevious.z / 2} - - local terrain1 = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, x1, 0, z1) - local terrain2 = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, x2, 0, z2) - local terrain3 = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPosMiddle.x, 0, worldPosMiddle.z) - local length = MathUtil.vector3Length(x1 - x2, terrain1 - terrain2, z1 - z2) - local lengthMiddle = MathUtil.vector3Length(worldPosMiddle.x - x2, terrain3 - terrain2, worldPosMiddle.z - z2) - local angleBetween = math.atan(math.abs(terrain1 - terrain2) / length) - local angleBetweenCenter = math.atan(math.abs(terrain3 - terrain2) / lengthMiddle) - - local angleLeft = 0 - local angleRight = 0 - - if self.cos90 == nil then - -- speed up the calculation - self.cos90 = math.cos(math.rad(90)) - self.sin90 = math.sin(math.rad(90)) - self.cos270 = math.cos(math.rad(270)) - self.sin270 = math.sin(math.rad(270)) - end - - local rotX = vectorFromPrevious.x * self.cos90 - vectorFromPrevious.z * self.sin90 - local rotZ = vectorFromPrevious.x * self.sin90 + vectorFromPrevious.z * self.cos90 - local vectorLeft = {x = rotX, z = rotZ} - - local rotX = vectorFromPrevious.x * self.cos270 - vectorFromPrevious.z * self.sin270 - local rotZ = vectorFromPrevious.x * self.sin270 + vectorFromPrevious.z * self.cos270 - local vectorRight = {x = rotX, z = rotZ} - - local worldPosLeft = {x = x1 + vectorLeft.x / 2, z = z1 + vectorLeft.z / 2} - local worldPosRight = {x = x1 + vectorRight.x / 2, z = z1 + vectorRight.z / 2} - local terrainLeft = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPosLeft.x, 0, worldPosLeft.z) - local terrainRight = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, worldPosRight.x, 0, worldPosRight.z) - local lengthLeft = MathUtil.vector3Length(worldPosLeft.x - x1, terrainLeft - terrain1, worldPosLeft.z - z1) - local lengthRight = MathUtil.vector3Length(worldPosRight.x - x1, terrainRight - terrain1, worldPosRight.z - z1) - angleLeft = math.atan(math.abs(terrainLeft - terrain1) / lengthLeft) - angleRight = math.atan(math.abs(terrainRight - terrain1) / lengthRight) - - local waterY = g_currentMission.environmentAreaSystem:getWaterYAtWorldPosition(worldPosMiddle.x, terrain3, worldPosMiddle.z) or -200 - - local belowGroundLevel = terrain1 < waterY - 0.5 or terrain2 < waterY - 0.5 or terrain3 < waterY - 0.5 - - if belowGroundLevel then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkSlopeAngle belowGroundLevel x,z %d %d", - math.floor(x1), - math.floor(z1) - ) - ) - end - end - - if (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkSlopeAngle (angleBetween * 1.25) > PathFinderModule.SLOPE_DETECTION_THRESHOLD x,z %d %d", - math.floor(x1), - math.floor(z1) - ) - ) - end - end - - if (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then - if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM checkSlopeAngle (angleBetweenCenter * 1.25) > PathFinderModule.SLOPE_DETECTION_THRESHOLD x,z %d %d", - math.floor(x1), - math.floor(z1) - ) - ) - end - end - - if belowGroundLevel or (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD or (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD - or (angleLeft > PathFinderModule.SLOPE_DETECTION_THRESHOLD or angleRight > PathFinderModule.SLOPE_DETECTION_THRESHOLD) - then - return true, angleBetween - end - return false, angleBetween -end - -function PathFinderModule.debugVehicleMsg(vehicle, msg) - -- collect output for single vehicle - help to examine sequences for a single vehicle - if vehicle ~= nil and vehicle.ad ~= nil and vehicle.ad.debug ~= nil then - if AutoDrive.debugVehicleMsg ~= nil then - AutoDrive.debugVehicleMsg(vehicle, msg) - end - end -end - -function PathFinderModule:drawDebugNewPF() - if self.closedset then - for k, node in pairs(self.closedset) do - if k.text then - local point = self:gridLocationToWorldLocation({k.x, k.z}) - point.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, point.x, 1, point.z) - Utils.renderTextAtWorldPosition(point.x, point.y + 4, point.z, tostring(k.text), getCorrectTextSize(0.013), 0) - end - end - end - if self.fruitAreas then - for _, corners in ipairs(self.fruitAreas) do - local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corners[1].x, 1, corners[1].z) - ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) - ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) - ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) - ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) - end - end -end - -function PathFinderModule:isDriveable(cell) - cell.isRestricted = false - cell.incoming = cell.from_node - cell.hasCollision = false - - local worldPos = self:gridLocationToWorldLocation(cell) - --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped - - cell.isOnField = AutoDrive.checkIsOnField(worldPos.x, 0, worldPos.z) - - -- check the most probable restrictions on field first to prevent unneccessary checks - if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 or self.fallBackMode2) then - -- in fallBackMode1 we ignore the field restriction - cell.isRestricted = cell.isRestricted or (not cell.isOnField) - if not cell.isOnField then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable not cell.isOnField xz %d,%d " - , cell.x, cell.z - ) - end - end - - local gridFactor = PathFinderModule.GRID_SIZE_FACTOR - if self.isSecondChasingVehicle then - gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER - end - local corners = self:getCorners(cell, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) - - if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then - -- check for fruit - self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found - table.insert(self.fruitAreas, corners) - if cell.isRestricted then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.isRestricted xz %d,%d fruit found %s" - , cell.x, cell.z - , self.fruitToCheck - ) - end - end - - if not cell.isRestricted and cell.incoming ~= nil then - -- check for up/down is to big or below water level - local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) - local angelToSlope, angle = self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level - cell.angle = angle - cell.hasCollision = cell.hasCollision or angelToSlope - cell.isRestricted = cell.isRestricted or cell.hasCollision - if angelToSlope then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable angelToSlope xz %d,%d" - , cell.x, cell.z - ) - end - end - - if not cell.isRestricted then - -- check for obstacles - local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height - self.collisionhits = 0 - local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, shapeDefinition.height, shapeDefinition.widthZ, "collisionTestCallback", self, self.mask, true, true, true) - cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) - cell.isRestricted = cell.isRestricted or cell.hasCollision - if cell.hasCollision then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cell.hasCollision xz %d,%d collision" - , cell.x, cell.z - ) - end - end - - if not cell.isRestricted and cell.incoming ~= nil then - local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) - local vectorX = worldPosPrevious.x - worldPos.x - local vectorZ = worldPosPrevious.z - worldPos.z - local dirVec = { x=vectorX, z = vectorZ} - - local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) - cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath - self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath - if cellUsedByVehiclePath then - PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveable cellUsedByVehiclePath xz %d,%d vehicle" - , cell.x, cell.z - ) - end - end - return not(cell.isRestricted) -end - - -- Cost of two adjacent nodes --- current, neighbor -function PathFinderModule:get_cost(from_node, to_node) - local dx, dz = from_node.x - to_node.x, from_node.z - to_node.z - return math.sqrt(dx * dx + dz * dz) + (from_node.cost + to_node.cost) * 0.5 -end - --- For heuristic. Estimate cost of current node to goal node --- neighbor, goal -function PathFinderModule:estimate_cost(node, goal_node) - return self:get_cost(node, goal_node) * 1.5 + (node.cost + goal_node.cost) * 0.5 -end - --- current = self:pop_best_node(self.openset, self.f_score) --- return: node / nil --- self.openset, f_score -function PathFinderModule:pop_best_node(set, score) - local best, node = math.huge, nil - - for k, v in pairs(set) do - local s = score[k] - - if s < best then - best = s or math.huge - node = k - end - end - if not node then return end - set[node] = nil - return node -end - --- {}, self.came_from, self.nodeGoal -function PathFinderModule:unwind_path(flat_path, came_from, goal) - if came_from[goal] and (came_from[goal] ~= self.nodeGoal) then - table.insert(flat_path, 1, came_from[goal]) - return self:unwind_path(flat_path, came_from, came_from[goal]) - else - return flat_path - end -end - --- Node must be able to check if they are the same --- so the example cannot directly return a different table for same coord -function PathFinderModule:get_node(x, z) - local row = self.cached_nodes[z] - if not row then row = {}; self.cached_nodes[z] = row end - local node = row[x] - if not node then node = { x = x, z = z, cost = 0 }; row[x] = node end - if node.text == nil then - node.text = string.format("%d,%d", x, z) - end - return node -end - -function PathFinderModule:getDirections(fromNode, node) - if node == nil then - AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node %s" - , tostring(fromNode) - , tostring(node) - ) - return - end - local directions = {} - - if fromNode then - PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %d,%d fromNode.direction %s node xz %d,%d node.direction %s" - , fromNode.x, fromNode.z - , tostring(self.direction_to_text[fromNode.direction+1]) - , node.x, node.z - , tostring(self.direction_to_text[node.direction+1]) - ) - else - PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %s node xz %d,%d node.direction %s" - , tostring(fromNode) - , node.x, node.z - , tostring(self.direction_to_text[node.direction+1]) - ) - end - if (fromNode == nil and node.direction == self.PP_RIGHT) or (fromNode and fromNode.x == node.x and fromNode.z < node.z) then - directions[1] = { -1, 1 } - directions[1].direction = self.PP_DOWN_RIGHT - directions[2] = { 0, 1 } - directions[2].direction = self.PP_RIGHT - directions[3] = { 1, 1 } - directions[3].direction = self.PP_UP_RIGHT - elseif (fromNode == nil and node.direction == self.PP_LEFT) or (fromNode and fromNode.x == node.x and fromNode.z > node.z) then - directions[1] = { -1, -1 } - directions[1].direction = self.PP_DOWN_LEFT - directions[2] = { 0, -1 } - directions[2].direction = self.PP_LEFT - directions[3] = { 1, -1 } - directions[3].direction = self.PP_UP_LEFT - elseif (fromNode == nil and node.direction == self.PP_UP) or (fromNode and fromNode.x < node.x and fromNode.z == node.z) then - directions[1] = { 1, -1 } - directions[1].direction = self.PP_UP_LEFT - directions[2] = { 1, 0 } - directions[2].direction = self.PP_UP - directions[3] = { 1, 1 } - directions[3].direction = self.PP_UP_RIGHT - elseif (fromNode == nil and node.direction == self.PP_DOWN) or (fromNode and fromNode.x > node.x and fromNode.z == node.z) then - directions[1] = { -1, -1 } - directions[1].direction = self.PP_DOWN_LEFT - directions[2] = { -1, 0 } - directions[2].direction = self.PP_DOWN - directions[3] = { -1, 1 } - directions[3].direction = self.PP_DOWN_RIGHT - elseif (fromNode == nil and node.direction == self.PP_UP_RIGHT) or (fromNode and fromNode.x < node.x and fromNode.z < node.z) then - directions[1] = { 1, 0 } - directions[1].direction = self.PP_UP - directions[2] = { 1, 1 } - directions[2].direction = self.PP_UP_RIGHT - directions[3] = { 0, 1 } - directions[3].direction = self.PP_RIGHT - elseif (fromNode == nil and node.direction == self.PP_DOWN_LEFT) or (fromNode and fromNode.x > node.x and fromNode.z > node.z) then - directions[1] = { 0, -1 } - directions[1].direction = self.PP_LEFT - directions[2] = { -1, -1 } - directions[2].direction = self.PP_DOWN_LEFT - directions[3] = { -1, 0 } - directions[3].direction = self.PP_DOWN - elseif (fromNode == nil and node.direction == self.PP_UP_LEFT) or (fromNode and fromNode.x < node.x and fromNode.z > node.z) then - directions[1] = { 0, -1 } - directions[1].direction = self.PP_LEFT - directions[2] = { 1, -1 } - directions[2].direction = self.PP_UP_LEFT - directions[3] = { 1, 0 } - directions[3].direction = self.PP_UP - elseif (fromNode == nil and node.direction == self.PP_DOWN_RIGHT) or (fromNode and fromNode.x > node.x and fromNode.z < node.z) then - directions[1] = { -1, 0 } - directions[1].direction = self.PP_DOWN - directions[2] = { -1, 1 } - directions[2].direction = self.PP_DOWN_RIGHT - directions[3] = { 0, 1 } - directions[3].direction = self.PP_RIGHT - else - if fromNode then - AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode xz %d,%d" - , fromNode.x - , fromNode.z - ) - end - AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node xz %d,%d node.direction %s" - , tostring(fromNode) - , node.x, node.z - , tostring(node.direction) - ) - end - PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections %d,%d direction %s %d,%d direction %s %d,%d direction %s" - , directions[1][1], directions[1][2] - , tostring(self.direction_to_text[directions[1].direction+1]) - , directions[2][1], directions[2][2] - , tostring(self.direction_to_text[directions[2].direction+1]) - , directions[3][1], directions[3][2] - , tostring(self.direction_to_text[directions[3].direction+1]) - ) - return directions -end - --- Return all neighbor nodes. Means a target that can be moved from the current node --- current, from_node, add_neighbor_fn -function PathFinderModule:get_neighbors(node, fromNode, add_neighbor_fn) - local x, z = node.x, node.z - local directions = self:getDirections(fromNode, node) - if directions then - for i, offset in ipairs(directions) do - local tnode = self:get_node(x + offset[1], z + offset[2]) - tnode.direction = offset.direction - tnode.from_node = fromNode - add_neighbor_fn(tnode) - end - end -end - -local all_neighbors_offset = { - { -1, -1 }, { 0, -1 }, { 1, -1 }, - { -1, 0 }, { 1, 0 }, - { -1, 1 }, { 0, 1 }, { 1, 1 } -} - -function PathFinderModule:setBlockedGoal() - local x, z = self.targetAheadCell.x, self.targetAheadCell.z - for i, offset in ipairs(all_neighbors_offset) do - if not (self.targetCell.x == (x + offset[1]) and self.targetCell.z == (z + offset[2])) then - local tnode = self:get_node(x + offset[1], z + offset[2]) - tnode.text = string.format("G %d,%d",x + offset[1], z + offset[2]) - self.closedset[tnode] = true - end - end -end - -function PathFinderModule:reachedGoal(current, goal) - if math.abs(current.x - goal.x) < 2 and math.abs(current.z - goal.z) < 2 then - return true - else - return false - end -end - -function PathFinderModule:setupNew(behindStartCell, startCell, targetCell, userdata) - PathFinderModule.debugMsg(self.vehicle, "PFM:setupNew behindStartCell %s,%s startCell %s,%s targetCell %s,%s" - , tostring(behindStartCell.x) - , tostring(behindStartCell.z) - , tostring(startCell.x) - , tostring(startCell.z) - , tostring(targetCell.x) - , tostring(targetCell.z) - ) - self.cached_nodes = {} - self.openset = {} - self.closedset = {} - self.came_from = {} - self.g_score = {} - self.h_score = {} - self.f_score = {} - - self.nodeBehindStart = self:get_node(behindStartCell.x, behindStartCell.z) - self.nodeBehindStart.text = string.format("B %d,%d",behindStartCell.x, behindStartCell.z) - self.nodeBehindStart.direction = behindStartCell.direction - - self.nodeStart = self:get_node(startCell.x, startCell.z) - self.nodeStart.text = string.format("S %d,%d",startCell.x, startCell.z) - self.nodeStart.direction = startCell.direction - - self.nodeGoal = self:get_node(targetCell.x, targetCell.z) - self.nodeGoal.text = string.format("T %d,%d",targetCell.x, targetCell.z) - - self.g_score[self.nodeBehindStart] = math.huge - self.h_score[self.nodeBehindStart] = math.huge - self.f_score[self.nodeBehindStart] = math.huge - - self.g_score[self.nodeStart] = 0 - self.h_score[self.nodeStart] = self:estimate_cost(self.nodeStart, self.nodeGoal) - self.f_score[self.nodeStart] = self.h_score[self.nodeStart] - self.came_from[self.nodeStart] = self.nodeBehindStart - - self.openset[self.nodeStart] = true - self.closedset[self.nodeBehindStart] = true - self:isDriveable(self.nodeStart) - self:setBlockedGoal() - self.initNew = true -end - -function PathFinderModule:createWayPointsNew() - if self.smoothStep == 0 then - self.wayPoints = {} - for index, cell in ipairs(self.path) do - self.wayPoints[index] = self:gridLocationToWorldLocation(cell) - self.wayPoints[index].y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, self.wayPoints[index].x, 1, self.wayPoints[index].z) - self.wayPoints[index].direction = cell.direction - end - -- remove zig zag line - self:smoothResultingPPPath() - end - -- shortcut the path if possible - self:smoothResultingPPPath_Refined() - - if self.smoothStep == 2 then - -- When going to network, dont turn actual road network nodes into pathFinderPoints - if self.goingToNetwork then - for i = 1, #self.wayPoints, 1 do - self.wayPoints[i].isPathFinderPoint = true - end - end - - if self.appendWayPoints ~= nil then - for i = 1, #self.appendWayPoints, 1 do - self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] - end - self.smoothStep = 3 - end - - -- See comment above - if not self.goingToNetwork then - for i = 1, #self.wayPoints, 1 do - self.wayPoints[i].isPathFinderPoint = true - end - end - end -end - -function PathFinderModule:collisionTestCallback(transformId) - if transformId ~= 0 and transformId ~= g_currentMission.terrainRootNode then - local object = g_currentMission:getNodeObject(transformId) - if (object == nil) or (object ~= nil and not (object == self.vehicle or object.rootVehicle == self.vehicle)) then - self.collisionhits = self.collisionhits + 1 - end - end -end - -function PathFinderModule.debugMsg(vehicle, debugText, ...) - if PathFinderModule.debug == true then - AutoDrive.debugMsg(vehicle, debugText, ...) - else - AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, debugText, ...) - end -end From fcb9eaa0ee10333ca075a05ad1a6608f122cc9cf Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 1 Sep 2024 12:35:42 +0200 Subject: [PATCH 36/44] improve chopper unloader --- scripts/Modules/PathFinderModule.lua | 8 +++++++- scripts/Tasks/FollowCombineTask.lua | 4 +++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 1758c1fb..92d7098c 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -805,7 +805,13 @@ function PathFinderModule:update(dt) end if not self.initNew then self:setupNew(self.behindStartCell, self.startCell,self.targetCell) - if self.nodeBehindStart == self.nodeGoal then + local dx, dz = self.nodeStart.x - self.nodeGoal.x, self.nodeStart.z - self.nodeGoal.z + local diff = math.sqrt(dx * dx + dz * dz) + local toCloseToTarget = (diff < 3) + dx, dz = self.nodeBehindStart.x - self.nodeGoal.x, self.nodeBehindStart.z - self.nodeGoal.z + diff = math.sqrt(dx * dx + dz * dz) + toCloseToTarget = toCloseToTarget or (diff < 3) + if (self.nodeBehindStart == self.nodeGoal) or toCloseToTarget then self.completelyBlocked = true return -- no valid path end diff --git a/scripts/Tasks/FollowCombineTask.lua b/scripts/Tasks/FollowCombineTask.lua index b6105a4f..69f956a5 100644 --- a/scripts/Tasks/FollowCombineTask.lua +++ b/scripts/Tasks/FollowCombineTask.lua @@ -112,7 +112,9 @@ function FollowCombineTask:update(dt) return end - if self.angleWrongTimer.elapsedTime > 15000 then + local wrongChopperHeading = self.combine.ad.isChopper and (self.angleToCombineHeading > 90 and self.distanceToCombine < 30) + + if (self.angleWrongTimer.elapsedTime > 15000) or wrongChopperHeading then -- if stuck with harvester - try reverse if (g_updateLoopIndex % 60 == 0) or self.loop5 == nil then self.loop5 = true From cedba1edf3511d31d1357c49f16c76c285c9955c Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Mon, 2 Sep 2024 18:51:58 +0200 Subject: [PATCH 37/44] FIX: unintended change of vehicle destinations if destinations are deleted it could happen that selected destinations of vehicles got changed to the first destination unintended --- scripts/Manager/GraphManager.lua | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/scripts/Manager/GraphManager.lua b/scripts/Manager/GraphManager.lua index 6cf9647c..0af1154a 100644 --- a/scripts/Manager/GraphManager.lua +++ b/scripts/Manager/GraphManager.lua @@ -487,6 +487,9 @@ function ADGraphManager:removeMapMarker(markerId, sendEvent) AutoDriveDeleteMapMarkerEvent.sendEvent(markerId) else if self.mapMarkers[markerId] ~= nil then + -- remove deleted marker from vehicle destinations + ADGraphManager:checkResetVehicleDestinations(markerId) + table.remove(self.mapMarkers, markerId) --Readjust stored markerIndex values to point to corrected ID for markerID, marker in pairs(self.mapMarkers) do @@ -530,8 +533,6 @@ function ADGraphManager:removeMapMarker(markerId, sendEvent) end end end - -- remove deleted marker from vehicle destinations - ADGraphManager:checkResetVehicleDestinations(markerId) end -- Calling external interop listeners From 8f49dfe05af49c2233ae8dc8e4eaad6a0199a4cf Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 15 Sep 2024 10:55:53 +0200 Subject: [PATCH 38/44] TWEAK: LCTRL for fast scroll in Pulldown-List hold LCTRL while scroll with mousewheel will move +-10 items with each tick --- scripts/Hud/PullDownList.lua | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/scripts/Hud/PullDownList.lua b/scripts/Hud/PullDownList.lua index 7bcbbb7a..657bc1a9 100644 --- a/scripts/Hud/PullDownList.lua +++ b/scripts/Hud/PullDownList.lua @@ -703,21 +703,29 @@ function ADPullDownList:act(vehicle, posX, posY, isDown, isUp, button) elseif button == 4 and isUp then -- mouse wheel local oldSelected = self.selected - self.selected = math.max(1, self.selected - 1) + local decrement = 1 + if AutoDrive.leftCTRLmodifierKeyPressed then + decrement = 10 + end + self.selected = math.max(1, self.selected - decrement) if oldSelected ~= self.selected then - self.hovered = math.max(1, self.hovered - 1) + self.hovered = math.max(1, self.hovered - decrement) end - if self.hovered > (self.selected + ADPullDownList.MAX_SHOWN - 1) then - self.hovered = self.selected + ADPullDownList.MAX_SHOWN - 1 + if self.hovered > (self.selected + ADPullDownList.MAX_SHOWN - decrement) then + self.hovered = self.selected + ADPullDownList.MAX_SHOWN - decrement end AutoDrive.mouseWheelActive = true return true elseif button == 5 and isUp then -- mouse wheel - if self:getListElementByIndex(vehicle, self.selected + 1 + ADPullDownList.MAX_SHOWN - 3) ~= nil then - self.selected = self.selected + 1 - if self:getListElementByIndex(vehicle, self.hovered + 1 + ADPullDownList.MAX_SHOWN - 3) ~= nil then - self.hovered = self.hovered + 1 + local increment = 1 + if AutoDrive.leftCTRLmodifierKeyPressed then + increment = 10 + end + if self:getListElementByIndex(vehicle, self.selected + increment + ADPullDownList.MAX_SHOWN - 3) ~= nil then + self.selected = self.selected + increment + if self:getListElementByIndex(vehicle, self.hovered + increment + ADPullDownList.MAX_SHOWN - 3) ~= nil then + self.hovered = self.hovered + increment end end AutoDrive.mouseWheelActive = true From 09a210ca9af779d5abb2e72dca2a81b3dab0f764 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Sun, 3 Nov 2024 20:20:47 +0100 Subject: [PATCH 39/44] added some debug drawing --- scripts/Modules/PathFinderModule.lua | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 92d7098c..22dd2b57 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -643,6 +643,32 @@ function PathFinderModule:update(dt) if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then if self.isNewPF then self:drawDebugNewPF() + if self.isFinished and self.smoothDone and self.wayPoints ~= nil then + local lastPoint = nil + for index, point in ipairs(self.wayPoints) do + if point.isPathFinderPoint and lastPoint ~= nil then + ADDrawingManager:addLineTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + + if AutoDrive.getSettingState("lineHeight") == 1 then + local gy = point.y - AutoDrive.drawHeight + 4 + local ty = lastPoint.y - AutoDrive.drawHeight + 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + else + local gy = point.y - AutoDrive.drawHeight - 4 + local ty = lastPoint.y - AutoDrive.drawHeight - 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + end + end + lastPoint = point + end + end else if self.isFinished and self.smoothDone and self.wayPoints ~= nil and self.chainStartToTarget ~= nil and #self.chainStartToTarget > 0 and self.vehicle.ad.stateModule:getSpeedLimit() > 40 then self:drawDebugForCreatedRoute() From 033dae55ef504399d292dd4bd691cebcd4fbc399 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Fri, 8 Nov 2024 20:35:53 +0100 Subject: [PATCH 40/44] small fix --- scripts/Modules/PathFinderModule.lua | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 22dd2b57..64e0ad1a 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -118,6 +118,10 @@ function PathFinderModule:reset() self.isFinished = true self.smoothDone = true self.fruitAreas = {} + self.goingToNetwork = false + self.goingToPipe = false + self.chasingVehicle = false + self.isSecondChasingVehicle = false if AutoDrive.experimentalFeatures.NewPathfinder then self.PP_UP = 0 From 931b47794d56a008b8741ef3ce77d3b7716110f2 Mon Sep 17 00:00:00 2001 From: Axel32019 <52026009+Axel32019@users.noreply.github.com> Date: Tue, 12 Nov 2024 19:56:41 +0100 Subject: [PATCH 41/44] added nil check --- scripts/Hud/PullDownList.lua | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/scripts/Hud/PullDownList.lua b/scripts/Hud/PullDownList.lua index 657bc1a9..aa281c31 100644 --- a/scripts/Hud/PullDownList.lua +++ b/scripts/Hud/PullDownList.lua @@ -497,14 +497,16 @@ function ADPullDownList:createSelection_FillType() if vehicle ~= nil then local trailers, _ = AutoDrive.getAllUnits(vehicle) - supportedFillTypes = {} - for _, trailer in ipairs(trailers) do - if AutoDrive:hasAL(trailer) then - local alFillTypes = AutoDrive:getALFillTypes(trailer) - if alFillTypes ~= nil and #alFillTypes > 0 then - -- self.autoLoadFillTypes is either nil or it contains items. It is never empty. - self.autoLoadFillTypes = alFillTypes - break + if trailers then + supportedFillTypes = {} + for _, trailer in ipairs(trailers) do + if AutoDrive:hasAL(trailer) then + local alFillTypes = AutoDrive:getALFillTypes(trailer) + if alFillTypes ~= nil and #alFillTypes > 0 then + -- self.autoLoadFillTypes is either nil or it contains items. It is never empty. + self.autoLoadFillTypes = alFillTypes + break + end end end end From a4ead1520964c6f1c7f74e86bafa18697a561cca Mon Sep 17 00:00:00 2001 From: Iwan1803 <47528836+Iwan1803@users.noreply.github.com> Date: Tue, 19 Nov 2024 08:11:29 +0100 Subject: [PATCH 42/44] Update modDesc.xml --- modDesc.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modDesc.xml b/modDesc.xml index 10d79dfc..f2f46c69 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -32,7 +32,7 @@ Différents modes d'utilisation ont été ajoutés depuis les premières version - 2.0.1.7-RC4 + 2.0.1.8 icon.dds From 5a7c2add4c11681070052e63eb2550e7761de3b7 Mon Sep 17 00:00:00 2001 From: Iwan1803 <47528836+Iwan1803@users.noreply.github.com> Date: Tue, 19 Nov 2024 08:12:09 +0100 Subject: [PATCH 43/44] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b73767b6..e2520072 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # FS22_AutoDrive FS22 version of the AutoDrive mod -### Latest Release: 2.0.1.7-RC +### Latest Release: 2.0.1.8 ![GitHub all releases](https://img.shields.io/github/downloads/Stephan-S/FS22_AutoDrive/total?label=Downloads&style=plastic) Direct Download: https://github.com/Stephan-S/FS22_AutoDrive/releases/latest/download/FS22_AutoDrive.zip From fe8430dced548ed031ff399c6238b8c1729c1fe7 Mon Sep 17 00:00:00 2001 From: Iwan1803 <47528836+Iwan1803@users.noreply.github.com> Date: Tue, 19 Nov 2024 08:12:24 +0100 Subject: [PATCH 44/44] Update AutoDrive.lua --- scripts/AutoDrive.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index dcccb966..5b7cf3c3 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -1,5 +1,5 @@ AutoDrive = {} -AutoDrive.version = "2.0.1.7-RC4" +AutoDrive.version = "2.0.1.8" AutoDrive.directory = g_currentModDirectory