ASTAR/PATHLINE

Moose Development/Moose/Core/Astar.lua

@@ -281,16 +281,23 @@ function ASTAR:AddNodeFromPathlineName(Pathline)
   
     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)
+      -- Create node from point coordinate.
+      local node=self:AddNodeFromCoordinate(COORDINATE:NewFromVec3(point.vec3))
+      
+      -- Add pathline parameters.
       node.pathline=Pathline
       node.pathpoint=point
       
+      -- Debug.
       local name=node.pathline and node.pathline.name or "N/A"
       local idx=node.pathline and node.pathline:_GetPointIndex(node.pathpoint) or "N/A"
       
+      -- Debug message.
       self:T(self.lid..string.format("Adding node UID=%d pathline=%s [%s]", node.id, name, tostring(idx)))
-      node.coordinate:MarkToAll(string.format("Node ID=%d\npathline=%s [%s]", node.id, name, tostring(idx)))      
+            
+      -- Debug mark
+      --node.coordinate:MarkToAll(string.format("Node ID=%d\npathline=%s [%s]", node.id, name, tostring(idx)))
       
     end    
     
@@ -604,7 +611,7 @@ function ASTAR.Pathline(nodeA, nodeB, distmax)
 
   distmax=distmax or 10
 
-  if nodeA.pathpoint.name==nodeB.pathpoint.name then
+  if nodeA.pathline.name==nodeB.pathline.name then
   
     -- Nodes are on the same pathline. We use the index to check if they are neighbours.
   
@@ -619,27 +626,21 @@ function ASTAR.Pathline(nodeA, nodeB, distmax)
     
   else
     
---    local dist=nodeA.coordinate:Get2DDistance(nodeB.coordinate)
---    if dist<distmax then
---      return true
---    end
-
-    local idxA=nodeA.pathline:_GetPointIndex(nodeA.pathpoint)
-    local idxB=nodeB.pathline:_GetPointIndex(nodeB.pathpoint)
-    
     -- Check if nodeB is close to pathline of nodeA.
     local c, dist, segA=nodeA.pathline:GetClosestPoint3D(nodeB.coordinate)
     local seg=segA --Core.Pathline#PATHLINE.Segment
     
-    --TODO: Check that 
     if dist<distmax and (nodeA.pathpoint.uid==seg.p1.uid or nodeA.pathpoint.uid==seg.p2.uid) then
       env.info(string.format("FF NodeB=%d [pathline=%s] is close to NodeA=%d [pathline=%s] ==> valid neighbour", nodeB.id, nodeB.pathline.name, nodeA.id, nodeA.pathline.name))
       return true
     end
 
+
     -- Check if nodeA is close to pathline of nodeB.
-    local c, dist=nodeB.pathline:GetClosestPoint3D(nodeA.coordinate)
+    local c, dist, segB=nodeB.pathline:GetClosestPoint3D(nodeA.coordinate)
-    if dist<distmax then
+    local seg=segB --Core.Pathline#PATHLINE.Segment
+
+    if dist<distmax and (nodeB.pathpoint.uid==seg.p1.uid or nodeB.pathpoint.uid==seg.p2.uid) then
       env.info(string.format("FF NodeA=%d [pathline=%s] is close to NodeB=%d [pathline=%s] ==> valid neighbour", nodeA.id, nodeA.pathline.name, nodeB.id, nodeB.pathline.name))
       return true
     end    
@@ -674,6 +675,8 @@ end
 -- @return #number Distance between the two nodes.
 function ASTAR.Dist2D(nodeA, nodeB)
   local dist=nodeA.coordinate:Get2DDistance(nodeB.coordinate)
+  local text=string.format("FF Cost Dist2D NodeA=%d-->NodeB=%d = %.1f", nodeA.id, nodeB.id, dist)
+  env.info(text)
   return dist
 end
 
@@ -797,24 +800,25 @@ function ASTAR:FindStartNode()
   -- Find the closest pathline to the 
   local pathline, dist, vec3, s=self:FindClosestPathline(self.startCoord)
   
-  if pathline and vec3 then
+  if pathline and vec3 and dist and dist>10 then
 
