From bda7988118ba567df811ce704ffd65c3a8c5a281 Mon Sep 17 00:00:00 2001
From: Frank <frank@inter-zone.de>
Date: Mon, 4 Sep 2023 23:34:23 +0200
Subject: [PATCH] Astar

---
 Moose Development/Moose/Core/Astar.lua      |  90 +++++++--
 Moose Development/Moose/Core/Pathline.lua   | 199 +++++++++++++++++++-
 Moose Development/Moose/Utilities/Utils.lua |  16 ++
 3 files changed, 277 insertions(+), 28 deletions(-)

diff --git a/Moose Development/Moose/Core/Astar.lua b/Moose Development/Moose/Core/Astar.lua
index 343773713..1b43bea8a 100644
--- a/Moose Development/Moose/Core/Astar.lua	
+++ b/Moose Development/Moose/Core/Astar.lua	
@@ -369,9 +369,9 @@ function ASTAR:SetValidNeighbourRoad(MaxDistance)
   return self
 end
 
---- Set valid neighbours to be on the same pathline or not further apart than 10 meters.
+--- Set valid neighbours to be on the same pathline or not further apart than 10 meters to jump from one pathline to another.
 -- @param #ASTAR self
--- @param #number MaxDistance Max distance between nodes in meters. Default is 2000 m.
+-- @param #number MaxDistance Max allowed distance between nodes of different pathlines in meters. Default is 10 m.
 -- @return #ASTAR self
 function ASTAR:SetValidNeighbourPathline(MaxDistance)
 
@@ -666,7 +666,7 @@ function ASTAR.DistRoad(nodeA, nodeB)
     local dist=0
     
     for i=2,#path do
-      local b=path[i] --DCS#Vec2
+      local b=path[i]   --DCS#Vec2
       local a=path[i-1] --DCS#Vec2
       
       dist=dist+UTILS.VecDist2D(a,b)
@@ -676,7 +676,6 @@ function ASTAR.DistRoad(nodeA, nodeB)
     return dist
   end
   
-
   return math.huge
 end
 
@@ -686,10 +685,11 @@ 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.
+-- @param Core.Point#COORDINATE Coordinate Reference coordinate.
+-- @param #table ExcludeNodes Table of nodes that are excluded.
+-- @return #ASTAR.Node Closest node to the coordinate.
 -- @return #number Distance to closest node in meters.
-function ASTAR:FindClosestNode(Coordinate)
+function ASTAR:FindClosestNode(Coordinate, ExcludeNodes)
 
   local distMin=math.huge
   local closeNode=nil
@@ -697,11 +697,15 @@ function ASTAR:FindClosestNode(Coordinate)
   for _,_node in pairs(self.nodes) do
     local node=_node --#ASTAR.Node
     
-    local dist=node.coordinate:Get2DDistance(Coordinate)
+    if ExcludeNodes==nil or self:_IsNodeNotInTable(ExcludeNodes, node) then
     
-    if dist<distMin then
-      distMin=dist
-      closeNode=node
+      local dist=node.coordinate:Get2DDistance(Coordinate)
+      
+      if dist<distMin then
+        distMin=dist
+        closeNode=node
+      end
+      
     end
     
   end
@@ -711,7 +715,6 @@ end
 
 --- Find the start node.
 -- @param #ASTAR self
--- @param #ASTAR.Node Node The node to be added to the nodes table.
 -- @return #ASTAR self
 function ASTAR:FindStartNode()
 
@@ -727,13 +730,12 @@ function ASTAR:FindStartNode()
   return self
 end
 
---- Add a node.
+--- Find the end node.
 -- @param #ASTAR self
--- @param #ASTAR.Node Node The node to be added to the nodes table.
 -- @return #ASTAR self
 function ASTAR:FindEndNode()
 
-  local node, dist=self:FindClosestNode(self.endCoord)
+  local node, dist=self:FindClosestNode(self.endCoord, {self.startNode})
 
   self.endNode=node
   
@@ -762,6 +764,8 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
   local nodes=self.nodes
   local start=self.startNode
   local goal=self.endNode
+  
+  self:I(self.lid..string.format("GetPath Start Node=%d, End Node=%d", start.id, goal.id))
 
   -- Sets.
   local openset   = {}
@@ -837,7 +841,6 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
     
     -- Loop over neighbours.
     for _,neighbor in pairs(neighbors) do
