FlightControl-Master/MOOSE
1
0
Fork 0
mirror of https://github.com/FlightControl-Master/MOOSE.git synced 2025-10-29 16:58:06 +00:00
Code Issues Packages Projects Releases Wiki Activity

Astar

Browse Source
This commit is contained in:
Frank
2020-07-23 00:24:59 +02:00
parent ec809085b4
commit 98971736f8
8 changed files with 375 additions and 327 deletions
Download Patch File Download Diff File Expand all files Collapse all files

481
Moose Development/Moose/Core/Astar.lua
View File

@@ -21,10 +21,11 @@
-- @field #ASTAR.Node endNode End node.
-- @field Core.Point#COORDINATE startCoord Start coordinate.
-- @field Core.Point#COORDINATE endCoord End coordinate.
-- @field #func CheckNodeValid Function to check if a node is valid.
-- @field #func ValidNeighbourFunc Function to check if a node is valid.
-- @field #table ValidNeighbourArg Optional arguments passed to the valid neighbour function.
-- @extends Core.Base#BASE
--- Be surprised!
--- When nothing goes right... Go left!
--
-- ===
--
@@ -42,7 +43,6 @@ ASTAR = {
Debug = nil,
lid = nil,
nodes = {},
CheckNodeValid = nil,
}
--- Defence condition.
@@ -56,19 +56,20 @@ ASTAR.INF=1/0
--- ASTAR class version.
-- @field #string version
ASTAR.version="0.0.1"
ASTAR.version="0.1.0"
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- TODO list
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- TODO: A lot.
-- TODO: Add more valid neighbour functions.
-- TODO: Write docs.
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- Constructor
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
--- Create a new ASTAR object and start the FSM.
--- Create a new ASTAR object.
-- @param #ASTAR self
-- @return #ASTAR self
function ASTAR:New()
@@ -76,6 +77,9 @@ function ASTAR:New()
-- Inherit everything from INTEL class.
local self=BASE:Inherit(self, BASE:New()) --#ASTAR
self.lid="ASTAR | "
self.Debug=true
return self
end
@@ -95,7 +99,7 @@ function ASTAR:SetStartCoordinate(Coordinate)
return self
end
--- Set coordinate from where to go.
--- Set coordinate where you want to go.
-- @param #ASTAR self
-- @param Core.Point#COORDINATE Coordinate end coordinate.
-- @return #ASTAR self
@@ -106,9 +110,9 @@ function ASTAR:SetEndCoordinate(Coordinate)
return self
end
--- Add a node.
--- Create a node from a given coordinate.
-- @param #ASTAR self
-- @param Core.Point#COORDINATE Coordinate The coordinate.
-- @param Core.Point#COORDINATE Coordinate The coordinate where to create the node.
-- @return #ASTAR.Node The node.
function ASTAR:GetNodeFromCoordinate(Coordinate)
@@ -121,9 +125,9 @@ function ASTAR:GetNodeFromCoordinate(Coordinate)
end
--- Add a node.
--- Add a node to the table of grid nodes.
-- @param #ASTAR self
-- @param #ASTAR.Node Node The node to be added to the nodes table.
-- @param #ASTAR.Node Node The node to be added.
-- @return #ASTAR self
function ASTAR:AddNode(Node)
@@ -132,18 +136,171 @@ function ASTAR:AddNode(Node)
return self
end
--- Check if the coordinate of a node has is at a valid surface type.
-- @param #ASTAR self
-- @param #ASTAR.Node Node The node to be added.
-- @param #table SurfaceTypes Surface types, for example `{land.SurfaceType.WATER}`. By default all surface types are valid.
-- @return #boolean If true, surface type of node is valid.
function ASTAR:CheckValidSurfaceType(Node, SurfaceTypes)
if SurfaceTypes then
if type(SurfaceTypes)~="table" then
SurfaceTypes={SurfaceTypes}
end
for _,surface in pairs(SurfaceTypes) do
if surface==Node.surfacetype then
return true
end
end
return false
else
return true
end
end
--- Set valid neighbours to require line of sight between two nodes.
-- @param #ASTAR self
-- @param #number CorridorWidth Width of LoS corridor in meters.
-- @return #ASTAR self
function ASTAR:SetValidNeighbourLoS(CorridorWidth)
self:SetValidNeighbourFunction(ASTAR.LoS, CorridorWidth)
return self
end
--- Add a function to determine if a neighbour of a node is valid.
-- @param #ASTAR self
-- @param #function NeighbourFunction Function that needs to return *true* for a neighbour to be valid.
-- @param ... Condition function arguments if any.
-- @return #ASTAR self
function ASTAR:SetValidNeighbourFunction(NeighbourFunction, ...)
self.ValidNeighbourFunc=NeighbourFunction
self.ValidNeighbourArg={}
if arg then
self.ValidNeighbourArg=arg
end
return self
end
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- User functions
-- Grid functions
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
--- Find the closest node from a given coordinate.
-- @param #ASTAR.Node nodeA
-- @param #ASTAR.Node nodeB
function ASTAR.LoS(nodeA, nodeB)
--- Create a rectangular grid of nodes between star and end coordinate.
-- The coordinate system is oriented along the line between start and end point.
-- @param #ASTAR self
-- @param #table ValidSurfaceTypes Valid surface types. By default is all surfaces are allowed.
-- @param #number BoxHY Box "height" in meters along the y-coordinate. Default 40000 meters (40 km).
-- @param #number SpaceX Additional space in meters before start and after end coordinate. Default 10000 meters (10 km).
-- @param #number deltaX Increment in the direction of start to end coordinate in meters. Default 2000 meters.
-- @param #number deltaY Increment perpendicular to the direction of start to end coordinate in meters. Default is same as deltaX.
-- @param #boolean MarkGrid If true, create F10 map markers at grid nodes.
-- @return #ASTAR self
function ASTAR:CreateGrid(ValidSurfaceTypes, BoxHY, SpaceX, deltaX, deltaY, MarkGrid)
-- Note that internally
-- x coordinate is z: x-->z Line from start to end
-- y coordinate is x: y-->x Perpendicular
-- Grid length and width.
local Dz=SpaceX or 10000
local Dx=BoxHY and BoxHY/2 or 20000
-- Increments.
local dz=deltaX or 2000
local dx=deltaY or dz
-- Heading from start to end coordinate.
local angle=self.startCoord:HeadingTo(self.endCoord)
--Distance between start and end.
local dist=self.startCoord:Get2DDistance(self.endCoord)+2*Dz
-- Origin of map. Needed to translate back to wanted position.
local co=COORDINATE:New(0, 0, 0)
local do1=co:Get2DDistance(self.startCoord)
local ho1=co:HeadingTo(self.startCoord)
-- Start of grid.
local xmin=-Dx
local zmin=-Dz
-- Number of grid points.
local nz=dist/dz+1
local nx=2*Dx/dx+1
-- Debug info.
local text=string.format("Building grid with nx=%d ny=%d => total=%d nodes", nx, nz, nx*nz)
self:I(self.lid..text)
MESSAGE:New(text, 10, "ASTAR"):ToAllIf(self.Debug)
-- Loop over x and z coordinate to create a 2D grid.
for i=1,nx do
-- x coordinate perpendicular to z.
local x=xmin+dx*(i-1)
for j=1,nz do
-- z coordinate connecting start and end.
local z=zmin+dz*(j-1)
-- Rotate 2D.
local vec3=UTILS.Rotate2D({x=x, y=0, z=z}, angle)
-- Coordinate of the node.
local c=COORDINATE:New(vec3.z, vec3.y, vec3.x):Translate(do1, ho1, true)
-- Create a node at this coordinate.
local node=self:GetNodeFromCoordinate(c)
-- Check if node has valid surface type.
if self:CheckValidSurfaceType(node, ValidSurfaceTypes) then
if MarkGrid then
c:MarkToAll(string.format("i=%d, j=%d surface=%d", i, j, node.surfacetype))
end
-- Add node to grid.
self:AddNode(node)
end
end
end
-- Debug info.
local text=string.format("Done building grid!")
self:I(self.lid..text)
MESSAGE:New(text, 10, "ASTAR"):ToAllIf(self.Debug)
return self
end
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- Valid neighbour functions
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
--- Function to check if two nodes have line of sight (LoS).
-- @param #ASTAR.Node nodeA First node.
-- @param #ASTAR.Node nodeB Other node.
-- @param #number corridor (Optional) Width of corridor in meters.
-- @return #boolean If true, two nodes have LoS.
function ASTAR.LoS(nodeA, nodeB, corridor)
local offset=0.1
local dx=200
local dx=corridor and corridor/2 or nil
local dy=dx
local cA=nodeA.coordinate:SetAltitude(0, true)
@@ -151,7 +308,7 @@ function ASTAR.LoS(nodeA, nodeB)
local los=cA:IsLOS(cB, offset)
if los then
if los and corridor then
local heading=cA:HeadingTo(cB)
local Ap=cA:Translate(dx, heading+90)
@@ -173,62 +330,9 @@ function ASTAR.LoS(nodeA, nodeB)
end
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- User functions
-- Misc functions
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
--- Find the closest node from a given coordinate.
-- @param #ASTAR self
-- @param #number DeltaX Increment in the direction of start to end coordinate in meters. Default 2000 meters.
-- @param #number DeltaY Increment perpendicular to the direction of start to end coordinate in meters. Default is same as DeltaX.
-- @return #ASTAR self
function ASTAR:CreateGrid()
local Dx=20000
local Dz=10000
local delta=2000
local angle=self.startCoord:HeadingTo(self.endCoord)
local dist=self.startCoord:Get2DDistance(self.endCoord)+2*Dz
local co=COORDINATE:New(0, 0, 0)
local do1=co:Get2DDistance(self.startCoord)
local ho1=co:HeadingTo(self.startCoord)
local xmin=-Dx
local zmin=-Dz
local nz=dist/delta+1
local nx=2*Dx/delta+1
env.info(string.format("FF building grid with nx=%d ny=%d total=%d nodes. Angle=%d, dist=%d meters", nx, nz, nx*nz, angle, dist))
for i=1,nx do
local x=xmin+delta*(i-1)
for j=1,nz do
local z=zmin+delta*(j-1)
local vec3=UTILS.Rotate2D({x=x, y=0, z=z}, angle)
local c=COORDINATE:New(vec3.z, vec3.y, vec3.x):Translate(do1, ho1, true)
if c:IsSurfaceTypeWater() then
--c:MarkToAll(string.format("i=%d, j=%d", i, j))
local node=self:GetNodeFromCoordinate(c)
self:AddNode(node)
end
end
end
env.info("FF Done building grid!")
return self
end
--- Find the closest node from a given coordinate.
-- @param #ASTAR self
@@ -276,41 +380,147 @@ function ASTAR:FindEndNode()
return self
end
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- Main A* pathfinding function
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
--- Function
--- A* pathfinding function. This seaches the path along nodes between start and end nodes/coordinates.
-- @param #ASTAR self
-- @param #boolean ExcludeStartNode If *true*, do not include start node in found path. Default is to include it.
-- @param #boolean ExcludeEndNode If *true*, do not include end node in found path. Default is to include it.
-- @return #table Table of nodes from start to finish.
function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
self:FindStartNode()
self:FindEndNode()
local nodes=self.nodes
local start=self.startNode
local goal=self.endNode
local closedset = {}
local openset = { start }
local came_from = {}
local g_score, f_score = {}, {}
g_score[start]=0
f_score[start]=g_score[start]+self:HeuristicCost(start, goal)
-- Set start time.
local T0=timer.getAbsTime()
-- Debug message.
local text=string.format("Starting A* pathfinding")
self:I(self.lid..text)
MESSAGE:New(text, 10, "ASTAR"):ToAllIf(self.Debug)
while #openset > 0 do
local current=self:LowestFscore(openset, f_score)
-- Check if we are at the end node.
if current==goal then
local path=self:UnwindPath({}, came_from, goal)
if not ExcludeEndNode then
table.insert(path, goal)
end
if ExcludeStartNode then
table.remove(path, 1)
end
-- Set end time.
local T9=timer.getAbsTime()
-- Debug message.
local text=string.format("Found path with %d nodes", #path)
self:I(self.lid..text)
MESSAGE:New(text, 60, "ASTAR"):ToAllIf(self.Debug)
return path
end
self:RemoveNode(openset, current)
table.insert(closedset, current)
local neighbors=self:NeighbourNodes(current, nodes)
-- Loop over neighbours.
for _,neighbor in ipairs(neighbors) do
if self:NotIn(closedset, neighbor) then
local tentative_g_score=g_score[current]+self:DistNodes(current, neighbor)
if self:NotIn(openset, neighbor) or tentative_g_score < g_score[neighbor] then
came_from[neighbor]=current
g_score[neighbor]=tentative_g_score
f_score[neighbor]=g_score[neighbor]+self:HeuristicCost(neighbor, goal)
if self:NotIn(openset, neighbor) then
table.insert(openset, neighbor)
end
end
end
end
end
-- Debug message.
local text=string.format("WARNING: Could NOT find valid path!")
self:I(self.lid..text)
MESSAGE:New(text, 60, "ASTAR"):ToAllIf(self.Debug)
return nil -- no valid path
end
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-- A* pathfinding helper functions
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
--- Calculate 2D distance between two nodes.
-- @param #ASTAR self
-- @param #ASTAR.Node nodeA Node A.
-- @param #ASTAR.Node nodeB Node B.
-- @return #number Distance between nodes in meters.
function ASTAR:DistNodes ( nodeA, nodeB )
function ASTAR:DistNodes(nodeA, nodeB)
return nodeA.coordinate:Get2DDistance(nodeB.coordinate)
end
--- Function
--- Heuristic cost function to go from node A to node B. That is simply the distance here.
-- @param #ASTAR self
-- @param #ASTAR.Node nodeA Node A.
-- @param #ASTAR.Node nodeB Node B.
-- @return #number Distance between nodes in meters.
function ASTAR:HeuristicCost( nodeA, nodeB )
function ASTAR:HeuristicCost(nodeA, nodeB)
return self:DistNodes(nodeA, nodeB)
end
--- Function
--- Check if going from a node to a neighbour is possible.
-- @param #ASTAR self
function ASTAR:is_valid_node ( node, neighbor )
-- @param #ASTAR.Node node A node.
-- @param #ASTAR.Node neighbor Neighbour node.
-- @return #boolean If true, transition between nodes is possible.
function ASTAR:IsValidNeighbour(node, neighbor)
self.CheckNodeValid=ASTAR.LoS
if self.CheckNodeValid then
return self.CheckNodeValid(node, neighbor)
if self.ValidNeighbourFunc then
return self.ValidNeighbourFunc(node, neighbor, unpack(self.ValidNeighbourArg))
else
return true
end
end
--- Function
-- @param #ASTAR self
function ASTAR:lowest_f_score(set, f_score)
function ASTAR:LowestFscore(set, f_score)
local lowest, bestNode = ASTAR.INF, nil
@@ -326,25 +536,37 @@ function ASTAR:lowest_f_score(set, f_score)
return bestNode
end
--- Function
--- Function to get valid neighbours of a node.
-- @param #ASTAR self
function ASTAR:neighbor_nodes(theNode, nodes)
-- @param #ASTAR.Node theNode The node.
-- @param #table nodes Possible neighbours.
-- @param #table Valid neighbour nodes.
function ASTAR:NeighbourNodes(theNode, nodes)
local neighbors = {}
for _, node in ipairs ( nodes ) do
if theNode ~= node and self:is_valid_node ( theNode, node ) then
table.insert ( neighbors, node )
if theNode~=node then
local isvalid=self:IsValidNeighbour(theNode, node)
if isvalid then
table.insert(neighbors, node)
end
end
end
return neighbors
end
--- Function
--- Function to check if a node is not in a set.
-- @param #ASTAR self
function ASTAR:not_in ( set, theNode )
-- @param #table set Set of nodes.
-- @param #ASTAR.Node theNode The node to check.
-- @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
@@ -355,9 +577,11 @@ function ASTAR:not_in ( set, theNode )
return true
end
--- Function
--- Function to remove a node from a set.
-- @param #ASTAR self
function ASTAR:remove_node(set, theNode)
-- @param #table set Set of nodes.
-- @param #ASTAR.Node theNode The node to check.
function ASTAR:RemoveNode(set, theNode)
for i, node in ipairs ( set ) do
if node == theNode then
@@ -365,11 +589,16 @@ function ASTAR:remove_node(set, theNode)
set [ #set ] = nil
break
end
end
end
end
--- Function
--- Unwind path function.
-- @param #ASTAR self
-- @param #table flat_path Flat path.
-- @param #table map Map.
-- @param #ASTAR.Node current_node The current node.
-- @return #table Unwinded path.
function ASTAR:UnwindPath( flat_path, map, current_node )
if map [ current_node ] then
@@ -380,70 +609,6 @@ function ASTAR:UnwindPath( flat_path, map, current_node )
end
end
----------------------------------------------------------------
-- pathfinding functions
----------------------------------------------------------------
--- Function
-- @param #ASTAR self
function ASTAR:GetPath()
self:FindStartNode()
self:FindEndNode()
local nodes=self.nodes
local start=self.startNode
local goal=self.endNode
local closedset = {}
local openset = { start }
local came_from = {}
local g_score, f_score = {}, {}
g_score [ start ] = 0
f_score [ start ] = g_score [ start ] + self:HeuristicCost ( start, goal )
while #openset > 0 do
local current = self:lowest_f_score ( openset, f_score )
if current == goal then
local path = self:UnwindPath ( {}, came_from, goal )
table.insert(path, goal)
return path
end
self:remove_node( openset, current )
table.insert ( closedset, current )
local neighbors = self:neighbor_nodes( current, nodes )
for _, neighbor in ipairs ( neighbors ) do
if self:not_in ( closedset, neighbor ) then
local tentative_g_score = g_score [ current ] + self:DistNodes ( current, neighbor )
if self:not_in ( openset, neighbor ) or tentative_g_score < g_score [ neighbor ] then
came_from [ neighbor ] = current
g_score [ neighbor ] = tentative_g_score
f_score [ neighbor ] = g_score [ neighbor ] + self:HeuristicCost ( neighbor, goal )
if self:not_in ( openset, neighbor ) then
table.insert ( openset, neighbor )
end
end
end
end
end
return nil -- no valid path
end
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------