Astar Pathline

Moose Development/Moose/Core/Astar.lua

@@ -713,6 +713,43 @@ function ASTAR:FindClosestNode(Coordinate, ExcludeNodes)
   return closeNode, distMin
 end
 
+--- Find the closest pathline to a given reference coordinate.
+-- @param #ASTAR self
+-- @param Core.Point#COORDINATE Coordinate Reference coordinate.
+-- @return Core.Pathline#PATHLINE Closest pathline
+-- @return #number Distance in meters.
+-- @return DCS#Vec3 Closest point on pathline to the ref coordinate.
+-- @return Core.Pathline#PATHLINE.Segment Segment.
+function ASTAR:FindClosestPathline(Coordinate)
+
+  local pathline=nil --Core.Pathline#PATHLINE
+  local dist=math.huge
+  local vec3=nil
+  local S=nil
+  
+  for _,_node in pairs(self.nodes) do
+    local node=_node --#ASTAR.Node
+    
+    if node.pathline then
+    
+      local vec, d, s=node.pathline:GetClosestPoint3D(Coordinate)
+      
+      if d<dist then
+        pathline=node.pathline
+        dist=d
+        vec3=vec
+        S=s        
+      end
+    
+    
+    end
+    
+  end
+
+  return pathline, dist, vec3, S
+end
+
+
 --- Find the start node.
 -- @param #ASTAR self
 -- @return #ASTAR self
@@ -722,10 +759,20 @@ function ASTAR:FindStartNode()
   
   self.startNode=node
   
-  if dist>1000 then
-    self:T(self.lid.."Adding start node to node grid!")
-    self:AddNode(node)
+  local pathline, dist, vec3, s=self:FindClosestPathline(self.startCoord)
+  if pathline and vec3 then
+    local Coordinate=COORDINATE:NewFromVec3(vec3)
+    node=self:AddNodeFromCoordinate(Coordinate)
+    node.pathline=pathline
+    local point=pathline:AddPointFromVec3(vec3, nil, s.p1)
+    node.pathpoint=point
   end
+  
+  -- Not sure why I did this. The node does not need to be added again as it is already contained in self.nodes!
+--  if dist>1000 then
+--    self:T(self.lid.."Adding start node to node grid!")
+--    self:AddNode(node)
+--  end
     
   return self
 end
@@ -739,10 +786,18 @@ function ASTAR:FindEndNode()
 
   self.endNode=node
   
-  if dist>1000 then
-    self:T(self.lid.."Adding end node to node grid!")
-    self:AddNode(node)
+  local pathline, dist, vec3=self:FindClosestPathline(self.startCoord)
+  if pathline and vec3 then
+    local Coordinate=COORDINATE:NewFromVec3(vec3)
+    self:AddNodeFromCoordinate(Coordinate)
   end
+  
+  
+  -- Not sure why I did this. The node does not need to be added again as it is already contained in self.nodes!
+--  if dist>1000 then
+--    self:T(self.lid.."Adding end node to node grid!")
+--    self:AddNode(node)
+--  end
     
   return self
 end

Moose Development/Moose/Core/Pathline.lua

@@ -77,6 +77,11 @@ PATHLINE = {
 -- @field #number markerID Marker ID.
 -- @field #number lineID Marker of pathline ID.
 
+--- Segment of line.
+-- @type PATHLINE.Segment
+-- @field #PATHLINE.Point p1 First point.
+-- @field #PATHLINE.Point p2 Second point.
+
 
 --- PATHLINE class version.
 -- @field #string version
@@ -159,17 +164,28 @@ end
 -- @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.
+-- @param #PATHLINE.Point Point Add point after given point. Default is at the end or at given index.
 -- @return #PATHLINE self
-function PATHLINE:AddPointFromVec2(Vec2, Index)
+function PATHLINE:AddPointFromVec2(Vec2, Index, Point)
 
   if Vec2 then
   
+    -- Create a new point.
     local point=self:_CreatePoint(Vec2)
 
     if Index then
+      -- Add at given index.
       table.insert(self.points, Index, point)
     else
-      table.insert(self.points, point)
+      if Point then
+        -- Get index of given point.
+        local i=self:_GetPointIndex(Point)
+        -- Add new point after given point.
+        table.insert(self.points, i+1, point)
+      else
+        -- Add add the end.
+        table.insert(self.points, point)
+      end
     end 
   end
   
@@ -180,22 +196,31 @@ end
 -- @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, Index)
+-- @param #PATHLINE.Point Point Add point after given point. Default is at the end or at given index.
+-- @return #PATHLINE.Point Point that was added.
+function PATHLINE:AddPointFromVec3(Vec3, Index, Point)
 
   if Vec3 then
   
     local point=self:_CreatePoint(Vec3)
 
     if Index then
+      -- Add add given index.
       table.insert(self.points, Index, point)
     else
-      table.insert(self.points, point)
+      if Point then
+        local i=self:_GetPointIndex(Point)
+        table.insert(self.points, i+1, point)
+      else
+        -- Add add the end.
+        table.insert(self.points, point)
+      end
     end
-    
+  
+    return point  
   end
   
-  return self
+  return nil
 end
 
 --- Get name of pathline.
@@ -380,7 +405,6 @@ function PATHLINE:Draw(Switch, Coalition, Color, LineType)
 
     for i=2,#self.points do
       
-    
       local p1=self.points[i-1] --#PATHLINE.Point
       local p2=self.points[i]   --#PATHLINE.Point
       
@@ -404,11 +428,12 @@ end
 -- @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.
+-- @return #PATHLINE.Segment Closest segment of ref point.
 function PATHLINE:GetClosestPoint2D(Vec2)
 
-
   local P=nil --DCS#Vec2  
   local D=math.huge
+  local S={} --#PATHLINE.Segment
   
   for i=2,#self.points do
 
@@ -448,26 +473,26 @@ function PATHLINE:GetClosestPoint2D(Vec2)
     if d<=D then
       D=d
       P=p
+      S.p1=A
+      S.p2=B
     end
     
   end
   
-    
-  --local c=COORDINATE:NewFromVec2(P)  
-  --c:MarkToAll(string.format("Point D=%.1f m", D))
-
-  return P, D
+  return P, D, S
 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.
+-- @param DCS#Vec3 Vec3 Reference Point in 3D. Can also be a `COORDINATE`.
+-- @return DCS#Vec3 Closest point on pathline.
 -- @return #number Distance from closest point to ref point in meters.
+-- @return #PATHLINE.Segment Closest segment of ref point.
 function PATHLINE:GetClosestPoint3D(Vec3)
 
   local P=nil --DCS#Vec3
   local D=math.huge
+  local S={} --#PATHLINE.Segment
   
   for i=2,#self.points do
 
@@ -507,18 +532,15 @@ function PATHLINE:GetClosestPoint3D(Vec3)
     if d<=D then
       D=d
       P=p
+      S.p1=A
+      S.p2=B
     end
     
   end
-  
-    
-  --local c=COORDINATE:NewFromVec2(P)  
-  --c:MarkToAll(string.format("Point D=%.1f m", D))
-
-  return P, D
+ 
+  return P, D, S
 end
 
-
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -- Private functions
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -559,12 +581,12 @@ end
 
 --- Get index of point in the lua table.
 -- @param #PATHLINE self
--- @param #Pathline.Point Point Given point.
+-- @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
+    local point=_point --#PATHLINE.Point
    
     if point.uid==Point.uid then
       return i