A*

Moose Development/Moose/Core/Astar.lua

@@ -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!
+--
+-- ===
+--
+-- ![Banner Image](..\Presentations\WingCommander\ASTAR_Main.jpg)
+--
+-- # 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
+
+-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------