6.03.00024 Bridge and proximity sensor fixes

- Goal node elevation fixed (#6665)
- Proximity sensor directly on the back of the combine/chopper
allowing for the unloader to get closer.

When driving over non-level bridges, the controlled node isn't vertical and
the goal node was positioned on the ground level which is way below the vehicle.

Due to this, the goal's position in the controlled node's reference wasn't accurate
in this situation, seemed to be a lot closer when driving uphill, lot further when
driving downhill. Uphill goal being too close caused oscillations in PPC.

Moved the goal point on the level of the vehicle (controlled node), so the error
caused by the non-vertical controlled node is negligible.

Few proximity sensor and vehicle size fixes

- Proximity sensor sits on the back of the combine now
- fixed pathfinder vehicle data rectangles, clarified the
misconception that the direction node is always on the root node,
it is not, the direction node is the turning node, usually sits
between the non-steered wheels.

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

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
 

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,26 +249,26 @@ 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)
     end
     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);
             local x2,y2,z2 = localToWorld(g_devHelper.helperNode, rectangle.dLeft, 2, rectangle.dFront);
             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)

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)

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

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(...)

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

modDesc.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="utf-8" standalone="no" ?>
 <modDesc descVersion="47">
-	<version>6.03.00023</version>
+	<version>6.03.00024</version>
 	<author><![CDATA[Courseplay.devTeam]]></author>
 	<title><!-- 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>