Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion scripts/pathfinder/Dubins.lua
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,9 @@ function DubinsSolution:getLength(turnRadius)
end

function DubinsSolution:getWaypoints(start, turnRadius)
return dubins_path_sample_many(self.pathDescriptor, 1)
-- waypoints are 1 m apart, but make sure there are at least 12 waypoints per half circle
-- even with very small radii
return dubins_path_sample_many(self.pathDescriptor, math.min(1, turnRadius * math.pi / 12))
end

local twoPi = 2 * math.pi
Expand Down
10 changes: 6 additions & 4 deletions scripts/pathfinder/ReedsShepp.lua
Original file line number Diff line number Diff line change
Expand Up @@ -138,12 +138,12 @@ function ReedsShepp.ActionSet:addAction(steer, gear, length)
end

function ReedsShepp.ActionSet:calculateCost(unit, reverseCostMultiplier, gearSwitchCost)
if reverseCostMultiplier == 1 and gearSwitchCost == 0 then return self.Length * unit end
if self.Length == math.huge or #self.ctions == 0 then return math.huge end
if reverseCostMultiplier == 1 and gearSwitchCost == 0 then return self.length * unit end
if self.length == math.huge or #self.actions == 0 then return math.huge end
local cost = 0
local prevGear = self.actions[1].gear
for _, a in ipairs(self.actions) do
local actionCost = a.Length * unit
local actionCost = a.length * unit
if a.gear == Gear.Backward then
actionCost = actionCost * reverseCostMultiplier
end
Expand Down Expand Up @@ -174,7 +174,9 @@ function ReedsShepp.ActionSet:getWaypoints(start, turnRadius)
local waypoints = {}
table.insert(waypoints, prev)
for _, action in ipairs(self.actions) do
local n = math.ceil(action.length * turnRadius)
-- waypoints are 1 m apart, but make sure there are at least 12 waypoints per half circle
-- even with very small radii
local n = math.ceil(math.max(action.length * turnRadius, action.length / math.pi * 12))
if action.steer ~= Steer.Straight then
local pieceAngle = action.length / n

Expand Down
Loading