Merge pull request #25 from Courseplay/20-headlands-around-big-island…

…s-not-used-in-turns

Find path on headland to next row

CourseGenerator/Block.lua

@@ -118,17 +118,14 @@ function Block:finalize(entry)
         -- this assumes row has not been manipulated other than reversed
         local rowOnLeftWorked, rowOnRightWorked, leftSideBlockBoundary, rightSideBlockBoundary =
             self:_getAdjacentRowInfo(rowInfo.rowIx, rowInfo.reverse, self.rows, self.rowsInWorkSequence)
+        row:setAdjacentRowInfo(rowOnLeftWorked, rowOnRightWorked, leftSideBlockBoundary, rightSideBlockBoundary)
         self.logger:debug('row %d is now at position %d, left/right worked %s/%s, headland %s/%s',
                 row:getOriginalSequenceNumber(), i, rowOnLeftWorked, rowOnRightWorked, leftSideBlockBoundary, rightSideBlockBoundary)
         -- need vertices close enough so the smoothing in goAround() only starts close to the island
         row:splitEdges(cg.cRowWaypointDistance)
         row:adjustLength()
         row:setRowNumber(i)
         row:setAllAttributes()
-        row:setAttribute(nil, cg.WaypointAttributes.setLeftSideWorked, rowOnLeftWorked)
-        row:setAttribute(nil, cg.WaypointAttributes.setRightSideWorked, rowOnRightWorked)
-        row:setAttribute(nil, cg.WaypointAttributes.setLeftSideBlockBoundary, leftSideBlockBoundary)
-        row:setAttribute(nil, cg.WaypointAttributes.setRightSideBlockBoundary, rightSideBlockBoundary)
         table.insert(self.rowsInWorkSequence, row)
     end
     return exit

CourseGenerator/CacheMap.lua

@@ -0,0 +1,72 @@
+local CacheMap = CpObject()
+
+--- A cached map to store key-value pairs
+--- The map can be multi-dimensional, just make sure to pass in a key for each dimension.
+function CacheMap:init(dimensions)
+    self.logger = Logger('CacheMap')
+    self.dimensions = dimensions or 1
+    self.map = {}
+end
+
+--- Get the value at the given key
+---@param k1[, k2[, k3]]] ... the keys
+---@return any the value
+function CacheMap:get(...)
+    return self:_getMap(...)[select(self.dimensions, ...)]
+end
+
+--- Set the value at the given key
+---@param k1[, k2[, k3 ...]]], value : the keys in the map, according to the dimension and a value
+---@return any the value
+function CacheMap:put(...)
+    local lastKey = select(self.dimensions, ...)
+    local value = select(self.dimensions + 1, ...)
+    self:_getMap(...)[lastKey] = value
+    return value
+end
+
+--- Get the value defined by the key, if not found, call func with the keys and set value to the result.
+---@param k1[, k2[, k3]]] ... the keys
+---@param lambda function the function to set the value
+---@return any the value pointed to by keys. If it is not cached yet, call lambda and set the value to the value
+--- returned by lambda
+function CacheMap:getWithLambda(...)
+    local nArgs = select('#', ...)
+    if nArgs < self.dimensions + 1 then
+        self.logger:error('getWithFunc() with %d dimension(s) needs %d key(s) and a func, got only %d argument(s)',
+                self.dimensions, self.dimensions, nArgs)
+        return
+    end
+    local map = self:_getMap(...)
+    local lastKey = select(self.dimensions, ...)
+    local entry = map[lastKey]
+    if entry == nil then
+        map[lastKey] = select(nArgs, ...)()
+        return map[lastKey]
+    else
+        return entry
+    end
+end
+
+function CacheMap:_getMap(...)
+    local nArgs = select('#', ...)
+    if nArgs < self.dimensions then
+        self.logger:error('get() with %d dimension(s) needs %d key(s), got only %d argument(s)',
+                self.dimensions, self.dimensions, nArgs)
+        return
+    end
+    -- drill down to the entry in the map pointed to by the keys, creating non-existing dimensions
+    -- on the way when needed
+    local map = self.map
+    for i = 1, self.dimensions - 1 do
+        local key = select(i, ...)
+        if not map[key] then
+            map[key] = {}
+        end
+        map = map[key]
+    end
+    return map
+end
+
+---@class cg.CacheMap
+cg.CacheMap = CacheMap

CourseGenerator/Center.lua

@@ -24,7 +24,7 @@ function Center:init(context, boundary, headland, startLocation, bigIslands)
             virtualHeadland:sharpenCorners(self.context.turningRadius)
         end
         self.headlandPolygon = virtualHeadland:getPolygon()
