ASTAR & PATHLINE

Moose Development/Moose/Core/Astar.lua

@@ -157,6 +157,8 @@ ASTAR = {
 -- @field #number surfacetype Surface type.
 -- @field #table valid Cached valid/invalid nodes.
 -- @field #table cost Cached cost.
+-- @field Core.Pathline#PATHLINE pathline Pathline that node is part of.
+-- @field Core.Pathline#PATHLINE.Point pathpoint Pathline point.
 
 --- ASTAR infinity.
 -- @field #number INF
@@ -264,21 +266,24 @@ end
 
 --- Adds nodes to the table of grid nodes from a PATHLINE.
 -- @param #ASTAR self
--- @param #string PathlineName Name of pathline. Has to exist.
+-- @param Core.Pathline#PATHLINE Pathline Pathline or name of pathline. Has to exist.
 -- @return #ASTAR self
-function ASTAR:AddNodeFromPathlineName(PathlineName)
+function ASTAR:AddNodeFromPathlineName(Pathline)
 
-  local pathline=PATHLINE:FindByName(PathlineName)
+  if type(Pathline)=="string" then
+    Pathline=PATHLINE:FindByName(Pathline)
+  end
   
-  if pathline then
+  if Pathline then
   
-    local coords=pathline:GetCoordinats()
+    for i,_point in pairs(Pathline.points) do
+      local point=_point --Core.Pathline#PATHLINE.Point
+      local coord=COORDINATE:NewFromVec3(point.vec3)
+      local node=self:AddNodeFromCoordinate(coord)
+      node.pathline=Pathline
+      node.pathpoint=point
+    end    
     
-    for _,_coord in pairs(coords) do
-      local c=_coord --Core.Point#COORDINATE
-      env.info("FF 100")
-      self:AddNodeFromCoordinate(c)  
-    end
   else
     env.error("FF error pathline")
   end
@@ -364,6 +369,18 @@ function ASTAR:SetValidNeighbourRoad(MaxDistance)
   return self
 end
 
+--- Set valid neighbours to be on the same pathline or not further apart than 10 meters.
+-- @param #ASTAR self
+-- @param #number MaxDistance Max distance between nodes in meters. Default is 2000 m.
+-- @return #ASTAR self
+function ASTAR:SetValidNeighbourPathline(MaxDistance)
+
+  self:SetValidNeighbourFunction(ASTAR.Pathline, MaxDistance)
+
+  return self
+end
+
+
 --- Set the function which calculates the "cost" to go from one to another node.
 -- The first to arguments of this function are always the two nodes under consideration. But you can add optional arguments.
 -- Very often the distance between nodes is a good measure for the cost.
@@ -568,6 +585,37 @@ function ASTAR.Road(nodeA, nodeB)
 
 end
 
+--- Function to check if two nodes are on the same pathline or if nodes are less than 10 meters apart.
+-- @param #ASTAR.Node nodeA First node.
+-- @param #ASTAR.Node nodeB Other node.
+-- @param #number distmax Max distance in meters. Default is 10 m.
+-- @return #boolean If true, two nodes are connected.
+function ASTAR.Pathline(nodeA, nodeB, distmax)
+
+  distmax=distmax or 10
+
+  if nodeA.pathpoint.name==nodeB.pathpoint.name then
+  
+    local pathline=nodeA.pathline
+    
+    local idxA=pathline:_GetPointIndex(nodeA.pathpoint)
+    local idxB=pathline:_GetPointIndex(nodeB.pathpoint)
+    
+    if math.abs(idxA-idxB)<=1 then
+      return true
+    end
+    
+  else
+    local dist=nodeA.coordinate:Get2DDistance(nodeB.coordinate)
+    if dist<distmax then
+      return true
+    end
+  end
+
+  return false
+end
+
+
 --- Function to check if distance between two nodes is less than a threshold distance.
 -- @param #ASTAR.Node nodeA First node.
 -- @param #ASTAR.Node nodeB Other node.
@@ -591,7 +639,7 @@ end
 -- @param #ASTAR.Node nodeB Other node.
 -- @return #number Distance between the two nodes.
 function ASTAR.Dist2D(nodeA, nodeB)
-  local dist=nodeA.coordinate:Get2DDistance(nodeB)
+  local dist=nodeA.coordinate:Get2DDistance(nodeB.coordinate)
   return dist
 end
 
@@ -744,6 +792,9 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
     -- Get current node.
     local current=self:_LowestFscore(openset, f_score)
     
+    self:I(current)
+    self:I(goal)
+    
     -- Check if we are at the end node.
     if current.id==goal.id then
     
@@ -871,6 +922,8 @@ function ASTAR:_HeuristicCost(nodeA, nodeB)
     cost=self:_DistNodes(nodeA, nodeB)
   end
   
+  self:T(self.lid..string.format("Cost nodeA=%d --> nodeB=%d = %.1f", nodeA.id, nodeB.id, cost))
+  
   nodeA.cost[nodeB.id]=cost
   nodeB.cost[nodeA.id]=cost  -- Symmetric problem. 
   

Moose Development/Moose/Core/Pathline.lua

@@ -21,6 +21,7 @@
 -- @field #string lid Class id string for output to DCS log file.
 -- @field #string name Name of the path line.
 -- @field #table points List of 3D points defining the path.
+-- @field #number counter Running number counting the point IDs.
 -- @extends Core.Base#BASE
 
 --- *The shortest distance between two points is a straight line.* -- Archimedes
@@ -59,10 +60,13 @@ PATHLINE = {
   ClassName      = "PATHLINE",
   lid            =   nil,
   points         =    {},
+  counter        =     0,
 }
 
 --- Point of line.
 -- @type PATHLINE.Point
+-- @field #number uid Unique ID of this point.
+-- @field #string name Name of the pathline this point belongs to.
 -- @field DCS#Vec3 vec3 3D position.
 -- @field DCS#Vec2 vec2 2D position.
 -- @field #number surfaceType Surface type.
@@ -73,7 +77,7 @@ PATHLINE = {
 
 --- PATHLINE class version.
 -- @field #string version
-PATHLINE.version="0.1.0"
+PATHLINE.version="0.2.0"
 
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -- TODO list
@@ -312,7 +316,7 @@ function PATHLINE:MarkPoints(Switch)
     if Switch==false then
       
       if point.markerID then
-        UTILS.RemoveMark(point.markerID, Delay)
+        UTILS.RemoveMark(point.markerID)
       end
       
     else
@@ -323,7 +327,7 @@ function PATHLINE:MarkPoints(Switch)
     
       point.markerID=UTILS.GetMarkID()
       
-      local text=string.format("Pathline %s: Point #%d\nSurface Type=%d\nHeight=%.1f m\nDepth=%.1f m", self.name, i, point.surfaceType, point.landHeight, point.depth)
+      local text=string.format("Pathline %s: Point #%d [UID=%d]\nSurface Type=%d\nHeight=%.1f m\nDepth=%.1f m", self.name, i, point.uid, point.surfaceType, point.landHeight, point.depth)
       
       trigger.action.markToAll(point.markerID, text, point.vec3, "")
     
@@ -343,6 +347,11 @@ end
 function PATHLINE:_CreatePoint(Vec)
 
   local point={} --#PATHLINE.Point
+  
+  self.counter=self.counter+1
+  
+  point.uid=self.counter
+  point.name=self.name
 
   if Vec.z then
     -- Given vec is 3D
@@ -365,6 +374,23 @@ function PATHLINE:_CreatePoint(Vec)
   return point
 end
 
+--- Get index of point in the lua table.
+-- @param #PATHLINE self
+-- @param #Pathline.Point Point Given point.
+-- @return #number index
+function PATHLINE:_GetPointIndex(Point)
+
+  for i,_point in pairs(self.points) do
+    local point=_point --#Pathline.Point
+   
+    if point.uid==Point.uid then
+      return i
+    end
+  
+  end
+
+  return nil
+end
 
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------