ASTAR Pathlines

Moose Development/Moose/Core/Astar.lua

@@ -778,9 +778,7 @@ function ASTAR:FindClosestPathline(Coordinate)
         S=s        
       end
     
-    
     end    
-    
   end
   
   if pathline then
@@ -800,7 +798,10 @@ function ASTAR:FindStartNode()
   -- Find the closest pathline to the 
   local pathline, dist, vec3, s=self:FindClosestPathline(self.startCoord)
 
-  if pathline and vec3 and dist and dist>10 then
+  -- Find the closest node to the given start coordinate.
+  local node, dist2=self:FindClosestNode(self.startCoord)
+  
+  if pathline and vec3 and dist and dist2>dist then
 
     -- 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))
@@ -815,7 +816,7 @@ function ASTAR:FindStartNode()
   end
 
   -- Find the closest node to the given start coordinate.
-  self.startNode, dist=self:FindClosestNode(self.startCoord)
+  self.startNode, dist2=self:FindClosestNode(self.startCoord)
   
   --self.startNode.coordinate:MarkToAll("Start Node")
   
@@ -838,7 +839,10 @@ function ASTAR:FindEndNode()
   
   local pathline, dist, vec3, s=self:FindClosestPathline(self.endCoord)
   
-  if pathline and vec3 and dist and dist>10 then
+  -- Find the closest node to the given start coordinate.
+  local node, dist2=self:FindClosestNode(self.endCoord)  
+  
+  if pathline and vec3 and dist and dist2>dist then
 
     -- 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))

Moose Development/Moose/Utilities/Utils.lua

@@ -1413,6 +1413,24 @@ function UTILS.VecAngle(a, b)
   return math.deg(alpha)
 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.
+-- @return #number Angle alpha between and b in degrees.
+function UTILS.VecAngleSigned(a, b)
+
+      local a=UTILS.VecSubstract(s1.p2.vec3, s1.p1.vec3)
+      
+      local b=UTILS.VecSubstract(s2.p2.vec3, s2.p1.vec3)
+      
+      local h1=UTILS.VecHdg(a)
+      local h2=UTILS.VecHdg(b)
+      
+      local angle=h1-h2 --UTILS.VecAngle(a, b)
+
+
+end
+
 --- Calculate "heading" of a 3D vector in the X-Z plane.
 -- @param DCS#Vec3 a Vector in 3D with x, y, z components.
 -- @return #number Heading in degrees in [0,360).
@@ -1435,6 +1453,20 @@ function UTILS.Vec2Hdg(a)
   return h
 end
 
+--- Calculate the difference between two "heading", i.e. angles in [0,360) deg.
+-- @param DCS#Vec3 a Vector a.
+-- @param DCS#Vec3 a Vector b.
+-- @return #number Heading difference in degrees.
+function UTILS.VecHdgDiff(a, b)
+
+  local ha=math.deg(math.atan2(a.z, a.x))
+  local hb=math.deg(math.atan2(b.z, b.x))
+  
+  local angle=ha-hb
+
+  return angle
+end
+
 --- Calculate the difference between two "heading", i.e. angles in [0,360) deg.
 -- @param #number h1 Heading one.
 -- @param #number h2 Heading two.