-    local Coordinate=COORDINATE:NewFromVec3(vec3)
+    -- Create a node on the closest pathline so we first go straight there and then along the pathline.
-    
+    local node=self:AddNodeFromCoordinate(COORDINATE:NewFromVec3(vec3))
-    local node=self:AddNodeFromCoordinate(Coordinate)
-    
-    node.pathline=pathline
     
+    -- We also need the pathline point.    
     local point=pathline:AddPointFromVec3(vec3, nil, s.p1)
 
+    node.pathline=pathline    
     node.pathpoint=point
     
-    self.startNode=node
+    self:T2(self.lid..string.format("Added new node=%d, which is closest to start coord. dist=%.1f m", node.id, dist))
-  else
-    -- Find the closest node to the given start coordinate.
-    self.startNode, dist=self:FindClosestNode(self.startCoord)
   end
 
+  -- Find the closest node to the given start coordinate.
+  self.startNode, dist=self:FindClosestNode(self.startCoord)
+  
+  --self.startNode.coordinate:MarkToAll("Start Node")
+  
   -- Debug info.
   self:T(self.lid..string.format("START node ID=%d", self.startNode.id))
   
@@ -834,23 +838,26 @@ function ASTAR:FindEndNode()
   
   local pathline, dist, vec3, s=self:FindClosestPathline(self.endCoord)
   
-  if pathline and vec3 then
+  if pathline and vec3 and dist and dist>10 then
 
-    local Coordinate=COORDINATE:NewFromVec3(vec3)
+    -- Create a node on the closest pathline so we first go straight there and then along the pathline.
-    
+    local node=self:AddNodeFromCoordinate(COORDINATE:NewFromVec3(vec3))
-    local node=self:AddNodeFromCoordinate(Coordinate)
-    
-    node.pathline=pathline
         
+    -- We also need the point.
     local point=pathline:AddPointFromVec3(vec3, nil, s.p1)
     
+    -- Add pathline parameters to node.
+    node.pathline=pathline
     node.pathpoint=point
     
-    self.endNode=node
+    self:T2(self.lid..string.format("Added new node=%d, which is closest to END coord: dist=%.1f m", node.id, dist))
-  else
-    self.endNode, dist=self:FindClosestNode(self.endCoord, {self.startNode})
   end
   
