From d9b7cc18f3f49cbcb22bad8364ba616e4a2ab851 Mon Sep 17 00:00:00 2001
From: Frank <frank@inter-zone.de>
Date: Tue, 21 Jul 2020 01:19:03 +0200
Subject: [PATCH] A*

---
 Moose Development/Moose/Core/Astar.lua       | 439 +++++++++++++++++++
 Moose Development/Moose/Core/Point.lua       |   9 +-
 Moose Development/Moose/Modules.lua          |   1 +
 Moose Development/Moose/Ops/Airboss.lua      |   2 +-
 Moose Development/Moose/Ops/Intelligence.lua |   9 +-
 Moose Development/Moose/Ops/NavyGroup.lua    |   2 +-
 6 files changed, 453 insertions(+), 9 deletions(-)
 create mode 100644 Moose Development/Moose/Core/Astar.lua

diff --git a/Moose Development/Moose/Core/Astar.lua b/Moose Development/Moose/Core/Astar.lua
new file mode 100644
index 000000000..20db867ba
--- /dev/null
+++ b/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
+
+-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
\ No newline at end of file
diff --git a/Moose Development/Moose/Core/Point.lua b/Moose Development/Moose/Core/Point.lua
index 0559ed660..4b7b9a7ef 100644
--- a/Moose Development/Moose/Core/Point.lua	
+++ b/Moose Development/Moose/Core/Point.lua	
@@ -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 )
 
diff --git a/Moose Development/Moose/Modules.lua b/Moose Development/Moose/Modules.lua
index 7f49bf4ba..f4a6ab8b6 100644
--- a/Moose Development/Moose/Modules.lua	
+++ b/Moose Development/Moose/Modules.lua	
@@ -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' )
diff --git a/Moose Development/Moose/Ops/Airboss.lua b/Moose Development/Moose/Ops/Airboss.lua
index aa34cee89..16de9bf86 100644
--- a/Moose Development/Moose/Ops/Airboss.lua	
+++ b/Moose Development/Moose/Ops/Airboss.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
diff --git a/Moose Development/Moose/Ops/Intelligence.lua b/Moose Development/Moose/Ops/Intelligence.lua
index 8d62a180a..4773499a7 100644
--- a/Moose Development/Moose/Ops/Intelligence.lua	
+++ b/Moose Development/Moose/Ops/Intelligence.lua	
@@ -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
diff --git a/Moose Development/Moose/Ops/NavyGroup.lua b/Moose Development/Moose/Ops/NavyGroup.lua
index bada43c13..d6fe5816f 100644
--- a/Moose Development/Moose/Ops/NavyGroup.lua	
+++ b/Moose Development/Moose/Ops/NavyGroup.lua	
@@ -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