-    
       if self:_NotIn(closedset, neighbor.id) then
       
         local tentative_g_score=g_score[current.id]+self:_DistNodes(current, neighbor)
@@ -868,11 +871,13 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
   return nil -- no valid path
 end
 
---- A* pathfinding function. This seaches the path along nodes between start and end nodes/coordinates.
+--- A* pathfinding function. This seaches the path along nodes between start and end nodes/coordinates. 
+-- It automatically creates a PATHLINE object that is returned in combination with the nodes of the optimal path. 
 -- @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 Core.Pathline#PATHLINE Pathline
+-- @return Core.Pathline#PATHLINE Pathline.
+-- @return #table Nodes of path.
 function ASTAR:GetPathline(ExcludeStartNode, ExcludeEndNode)
 
   local nodes=self:GetPath(ExcludeStartNode, ExcludeEndNode)
@@ -891,7 +896,23 @@ function ASTAR:GetPathline(ExcludeStartNode, ExcludeEndNode)
       
   end
 
-  return pathline
+  return pathline, nodes
+end
+
+--- Get pathlines from nodes.
+-- @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 PATHLINES used in the path. Key is the pathline name and value is the PATHLINE.
+function ASTAR:GetPathlinesFromNodes(Nodes)
+
+  local pathlines={}
+  for _,_node in pairs(Nodes or {}) do
+    local node=_node --#ASTAR.Node
+    pathlines[node.pathline.name]=node.pathline
+  end
+
+  return pathlines
 end
 
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -987,6 +1008,8 @@ function ASTAR:_LowestFscore(set, f_score)
     end
   end
   
+  self:I(self.lid..string.format("Lowest Fscore=%.1f, Node=%d", lowest, bestNode))
+  
   return self.nodes[bestNode]
 end
 
@@ -1033,7 +1056,7 @@ end
 -- @return #table Unwinded path.
 function ASTAR:_UnwindPath( flat_path, map, current_node )
 
-  if map [current_node] then
+  if map[current_node] then
     table.insert (flat_path, 1, map[current_node]) 
     return self:_UnwindPath(flat_path, map, map[current_node])
   else
@@ -1041,6 +1064,33 @@ function ASTAR:_UnwindPath( flat_path, map, current_node )
   end
 end
 
+--- Function to check if a certain node is in a given table.
+-- @param #ASTAR self
+-- @param #table Nodes Nodes table.
+-- @param #ASTAR.Node Node The node to check.
+-- @return #boolean If true, the node is not in the set.
+function ASTAR:_IsNodeInTable(Nodes, Node)
+
+  for _,_node in pairs(Nodes) do
+    local node=_node --#ASTAR.Node
+    if node.id==Node.id then
+      return true
+    end
+  end
+
+  return false
+end
+
+--- Function to check if a certain node is **not** in a given table.
+-- @param #ASTAR self
+-- @param #table Nodes Nodes table.
+-- @param #ASTAR.Node Node The node to check.
+-- @return #boolean If true, the node is not in the set.
+function ASTAR:_IsNodeNotInTable(Nodes, Node)
+  local is=self:_IsNodeInTable(Nodes, Node)
+  return not is
+end
+
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
\ No newline at end of file
diff --git a/Moose Development/Moose/Core/Pathline.lua b/Moose Development/Moose/Core/Pathline.lua
index 0088a97f1..435073612 100644
--- a/Moose Development/Moose/Core/Pathline.lua	
+++ b/Moose Development/Moose/Core/Pathline.lua	
@@ -5,6 +5,8 @@
 --    * Path from A to B
 --    * Arbitrary number of points
 --    * Automatically from lines drawtool
+--    * Draw line or mark points on F10 map
+--    * Find closest points to path
 --
 -- ===
 --
