refactor: preparing for the new feature

Fixed a bug in the Slider which caused callstacks and
refactored the GraphPathfinder to allow for extending
later.

Author: Peter Vaiko

scripts/courseGenerator/geometry/Slider.lua

@@ -6,11 +6,12 @@ local Slider = CpObject(CourseGenerator.LineSegment)
 ---@param d number distance from polyline[ix] for the initial position
 function Slider:init(polyline, ix, d)
     self.polyline = polyline
-    self._isAtEnd = false
     self:set(ix, 0)
     self:move(d or 0)
 end
 
+--- The slider's position is defined by the index of a vertex and the distance from that vertex on
+--- the exit edge, therefore, d is always positive.
 function Slider:set(ix, d)
     -- index of the current vertex
     self.ix = ix
@@ -19,7 +20,12 @@ function Slider:set(ix, d)
     self.base = Vector(self:vertex().x, self:vertex().y)
     -- unit vector pointing to the exit heading of the vertex
     self.slope = Vector(1, 0)
-    self.slope:setHeading(self:vertex():getExitHeading())
+    local exitHeading = self:vertex():getExitHeading()
+    if exitHeading then
+        -- leave the slope as it was when there is no exit heading. This can happen if the caller did not
+        -- properly ran calculateProperties() on the polyline.
+        self.slope:setHeading(exitHeading)
+    end
     -- move to the current offset
     self:offset(d, 0)
 end
@@ -58,17 +64,17 @@ function Slider:move(d)
     end
 
     local function backward()
-        local entryEdge = self:vertex(ix):getEntryEdge()
-        if not entryEdge then
-            -- reached the start of polyline
-            dRemaining = 0
-            endReached = true
-            return false
-        end
         if dRemaining > offset then
-            dRemaining = dRemaining - offset
-            ix = ix - 1
-            offset = entryEdge:getLength()
+            if self:vertex(ix):getEntryEdge() == nil then
+                -- reached the start of polyline
+                offset = 0
+                endReached = true
+                return false
+            else
+                dRemaining = dRemaining - offset
+                ix = ix - 1
+                offset = self:vertex(ix):getExitEdge():getLength()
+            end
         else
             offset = offset - dRemaining
             return false

scripts/courseGenerator/test/PolylineTest.lua

@@ -424,6 +424,35 @@ function testSplitEdges()
     p[3]:assertAlmostEquals(Vertex(85, 0))
     p[2]:assertAlmostEquals(Vertex(95, 0))
     p[1]:assertAlmostEquals(Vertex(105, 0))
