diff --git a/AIDriver.lua b/AIDriver.lua index cedd1aed8..27fc270c3 100644 --- a/AIDriver.lua +++ b/AIDriver.lua @@ -1826,7 +1826,10 @@ function AIDriver:setBackMarkerNode(vehicle) local lastImplement lastImplement, backMarkerOffset = AIDriverUtil.getLastAttachedImplement(vehicle) referenceNode = vehicle.rootNode - self:debug('Using the last implement\'s rear distance for the rear proximity sensor, %d m from root node', backMarkerOffset) + self:debug('Using the last implement\'s rear distance for the rear proximity sensor, %d m from root node', backMarkerOffset) elseif self.measuredBackDistance then + referenceNode = vehicle.rootNode + backMarkerOffset = -self.measuredBackDistance + self:debug('back marker node on measured back distance %.1f', self.measuredBackDistance) elseif reverserNode then -- if there is a reverser node, use that, mainly because that most likely will turn with an implement -- or with the back component of an articulated vehicle. Just need to find out the distance correctly @@ -1838,7 +1841,7 @@ function AIDriver:setBackMarkerNode(vehicle) debugText, backMarkerOffset, dBetweenRootAndReverserNode) else referenceNode = vehicle.rootNode - backMarkerOffset = - vehicle.sizeLength / 2 - vehicle.lengthOffset + backMarkerOffset = - vehicle.sizeLength / 2 + vehicle.lengthOffset self:debug('Using the vehicle\'s root node for the rear proximity sensor, %d m from root node', backMarkerOffset) end if not vehicle.cp.driver.aiDriverData.backMarkerNode then diff --git a/CombineAIDriver.lua b/CombineAIDriver.lua index e98ce9b25..b509abf70 100644 --- a/CombineAIDriver.lua +++ b/CombineAIDriver.lua @@ -1640,8 +1640,8 @@ end function CombineAIDriver:measureBackDistance() self.measuredBackDistance = 0 -- raycast from a point behind the vehicle forward towards the direction node - local nx, ny, nz = localDirectionToWorld(self:getDirectionNode(), 0, 0, 1) - local x, y, z = localToWorld(self:getDirectionNode(), 0, 1.5, - self.maxBackDistance) + local nx, ny, nz = localDirectionToWorld(self.vehicle.rootNode, 0, 0, 1) + local x, y, z = localToWorld(self.vehicle.rootNode, 0, 1.5, - self.maxBackDistance) raycastAll(x, y, z, nx, ny, nz, 'raycastBackCallback', self.maxBackDistance, self) end @@ -1661,7 +1661,6 @@ function CombineAIDriver:raycastBackCallback(hitObjectId, x, y, z, distance, nx, end end - function CombineAIDriver:setStrawSwath(enable) local strawSwathCanBeEnabled = false local fruitType = g_fruitTypeManager:getFruitTypeIndexByFillTypeIndex(self.vehicle:getFillUnitFillType(self.combine.fillUnitIndex)) @@ -1683,6 +1682,10 @@ function CombineAIDriver:onDraw() DebugUtil.drawDebugNode(dischargeNode.node, 'discharge') end + if self.aiDriverData.backMarkerNode then + DebugUtil.drawDebugNode(self.aiDriverData.backMarkerNode, 'back marker') + end + UnloadableFieldworkAIDriver.onDraw(self) end diff --git a/DevHelper.lua b/DevHelper.lua index 19e0f1432..bf7b2b535 100644 --- a/DevHelper.lua +++ b/DevHelper.lua @@ -48,12 +48,18 @@ function DevHelper:update() end self.vehicle = g_currentMission.controlledVehicle - self.node = AIDriverUtil.getDirectionNode(g_currentMission.controlledVehicle) + self.node = g_currentMission.controlledVehicle.rootNode lx, _, lz = localDirectionToWorld(self.node, 0, 0, 1) else -- camera node looks backwards so need to flip everything by 180 degrees self.node = g_currentMission.player.cameraNode lx, _, lz = localDirectionToWorld(self.node, 0, 0, -1) + if not self.proxySensor then + self.proxySensor = ProximitySensor(self.node, 180, 10, 1, 0) + else + self.proxySensor:update() + self.proxySensor:showDebugInfo() + end end if self.vehicleData then @@ -243,7 +249,7 @@ end function DevHelper:showVehicleSize() local vehicle = g_currentMission.controlledVehicle if not vehicle then return end - local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(AIDriverUtil.getDirectionNode(vehicle)) + local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicle.rootNode) local node = State3D(x, -z, courseGenerator.fromCpAngle(yRot)) if not g_devHelper.helperNode then g_devHelper.helperNode = courseplay.createNode('pathfinderHelper', node.x, -node.y, 0) @@ -251,7 +257,7 @@ function DevHelper:showVehicleSize() local y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, node.x, 0, -node.y); setTranslation(g_devHelper.helperNode, node.x, y, -node.y) setRotation(g_devHelper.helperNode, 0, courseGenerator.toCpAngle(node.t), 0) - + if self.vehicleData then for _, rectangle in ipairs(self.vehicleData.rectangles) do local x1,y1,z1 = localToWorld(g_devHelper.helperNode, rectangle.dRight, 2, rectangle.dFront); @@ -259,10 +265,10 @@ function DevHelper:showVehicleSize() local x3,y3,z3 = localToWorld(g_devHelper.helperNode, rectangle.dRight, 2, rectangle.dRear); local x4,y4,z4 = localToWorld(g_devHelper.helperNode, rectangle.dLeft, 2, rectangle.dRear); - drawDebugLine(x1,y1,z1,0,0,1,x2,y2,z2,0,0,1); - drawDebugLine(x1,y1,z1,0,0,1,x3,y3,z3,0,0,1); - drawDebugLine(x2,y2,z2,0,0,1,x4,y4,z4,0,0,1); - drawDebugLine(x3,y3,z3,0,0,1,x4,y4,z4,0,0,1); + drawDebugLine(x1,y1,z1,0.2, 0.2 ,1,x2,y2,z2,0.2, 0.2,1); + drawDebugLine(x1,y1,z1,0.2, 0.2 ,1,x3,y3,z3,0.2, 0.2,1); + drawDebugLine(x2,y2,z2,0.2, 0.2 ,1,x4,y4,z4,0.2, 0.2,1); + drawDebugLine(x3,y3,z3,0.2, 0.2 ,1,x4,y4,z4,0.2, 0.2,1); end if self.vehicleData.trailerRectangle then local x, y, z = localToWorld(g_devHelper.helperNode, 0, 0, self.vehicleData.trailerHitchOffset) @@ -283,9 +289,10 @@ function DevHelper:showVehicleSize() for i = 1, 4 do local cp = self.collisionData.corners[i] local pp = self.collisionData.corners[i > 1 and i - 1 or 4] - cpDebug:drawLine(cp.x, cp.y + 0.4, cp.z, 1, 1, 0, pp.x, pp.y + 0.4, pp.z) + cpDebug:drawLine(cp.x, cp.y + 0.4, cp.z, 1, 0, 0, pp.x, pp.y + 0.4, pp.z) end end + DebugUtil.drawDebugNode(g_devHelper.helperNode, 'devhelper') end function DevHelper.saveVehiclePosition(vehicle, vehiclePositionData) diff --git a/FieldworkAIDriver.lua b/FieldworkAIDriver.lua index 3d5ea839c..645eac632 100644 --- a/FieldworkAIDriver.lua +++ b/FieldworkAIDriver.lua @@ -1340,7 +1340,10 @@ function FieldworkAIDriver:onDraw() DebugUtil.drawDebugNode(aiSizeBackMarker, object:getName() .. ' AI Size Back') end end - DebugUtil.drawDebugNode(object.cp.directionNode or object.rootNode, object:getName() .. ' root') + DebugUtil.drawDebugNode(object.rootNode, object:getName() .. ' root') + if object.cp.directionNode then + DebugUtil.drawDebugNode(object.cp.directionNode, object:getName() .. ' dir') + end end showAIMarkersOfObject(self.vehicle) diff --git a/ProximitySensor.lua b/ProximitySensor.lua index bbf613ef8..20c4364c0 100644 --- a/ProximitySensor.lua +++ b/ProximitySensor.lua @@ -63,8 +63,10 @@ function ProximitySensor:update() self.distanceOfClosestObject = math.huge self.objectId = nil if self.enabled then - raycastClosest(x, y + self.height, z, nx, ny, nz, 'raycastCallback', self.range, self, bitOR(AIVehicleUtil.COLLISION_MASK, 2)) --- cpDebug:drawLine(x, y + self.height, z, 0, 1, 0, tx, y2 + self.height, tz) + raycastClosest(x, y + self.height, z, nx, ny, nz, 'raycastCallback', self.range, self, + 0x1000 + 0x100 + 0x80 + 0x40 + 0x20) + --bitOR(AIVehicleUtil.COLLISION_MASK, 0x1f0)) + --cpDebug:drawLine(x, y + self.height, z, 0, 1, 0, x + 5 * nx, y + self.height + 5 * ny, z + 5 * nz) end if courseplay.debugChannels[12] and self.distanceOfClosestObject <= self.range then local green = self.distanceOfClosestObject / self.range @@ -104,7 +106,7 @@ function ProximitySensor:showDebugInfo() local object = g_currentMission:getNodeObject(self.objectId) if object then if object.getRootVehicle then - text = text .. 'vehicle' .. object:getName() + text = text .. 'vehicle ' .. object:getName() else text = text .. object:getName() end diff --git a/PurePursuitController.lua b/PurePursuitController.lua index 2d971ab9e..147291e53 100644 --- a/PurePursuitController.lua +++ b/PurePursuitController.lua @@ -413,6 +413,7 @@ function PurePursuitController:findGoalPoint() -- If we weren't on track yet (after initialization, on our way to the first/initialized waypoint) -- set the goal to the relevant WP self.goalWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) + self:setGoalTranslation() self:showGoalpointDiag(1, 'initializing, ix=%d, q1=%.1f, q2=%.1f, la=%.1f', ix, q1, q2, self.lookAheadDistance) -- and also the current waypoint is now at the relevant WP self:setCurrentWaypoint(self.relevantWpNode.ix) @@ -428,8 +429,8 @@ function PurePursuitController:findGoalPoint() end local cosGamma = ( q2 * q2 - q1 * q1 - l * l ) / (-2 * l * q1) local p = q1 * cosGamma + math.sqrt(q1 * q1 * (cosGamma * cosGamma - 1) + self.lookAheadDistance * self.lookAheadDistance) - local gx, gy, gz = localToWorld(node1.node, 0, 0, p) - setTranslation(self.goalWpNode.node, gx, gy + 1, gz) + local gx, _, gz = localToWorld(node1.node, 0, 0, p) + self:setGoalTranslation(gx, gz) self.wpBeforeGoalPointIx = ix self:showGoalpointDiag(2, 'common case, ix=%d, q1=%.1f, q2=%.1f la=%.1f', ix, q1, q2, self.lookAheadDistance) -- current waypoint is the waypoint at the end of the path segment @@ -445,8 +446,8 @@ function PurePursuitController:findGoalPoint() if math.abs(self.crossTrackError) <= self.lookAheadDistance then -- case iii (two intersection points) local p = math.sqrt(self.lookAheadDistance * self.lookAheadDistance - self.crossTrackError * self.crossTrackError) - local gx, gy, gz = localToWorld(self.projectedPosNode, 0, 0, p) - setTranslation(self.goalWpNode.node, gx, gy + 1, gz) + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, p) + self:setGoalTranslation(gx, gz) self.wpBeforeGoalPointIx = ix self:showGoalpointDiag(3, 'two intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, self.lookAheadDistance, self.crossTrackError) @@ -456,8 +457,8 @@ function PurePursuitController:findGoalPoint() -- case iv (no intersection points) -- case v ( goal point dead zone) -- set the goal to the projected position - local gx, gy, gz = localToWorld(self.projectedPosNode, 0, 0, 0) - setTranslation(self.goalWpNode.node, gx, gy + 1, gz) + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, 0) + self:setGoalTranslation(gx, gz) self.wpBeforeGoalPointIx = ix self:showGoalpointDiag(4, 'no intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, self.lookAheadDistance, self.crossTrackError) @@ -490,6 +491,18 @@ function PurePursuitController:findGoalPoint() end end +-- set the goal WP node's position. This will make sure the goal node is on the same height +-- as the controlled node, avoiding issues when driving on non-level bridges where the controlled node +-- is not vertical and since the goal is very far below it, it will be much closer/further in the controlled +-- node's reference frame. +-- If everyone is on the same height, this error is negligible +function PurePursuitController:setGoalTranslation(x, z) + local gx, _, gz = getWorldTranslation(self.goalWpNode.node) + local _, cy, _ = getWorldTranslation(self.controlledNode) + -- if there's an x, z passed in, use that, otherwise only adjust y to be the same as the controlled node + setTranslation(self.goalWpNode.node, x or gx, cy + 1, z or gz) +end + -- set the current waypoint for the rest of Courseplay and to notify listeners function PurePursuitController:setCurrentWaypoint(ix) -- this is the current waypoint for the rest of Courseplay code, the waypoint we are driving to @@ -514,6 +527,7 @@ function PurePursuitController:showGoalpointDiag(case, ...) local diagText = string.format(...) if courseplay.debugChannels[12] then DebugUtil.drawDebugNode(self.goalWpNode.node, diagText) + DebugUtil.drawDebugNode(self.controlledNode, 'controlled') end if case ~= self.case then self:debug(...) diff --git a/course-generator/PathfinderUtil.lua b/course-generator/PathfinderUtil.lua index 0b23a2813..352b0eb28 100644 --- a/course-generator/PathfinderUtil.lua +++ b/course-generator/PathfinderUtil.lua @@ -38,8 +38,8 @@ function PathfinderUtil.VehicleData:init(vehicle, withImplements, buffer) self.vehicle = vehicle self.rootVehicle = vehicle:getRootVehicle() self.name = vehicle.getName and vehicle:getName() or 'N/A' - -- distance of the sides of a rectangle from the direction node of the vehicle - -- in other words, the X and Z offsets of the corners from the direction node + -- distance of the sides of a rectangle from the root node of the vehicle + -- in other words, the X and Z offsets of the corners from the root node -- negative is to the rear and to the right -- this is the bounding box of the entire vehicle with all attached implements, -- except a towed trailer as that is calculated independently @@ -62,6 +62,7 @@ function PathfinderUtil.VehicleData:init(vehicle, withImplements, buffer) dLeft = self.trailer.sizeWidth / 2, dRight = -self.trailer.sizeWidth / 2 } + -- TODO: this should be the attacher joint's offset, not the back of the vehicle. self.trailerHitchOffset = self.dRear self.trailerHitchLength = AIDriverUtil.getTowBarLength(vehicle) courseplay.debugVehicle(7, vehicle, 'trailer for the pathfinding is %s, hitch offset is %.1f', @@ -74,22 +75,29 @@ end --- Calculate the relative coordinates of a rectangle's corners around a reference node, representing the implement function PathfinderUtil.VehicleData:getRectangleForImplement(implement, referenceNode, buffer) - local rootToReferenceNodeDistance = 0 + local rootToReferenceNodeOffset = 0 local attacherJoint = implement.object.getActiveInputAttacherJoint and implement.object:getActiveInputAttacherJoint() if attacherJoint and attacherJoint.node then -- the implement may not be aligned with the vehicle so we need to calculate this distance in two - -- steps, first the distance between the vehicle's direction node and the attacher joint and then + -- steps, first the distance between the vehicle's root node and the attacher joint and then -- from the attacher joint to the implement's root node + -- < 0 when the attacher joint is behind the reference node local _, _, referenceToAttacherJoint = localToLocal(attacherJoint.node, referenceNode, 0, 0, 0) + -- > 0 when the attacher node is in front of the implement's root node (we don't use the attacher joint node + -- as a reference as it may point to any direction, we know the implement's root node points forward local _, _, attacherJointToImplementRoot = localToLocal(attacherJoint.node, implement.object.rootNode, 0, 0, 0) - rootToReferenceNodeDistance = attacherJointToImplementRoot - referenceToAttacherJoint + -- we call this offset, and is negative when behind the reference node, positive when in front of it + -- (need to reverse attacherJointToImplementRoot) + rootToReferenceNodeOffset = - attacherJointToImplementRoot + referenceToAttacherJoint + courseplay.debugFormat(7, '%s: ref to attacher joint %.1f, att to implement root %.1f, impl root to ref %.1f', + nameNum(implement.object), referenceToAttacherJoint, attacherJointToImplementRoot, rootToReferenceNodeOffset) else - _, _, rootToReferenceNodeDistance = localToLocal(implement.object.rootNode, referenceNode, 0, 0, 0) + _, _, rootToReferenceNodeOffset = localToLocal(implement.object.rootNode, referenceNode, 0, 0, 0) end -- default size, used by Giants to determine the drop area when buying something local rectangle = { - dFront = rootToReferenceNodeDistance + implement.object.sizeLength / 2 + implement.object.lengthOffset + (buffer or 0), - dRear = rootToReferenceNodeDistance - implement.object.sizeLength / 2 - implement.object.lengthOffset - (buffer or 0), + dFront = rootToReferenceNodeOffset + implement.object.sizeLength / 2 + implement.object.lengthOffset + (buffer or 0), + dRear = rootToReferenceNodeOffset - implement.object.sizeLength / 2 + implement.object.lengthOffset - (buffer or 0), dLeft = implement.object.sizeWidth / 2, dRight = -implement.object.sizeWidth / 2 } @@ -121,7 +129,7 @@ end function PathfinderUtil.VehicleData:calculateSizeOfObjectList(vehicle, implements, buffer, rectangles) for _, implement in ipairs(implements) do --print(implement.object:getName()) - local referenceNode = AIDriverUtil.getDirectionNode(vehicle) + local referenceNode = vehicle.rootNode if implement.object ~= self.trailer then -- everything else is attached to the root vehicle and calculated as it was moving with it (having -- the same heading) @@ -306,7 +314,7 @@ end function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData, vehiclesToIgnore, log) self.vehiclesToIgnore = vehiclesToIgnore or {} self.vehicleData = vehicleData - -- the box for overlapBox() is symmetric, so if our direction node is not in the middle of the vehicle rectangle, + -- the box for overlapBox() is symmetric, so if our root node is not in the middle of the vehicle rectangle, -- we have to translate it into the middle -- right/rear is negative local xOffset = vehicleData.dRight + vehicleData.dLeft diff --git a/modDesc.xml b/modDesc.xml index c8e3848c7..ced309ef5 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -1,6 +1,6 @@ - 6.03.00023 + 6.03.00024 <!-- en=English de=German fr=French es=Spanish ru=Russian pl=Polish it=Italian br=Brazilian-Portuguese cs=Chinese(Simplified) ct=Chinese(Traditional) cz=Czech nl=Netherlands hu=Hungary jp=Japanese kr=Korean pt=Portuguese ro=Romanian tr=Turkish --> <en>CoursePlay SIX</en>