@@ -73,11 +75,12 @@ PATHLINE = {
 -- @field #number landHeight Land height in meters.
 -- @field #number depth Water depth in meters.
 -- @field #number markerID Marker ID.
+-- @field #number lineID Marker of pathline ID.
 
 
 --- PATHLINE class version.
 -- @field #string version
-PATHLINE.version="0.2.0"
+PATHLINE.version="0.3.0"
 
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -- TODO list
@@ -155,15 +158,19 @@ end
 --- Add a point to the path from a given 2D position. The third dimension is determined from the land height.
 -- @param #PATHLINE self
 -- @param DCS#Vec2 Vec2 The 2D vector (x,y) to add.
+-- @param #number Index Index to add this point, *e.g.* 1 for first point or 2 for second point. Default is at the end.
 -- @return #PATHLINE self
-function PATHLINE:AddPointFromVec2(Vec2)
+function PATHLINE:AddPointFromVec2(Vec2, Index)
 
   if Vec2 then
   
     local point=self:_CreatePoint(Vec2)
 
-    table.insert(self.points, point)
-    
+    if Index then
+      table.insert(self.points, Index, point)
+    else
+      table.insert(self.points, point)
+    end 
   end
   
   return self
@@ -172,14 +179,19 @@ end
 --- Add a point to the path from a given 3D position.
 -- @param #PATHLINE self
 -- @param DCS#Vec3 Vec3 The 3D vector (x,y) to add.
+-- @param #number Index Index to add this point, *e.g.* 1 for first point or 2 for second point. Default is at the end.
 -- @return #PATHLINE self
-function PATHLINE:AddPointFromVec3(Vec3)
+function PATHLINE:AddPointFromVec3(Vec3, Index)
 
   if Vec3 then
   
     local point=self:_CreatePoint(Vec3)
 
-    table.insert(self.points, point)
+    if Index then
+      table.insert(self.points, Index, point)
+    else
+      table.insert(self.points, point)
+    end
     
   end
   
@@ -309,10 +321,12 @@ end
 --- Mark points on F10 map.
 -- @param #PATHLINE self
 -- @param #boolean Switch If `true` or nil, set marks. If `false`, remove marks.
--- @return <DCS#Vec3> List of DCS#Vec3 points.
+-- @return #PATHLINE self
 function PATHLINE:MarkPoints(Switch)
+
   for i,_point in pairs(self.points) do
     local point=_point --#PATHLINE.Point
+    
     if Switch==false then
       
       if point.markerID then
@@ -333,6 +347,175 @@ function PATHLINE:MarkPoints(Switch)
     
     end
   end
+  
+  return self
+end
+
+
+--- Draw line on F10 map.
+-- @param #PATHLINE self
+-- @param #boolean Switch If `true` or nil, draw pathline. If `false`, remove drawing.
+-- @param #number Coalition Coalition side. Default -1 for all.
+-- @param #table Color RGB color and alpha `{r, g, b, a}`. Default {0, 1, 0, 0.5}.
+-- @param #number LineType Line type. Default 1=solid.
+-- @return #PATHLINE self
+function PATHLINE:Draw(Switch, Coalition, Color, LineType)
+
+  Coalition=Coalition or -1
+  Color=Color or {0, 1, 0, 0.5}
+  LineType=LineType or -1
+
+  if Switch==false then
+
+    for i,_point in pairs(self.points) do
+      local point=_point --#PATHLINE.Point
+        
+      if point.lineID then
+        UTILS.RemoveMark(point.lineID)
+      end
+      
+    end
+      
+  else
+
+    for i=2,#self.points do
+      
+    
+      local p1=self.points[i-1] --#PATHLINE.Point
+      local p2=self.points[i]   --#PATHLINE.Point
+      
+      if p2.lineID then
+        UTILS.RemoveMark(p2.lineID)
+      end
+    
+      p2.lineID=UTILS.GetMarkID()      
+    
+      trigger.action.lineToAll(Coalition, p2.lineID, p1.vec3, p2.vec3, Color, LineType)
+      
+    end
+    
+  end
+  
+  return self
+end
+
+--- Get the closest point on the pathline for a given reference point.
+-- @param #PATHLINE self
+-- @param DCS#Vec2 Vec2 Reference Point in 2D.
+-- @return DCS#Vec2 Cloest point on pathline.
+-- @return #number Distance from closest point to ref point in meters.
+function PATHLINE:GetClosestPoint2D(Vec2)
+
+
+  local P=nil --DCS#Vec2  
+  local D=math.huge
+  
+  for i=2,#self.points do
+
+    local A=self.points[i-1] --#PATHLINE.Point
+    local B=self.points[i]   --#PATHLINE.Point
+    
+    local a=A.vec2
+    local b=B.vec2
+        
+    local ab=UTILS.Vec2Substract(b, a)  
+    local ap=UTILS.Vec2Substract(Vec2, a)
+    
+    local proj=UTILS.Vec2Dot(ap, ab)
+    
+    local lab=UTILS.Vec2Norm(ab)
+    
+    local f=proj/lab/lab
+    
+    -- Debug info.
+    local text=string.format("FF Proj=%.1f, |ab|=%.1f, f=%.1f", proj, lab, f)
+    self:T(self.lid..text)
+    
+    -- Cases for finite segment.
+    local p=nil --DCS#Vec2
+    if f<0 then
+      p=a
+    elseif f>1 then
+      p=b
+    else
+      local r=UTILS.Vec2Mult(ab, f)
+      p=UTILS.Vec2Add(a, r)  
+    end
+    
+    -- Distance.
+    local d=UTILS.VecDist2D(p, Vec2)
+    
+    if d<=D then
+      D=d
+      P=p
+    end
+    
+  end
+  
+    
+  --local c=COORDINATE:NewFromVec2(P)  
+  --c:MarkToAll(string.format("Point D=%.1f m", D))
+
+  return P, D
+end
+
+--- Get the closest point on the pathline for a given reference point.
+-- @param #PATHLINE self
+-- @param DCS#Vec3 Vec3 Reference Point in 2D.
+-- @return DCS#Vec3 Cloest point on pathline.
+-- @return #number Distance from closest point to ref point in meters.
+function PATHLINE:GetClosestPoint3D(Vec3)
+
+  local P=nil --DCS#Vec3
+  local D=math.huge
+  
+  for i=2,#self.points do
+
+    local A=self.points[i-1] --#PATHLINE.Point
+    local B=self.points[i]   --#PATHLINE.Point
+    
+    local a=A.vec3
+    local b=B.vec3
+        
+    local ab=UTILS.VecSubstract(b, a)  
+    local ap=UTILS.VecSubstract(Vec3, a)
+    
+    local proj=UTILS.VecDot(ap, ab)
+    
+    local lab=UTILS.VecNorm(ab)
+    
+    local f=proj/lab/lab
+    
+    -- Debug info.
+    local text=string.format("FF Proj=%.1f, |ab|=%.1f, f=%.1f", proj, lab, f)
+    self:T(self.lid..text)
+    
+    -- Cases for finite segment.
+    local p=nil --DCS#Vec2
+    if f<0 then
+      p=a
+    elseif f>1 then
+      p=b
+    else
+      local r=UTILS.VecMult(ab, f)
+      p=UTILS.VecAdd(a, r)  
+    end
+    
+    -- Distance.
+    local d=UTILS.VecDist3D(p, Vec3)
+    
+    if d<=D then
+      D=d
+      P=p
+    end
+    
+  end
+  
+    
+  --local c=COORDINATE:NewFromVec2(P)  
+  --c:MarkToAll(string.format("Point D=%.1f m", D))
+
+  return P, D
 end
 
 
@@ -343,7 +526,7 @@ end
 --- Get 3D points of pathline.
 -- @param #PATHLINE self
 -- @param DCS#Vec3 Vec Position vector. Can also be a DCS#Vec2 in which case the altitude at landheight is taken.
--- @return #PATHLINE.Point
+-- @return #PATHLINE.Point Pathline Point.
 function PATHLINE:_CreatePoint(Vec)
 
   local point={} --#PATHLINE.Point
diff --git a/Moose Development/Moose/Utilities/Utils.lua b/Moose Development/Moose/Utilities/Utils.lua
index 9170a95b1..f2ee13a63 100644
--- a/Moose Development/Moose/Utilities/Utils.lua	
+++ b/Moose Development/Moose/Utilities/Utils.lua	
@@ -1375,6 +1375,22 @@ function UTILS.Vec2Add(a, b)
   return {x=a.x+b.x, y=a.y+b.y}
 end
 
+--- Multiply 2D vector by a scalar value.
+-- @param DCS#Vec2 a Vector in 2D with x, y components.
+-- @param #number c Scalar value.
+-- @return DCS#Vec2 Vector 
+function UTILS.Vec2Mult(a, c)
+  return {x=c*a.x, y=c*a.y}
+end
+
+--- Multiply 3D vector by a scalar value.
+-- @param DCS#Vec3 a Vector in 3D with x, y, z components.
+-- @param #number c Scalar value.
+-- @return DCS#Vec3 Vector 
+function UTILS.VecMult(a, c)
+  return {x=c*a.x, y=c*a.y, z=c*a.z}
+end
+
 --- Calculate the angle between two 3D vectors.
 -- @param DCS#Vec3 a Vector in 3D with x, y, z components.
 -- @param DCS#Vec3 b Vector in 3D with x, y, z components.