Astar

2025-10-29 16:58:06 +00:00 · 2023-09-04 23:34:23 +02:00 · 2023-09-04 23:34:23 +02:00 · bda7988118
commit bda7988118
parent 47c0006537
3 changed files with 277 additions and 28 deletions
--- a/Development/Moose/Core/Astar.lua
+++ b/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)

@ -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,6 +697,8 @@ function ASTAR:FindClosestNode(Coordinate)
  for _,_node in pairs(self.nodes) do
    local node=_node --#ASTAR.Node
    
+    if ExcludeNodes==nil or self:_IsNodeNotInTable(ExcludeNodes, node) then
+    
      local dist=node.coordinate:Get2DDistance(Coordinate)
      
      if dist<distMin then
@ -706,12 +708,13 @@ function ASTAR:FindClosestNode(Coordinate)
      
    end
    
+  end
+    
  return closeNode, distMin
 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
  
@ -763,6 +765,8 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
  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   = {}
  local closedset = {}
@ -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)
@ -869,10 +872,12 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
 end

 --- 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

@ -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
+
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
--- a/Development/Moose/Core/Pathline.lua
+++ b/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)

+    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)

+    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
--- a/Development/Moose/Utilities/Utils.lua
+++ b/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.