perf: pathfinder penalty cache

Once calculated for a grid cell, cache the penalty
in the pathfinder. The penalty depends currently only
on the position and not the heading, so this requires
a limited amount of memory.

This should reduce the calls for getNodePenalty() by
an order of magnitude, but needs to be tested on a
bigger field with difficult situations.

Author: Peter Vaiko

scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua

@@ -232,7 +232,7 @@ function DummyAStar:init(vehicle, path)
 end
 
 function DummyAStar:run()
-    return true, self.path
+    return PathfinderResult(true, self.path)
 end
 
 --- Similar to HybridAStarWithAStarInTheMiddle, but the middle section is not calculated using the A*, instead

scripts/pathfinder/PathfinderConstraints.lua

@@ -76,7 +76,7 @@ function PathfinderConstraints:init(context)
     self:resetCounts()
     local areaToAvoidText = self.areaToAvoid and
             string.format('are to avoid %.1f x %.1f m', self.areaToAvoid.length, self.areaToAvoid.width) or 'none'
-    self.logger:debug('off field penalty %.1f, max fruit percent: %.1f, field number %d, %s, ignore fruit %s, ignore off-field penalty %s',
+    self.logger:debug(self.vehicle, 'off field penalty %.1f, max fruit percent: %.1f, field number %d, %s, ignore fruit %s, ignore off-field penalty %s',
             self.offFieldPenalty, self.maxFruitPercent, self.fieldNum, areaToAvoidText,
             self.areaToIgnoreFruit or 'none', self.areaToIgnoreOffFieldPenalty or 'none')
 end
@@ -172,7 +172,7 @@ function PathfinderConstraints:isValidAnalyticSolutionNode(node, log)
     local analyticLimit = self.maxFruitPercent * 2
     if hasFruit and fruitValue > analyticLimit then
         if log then
-            self.logger:debug('isValidAnalyticSolutionNode: fruitValue %.1f, max %.1f @ %.1f, %.1f',
+            self.logger:debug(self.vehicle, 'isValidAnalyticSolutionNode: fruitValue %.1f, max %.1f @ %.1f, %.1f',
                     fruitValue, analyticLimit, node.x, -node.y)
         end
         return false
@@ -239,10 +239,10 @@ function PathfinderConstraints:resetStrictMode()
 end
 
 function PathfinderConstraints:showStatistics()
-    self.logger:debug('Nodes: %d, Penalties: fruit: %d, off-field: %d, not owned field: %d, collisions: %d, trailer collisions: %d, area to avoid: %d, preferred path: %d',
+    self.logger:debug(self.vehicle, 'Nodes: %d, Penalties: fruit: %d, off-field: %d, not owned field: %d, collisions: %d, trailer collisions: %d, area to avoid: %d, preferred path: %d',
             self.totalNodeCount, self.fruitPenaltyNodeCount, self.offFieldPenaltyNodeCount, self.notOwnedFieldPenaltyNodeCount,
             self.collisionNodeCount, self.trailerCollisionNodeCount, self.areaToAvoidPenaltyCount, self.preferredPathPenaltyCount)
-    self.logger:debug('  max fruit %.1f %%, off-field penalty: %.1f',
+    self.logger:debug(self.vehicle, '  max fruit %.1f %%, off-field penalty: %.1f',
             self.maxFruitPercent, self.offFieldPenalty)
 end
 

scripts/pathfinder/PathfinderUtil.lua

@@ -477,12 +477,12 @@ function PathfinderUtil.findPathForTurn(vehicle, startOffset, goalReferenceNode,
             local _, y, _ = getWorldTranslation(vehicle:getAIDirectionNode())
             local dx, _, dz = worldToLocal(vehicle:getAIDirectionNode(), headlandPath[1].x, y, -headlandPath[1].y)
             local dirDeg = math.deg(math.abs(math.atan2(dx, dz)))
-            PathfinderUtil.logger:debug('First headland waypoint isn\'t in front of us (%.1f), remove first few waypoints to avoid making a circle %.1f %.1f', dirDeg, dx, dz)
+            PathfinderUtil.logger:debug(vehicle, 'First headland waypoint isn\'t in front of us (%.1f), remove first few waypoints to avoid making a circle %.1f %.1f', dirDeg, dx, dz)
             pathfinder = HybridAStarWithPathInTheMiddle(vehicle, 200, headlandPath, true, analyticSolver)
         end
     end
     if pathfinder == nil then
-        PathfinderUtil.logger:debug('No headland, or there is a headland but wasn\'t able to get the shortest path on the headland to the next row, falling back to hybrid A*')
+        PathfinderUtil.logger:debug(vehicle, 'No headland, or there is a headland but wasn\'t able to get the shortest path on the headland to the next row, falling back to hybrid A*')
         pathfinder = HybridAStarWithAStarInTheMiddle(vehicle, 200, 10000, true, analyticSolver)
     end