-        cg.addDebugPolyline(self.headlandPolygon, {1, 1, 0, 0.5})
+        cg.addDebugPolyline(self.headlandPolygon, { 1, 1, 0, 0.5 })
         self.headland = virtualHeadland
         self.mayOverlapHeadland = false
     else
@@ -103,7 +103,7 @@ function Center:generate()
     -- headland from one block to the other
 
     -- clear cache for the block sequencer
-    self.closestVertexCache, self.pathCache = {}, {}
+    self.closestVertexCache, self.pathCache = cg.CacheMap(2), cg.CacheMap(3)
 
     -- first run of the genetic search will restrict connecting path between blocks to the same
     -- headland, that is, the entry to the next block must be adjacent to the headland where the
@@ -463,27 +463,18 @@ end
 ---@param v2 cg.Vector
 ---@return Polyline always has at least one vertex
 function Center:_findShortestPathOnHeadland(headland, v1, v2)
-    if not self.closestVertexCache[headland] then
-        self.closestVertexCache[headland] = {}
-    end
-    local cvc = self.closestVertexCache[headland]
-    if not cvc[v1] then
-        cvc[v1] = headland:getPolygon():findClosestVertexToPoint(v1)
-    end
-    if not cvc[v2] then
-        cvc[v2] = headland:getPolygon():findClosestVertexToPoint(v2)
-    end
-    if not self.pathCache[headland] then
-        self.pathCache[headland] = {}
-    end
-    local pc = self.pathCache[headland]
-    if not pc[v1] then
-        pc[v1] = {}
-    end
-    if not pc[v1][v2] then
-        pc[v1][v2] = headland:getPolygon():getShortestPathBetween(cvc[v1].ix, cvc[v2].ix)
-    end
-    return pc[v1][v2]
+    local cv1 = self.closestVertexCache:getWithLambda(headland, v1,
+            function()
+                return headland:getPolygon():findClosestVertexToPoint(v1)
+            end)
+    local cv2 = self.closestVertexCache:getWithLambda(headland, v2,
+            function()
+                return headland:getPolygon():findClosestVertexToPoint(v2)
+            end)
+    return self.pathCache:getWithLambda(headland, v1, v2,
+            function()
+                return headland:getPolygon():getShortestPathBetween(cv1.ix, cv2.ix)
+            end)
 end
 
 function Center:_wrapUpConnectingPaths()

CourseGenerator/FieldworkCourse.lua

@@ -14,6 +14,7 @@ function FieldworkCourse:init(context)
     self:_setContext(context)
     self.headlandPath = cg.Polyline()
     self.circledIslands = {}
+    self.headlandCache = cg.CacheMap()
 
     self.logger:debug('### Generating headlands around the field perimeter ###')
     self:generateHeadlands()
@@ -25,22 +26,27 @@ function FieldworkCourse:init(context)
     end
 
     if self.context.headlandFirst then