+  -- Find closest node to the end coordinate (exclude the start coordinate.
+  self.endNode, dist=self:FindClosestNode(self.endCoord, {self.startNode})
+  
+  --self.endNode.coordinate:MarkToAll("End Node")
+  
   -- Debug info.
   self:T(self.lid..string.format("END node ID=%d", self.endNode.id))
   
@@ -913,22 +920,21 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
     -- Get current node.
     local current=self:_LowestFscore(openset, f_score)
     
-    env.info("FF current node="..current.id)
+    -- Debug
-    env.info("FF came_from")
+--    self:I("FF current node="..current.id)
-    for _neighbour,_current in pairs(came_from) do
+--    self:I("FF came_from")
-      local neighbour=_neighbour --#ASTAR.Node
+--    for _neighbour,_current in pairs(came_from) do
-      local current=_current --#ASTAR.Node
+--      local neighbour=_neighbour --#ASTAR.Node
-      env.info(string.format("neighbour=%d --> %d=current", neighbour.id, current.id))
+--      local current=_current --#ASTAR.Node
-    end
+--      self:I(string.format("neighbour=%d --> %d=current", neighbour.id, current.id))
-    --self:I(came_from)
+--    end
-    
+--    local text="Path:"
-    local text="Path:"
+--    local path=self:_UnwindPath({}, came_from, current)
-    local path=self:_UnwindPath({}, came_from, current)
+--    for i,_node in pairs(path) do
-    for i,_node in pairs(path) do
+--      local node=_node --#ASTAR.Node
-      local node=_node --#ASTAR.Node
+--      text=text..string.format("\n[%d] Node=%d", i, node.id)
-      text=text..string.format("\n[%d] Node=%d", i, node.id)
+--    end
-    end
+--    self:I(text)
-    env.info(text)
     
     -- Check if we are at the end node.
     if current.id==goal.id then
@@ -975,12 +981,12 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
     -- Get neighbour nodes.
     local neighbors=self:_NeighbourNodes(current, nodes)
     
-    local text=string.format("Current node UID=%d pathline=%s", current.id, current.pathline and current.pathline.name or "N/A")
+--    local text=string.format("Current node UID=%d pathline=%s", current.id, current.pathline and current.pathline.name or "N/A")
-    for _,_node in pairs(neighbors) do
+--    for _,_node in pairs(neighbors) do
-      local node=_node --#ASTAR.Node
+--      local node=_node --#ASTAR.Node
-      text=text..string.format("\nNeighbour node UID=%d pathline=%s", node.id, node.pathline and node.pathline.name or "N/A")
+--      text=text..string.format("\nNeighbour node UID=%d pathline=%s", node.id, node.pathline and node.pathline.name or "N/A")
-    end
+--    end
-    self:T(self.lid..text)
+--    self:T(self.lid..text)
     
     -- Loop over neighbours.
     for _,neighbor in pairs(neighbors) do
@@ -1034,10 +1040,11 @@ function ASTAR:GetPathline(ExcludeStartNode, ExcludeEndNode)
   
     pathline=PATHLINE:New("Astar")
   
-    for _,_note in pairs(nodes) do
+    for _,_node in pairs(nodes) do
-      local note=_note --#ASTAR.Node 
+      local node=_node --#ASTAR.Node 
       
-      local point=pathline:AddPointFromVec3(note.coordinate)
+      local point=pathline:AddPointFromVec3(node.coordinate)
+      point.name=node.pathline.name
           
     end
       
@@ -1075,10 +1082,6 @@ function ASTAR:GetPathlinesFromNodes(Nodes)
       
     end
 
-    -- Add pathline to table (if it is not already in).
-    --if not UTILS.IsAnyInTable(pathlines, {pathline}, "name") then
-      --env.info(string.format("Adding pathline %s", pathline.name, tostring(UTILS.IsAnyInTable(pathlines, pathline))))
-      
     -- We do not want to add the same pathline two times in a row.
     if #pathlines==0 or (#pathlines>0 and pathlines[#pathlines].name~=pathline.name) then      
       table.insert(pathlines, pathline)
@@ -1110,7 +1113,6 @@ function ASTAR:_HeuristicCost(nodeA, nodeB)
     return cost
   end
 
-  local cost=nil
   if self.CostFunc then
     cost=self.CostFunc(nodeA, nodeB, unpack(self.CostArg))
   else

Moose Development/Moose/Core/Pathline.lua

@@ -68,7 +68,8 @@ PATHLINE = {
 --- 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 #string mother Name of the pathline this point belongs to.
+-- @field #string name Name of this point.
 -- @field DCS#Vec3 vec3 3D position.
 -- @field DCS#Vec2 vec2 2D position.
 -- @field #number surfaceType Surface type.
@@ -240,11 +241,28 @@ end
 
 --- Get points of pathline. Not that points are tables, that contain more information as just the 2D or 3D position but also the surface type etc.
 -- @param #PATHLINE self
--- @return <#PATHLINE.Point> List of points.
+-- @return <Core.Pathline#PATHLINE.Point> List of points.
 function PATHLINE:GetPoints()  
   return self.points
 end
 
+--- Get segments of pathline.
+-- @param #PATHLINE self
+-- @return <Core.Pathline#PATHLINE.Segment> List of points.
+function PATHLINE:GetSetments()
+
+  local segments={}
+
+  for i=1,#self.points-1 do
+    local segment={} --#PATHLINE.Segment
+    segment.p1=self.points[i]
+    segment.p2=self.points[i+1]
+    table.insert(segments, segment)
+  end
+  
+  return segments
+end
+
 --- Get 3D points of pathline.
 -- @param #PATHLINE self
 -- @return <DCS#Vec3> List of DCS#Vec3 points.
@@ -557,7 +575,9 @@ function PATHLINE:_CreatePoint(Vec)
   self.counter=self.counter+1
   
   point.uid=self.counter
-  point.name=self.name
+  point.mother=self.name
+  
+  point.name=string.format("%s #%d", self.name, point.uid)
 
   if Vec.z then
     -- Given vec is 3D