feat: graph pathfinder extension

Works ok for unidirectional edges. Needs more tuning.

Author: Peter Vaiko

scripts/geometry/Polyline.lua

@@ -227,7 +227,7 @@ function Polyline:extendStart(length)
     return self
 end
 
----@param length number shorten the Polyline at the last vertex
+---@param length number shorten the Polyline starting at the last vertex
 function Polyline:cutEnd(length)
     if #self == 2 then
         local theOnlyEdge = self[1]:getExitEdge()
@@ -248,7 +248,7 @@ function Polyline:cutEnd(length)
     end
 end
 ---
----@param length number shorten the polyline at the first vertex
+---@param length number shorten the polyline starting at the first vertex
 function Polyline:cutStart(length)
     if #self == 2 then
         local theOnlyEdge = self[1]:getExitEdge()

scripts/pathfinder/GraphPathfinder.lua

@@ -154,12 +154,16 @@ end
 ---@param range number when an edge's exit is closer than range to another edge's entry, the
 --- two edges are considered as connected (and thus can traverse from one to the other)
 ---@param graph GraphPathfinder.GraphEdge[] Array of edges, the graph as described in the file header
-function GraphPathfinder:init(yieldAfter, maxIterations, range, graph)
+---@param extension number|nil how far to extend the path after the goal, if nil, no extension is done. Extension is
+--- always follows the edge which has the goal on it, and ends where the edge ends if the edge is not long enough to
+--- reach the extension distance.
+function GraphPathfinder:init(yieldAfter, maxIterations, range, graph, extension)
     HybridAStar.init(self, { }, yieldAfter, maxIterations)
     self.logger = Logger('GraphPathfinder', Logger.level.debug, CpDebug.DBG_PATHFINDER)
     self.range = range
     self.transitionRange = range
     self.originalGraph = graph
+    self.extension = extension
     self.deltaPosGoal = self.range
     self.deltaThetaDeg = 181
     self.deltaThetaGoal = math.rad(self.deltaThetaDeg)
@@ -183,6 +187,7 @@ end
 function GraphPathfinder:rollUpPath(lastNode, goal)
     local edges = {}
     local currentNode = lastNode
+    local lastEdge
     self:debug('Goal node at %.2f/%.2f, cost %.1f (%.1f - %.1f)', goal.x, goal.y, lastNode.cost,
             self.nodes.lowestCost, self.nodes.highestCost)
     while currentNode.pred and currentNode ~= currentNode.pred do
@@ -197,15 +202,43 @@ function GraphPathfinder:rollUpPath(lastNode, goal)
         if #edge > 0 then
             edge:calculateProperties()
             table.insert(edges, 1, edge)
+            lastEdge = lastEdge or currentNode.edge
         end
         currentNode = currentNode.pred
     end
     local path = self:addTransitions(edges)
     self:debug('Nodes %d, iterations %d, yields %d, deltaTheta %.1f', #path, self.iterations, self.yields,
             math.deg(self.deltaThetaGoal))
+    if self.extension then
+        self:debug('Extending path by %.1f meters', self.extension)
+        self:extendPath(path, lastEdge, self.extension)
+    end
     return path
 end
 
+function GraphPathfinder:extendPath(path, lastEdge, extension)
+    if lastEdge.cutEdge then
+        local cutEdgeLength = lastEdge.cutEdge:getLength()
+        if cutEdgeLength > extension then
+            lastEdge.cutEdge:cutEnd(cutEdgeLength - extension)
+        else
+            self:debug('Cut edge not long enough, extending by %.1f meters straight', extension - cutEdgeLength)
+            lastEdge.cutEdge:extendEnd(extension - cutEdgeLength)
+        end
+        -- remove the first vertex of the cut edge, as it is the same as the last vertex of the path
+        if #lastEdge.cutEdge == 2 then
+            path:append(lastEdge.cutEdge[2])
+            path:calculateProperties()
+        else
+            lastEdge.cutEdge:cutStartAtIx(2)
+            path:appendMany(lastEdge.cutEdge)
+        end
+    else
+        self:debug('Last edge has no cut edge, extending straight')
+        path:extendEnd(extension)
+    end
+end
+
 function GraphPathfinder:start(start, goal, turnRadius, ...)
     -- at each run, make a copy of the graph as we'll modify it
     ---@type GraphPathfinder.GraphEdge[]

scripts/pathfinder/test/GraphPathfinderTest.lua

