mirror of
https://github.com/FlightControl-Master/MOOSE.git
synced 2025-10-29 16:58:06 +00:00
A*
- Improved performance by caching cost and valid/invalid neighbours.
This commit is contained in:
Frank
parent
0d42b12658
commit
cf9248ecc8
@ -20,6 +20,7 @@
|
||||
-- @field #boolean Debug Debug mode. Messages to all about status.
|
||||
-- @field #string lid Class id string for output to DCS log file.
|
||||
-- @field #table nodes Table of nodes.
|
||||
-- @field #number counter Node counter.
|
||||
-- @field #ASTAR.Node startNode Start node.
|
||||
-- @field #ASTAR.Node endNode End node.
|
||||
-- @field Core.Point#COORDINATE startCoord Start coordinate.
|
||||
@ -136,12 +137,16 @@ ASTAR = {
|
||||
Debug = nil,
|
||||
lid = nil,
|
||||
nodes = {},
|
||||
counter = 1,
|
||||
}
|
||||
|
||||
--- Node data.
|
||||
-- @type ASTAR.Node
|
||||
-- @field #number id Node id.
|
||||
-- @field Core.Point#COORDINATE coordinate Coordinate of the node.
|
||||
-- @field #number surfacetype Surface type.
|
||||
-- @field #table valid Cached valid/invalid nodes.
|
||||
-- @field #table cost Cached cost.
|
||||
|
||||
--- ASTAR infinity.
|
||||
-- @field #number INF
|
||||
@ -149,7 +154,7 @@ ASTAR.INF=1/0
|
||||
|
||||
--- ASTAR class version.
|
||||
-- @field #string version
|
||||
ASTAR.version="0.1.0"
|
||||
ASTAR.version="0.2.0"
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- TODO list
|
||||
@ -211,6 +216,12 @@ function ASTAR:GetNodeFromCoordinate(Coordinate)
|
||||
|
||||
node.coordinate=Coordinate
|
||||
node.surfacetype=Coordinate:GetSurfaceType()
|
||||
node.id=self.counter
|
||||
|
||||
node.valid={}
|
||||
node.cost={}
|
||||
|
||||
self.counter=self.counter+1
|
||||
|
||||
return node
|
||||
end
|
||||
@ -518,6 +529,23 @@ function ASTAR.Dist3D(nodeA, nodeB)
|
||||
return nodeA.coordinate:Get3DDistance(nodeB.coordinate)
|
||||
end
|
||||
|
||||
--- Heuristic cost is given by the distance between the nodes on road.
|
||||
-- @param #ASTAR.Node nodeA First node.
|
||||
-- @param #ASTAR.Node nodeB Other node.
|
||||
-- @return #number Distance between the two nodes.
|
||||
function ASTAR.DistRoad(nodeA, nodeB)
|
||||
|
||||
local path,dist,gotpath=nodeA.coordinate:GetPathOnRoad(nodeB.coordinate,IncludeEndpoints,Railroad,MarkPath,SmokePath)
|
||||
|
||||
if gotpath then
|
||||
return dist
|
||||
else
|
||||
return math.huge
|
||||
end
|
||||
|
||||
return nodeA.coordinate:Get3DDistance(nodeB.coordinate)
|
||||
end
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- Misc functions
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
@ -616,13 +644,15 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
|
||||
local text=string.format("Starting A* pathfinding")
|
||||
self:I(self.lid..text)
|
||||
MESSAGE:New(text, 10, "ASTAR"):ToAllIf(self.Debug)
|
||||
|
||||
local Tstart=UTILS.GetOSTime()
|
||||
|
||||
while #openset > 0 do
|
||||
|
||||
local current=self:_LowestFscore(openset, f_score)
|
||||
|
||||
-- Check if we are at the end node.
|
||||
if current==goal then
|
||||
if current.id==goal.id then
|
||||
|
||||
local path=self:_UnwindPath({}, came_from, goal)
|
||||
|
||||
@ -634,8 +664,20 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
|
||||
table.remove(path, 1)
|
||||
end
|
||||
|
||||
local Tstop=UTILS.GetOSTime()
|
||||
|
||||
local dT=nil
|
||||
if Tstart and Tstop then
|
||||
dT=Tstop-Tstart
|
||||
end
|
||||
|
||||
-- Debug message.
|
||||
local text=string.format("Found path with %d nodes", #path)
|
||||
local text=string.format("Found path with %d nodes (%d total nodes)", #path, #self.nodes)
|
||||
if dT then
|
||||
text=text..string.format(". OS Time %.6f seconds", dT)
|
||||
end
|
||||
text=text..string.format("\nNvalid = %d %d cached", self.nvalid, self.nvalidcache)
|
||||
text=text..string.format("\nNcost = %d %d cached", self.ncost, self.ncostcache)
|
||||
self:I(self.lid..text)
|
||||
MESSAGE:New(text, 60, "ASTAR"):ToAllIf(self.Debug)
|
||||
|
||||
@ -689,11 +731,34 @@ end
|
||||
-- @return #number "Cost" to go from node A to node B.
|
||||
function ASTAR:_HeuristicCost(nodeA, nodeB)
|
||||
|
||||
if self.CostFunc then
|
||||
return self.CostFunc(nodeA, nodeB, unpack(self.CostArg))
|
||||
if self.ncost then
|
||||
self.ncost=self.ncost+1
|
||||
else
|
||||
return self:_DistNodes(nodeA, nodeB)
|
||||
self.ncost=1
|
||||
end
|
||||
|
||||
-- Get chached cost if available.
|
||||
local cost=nodeA.cost[nodeB.id]
|
||||
if cost~=nil then
|
||||
if self.ncostcache then
|
||||
self.ncostcache=self.ncostcache+1
|
||||
else
|
||||
self.ncostcache=1
|
||||
end
|
||||
return cost
|
||||
end
|
||||
|
||||
local cost=nil
|
||||
if self.CostFunc then
|
||||
cost=self.CostFunc(nodeA, nodeB, unpack(self.CostArg))
|
||||
else
|
||||
cost=self:_DistNodes(nodeA, nodeB)
|
||||
end
|
||||
|
||||
nodeA.cost[nodeB.id]=cost
|
||||
nodeB.cost[nodeA.id]=cost -- Symmetric problem.
|
||||
|
||||
return cost
|
||||
end
|
||||
|
||||
--- Check if going from a node to a neighbour is possible.
|
||||
@ -703,14 +768,34 @@ end
|
||||
-- @return #boolean If true, transition between nodes is possible.
|
||||
function ASTAR:_IsValidNeighbour(node, neighbor)
|
||||
|
||||
if self.ValidNeighbourFunc then
|
||||
|
||||
return self.ValidNeighbourFunc(node, neighbor, unpack(self.ValidNeighbourArg))
|
||||
|
||||
if self.nvalid then
|
||||
self.nvalid=self.nvalid+1
|
||||
else
|
||||
return true
|
||||
self.nvalid=1
|
||||
end
|
||||
|
||||
local valid=node.valid[neighbor.id]
|
||||
if valid~=nil then
|
||||
--env.info(string.format("Node %d has valid=%s neighbour %d", node.id, tostring(valid), neighbor.id))
|
||||
if self.nvalidcache then
|
||||
self.nvalidcache=self.nvalidcache+1
|
||||
else
|
||||
self.nvalidcache=1
|
||||
end
|
||||
return valid
|
||||
end
|
||||
|
||||
local valid=nil
|
||||
if self.ValidNeighbourFunc then
|
||||
valid=self.ValidNeighbourFunc(node, neighbor, unpack(self.ValidNeighbourArg))
|
||||
else
|
||||
valid=true
|
||||
end
|
||||
|
||||
node.valid[neighbor.id]=valid
|
||||
neighbor.valid[node.id]=valid -- Symmetric problem.
|
||||
|
||||
return valid
|
||||
end
|
||||
|
||||
--- Calculate 2D distance between two nodes.
|
||||
@ -727,7 +812,7 @@ end
|
||||
-- @param #table set The set of nodes.
|
||||
-- @param #number f_score F score.
|
||||
-- @return #ASTAR.Node Best node.
|
||||
function ASTAR:_LowestFscore(set, f_score)
|
||||
function ASTAR:_LowestFscore2(set, f_score)
|
||||
|
||||
local lowest, bestNode = ASTAR.INF, nil
|
||||
|
||||
@ -743,6 +828,25 @@ function ASTAR:_LowestFscore(set, f_score)
|
||||
return bestNode
|
||||
end
|
||||
|
||||
--- Function that calculates the lowest F score.
|
||||
-- @param #ASTAR self
|
||||
-- @param #table set The set of nodes.
|
||||
-- @param #number f_score F score.
|
||||
-- @return #ASTAR.Node Best node.
|
||||
function ASTAR:_LowestFscore(set, f_score)
|
||||
|
||||
local function sort(A, B)
|
||||
local a=A --#ASTAR.Node
|
||||
local b=B --#ASTAR.Node
|
||||
return f_score[a]<f_score[b]
|
||||
end
|
||||
|
||||
table.sort(set, sort)
|
||||
|
||||
return set[1]
|
||||
end
|
||||
|
||||
|
||||
--- Function to get valid neighbours of a node.
|
||||
-- @param #ASTAR self
|
||||
-- @param #ASTAR.Node theNode The node.
|
||||
@ -751,9 +855,9 @@ end
|
||||
function ASTAR:_NeighbourNodes(theNode, nodes)
|
||||
|
||||
local neighbors = {}
|
||||
for _, node in ipairs ( nodes ) do
|
||||
for _, node in pairs ( nodes ) do
|
||||
|
||||
if theNode~=node then
|
||||
if theNode.id~=node.id then
|
||||
|
||||
local isvalid=self:_IsValidNeighbour(theNode, node)
|
||||
|
||||
@ -775,8 +879,8 @@ end
|
||||
-- @return #boolean If true, the node is not in the set.
|
||||
function ASTAR:_NotIn(set, theNode)
|
||||
|
||||
for _, node in ipairs ( set ) do
|
||||
if node == theNode then
|
||||
for _, node in pairs ( set ) do
|
||||
if node.id == theNode.id then
|
||||
return false
|
||||
end
|
||||
end
|
||||
@ -790,8 +894,9 @@ end
|
||||
-- @param #ASTAR.Node theNode The node to check.
|
||||
function ASTAR:_RemoveNode(set, theNode)
|
||||
|
||||
for i, node in ipairs ( set ) do
|
||||
if node == theNode then
|
||||
for i, node in pairs ( set ) do
|
||||
if node.id == theNode.id then
|
||||
--table.remove(set, i)
|
||||
set [ i ] = set [ #set ]
|
||||
set [ #set ] = nil
|
||||
break
|
||||
|
||||
@ -1524,27 +1524,8 @@ do -- COORDINATE
|
||||
local coord=COORDINATE:NewFromVec2(_vec2)
|
||||
|
||||
Path[#Path+1]=coord
|
||||
|
||||
if MarkPath then
|
||||
coord:MarkToAll(string.format("Path segment %d.", _i))
|
||||
end
|
||||
if SmokePath then
|
||||
coord:SmokeGreen()
|
||||
end
|
||||
end
|
||||
|
||||
-- Mark/smoke endpoints
|
||||
if IncludeEndpoints then
|
||||
if MarkPath then
|
||||
COORDINATE:NewFromVec2(path[1]):MarkToAll("Path Initinal Point")
|
||||
COORDINATE:NewFromVec2(path[1]):MarkToAll("Path Final Point")
|
||||
end
|
||||
if SmokePath then
|
||||
COORDINATE:NewFromVec2(path[1]):SmokeBlue()
|
||||
COORDINATE:NewFromVec2(path[#path]):SmokeBlue()
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
else
|
||||
self:E("Path is nil. No valid path on road could be found.")
|
||||
GotPath=false
|
||||
@ -1555,6 +1536,23 @@ do -- COORDINATE
|
||||
Path[#Path+1]=ToCoord
|
||||
end
|
||||
|
||||
-- Mark or smoke.
|
||||
if MarkPath or SmokePath then
|
||||
for i,c in pairs(Path) do
|
||||
local coord=c --#COORDINATE
|
||||
if MarkPath then
|
||||
coord:MarkToAll(string.format("Path segment %d", i))
|
||||
end
|
||||
if SmokePath then
|
||||
if i==1 or i==#Path then
|
||||
coord:SmokeBlue()
|
||||
else
|
||||
coord:SmokeGreen()
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
-- Sum up distances.
|
||||
if #Path>=2 then
|
||||
for i=1,#Path-1 do
|
||||
@ -1562,7 +1560,7 @@ do -- COORDINATE
|
||||
end
|
||||
else
|
||||
-- There are cases where no path on road can be found.
|
||||
return nil,nil
|
||||
return nil,nil,false
|
||||
end
|
||||
|
||||
return Path, Way, GotPath
|
||||
|
||||
@ -1535,12 +1535,6 @@ function NAVYGROUP:_FindPathToNextWaypoint()
|
||||
|
||||
for i,_node in ipairs(path) do
|
||||
local node=_node --Core.Astar#ASTAR.Node
|
||||
|
||||
-- Waypoint index.
|
||||
local wpindex=self:GetWaypointIndexCurrent()+i
|
||||
|
||||
-- ID of current waypoint.
|
||||
local uid=self:GetWaypointCurrent().uid
|
||||
|
||||
-- Add waypoints along detour path to next waypoint.
|
||||
local wp=self:AddWaypoint(node.coordinate, speed, uid)
|
||||
|
||||
@ -1400,4 +1400,14 @@ function UTILS.GetSunset(Day, Month, Year, Latitude, Longitude, Tlocal)
|
||||
local DayOfYear=UTILS.GetDayOfYear(Year, Month, Day)
|
||||
|
||||
return UTILS.GetSunRiseAndSet(DayOfYear, Latitude, Longitude, false, Tlocal)
|
||||
end
|
||||
|
||||
--- Get OS time. Needs os to be desanitized!
|
||||
-- @return #number Os time in seconds.
|
||||
function UTILS.GetOSTime()
|
||||
if os then
|
||||
return os.clock()
|
||||
end
|
||||
|
||||
return nil
|
||||
end
|
||||
Loading…
x
Reference in New Issue
Block a user