+        -- connect the headlands first as the center needs to start where the headlands finish
         self.logger:debug('### Connecting headlands (%d) from the outside towards the inside ###', #self.headlands)
         self.headlandPath = cg.HeadlandConnector.connectHeadlandsFromOutside(self.headlands,
                 context.startLocation, self.context.workingWidth, self.context.turningRadius)
         self:routeHeadlandsAroundSmallIslands()
         self.logger:debug('### Generating up/down rows ###')
         self:generateCenter()
     else
+        -- here, make the center first as we want to start on the headlands where the center was finished
         self.logger:debug('### Generating up/down rows ###')
         local endOfLastRow = self:generateCenter()
         self.logger:debug('### Connecting headlands (%d) from the inside towards the outside ###', #self.headlands)
         self.headlandPath = cg.HeadlandConnector.connectHeadlandsFromInside(self.headlands,
                 endOfLastRow, self.context.workingWidth, self.context.turningRadius)
         self:routeHeadlandsAroundSmallIslands()
     end
+
     if self.context.bypassIslands then
-        self:bypassIslands()
+        self:bypassSmallIslandsInCenter()
+        self.logger:debug('### Bypassing big islands in the center: create path around them ###')
+        self:circleBigIslands()
     end
 end
 
@@ -59,8 +65,8 @@ function FieldworkCourse:getPath()
             self.path:appendMany(self:getCenterPath())
             self.path:appendMany(self:getHeadlandPath())
         end
+        self.path:calculateProperties()
     end
-    self.path:calculateProperties()
     return self.path
 end
 
@@ -230,7 +236,8 @@ function FieldworkCourse:routeHeadlandsAroundBigIslands()
     end
 end
 
----
+--- We do this after we have connected the individual headlands so the links between the headlands
+--- are also routed around the islands.
 function FieldworkCourse:routeHeadlandsAroundSmallIslands()
     self.logger:debug('### Bypassing small islands on the headland ###')
     for _, island in pairs(self.smallIslands) do
@@ -246,14 +253,12 @@ function FieldworkCourse:routeHeadlandsAroundSmallIslands()
     end
 end
 
-function FieldworkCourse:bypassIslands()
+function FieldworkCourse:bypassSmallIslandsInCenter()
     self.logger:debug('### Bypassing small islands in the center ###')
     for _, island in pairs(self.smallIslands) do
         self.logger:debug('Bypassing small island %d on the center', island:getId())
         self.center:bypassSmallIsland(island:getInnermostHeadland():getPolygon(), not self.circledIslands[island])
     end
-    self.logger:debug('### Bypassing big islands in the center: create path around them ###')
-    self:circleBigIslands()
 end
 
 -- Once we have the whole course laid out, we add the headland passes around the big islands
@@ -305,7 +310,31 @@ function FieldworkCourse:circleBigIslands()
         end
         i = i + step
     end
+end
 
+--- Find the path to the next row on the headland.
+---@param boundaryId string the boundary ID, telling if this is a boundary around the field or around an island. Will
+--- only return a path when the next row can be reached while staying on the same boundary.
+---@param rowEnd cg.Vector Last waypoint of the row
+---@param rowStart cg.Vector First waypoint of the next row
+---@param minDistanceFromRowEnd number|nil minimum distance of the headland (default 0)we choose for the path,
+--- from the row end, this should be set so that the vehicle can make the turn from the position where it ended the
+--- work on the row into the headland. In case of a headland perpendicular to the rows, this is approximately the turn
+--- radius, at other angles it could be bigger or smaller, which we currently do not take into account
+---@return cg.Polyline The path on the headland to the next row. Users should consider shortening both ends of the
+--- path with the turning radius to leave enough room for the vehicle to cleanly make the turn from the row end into
+--- the headland path and from the headland into the next row
+function FieldworkCourse:findPathToNextRow(boundaryId, rowEnd, rowStart, minDistanceFromRowEnd)
+    local headlands = self:_getCachedHeadlands(boundaryId)
+    local headlandWidth = #headlands * self.context.workingWidth
+    local usableHeadlandWidth = headlandWidth - (minDistanceFromRowEnd or 0)
+    local headlandPassNumber = cg.Math.clamp(math.floor(usableHeadlandWidth / self.context.workingWidth), 1, #headlands)
+    local headland = headlands[headlandPassNumber]
+    local vx1 = headland:findClosestVertexToPoint(rowEnd)
+    local vx2 = headland:findClosestVertexToPoint(rowStart)
+    --self.logger:debug('Found shortest path to next row on boundary %s, headland %d, %d->%d',
+    --        boundaryId, headlandPassNumber, vx1.ix, vx2.ix)
+    return headland:getShortestPathBetween(vx1.ix, vx2.ix)
 end
 
 ------------------------------------------------------------------------------------------------------------------------
@@ -336,5 +365,29 @@ function FieldworkCourse:_removeHeadland(n)
             n, self.nHeadlands, self.nHeadlandsWithRoundCorners)
 end
 
+function FieldworkCourse:_getCachedHeadlands(boundaryId)
+    local headlands = self.headlandCache:get(boundaryId)
+    if not headlands then
+        headlands = {}
+        for _, v in ipairs(self:getPath()) do
+            local a = v:getAttributes()
+            if a:getBoundaryId() == boundaryId and not a:isIslandBypass() and not a:isHeadlandTransition() and
+                    not a:isOnConnectingPath() then
+                local pass = a:getHeadlandPassNumber()
+                if headlands[pass] == nil then
+                    headlands[pass] = cg.Polygon()
+                end
+                headlands[pass]:append(v)
+            end
+        end
+        for i, h in ipairs(headlands) do
+            self.logger:debug('cached boundary %s, headland %d with %d vertices', boundaryId, i, #h)
+            h:calculateProperties()
+        end
+        self.headlandCache:put(boundaryId, headlands)
+    end
+    return headlands
+end
+
 ---@class cg.FieldworkCourse
 cg.FieldworkCourse = FieldworkCourse

CourseGenerator/Headland.lua

@@ -72,6 +72,7 @@ end
 function Headland:getPath()
     -- make sure all attributes are set correctly
     self.polygon:setAttribute(nil, cg.WaypointAttributes.setHeadlandPassNumber, self.passNumber)
+    self.polygon:setAttribute(nil, cg.WaypointAttributes.setBoundaryId, self:getBoundaryId())
     -- mark corners as headland turns
     for _, v in ipairs(self.polygon) do
         if v.isCorner then
@@ -116,21 +117,6 @@ function Headland:getUnpackedVertices()
     return self.unpackedVertices
 end
 
---- We route headlands around small islands on the first island headland. Each island has to be
---- circled completely once by the headland first bypassing it, subsequent bypasses just pick the
---- shortest way around it.
-function Headland:bypassSmallIslands(smallIslands)
-    for _, island in pairs(smallIslands) do
-        local startIx, circled = 1, false
-        while startIx ~= nil do
-            self.logger:debug('Bypassing island %d, at %d', island:getId(), startIx)
-            circled, startIx = self.polygon:goAround(
-                    island:getHeadlands()[1]:getPolygon(), startIx, not self.circledIslands[island])
-            self.circledIslands[island] = circled or self.circledIslands[island]
-        end
-    end
-end
-
 --- Bypassing big island differs from small ones as:
 --- 1. no circling is needed as the big islands will have real headlands, so we always drive around them on the
 ---    shortest path
@@ -265,6 +251,12 @@ function Headland:_getHeadlandChangeMinRadius()
     return NewCourseGenerator.cHeadlandChangeMinRadius
 end
 
+--- A short ID to identify the boundary this headland is based on when serializing/deserializing. By default, this
+--- is the field boundary.
+function Headland:getBoundaryId()
+   return 'F'
+end
+
 function Headland:__tostring()
     return 'Headland ' .. self.passNumber
 end
@@ -314,6 +306,11 @@ function IslandHeadland:_getHeadlandChangeMinRadius()
     return 0
 end
 
+--- A short ID in the form I<island ID> to identify the boundary this headland is based on when serializing/deserializing
+function IslandHeadland:getBoundaryId()
+    return 'I' .. self.island:getId()
+end
+
 function IslandHeadland:__tostring()
     return 'Island ' .. self.island:getId() .. ' headland ' .. self.passNumber
 end

CourseGenerator/Row.lua

@@ -191,27 +191,43 @@ function Row:getMiddle()
     end
 end
 
+--- What is on the left and right side of the row?
+function Row:setAdjacentRowInfo(rowOnLeftWorked, rowOnRightWorked, leftSideBlockBoundary, rightSideBlockBoundary)
+    self.rowOnLeftWorked = rowOnLeftWorked
+    self.rowOnRightWorked = rowOnRightWorked
+    self.leftSideBlockBoundary = leftSideBlockBoundary
+    self.rightSideBlockBoundary = rightSideBlockBoundary
+end
+
 --- Update the attributes of the first and last vertex of the row based on the row's properties.
 --- We use these attributes when finding an entry to a block, to see if the entry is on an island headland
 --- or not. The attributes are set when the row is split at headlands but may need to be reapplied when
 --- we adjust the end of the row as we may remove the first/last vertex.
 function Row:setEndAttributes()
     self:setAttribute(1, cg.WaypointAttributes.setRowStart)
     self:setAttribute(1, cg.WaypointAttributes._setAtHeadland, self.startsAtHeadland)
+    self:setAttribute(1, cg.WaypointAttributes.setAtBoundaryId, self.startsAtHeadland:getBoundaryId())
     self:setAttribute(#self, cg.WaypointAttributes.setRowEnd)
     self:setAttribute(#self, cg.WaypointAttributes._setAtHeadland, self.endsAtHeadland)
+    self:setAttribute(#self, cg.WaypointAttributes.setAtBoundaryId, self.endsAtHeadland:getBoundaryId())
 end
 
 function Row:setAllAttributes()
     self:setEndAttributes()
     self:setAttribute(nil, cg.WaypointAttributes.setRowNumber, self.rowNumber)
     self:setAttribute(nil, cg.WaypointAttributes.setBlockNumber, self.blockNumber)
+    self:setAttribute(nil, cg.WaypointAttributes.setLeftSideWorked, self.rowOnLeftWorked)
+    self:setAttribute(nil, cg.WaypointAttributes.setRightSideWorked, self.rowOnRightWorked)
+    self:setAttribute(nil, cg.WaypointAttributes.setLeftSideBlockBoundary, self.leftSideBlockBoundary)
+    self:setAttribute(nil, cg.WaypointAttributes.setRightSideBlockBoundary, self.rightSideBlockBoundary)
 end
 
 function Row:reverse()
     cg.Polyline.reverse(self)
     self.startHeadlandAngle, self.endHeadlandAngle = self.endHeadlandAngle, self.startHeadlandAngle
     self.startsAtHeadland, self.endsAtHeadland = self.endsAtHeadland, self.startsAtHeadland
+    self.rowOnLeftWorked, self.rowOnRightWorked = self.rowOnRightWorked, self.rowOnLeftWorked
+    self.leftSideBlockBoundary, self.rightSideBlockBoundary = self.rightSideBlockBoundary, self.leftSideBlockBoundary
 end
 
 --- Adjust the length of this tow for full coverage where it meets the headland or field boundary