@@ -25,6 +25,7 @@ require('AnalyticHelper') -- for the test cases
 require('PathfinderUtil')
 require('HybridAStar')
 require('GraphPathfinder')
+lu.EPS = 0.01
 
 local GraphEdge = GraphPathfinder.GraphEdge
 local TestConstraints = CpObject(PathfinderConstraintInterface)
@@ -416,37 +417,70 @@ function TestWithTransitions:testTransition()
     start = State3D(-5, 0, 0, 0)
     goal = State3D(210, 205, 0, 0)
     done, path, _ = runPathfinder()
-    printPath()
     lu.assertIsTrue(done)
     lu.assertEquals(#path, 14)
     -- path contains all points of the edge it goes through
     path[1]:assertAlmostEquals(Vector(0, 0))
-    path[3]:assertAlmostEquals(Vector(200, 0))
+    path[3]:assertAlmostEquals(Vector(203.8, 0))
     -- here's the arch
-    path[13]:assertAlmostEquals(Vector(210, 10))
+    path[13]:assertAlmostEquals(Vector(210, 100))
     path[#path]:assertAlmostEquals(Vector(210, 200))
 end
 
-TestExtension = {}
-function TestExtension:testExtension()
+-- z to make it run last
+TestzExtension = {}
+function TestzExtension:testExtension()
     local edge = GraphEdge(GraphEdge.UNIDIRECTIONAL, {})
-    for i = 0, 100, 5 do
+    for i = 0, 30, 5 do
         edge:append(Vertex(i, i / 5))
     end
+    local len = edge:getLength()
     local graph = { edge }
-    pathfinder = GraphPathfinder(math.huge, 500, 20, graph)
+    edge = GraphEdge(GraphEdge.UNIDIRECTIONAL, {})
+    for i = 35, 100, 5 do
+        edge:append(Vertex(i, i / 5))
+    end
+    len = len + edge:getLength() + 5 / math.cos(math.atan(1/5))
+    table.insert(graph, edge)
+    lu.assertAlmostEquals(len, 101.98)
+    pathfinder = GraphPathfinder(math.huge, 500, 20, graph, 5)
     start = State3D(-5, 0, 0, 0)
-    goal = State3D(50, 10, 0, 0)
+    goal = State3D(50, 0, 0, 0)
     done, path, _ = runPathfinder()
-    printPath()
     lu.assertIsTrue(done)
-    lu.assertEquals(#path, 90)
+    lu.assertAlmostEquals(path:getLength(), len / 2 + 5)
+    lu.assertEquals(#path, 11)
     -- path contains all points of the edge it goes through
     path[1]:assertAlmostEquals(Vector(0, 0))
-    path[41]:assertAlmostEquals(Vector(200, 0))
-    -- here's the arch
-    path[52]:assertAlmostEquals(Vector(210, 10))
-    path[#path]:assertAlmostEquals(Vector(210, 200))
+    path[6]:assertAlmostEquals(Vector(25, 5))
+    path[10]:assertAlmostEquals(Vector(50, 10))
+    path[11]:assertAlmostEquals(Vector(54.9, 10.98))
+
+    pathfinder = GraphPathfinder(math.huge, 500, 20, graph, 15)
+    done, path, _ = runPathfinder()
+    lu.assertAlmostEquals(path:getLength(), len / 2 + 15)
+    lu.assertEquals(#path, 12)
+    path[11]:assertAlmostEquals(Vector(55, 11))
+    path[12]:assertAlmostEquals(Vector(64.71, 12.94))
+
+    -- cut edge not long enough
+    pathfinder = GraphPathfinder(math.huge, 500, 20, graph, 55)
+    done, path, _ = runPathfinder()
+    lu.assertAlmostEquals(path:getLength(), len / 2 + 55)
+    lu.assertEquals(#path, 20)
+    path[18]:assertAlmostEquals(Vector(90, 18))
+    path[19]:assertAlmostEquals(Vector(95, 19))
+    path[20]:assertAlmostEquals(Vector(103.93, 20.79))
+
+    -- no cut edge
+    pathfinder = GraphPathfinder(math.huge, 500, 20, graph, 5)
+    goal = State3D(100, 10, 0, 0)
+    done, path, _ = runPathfinder()
+    lu.assertAlmostEquals(path:getLength(), len + 5)
+    lu.assertEquals(#path, 20)
+    path[18]:assertAlmostEquals(Vector(90, 18))
+    path[19]:assertAlmostEquals(Vector(95, 19))
+    path[20]:assertAlmostEquals(Vector(104.90, 20.98))
 
 end