+
+    p = Polyline({
+        Vertex(100, 100),
+        Vertex(110, 100),
+        Vertex(120, 100),
+        Vertex(130, 100),
+        Vertex(140, 100),
+        Vertex(150, 100)
+    })
+    p:splitEdges(5)
+    lu.assertEquals(#p, 11)
+    p[1]:assertAlmostEquals(Vertex(100, 100))
+    p[2]:assertAlmostEquals(Vertex(105, 100))
+    p[10]:assertAlmostEquals(Vertex(145, 100))
+    p[11]:assertAlmostEquals(Vertex(150, 100))
+end
+
+function testEnsureMinimumRadius()
+    local p = Polyline({
+        Vertex(9.33, -13.66),
+        Vertex(24.13, -2.36),
+        Vertex(36.79, -13.78),
+        Vertex(41.12, -17.94),
+        Vertex(45.41, -22.13)
+    })
+    p:ensureMinimumRadius(9.3)
+    lu.assertEquals(#p, 18)
+    p[1]:assertAlmostEquals(Vertex(9.33, -13.66))
+    p[18]:assertAlmostEquals(Vertex(45.41, -22.13))
 end
 
 os.exit(lu.LuaUnit.run())

scripts/courseGenerator/test/SliderTest.lua

@@ -48,4 +48,21 @@ function testSlider()
     s:move(-1)
     s:assertAlmostEquals(CourseGenerator.LineSegment(0, 0, 0, 1))
 end
+
+function testShortPolyline()
+    local p = Polyline({
+        Vertex(0, 0),
+        Vertex(100, 0),
+        Vertex(100, 100),
+    })
+    local s = CourseGenerator.Slider(p, 2, 0)
+    s:assertAlmostEquals(CourseGenerator.LineSegment(100, 0, 100, 1))
+    s:move(1)
+    s:assertAlmostEquals(CourseGenerator.LineSegment(100, 1, 100, 2))
+    s:move(-1)
+    s:assertAlmostEquals(CourseGenerator.LineSegment(100, 0, 100, 1))
+    s:move(-1)
+    s:assertAlmostEquals(CourseGenerator.LineSegment(99, 0, 100, 0))
+end
+
 os.exit(lu.LuaUnit.run())

scripts/pathfinder/GraphPathfinder.lua

@@ -68,6 +68,23 @@ function GraphPathfinder.GraphEdge:getExit(entry)
     end
 end
 
+-- if the vertex is the first or last vertex of the edge, we can use it directly as the entry/exit,
+-- otherwise, we split the edge at the vertex so we can use it as an entry/exit point.
+---@return GraphPathfinder.GraphEdge|nil new edge if the edge was split, nil otherwise
+function GraphPathfinder.GraphEdge:splitWhenNeeded(closestVertex)
+    if closestVertex.ix ~= 1 and closestVertex.ix ~= #self then
+        local newEdge = GraphPathfinder.GraphEdge(self:getDirection())
+        for j = closestVertex.ix, #self do
+            newEdge:append(self[j])
+        end
+        newEdge:calculateProperties()
+        self:cutEndAtIx(closestVertex.ix)
+        -- remember the piece we cut off, we may need it if the path must be extended after the goal
+        self.cutEdge = newEdge
+        return newEdge
+    end
+end
+
 function GraphPathfinder.GraphEdge:rollUpIterator(entry)
     local from, to, step
     if entry == self[1] then
@@ -191,6 +208,7 @@ end
 
 function GraphPathfinder:start(start, goal, turnRadius, ...)
     -- at each run, make a copy of the graph as we'll modify it
+    ---@type GraphPathfinder.GraphEdge[]
     self.graph = {}
     for _, e in ipairs(self.originalGraph) do
         if #e > 1 then
@@ -238,24 +256,6 @@ end
 ---@return State3D the entry vertex of the graph, closest to start
 ---@return State3D the exit vertex of the graph, closest to goal
 function GraphPathfinder:createGraphEntryAndExit(start, goal)
-
-    local function splitEdgeWhenNeeded(edge, closestVertex)
-        -- if the vertex is the first or last vertex of the edge, we can use it directly as the entry/exit,
-        -- otherwise, we split the edge at the vertex so we can use it as an entry/exit point.
-        if closestVertex.ix ~= 1 and closestVertex.ix ~= #edge then
-            self.logger:debug('Graph entry/exit found and split at vertex %d, x: %.1f y: %.1f',
-                    closestVertex.ix, closestVertex.x, closestVertex.y)
-            local newEdge = GraphPathfinder.GraphEdge(edge:getDirection())
-            for j = closestVertex.ix, #edge do
-                newEdge:append(edge[j])
-            end
-            newEdge:calculateProperties()
-            table.insert(self.graph, newEdge)
-            edge:cutEndAtIx(closestVertex.ix)
-            return newEdge
-        end
-    end
-
     -- find the edges closest to node. If the closest vertex isn't the first or last vertex of the edge, we split the
     -- edge at that vertex so we can use it as an entry/exit point.
     local function findClosestEdges(node, isEntry)
@@ -275,8 +275,11 @@ function GraphPathfinder:createGraphEntryAndExit(start, goal)
         for i = 1, #self.graph do
             local edge = self.graph[i]
             local vertex, d = edge:findClosestVertexToPoint(node)
-            local newEdge = splitEdgeWhenNeeded(edge, vertex)
+            local newEdge = edge:splitWhenNeeded(vertex)
             if newEdge then
+                self.logger:debug('Graph entry/exit found and split at vertex %d, x: %.1f y: %.1f',
+                        vertex.ix, vertex.x, vertex.y)
+                table.insert(self.graph, newEdge)
                 addToClosestEdge(newEdge, newEdge[1], d)
             end
             addToClosestEdge(edge, vertex, d)
@@ -327,7 +330,6 @@ function GraphPathfinder:addTransitions(edges)
     end
     path:appendMany(edges[#edges]) -- append the last edge, this will be the exit edge
     path:calculateProperties()
-    path:splitEdges(5)
     path:ensureMinimumRadius(self.turnRadius, false, 0.5)
     return path
 end
@@ -341,7 +343,7 @@ GraphPathfinder.GraphMotionPrimitives = CpObject(HybridAStar.MotionPrimitives)
 --- two edges are considered as connected (and thus can traverse from one to the other)
 ---@param graph Vector[] the graph as described in the file header
 function GraphPathfinder.GraphMotionPrimitives:init(range, graph)
-    self.logger = Logger('GraphMotionPrimitives', Logger.level.trace, CpDebug.DBG_PATHFINDER)
+    self.logger = Logger('GraphMotionPrimitives', Logger.level.debug, CpDebug.DBG_PATHFINDER)
     self.range = range
     self.graph = graph
 end

scripts/pathfinder/State3D.lua

@@ -207,8 +207,8 @@ function State3D:__tostring()
         steer = 'Straight'
     end
     local gear = self.gear == Gear.Forward and 'Forward' or 'Backward'
-    local tTrailer = self.tTrailer and string.format(' (%d)', math.deg(self.tTrailer)) or ''
-    result = string.format('x: %.2f y:%.2f t:%d%s gear:%s steer:%s g:%.4f h:%.4f c:%.4f closed:%s open:%s',
+    local tTrailer = self.tTrailer and string.format(' (%.0f)', math.deg(self.tTrailer)) or ''
+    result = string.format('x: %.2f y:%.2f t:%.0f%s gear:%s steer:%s g:%.4f h:%.4f c:%.4f closed:%s open:%s',
             self.x, self.y, math.deg(self.t), tTrailer, gear, steer,
             self.g, self.h, self.cost, tostring(self.closed), tostring(self.onOpenList))
     return result

scripts/pathfinder/test/GraphPathfinderTest.lua

@@ -418,13 +418,36 @@ function TestWithTransitions:testTransition()
     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))
+    -- here's the arch
+    path[13]:assertAlmostEquals(Vector(210, 10))
+    path[#path]:assertAlmostEquals(Vector(210, 200))
+end
+
+TestExtension = {}
+function TestExtension:testExtension()
+    local edge = GraphEdge(GraphEdge.UNIDIRECTIONAL, {})
+    for i = 0, 100, 5 do
+        edge:append(Vertex(i, i / 5))
+    end
+    local graph = { edge }
+    pathfinder = GraphPathfinder(math.huge, 500, 20, graph)
+    start = State3D(-5, 0, 0, 0)
+    goal = State3D(50, 10, 0, 0)
+    done, path, _ = runPathfinder()
+    printPath()
+    lu.assertIsTrue(done)
     lu.assertEquals(#path, 90)
     -- 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))
+
 end
 
 os.exit(lu.LuaUnit.run())