diff --git a/modDesc.xml b/modDesc.xml index 647361a1c..f42ea6f16 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -190,6 +190,7 @@ Changelog 8.1.0.0 + diff --git a/scripts/ai/util/VehicleSizeScanner.lua b/scripts/ai/util/VehicleSizeScanner.lua new file mode 100644 index 000000000..d90f36def --- /dev/null +++ b/scripts/ai/util/VehicleSizeScanner.lua @@ -0,0 +1,102 @@ +--[[ +This file is part of Courseplay (https://github.com/Courseplay) +Copyright (C) 2025 Courseplay Dev Team + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +]] + +--- A utility to scan the actual vehicle size using overlap boxes. +VehicleSizeScanner = CpObject() + +function VehicleSizeScanner:init() + self.logger = Logger('VehicleSizeScanner', Logger.level.debug, CpDebug.DBG_IMPLEMENTS) +end + +--- Scan the size of the given vehicle, using the given reference node (or the root node if nil). +--- This function uses overlap boxes moved towards the vehicle from all four directions to find the extents +--- by detecting collisions between the box and the vehicle. +---@param vehicle Vehicle +---@param referenceNode number|nil +---@return number front distance of the frontmost point from the reference node +---@return number rear distance of the rearmost point from the reference node +---@return number left distance of the leftmost point from the reference node +---@return number right distance of the rightmost point from the reference node +function VehicleSizeScanner:scan(vehicle, referenceNode) + self.left = self:_measureDimension(vehicle, referenceNode, 50, 0.1, 'x') + self.right = self:_measureDimension(vehicle, referenceNode, -50, -0.1, 'x') + self.front = self:_measureDimension(vehicle, referenceNode, 50, 0.1, 'z') + self.rear = self:_measureDimension(vehicle, referenceNode, -50, -0.1, 'z') + self.logger:debug(vehicle, 'Front: %.1f Rear: %.1f Left: %.1f Right: %.1f ', + self.front, self.rear, self.left, self.right) + return self.front, self.rear, self.left, self.right +end + +function VehicleSizeScanner:getLength() + -- front is positive z, rear is negative z + return self.front - self.rear +end + +function VehicleSizeScanner:getWidth() + -- left is positive x, right is negative x + return self.left - self.right +end + +function VehicleSizeScanner:_measureDimension(vehicle, referenceNode, startDistance, endDistance, axis) + local step = endDistance > startDistance and 0.1 or -0.1 + local boxHalfSize = 25 + local obX, obY, obZ, getLocalPosition + if axis == 'x' then + getLocalPosition = function(node, d) + return localToWorld(node, d, 0, 0) + end + -- a rectangle (thin box) in the y-z plane, moving along the x axis + obX, obY, obZ = 0.1, 10, boxHalfSize + elseif axis == 'y' then + getLocalPosition = function(node, d) + return localToWorld(node, 0, d, 0) + end + -- a rectangle (thin box) in the x-z plane, moving along the y axis + obX, obY, obZ = boxHalfSize, 0.1, boxHalfSize + elseif axis == 'z' then + getLocalPosition = function(node, d) + return localToWorld(node, 0, 0, d) + end + -- a rectangle (thin box) in the x-y plane, moving along the z axis + obX, obY, obZ = boxHalfSize, 10, 0.1 + end + local dimension = 3 + local node = referenceNode or vehicle.rootNode + local xRot, yRot, zRot = getWorldRotation(node) + self.vehicleBeingScanned = vehicle + self.scannedVehicleFound = false + -- create an overlap box, very thin, like a plane and move it towards the vehicle until we find a collision + for ix = startDistance, endDistance, step do + local x, y, z = getLocalPosition(vehicle.rootNode, ix) + overlapBox(x, y, z, xRot, yRot, zRot, obX, obY, obZ, "_overlapBoxCallback", self, CpUtil.getDefaultCollisionFlags(), true, true, true, true) + if self.scannedVehicleFound then + --PathfinderUtil.addOverlapBox(x, y, z, xRot, yRot, zRot, obX, obY, obZ) + dimension = ix + break + end + end + return dimension +end + +function VehicleSizeScanner:_overlapBoxCallback(transformId) + local collidingObject = g_currentMission.nodeToObject[transformId] + if collidingObject and self.vehicleBeingScanned == collidingObject then + self.scannedVehicleFound = true + end +end + diff --git a/scripts/dev/DevHelper.lua b/scripts/dev/DevHelper.lua index 8b76c614f..dcbd930d9 100644 --- a/scripts/dev/DevHelper.lua +++ b/scripts/dev/DevHelper.lua @@ -45,7 +45,9 @@ function DevHelper:removedSelectedVehicle() end function DevHelper:update() - if not self.isEnabled then return end + if not self.isEnabled then + return + end local lx, lz, hasCollision, vehicle @@ -55,37 +57,53 @@ function DevHelper:update() if self.vehicle then self.vehicle:removeDeleteListener(self, "removedSelectedVehicle") end - local fieldCourseSettings, implementData = FieldCourseSettings.generate(CpUtil.getCurrentVehicle()) + self.vehicle = CpUtil.getCurrentVehicle() + local fieldCourseSettings, implementData = FieldCourseSettings.generate(self.vehicle) self.data.implementWidth = fieldCourseSettings.implementWidth self.data.sideOffset = fieldCourseSettings.sideOffset - self.data.cpImplementWidth, self.data.cpSideOffset, _, _ = WorkWidthUtil.getAutomaticWorkWidthAndOffset(CpUtil.getCurrentVehicle()) + self.data.cpImplementWidth, self.data.cpSideOffset, _, _ = WorkWidthUtil.getAutomaticWorkWidthAndOffset(self.vehicle) + self.vehicleData = PathfinderUtil.VehicleData(self.vehicle, true, 0.5) + self.towedImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle) + if self.towedImplement then + PathfinderUtil.clearOverlapBoxes() + self.vehicleSizeScanner = VehicleSizeScanner() + self.vehicleSizeScanner:scan(self.vehicle) + self.vehicleSizeScanner:scan(self.towedImplement) + end end - self.vehicle = CpUtil.getCurrentVehicle() self.vehicle:addDeleteListener(self, "removedSelectedVehicle") - self.node = CpUtil.getCurrentVehicle():getAIDirectionNode() + self.node = self.vehicle:getAIDirectionNode() lx, _, lz = localDirectionToWorld(self.node, 0, 0, 1) + if self.towedImplement then + if self.towedImplement.getAIAgentSize then + local valid, width, length, lengthOffset, frontOffset, height = CpUtil.try(self.towedImplement.getAIAgentSize, self.towedImplement) + self.data.aiAgent = string.format('%s width=%.2f length=%.2f lengthOffset=%.2f frontOffset=%.2f height=%.2f', + CpUtil.getName(self.towedImplement), width or 0, length or 0, lengthOffset or 0, frontOffset or 0, height or 0) + end + end else -- camera node looks backwards so need to flip everything by 180 degrees self.node = g_currentMission.playerSystem:getLocalPlayer():getCurrentCameraNode() lx, _, lz = localDirectionToWorld(self.node, 0, 0, -1) + self.vehicle, self.vehicleData, self.towedImplement = nil end - self.yRot = math.atan2( lx, lz ) + self.yRot = math.atan2(lx, lz) self.data.xyDeg = math.deg(CpMathUtil.angleFromGame(self.yRot)) self.data.yRotDeg = math.deg(self.yRot) local _, yRot, _ = getWorldRotation(self.node) self.data.yRotFromRotation = math.deg(yRot) self.data.yRotDeg2 = math.deg(MathUtil.getYRotationFromDirection(lx, lz)) self.data.x, self.data.y, self.data.z = getWorldTranslation(self.node) - -- y is always on the ground + -- y is always on the ground self.data.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, self.data.x, self.data.y, self.data.z) self.data.hasFruit, self.data.fruitValue, self.data.fruit = PathfinderUtil.hasFruit(self.data.x, self.data.z, 1, 1) - self.data.fieldId = CpFieldUtil.getFieldIdAtWorldPosition(self.data.x, self.data.z) + self.data.fieldId = CpFieldUtil.getFieldIdAtWorldPosition(self.data.x, self.data.z) --self.data.owned = PathfinderUtil.isWorldPositionOwned(self.data.x, self.data.z) - self.data.farmlandId = g_farmlandManager:getFarmlandIdAtWorldPosition(self.data.x, self.data.z) + self.data.farmlandId = g_farmlandManager:getFarmlandIdAtWorldPosition(self.data.x, self.data.z) self.data.isOnField, self.data.densityBits = FSDensityMapUtil.getFieldDataAtWorldPosition(self.data.x, self.data.y, self.data.z) self.data.isOnFieldArea, self.data.onFieldArea, self.data.totalOnFieldArea = CpFieldUtil.isOnFieldArea(self.data.x, self.data.z) @@ -116,11 +134,11 @@ function DevHelper:overlapBoxCallback(transformId, subShapeIndex) if collidingObject.getRootVehicle then text = text .. ' vehicle ' .. collidingObject:getName() else - if collidingObject:isa(Bale) then - text = text .. ' Bale ' .. tostring(collidingObject.id) .. ' ' .. tostring(collidingObject.nodeId) - else - text = text .. ' ' .. (collidingObject.getName and collidingObject:getName() or 'N/A') - end + if collidingObject:isa(Bale) then + text = text .. ' Bale ' .. tostring(collidingObject.id) .. ' ' .. tostring(collidingObject.nodeId) + else + text = text .. ' ' .. (collidingObject.getName and collidingObject:getName() or 'N/A') + end end end for i = 0, getNumOfUserAttributes(transformId) - 1 do @@ -138,7 +156,9 @@ end -- Left-Alt + Space = save current vehicle position -- Left-Ctrl + Space = restore current vehicle position function DevHelper:keyEvent(unicode, sym, modifier, isDown) - if not self.isEnabled then return end + if not self.isEnabled then + return + end if bitAND(modifier, Input.MOD_LALT) ~= 0 and isDown and sym == Input.KEY_period then -- Left Alt + > mark goal self.goal = State3D(self.data.x, -self.data.z, CpMathUtil.angleFromGameDeg(self.data.yRotDeg)) @@ -146,8 +166,8 @@ function DevHelper:keyEvent(unicode, sym, modifier, isDown) local x, y, z = getWorldTranslation(self.node) local _, yRot, _ = getRotation(self.node) if self.goalNode then - setTranslation( self.goalNode, x, y, z ); - setRotation( self.goalNode, 0, yRot, 0); + setTranslation(self.goalNode, x, y, z); + setRotation(self.goalNode, 0, yRot, 0); else self.goalNode = courseplay.createNode('devhelper', x, z, yRot) end @@ -172,7 +192,8 @@ function DevHelper:keyEvent(unicode, sym, modifier, isDown) CpFieldUtil.detectFieldBoundary(self.data.x, self.data.z, true) elseif bitAND(modifier, Input.MOD_LALT) ~= 0 and isDown and sym == Input.KEY_d then -- use the Giants field boundary detector - self.vehicle:cpDetectFieldBoundary(self.data.x, self.data.z, nil, function() end) + self.vehicle:cpDetectFieldBoundary(self.data.x, self.data.z, nil, function() + end) elseif bitAND(modifier, Input.MOD_LALT) ~= 0 and isDown and sym == Input.KEY_g then self.courseGeneratorInterface:generateDefaultCourse(CpUtil.getCurrentVehicle()) elseif bitAND(modifier, Input.MOD_LALT) ~= 0 and isDown and sym == Input.KEY_n then @@ -187,35 +208,37 @@ end --- Show the data in a table in the order we want it (quick AI generated boilerplate) function DevHelper:fillDisplayData() local displayData = {} - table.insert(displayData, {name = 'x', value = self.data.x}) - table.insert(displayData, {name = 'y', value = self.data.y}) - table.insert(displayData, {name = 'z', value = self.data.z}) - table.insert(displayData, {name = 'yRotDeg', value = self.data.yRotDeg}) - table.insert(displayData, {name = 'yRotDeg2', value = self.data.yRotDeg2}) - table.insert(displayData, {name = 'yRotFromRotation', value = self.data.yRotFromRotation}) - table.insert(displayData, {name = 'xyDeg', value = self.data.xyDeg}) - table.insert(displayData, {name = 'hasFruit', value = self.data.hasFruit}) - table.insert(displayData, {name = 'fruitValue', value = self.data.fruitValue}) - table.insert(displayData, {name = 'fruit', value = self.data.fruit}) - table.insert(displayData, {name = 'fieldId', value = self.data.fieldId}) - table.insert(displayData, {name = 'farmlandId', value = self.data.farmlandId}) - table.insert(displayData, {name = 'isOnField', value = self.data.isOnField}) - table.insert(displayData, {name = 'densityBits', value = self.data.densityBits}) - table.insert(displayData, {name = 'isOnFieldArea', value = self.data.isOnFieldArea}) - table.insert(displayData, {name = 'onFieldArea', value = self.data.onFieldArea}) - table.insert(displayData, {name = 'totalOnFieldArea', value = self.data.totalOnFieldArea}) - table.insert(displayData, {name = 'CP implementWidth', value = self.data.cpImplementWidth}) - table.insert(displayData, {name = 'Giants implementWidth', value = self.data.implementWidth}) - table.insert(displayData, {name = 'CP sideOffset', value = self.data.cpSideOffset}) - table.insert(displayData, {name = 'Giants sideOffset', value = self.data.sideOffset}) + table.insert(displayData, { name = 'x', value = self.data.x }) + table.insert(displayData, { name = 'y', value = self.data.y }) + table.insert(displayData, { name = 'z', value = self.data.z }) + table.insert(displayData, { name = 'yRotDeg', value = self.data.yRotDeg }) + table.insert(displayData, { name = 'yRotDeg2', value = self.data.yRotDeg2 }) + table.insert(displayData, { name = 'yRotFromRotation', value = self.data.yRotFromRotation }) + table.insert(displayData, { name = 'xyDeg', value = self.data.xyDeg }) + table.insert(displayData, { name = 'hasFruit', value = self.data.hasFruit }) + table.insert(displayData, { name = 'fruitValue', value = self.data.fruitValue }) + table.insert(displayData, { name = 'fruit', value = self.data.fruit }) + table.insert(displayData, { name = 'fieldId', value = self.data.fieldId }) + table.insert(displayData, { name = 'farmlandId', value = self.data.farmlandId }) + table.insert(displayData, { name = 'isOnField', value = self.data.isOnField }) + table.insert(displayData, { name = 'densityBits', value = self.data.densityBits }) + table.insert(displayData, { name = 'isOnFieldArea', value = self.data.isOnFieldArea }) + table.insert(displayData, { name = 'onFieldArea', value = self.data.onFieldArea }) + table.insert(displayData, { name = 'totalOnFieldArea', value = self.data.totalOnFieldArea }) + table.insert(displayData, { name = 'CP implementWidth', value = self.data.cpImplementWidth }) + table.insert(displayData, { name = 'Giants implementWidth', value = self.data.implementWidth }) + table.insert(displayData, { name = 'CP sideOffset', value = self.data.cpSideOffset }) + table.insert(displayData, { name = 'Giants sideOffset', value = self.data.sideOffset }) for i = 1, #self.data.collidingShapes do - table.insert(displayData, {name = 'collidingShapes ' .. i, value = self.data.collidingShapes[i]}) + table.insert(displayData, { name = 'collidingShapes ' .. i, value = self.data.collidingShapes[i] }) end return displayData end function DevHelper:draw() - if not self.isEnabled then return end + if not self.isEnabled then + return + end DebugUtil.renderTable(0.3, 0.95, 0.013, self:fillDisplayData(), 0.05) self:showFillNodes() @@ -226,27 +249,63 @@ function DevHelper:draw() CourseGenerator.drawDebugPolylines() CourseGenerator.drawDebugPoints() - if not self.tNode then - self.tNode = createTransformGroup("devhelper") - link(g_currentMission.terrainRootNode, self.tNode) - end + if not self.tNode then + self.tNode = createTransformGroup("devhelper") + link(g_currentMission.terrainRootNode, self.tNode) + end - DebugUtil.drawDebugNode(self.tNode, 'Terrain normal') - --local nx, ny, nz = getTerrainNormalAtWorldPos(g_currentMission.terrainRootNode, self.data.x, self.data.y, self.data.z) + DebugUtil.drawDebugNode(self.tNode, 'Terrain normal') + --local nx, ny, nz = getTerrainNormalAtWorldPos(g_currentMission.terrainRootNode, self.data.x, self.data.y, self.data.z) - --local x, y, z = localToWorld(self.node, 0, -1, -3) + --local x, y, z = localToWorld(self.node, 0, -1, -3) - --drawDebugLine(x, y, z, 1, 1, 1, x + nx, y + ny, z + nz, 1, 1, 1) - DebugUtil.drawOverlapBox(self.data.x, self.data.y + 0.2 + DevHelper.overlapBoxHeight / 2, self.data.z, 0, self.yRot, 0, - DevHelper.overlapBoxWidth / 2, DevHelper.overlapBoxHeight / 2, DevHelper.overlapBoxLength / 2, - 0, 100, 0) + --drawDebugLine(x, y, z, 1, 1, 1, x + nx, y + ny, z + nz, 1, 1, 1) + -- function DebugUtil.drawOverlapBox(x, y, z, rotX, rotY, rotZ, extendX, extendY, extendZ, r, g, b) + --[[ DebugUtil.drawOverlapBox(self.data.x, self.data.y + 0.2 + DevHelper.overlapBoxHeight / 2, self.data.z, 0, self.yRot, 0, + DevHelper.overlapBoxWidth / 2, DevHelper.overlapBoxHeight / 2, DevHelper.overlapBoxLength / 2, + 0, 100, 0)]] PathfinderUtil.showOverlapBoxes() + self:drawPathfinderCollisionBoxes() g_fieldScanner:draw() if self.vehicle then self.vehicle:cpDrawFieldPolygon() end end +function DevHelper:drawPathfinderCollisionBoxes() + if not self.vehicleData then + return + end + + if not self.overlapBoxNode then + self.overlapBoxNode = CpUtil.createNode('devhelperPathfinderBoxes', 0, 0, 0) + link(g_currentMission.terrainRootNode, self.overlapBoxNode) + end + + -- visualize here what the pathfinder does to create the collision boxes for the main and the towed vehicle + -- draw main vehicle + PathfinderUtil.setWorldPositionAndRotationOnTerrain(self.overlapBoxNode, self.data.x, self.data.z, self.yRot, 0) + local ob = self.vehicleData:getVehicleOverlapBoxParams() + local xRot, yRot, zRot = getWorldRotation(self.overlapBoxNode) + local x, y, z = localToWorld(self.overlapBoxNode, ob.xOffset, 1, ob.zOffset) + -- function DebugUtil.drawOverlapBox(x, y, z, rotX, rotY, rotZ, extendX, extendY, extendZ, r, g, b) + DebugUtil.drawOverlapBox(x, y, z, xRot, yRot, zRot, ob.width, 1, ob.length, 0, 0.5, 0.5) + + if not self.towedImplement then + return + end + + ob = self.vehicleData:getTowedImplementOverlapBoxParams() + -- move the helper node to the hitch + x, y, z = localToWorld(self.overlapBoxNode, 0, 0, self.vehicleData:getHitchOffset()) + local lx, _, lz = localDirectionToWorld(self.towedImplement.rootNode, 0, 0, 1) + local trailerYRot = math.atan2(lx, lz) + PathfinderUtil.setWorldPositionAndRotationOnTerrain(self.overlapBoxNode, x, z, trailerYRot, 0) + xRot, yRot, zRot = getWorldRotation(self.overlapBoxNode) + x, y, z = localToWorld(self.overlapBoxNode, ob.xOffset, 1, ob.zOffset) + DebugUtil.drawOverlapBox(x, y, z, xRot, yRot, zRot, ob.width, 1, ob.length, 0, 0.5, 0.5) +end + function DevHelper:showFillNodes() for _, vehicle in pairs(g_currentMission.vehicleSystem.vehicles) do if SpecializationUtil.hasSpecialization(Trailer, vehicle.specializations) then @@ -254,9 +313,13 @@ function DevHelper:showFillNodes() local fillUnits = vehicle:getFillUnits() for i = 1, #fillUnits do local fillRootNode = vehicle:getFillUnitExactFillRootNode(i) - if fillRootNode then DebugUtil.drawDebugNode(fillRootNode, 'Fill node ' .. tostring(i)) end + if fillRootNode then + DebugUtil.drawDebugNode(fillRootNode, 'Fill node ' .. tostring(i)) + end local autoAimNode = vehicle:getFillUnitAutoAimTargetNode(i) - if autoAimNode then DebugUtil.drawDebugNode(autoAimNode, 'Auto aim node ' .. tostring(i)) end + if autoAimNode then + DebugUtil.drawDebugNode(autoAimNode, 'Auto aim node ' .. tostring(i)) + end end end end @@ -264,7 +327,9 @@ end function DevHelper:showAIMarkers() - if not self.vehicle then return end + if not self.vehicle then + return + end local function showAIMarkersOfObject(object) if object.getAIMarkers then @@ -309,32 +374,36 @@ function DevHelper:showAIMarkers() local directionNode = self.vehicle:getAIDirectionNode() if directionNode then - CpUtil.drawDebugNode(self.vehicle:getAIDirectionNode(), false , 4, "AiDirectionNode") + CpUtil.drawDebugNode(self.vehicle:getAIDirectionNode(), false, 4, "AiDirectionNode") end local reverseNode = self.vehicle:getAIReverserNode() if reverseNode then - CpUtil.drawDebugNode(reverseNode, false , 4.2, "AiReverseNode") + CpUtil.drawDebugNode(reverseNode, false, 4.2, "AiReverseNode") end local steeringNode = self.vehicle:getAISteeringNode() if steeringNode then - CpUtil.drawDebugNode(steeringNode, false , 4.4, "AiSteeringNode") + CpUtil.drawDebugNode(steeringNode, false, 4.4, "AiSteeringNode") end local reverserNode = AIVehicleUtil.getAIToolReverserDirectionNode(self.vehicle) if reverserNode then - CpUtil.drawDebugNode(reverserNode, false , 4.8, "AIVehicleUtil.AIToolReverserDirectionNode()") + CpUtil.drawDebugNode(reverserNode, false, 4.8, "AIVehicleUtil.AIToolReverserDirectionNode()") end reverserNode = self.vehicle:getAIToolReverserDirectionNode() if reverserNode then - CpUtil.drawDebugNode(reverserNode, false , 5.0, 'vehicle:AIToolReverserDirectionNode()') + CpUtil.drawDebugNode(reverserNode, false, 5.0, 'vehicle:AIToolReverserDirectionNode()') end end function DevHelper:togglePpcControlledNode() - if not self.vehicle then return end + if not self.vehicle then + return + end local strategy = self.vehicle:getCpDriveStrategy() - if not strategy then return end + if not strategy then + return + end if strategy.ppc:getControlledNode() == AIUtil.getReverserNode(self.vehicle) then strategy.pcc:resetControlledNode() else @@ -343,9 +412,13 @@ function DevHelper:togglePpcControlledNode() end function DevHelper:showDriveData() - if not self.vehicle then return end + if not self.vehicle then + return + end local strategy = self.vehicle:getCpDriveStrategy() - if not strategy then return end + if not strategy then + return + end strategy.ppc:update() strategy.reverser:getDriveData() end diff --git a/scripts/pathfinder/PathfinderCollisionDetector.lua b/scripts/pathfinder/PathfinderCollisionDetector.lua index 4d798960d..0d48828ad 100644 --- a/scripts/pathfinder/PathfinderCollisionDetector.lua +++ b/scripts/pathfinder/PathfinderCollisionDetector.lua @@ -122,9 +122,7 @@ function PathfinderCollisionDetector:findCollidingShapes(node, vehicleToLog, ove self, self.collisionMask, true, true, true, true) if true and self.collidingShapes > 0 then - table.insert(PathfinderUtil.overlapBoxes, - { x = x, y = y + 0.2, z = z, xRot = xRot, yRot = yRot, zRot = zRot, width = overlapBoxParams.width, - length = overlapBoxParams.length }) + PathfinderUtil.addOverlapBox(x, y + 0.2, z, xRot, yRot, zRot, overlapBoxParams.width, 1, overlapBoxParams.length) self.logger:debug(self.vehicle,'my %s (%.1fx%.1f) is colliding with %s at x = %.1f, z = %.1f, yRot = %d', CpUtil.getName(vehicleToLog), 2 * overlapBoxParams.width, 2 * overlapBoxParams.length, self.collidingShapesText, x, z, math.deg(yRot)) end diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index 64561ce0a..ea8736e74 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -42,6 +42,7 @@ PathfinderUtil.VehicleData = CpObject() --- pathfinding to consider the towed implement's heading (which will be different from the vehicle's heading) function PathfinderUtil.VehicleData:init(vehicle, withImplements, buffer) self.vehicle = vehicle + self.vehicleSizeScanner = VehicleSizeScanner() self.rootVehicle = vehicle:getRootVehicle() self.name = vehicle.getName and vehicle:getName() or 'N/A' @@ -49,23 +50,32 @@ function PathfinderUtil.VehicleData:init(vehicle, withImplements, buffer) -- while turning. Get that object here, there may be more but we ignore that case. self.towedImplement = AIUtil.getFirstReversingImplementWithWheels(vehicle) if self.towedImplement then + local _, _, dLeft, dRight = self.vehicleSizeScanner:scan(self.towedImplement) -- the trailer's heading is different than the vehicle's heading and will be calculated and -- checked for collision independently at each waypoint. Also, the trailer is rotated to its heading -- around the hitch (which we approximate as the front side of the size rectangle), not around the root node local dFront = buffer or 0 - local dRear = -self.towedImplement.size.length - (buffer or 0) - local dLeft = AIUtil.getWidth(self.towedImplement) / 2 + (buffer or 0) - local dRight = -AIUtil.getWidth(self.towedImplement) / 2 - (buffer or 0) - self.towedImplementOverlapBoxParams = self:calculateOverlapBoxParams(dFront, dRear, dLeft, dRight) + local dRear = -self.vehicleSizeScanner:getLength() - (buffer or 0) + -- don't use just the width as the vehicle may be asymmetrical + dLeft = dLeft + (buffer or 0) + dRight = dRight - (buffer or 0) local inputAttacherJoint = self.towedImplement:getActiveInputAttacherJoint() + local towedImplementSideOffset = 0 if inputAttacherJoint then local _, _, dz = localToLocal(inputAttacherJoint.node, AIUtil.getDirectionNode(vehicle), 0, 0, 0) self.hitchOffset = dz + if self.towedImplement.rootNode then + -- for implements which are not centered on the hitch point, such as bale wrappers in the working position + towedImplementSideOffset, _, _ = localToLocal(inputAttacherJoint.node, self.towedImplement.rootNode, 0, 0, 0) + dLeft = dLeft - towedImplementSideOffset + dRight = dRight - towedImplementSideOffset + end else self.hitchOffset = self.dRear end - PathfinderUtil.logger:debug(vehicle, 'trailer for the pathfinding is %s, hitch offset is %.1f', - self.towedImplement:getName(), self.hitchOffset) + self.towedImplementOverlapBoxParams = self:calculateOverlapBoxParams(dFront, dRear, dLeft, dRight) + PathfinderUtil.logger:debug(vehicle, 'trailer for the pathfinding is %s, hitch offset is %.1f, side offset is %.1f', + self.towedImplement:getName(), self.hitchOffset, towedImplementSideOffset) end -- calculate the bounding box for the vehicle plus all implements when required (except the trailer) @@ -125,31 +135,12 @@ function PathfinderUtil.VehicleData:getRectangleForImplement(implement, referenc dLeft = AIUtil.getWidth(implement.object) / 2, dRight = -AIUtil.getWidth(implement.object) / 2 } - -- now see if we have something better, then use that. Since any of the six markers may be missing, we - -- check them one by one. - if implement.object.getAIMarkers then - -- otherwise try the AI markers (work area), this will be bigger than the vehicle's physical size, for example - -- in case of sprayers - local aiLeftMarker, aiRightMarker, aiBackMarker = implement.object:getAIMarkers() - if aiLeftMarker and aiRightMarker then - rectangle.dLeft, _, rectangle.dFront = localToLocal(aiLeftMarker, referenceNode, 0, 0, 0) - rectangle.dRight, _, _ = localToLocal(aiRightMarker, referenceNode, 0, 0, 0) - if aiBackMarker then - _, _, rectangle.dRear = localToLocal(aiBackMarker, referenceNode, 0, 0, 0) - end - end - end - if implement.object.getAISizeMarkers then - -- but the best case is if we have the AI size markers - local aiSizeLeftMarker, aiSizeRightMarker, aiSizeBackMarker = implement.object:getAISizeMarkers() - if aiSizeLeftMarker then - rectangle.dLeft, _, rectangle.dFront = localToLocal(aiSizeLeftMarker, referenceNode, 0, 0, 0) - end - if aiSizeRightMarker then - rectangle.dRight, _, _ = localToLocal(aiSizeRightMarker, referenceNode, 0, 0, 0) - end - if aiSizeBackMarker then - _, _, rectangle.dRear = localToLocal(aiSizeBackMarker, referenceNode, 0, 0, 0) + local aiLeftMarker, aiRightMarker, aiBackMarker = WorkWidthUtil.getAIMarkers(implement.object) + if aiLeftMarker and aiRightMarker then + rectangle.dLeft, _, rectangle.dFront = localToLocal(aiLeftMarker, referenceNode, 0, 0, 0) + rectangle.dRight, _, _ = localToLocal(aiRightMarker, referenceNode, 0, 0, 0) + if aiBackMarker then + _, _, rectangle.dRear = localToLocal(aiBackMarker, referenceNode, 0, 0, 0) end end return rectangle @@ -737,12 +728,22 @@ function PathfinderUtil.showNodes(pathfinder) end end +function PathfinderUtil.clearOverlapBoxes() + PathfinderUtil.overlapBoxes = {} +end + +function PathfinderUtil.addOverlapBox(x, y, z, xRot, yRot, zRot, width, height, length) + table.insert(PathfinderUtil.overlapBoxes, + { x = x, y = y + 0.2, z = z, xRot = xRot, yRot = yRot, zRot = zRot, + width = width, height = height, length = length }) +end + function PathfinderUtil.showOverlapBoxes() if not PathfinderUtil.overlapBoxes then return end for _, box in ipairs(PathfinderUtil.overlapBoxes) do - DebugUtil.drawOverlapBox(box.x, box.y, box.z, box.xRot, box.yRot, box.zRot, box.width, 1, box.length, 0, 100, 0) + DebugUtil.drawOverlapBox(box.x, box.y, box.z, box.xRot, box.yRot, box.zRot, box.width, box.height, box.length, 0, 1, 0) end end