mirror of
https://github.com/FlightControl-Master/MOOSE.git
synced 2025-08-15 10:47:21 +00:00
A*
This commit is contained in:
Frank
parent
ce1121a4c2
commit
d9b7cc18f3
439
Moose Development/Moose/Core/Astar.lua
Normal file
439
Moose Development/Moose/Core/Astar.lua
Normal file
@ -0,0 +1,439 @@
|
||||
--- **Core** - Pathfinding.
|
||||
--
|
||||
-- **Main Features:**
|
||||
--
|
||||
-- * Stuff
|
||||
--
|
||||
-- ===
|
||||
--
|
||||
-- ### Author: **funkyfranky**
|
||||
-- @module Core.Astar
|
||||
-- @image CORE_Atar.png
|
||||
|
||||
|
||||
--- ASTAR class.
|
||||
-- @type ASTAR
|
||||
-- @field #string ClassName Name of the class.
|
||||
-- @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 #ASTAR.Node startNode Start node.
|
||||
-- @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.
|
||||
-- @extends Core.Base#BASE
|
||||
|
||||
--- Be surprised!
|
||||
--
|
||||
-- ===
|
||||
--
|
||||
-- 
|
||||
--
|
||||
-- # The ASTAR Concept
|
||||
--
|
||||
-- Pathfinding algorithm.
|
||||
--
|
||||
--
|
||||
--
|
||||
-- @field #ASTAR
|
||||
ASTAR = {
|
||||
ClassName = "ASTAR",
|
||||
Debug = nil,
|
||||
lid = nil,
|
||||
nodes = {},
|
||||
CheckNodeValid = nil,
|
||||
}
|
||||
|
||||
--- Defence condition.
|
||||
-- @type ASTAR.Node
|
||||
-- @field Core.Point#COORDINATE coordinate Coordinate of the node.
|
||||
-- @field #number surfacetype Surface type.
|
||||
|
||||
--- ASTAR infinity
|
||||
-- @field #string INF
|
||||
ASTAR.INF=1/0
|
||||
|
||||
--- ASTAR class version.
|
||||
-- @field #string version
|
||||
ASTAR.version="0.0.1"
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- TODO list
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
-- TODO: A lot.
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- Constructor
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
--- Create a new ASTAR object and start the FSM.
|
||||
-- @param #ASTAR self
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:New()
|
||||
|
||||
-- Inherit everything from INTEL class.
|
||||
local self=BASE:Inherit(self, BASE:New()) --#ASTAR
|
||||
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- User functions
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
--- Set coordinate from where to start.
|
||||
-- @param #ASTAR self
|
||||
-- @param Core.Point#COORDINATE Coordinate Start coordinate.
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:SetStartCoordinate(Coordinate)
|
||||
|
||||
self.startCoord=Coordinate
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
--- Set coordinate from where to go.
|
||||
-- @param #ASTAR self
|
||||
-- @param Core.Point#COORDINATE Coordinate end coordinate.
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:SetEndCoordinate(Coordinate)
|
||||
|
||||
self.endCoord=Coordinate
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
--- Add a node.
|
||||
-- @param #ASTAR self
|
||||
-- @param Core.Point#COORDINATE Coordinate The coordinate.
|
||||
-- @return #ASTAR.Node The node.
|
||||
function ASTAR:GetNodeFromCoordinate(Coordinate)
|
||||
|
||||
local node={} --#ASTAR.Node
|
||||
|
||||
node.coordinate=Coordinate
|
||||
node.surfacetype=Coordinate:GetSurfaceType()
|
||||
|
||||
return node
|
||||
end
|
||||
|
||||
|
||||
--- Add a node.
|
||||
-- @param #ASTAR self
|
||||
-- @param #ASTAR.Node Node The node to be added to the nodes table.
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:AddNode(Node)
|
||||
|
||||
table.insert(self.nodes, Node)
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- User functions
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
--- Find the closest node from a given coordinate.
|
||||
-- @param #ASTAR.Node nodeA
|
||||
-- @param #ASTAR.Node nodeB
|
||||
function ASTAR.LoS(nodeA, nodeB)
|
||||
|
||||
local los=nodeA.coordinate:IsLOS(nodeB.coordinate, 0.5)
|
||||
|
||||
if los then
|
||||
local heading=nodeA.coordinate:HeadingTo(nodeB.coordinate)
|
||||
|
||||
local Ap=nodeA.coordinate:Translate(100, heading+90)
|
||||
local Bp=nodeA.coordinate:Translate(100, heading+90)
|
||||
|
||||
los=Ap:IsLOS(Bp, 0.5)
|
||||
|
||||
if los then
|
||||
|
||||
local Am=nodeA.coordinate:Translate(100, heading-90)
|
||||
local Bm=nodeA.coordinate:Translate(100, heading-90)
|
||||
|
||||
los=Am:IsLOS(Bm, 0.5)
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
return los
|
||||
end
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- User functions
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
--- Find the closest node from a given coordinate.
|
||||
-- @param #ASTAR self
|
||||
-- @param Core.Point#COORDINATE Coordinate.
|
||||
-- @return #ASTAR.Node Cloest node to the coordinate.
|
||||
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!")
|
||||
|
||||
end
|
||||
|
||||
--- Find the closest node from a given coordinate.
|
||||
-- @param #ASTAR self
|
||||
-- @param Core.Point#COORDINATE Coordinate.
|
||||
-- @return #ASTAR.Node Cloest node to the coordinate.
|
||||
function ASTAR:FindClosestNode(Coordinate)
|
||||
|
||||
local distMin=math.huge
|
||||
local closeNode=nil
|
||||
|
||||
for _,_node in pairs(self.nodes) do
|
||||
local node=_node --#ASTAR.Node
|
||||
|
||||
local dist=node.coordinate:Get2DDistance(Coordinate)
|
||||
|
||||
if dist<distMin then
|
||||
distMin=dist
|
||||
closeNode=node
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
return closeNode
|
||||
end
|
||||
|
||||
--- Add a node.
|
||||
-- @param #ASTAR self
|
||||
-- @param #ASTAR.Node Node The node to be added to the nodes table.
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:FindStartNode()
|
||||
|
||||
self.startNode=self:FindClosestNode(self.startCoord)
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
--- Add a node.
|
||||
-- @param #ASTAR self
|
||||
-- @param #ASTAR.Node Node The node to be added to the nodes table.
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:FindEndNode()
|
||||
|
||||
self.endNode=self:FindClosestNode(self.endCoord)
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
-- @param #ASTAR.Node nodeA Node A.
|
||||
-- @param #ASTAR.Node nodeB Node B.
|
||||
-- @return #number Distance between nodes in meters.
|
||||
function ASTAR:dist_between ( nodeA, nodeB )
|
||||
return nodeA.coordinate:Get2DDistance(nodeB.coordinate)
|
||||
end
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
-- @param #ASTAR.Node nodeA Node A.
|
||||
-- @param #ASTAR.Node nodeB Node B.
|
||||
-- @return #number Distance between nodes in meters.
|
||||
function ASTAR:heuristic_cost_estimate( nodeA, nodeB )
|
||||
return self:dist_between(nodeA, nodeB)
|
||||
end
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
function ASTAR:is_valid_node ( node, neighbor )
|
||||
|
||||
self.CheckNodeValid=ASTAR.LoS
|
||||
|
||||
if self.CheckNodeValid then
|
||||
return self.CheckNodeValid(node, neighbor)
|
||||
else
|
||||
return true
|
||||
end
|
||||
end
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
function ASTAR:lowest_f_score(set, f_score)
|
||||
|
||||
local lowest, bestNode = ASTAR.INF, nil
|
||||
|
||||
for _, node in ipairs ( set ) do
|
||||
|
||||
local score = f_score [ node ]
|
||||
|
||||
if score < lowest then
|
||||
lowest, bestNode = score, node
|
||||
end
|
||||
end
|
||||
|
||||
return bestNode
|
||||
end
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
function ASTAR:neighbor_nodes ( 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 )
|
||||
end
|
||||
|
||||
end
|
||||
return neighbors
|
||||
end
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
function ASTAR:not_in ( set, theNode )
|
||||
|
||||
for _, node in ipairs ( set ) do
|
||||
if node == theNode then
|
||||
return false
|
||||
end
|
||||
end
|
||||
|
||||
return true
|
||||
end
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
function ASTAR:remove_node(set, theNode)
|
||||
|
||||
for i, node in ipairs ( set ) do
|
||||
if node == theNode then
|
||||
set [ i ] = set [ #set ]
|
||||
set [ #set ] = nil
|
||||
break
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
--- Function
|
||||
-- @param #ASTAR self
|
||||
function ASTAR:unwind_path ( flat_path, map, current_node )
|
||||
|
||||
if map [ current_node ] then
|
||||
table.insert ( flat_path, 1, map [ current_node ] )
|
||||
return self:unwind_path ( flat_path, map, map [ current_node ] )
|
||||
else
|
||||
return flat_path
|
||||
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:heuristic_cost_estimate ( start, goal )
|
||||
|
||||
while #openset > 0 do
|
||||
|
||||
local current = self:lowest_f_score ( openset, f_score )
|
||||
|
||||
if current == goal then
|
||||
local path = self:unwind_path ( {}, 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:dist_between ( 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:heuristic_cost_estimate ( neighbor, goal )
|
||||
|
||||
if self:not_in ( openset, neighbor ) then
|
||||
table.insert ( openset, neighbor )
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
return nil -- no valid path
|
||||
end
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
@ -1921,15 +1921,18 @@ do -- COORDINATE
|
||||
--- Returns if a Coordinate has Line of Sight (LOS) with the ToCoordinate.
|
||||
-- @param #COORDINATE self
|
||||
-- @param #COORDINATE ToCoordinate
|
||||
-- @param #number OFfset Height offset in meters. Default 2 m.
|
||||
-- @return #boolean true If the ToCoordinate has LOS with the Coordinate, otherwise false.
|
||||
function COORDINATE:IsLOS( ToCoordinate )
|
||||
function COORDINATE:IsLOS( ToCoordinate, Offset )
|
||||
|
||||
Offset=Offset or 2
|
||||
|
||||
-- Measurement of visibility should not be from the ground, so Adding a hypotethical 2 meters to each Coordinate.
|
||||
local FromVec3 = self:GetVec3()
|
||||
FromVec3.y = FromVec3.y + 2
|
||||
FromVec3.y = FromVec3.y + Offset
|
||||
|
||||
local ToVec3 = ToCoordinate:GetVec3()
|
||||
ToVec3.y = ToVec3.y + 2
|
||||
ToVec3.y = ToVec3.y + Offset
|
||||
|
||||
local IsLOS = land.isVisible( FromVec3, ToVec3 )
|
||||
|
||||
|
||||
@ -26,6 +26,7 @@ __Moose.Include( 'Scripts/Moose/Core/Spawn.lua' )
|
||||
__Moose.Include( 'Scripts/Moose/Core/SpawnStatic.lua' )
|
||||
__Moose.Include( 'Scripts/Moose/Core/Goal.lua' )
|
||||
__Moose.Include( 'Scripts/Moose/Core/Spot.lua' )
|
||||
__Moose.Include( 'Scripts/Moose/Core/Astar.lua' )
|
||||
|
||||
__Moose.Include( 'Scripts/Moose/Wrapper/Object.lua' )
|
||||
__Moose.Include( 'Scripts/Moose/Wrapper/Identifiable.lua' )
|
||||
|
||||
@ -2464,7 +2464,7 @@ function AIRBOSS:AddRecoveryWindow(starttime, stoptime, case, holdingoffset, tur
|
||||
local Tstart=UTILS.ClockToSeconds(starttime)
|
||||
|
||||
-- Set stop time.
|
||||
local Tstop=UTILS.ClockToSeconds(stoptime or Tstart+90*60)
|
||||
local Tstop=stoptime and UTILS.ClockToSeconds(stoptime) or Tstart+90*60
|
||||
|
||||
-- Consistancy check for timing.
|
||||
if Tstart>Tstop then
|
||||
|
||||
@ -66,8 +66,10 @@ INTEL = {
|
||||
-- @field #number Tdetected Time stamp in abs. mission time seconds when this item was last detected.
|
||||
-- @field Core.Point#COORDINATE position Last known position of the item.
|
||||
-- @field DCS#Vec3 velocity 3D velocity vector. Components x,y and z in m/s.
|
||||
-- @field #number speed Last known speed.
|
||||
-- @field #number markerID F10 map marker ID.
|
||||
-- @field #number speed Last known speed in m/s.
|
||||
-- @field #boolean isship
|
||||
-- @field #boolean ishelo
|
||||
-- @field #boolean isgrund
|
||||
|
||||
--- Cluster info.
|
||||
-- @type INTEL.Cluster
|
||||
@ -921,7 +923,7 @@ function INTEL:GetClusterCoordinate(cluster)
|
||||
|
||||
x=x+contact.position.x
|
||||
y=y+contact.position.y
|
||||
y=y+contact.position.z
|
||||
z=z+contact.position.z
|
||||
n=n+1
|
||||
|
||||
end
|
||||
@ -970,7 +972,6 @@ function INTEL:UpdateClusterMarker(cluster, newcoordinate)
|
||||
local refresh=false
|
||||
|
||||
if cluster.marker.text~=text then
|
||||
--cluster.marker:UpdateText(text)
|
||||
cluster.marker.text=text
|
||||
refresh=true
|
||||
end
|
||||
|
||||
@ -242,7 +242,7 @@ function NAVYGROUP:CreateTurnIntoWind(starttime, stoptime, speed, uturn, offset)
|
||||
local Tstart=UTILS.ClockToSeconds(starttime)
|
||||
|
||||
-- Set stop time.
|
||||
local Tstop=UTILS.ClockToSeconds(stoptime or Tstart+90*60)
|
||||
local Tstop=stoptime and UTILS.ClockToSeconds(stoptime) or Tstart+90*60
|
||||
|
||||
-- Consistancy check for timing.
|
||||
if Tstart>Tstop then
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user