A*

- Improved performance by caching cost and valid/invalid neighbours.

Moose Development/Moose/Core/Astar.lua

@@ -20,6 +20,7 @@
 -- @field #boolean Debug Debug mode. Messages to all about status.
 -- @field #string lid Class id string for output to DCS log file.
 -- @field #table nodes Table of nodes.
+-- @field #number counter Node counter.
 -- @field #ASTAR.Node startNode Start node.
 -- @field #ASTAR.Node endNode End node.
 -- @field Core.Point#COORDINATE startCoord Start coordinate.
@@ -136,12 +137,16 @@ ASTAR = {
   Debug          =   nil,
   lid            =   nil,
   nodes          =    {},
+  counter        =     1,
 }
 
 --- Node data.
 -- @type ASTAR.Node
+-- @field #number id Node id.
 -- @field Core.Point#COORDINATE coordinate Coordinate of the node.
 -- @field #number surfacetype Surface type.
+-- @field #table valid Cached valid/invalid nodes.
+-- @field #table cost Cached cost.
 
 --- ASTAR infinity.
 -- @field #number INF
@@ -149,7 +154,7 @@ ASTAR.INF=1/0
 
 --- ASTAR class version.
 -- @field #string version
-ASTAR.version="0.1.0"
+ASTAR.version="0.2.0"
 
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -- TODO list
@@ -211,6 +216,12 @@ function ASTAR:GetNodeFromCoordinate(Coordinate)
   
   node.coordinate=Coordinate
   node.surfacetype=Coordinate:GetSurfaceType()
+  node.id=self.counter
+  
+  node.valid={}
+  node.cost={}
+  
+  self.counter=self.counter+1
   
   return node
 end
@@ -518,6 +529,23 @@ function ASTAR.Dist3D(nodeA, nodeB)
   return nodeA.coordinate:Get3DDistance(nodeB.coordinate)
 end
 
+--- Heuristic cost is given by the distance between the nodes on road. 
+-- @param #ASTAR.Node nodeA First node.
+-- @param #ASTAR.Node nodeB Other node.
+-- @return #number Distance between the two nodes.
+function ASTAR.DistRoad(nodeA, nodeB)
+
+  local path,dist,gotpath=nodeA.coordinate:GetPathOnRoad(nodeB.coordinate,IncludeEndpoints,Railroad,MarkPath,SmokePath)
+  
+  if gotpath then
+    return dist
+  else
+    return math.huge
+  end
+
+  return nodeA.coordinate:Get3DDistance(nodeB.coordinate)
+end
+
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 -- Misc functions
 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -616,13 +644,15 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
   local text=string.format("Starting A* pathfinding")
   self:I(self.lid..text)
   MESSAGE:New(text, 10, "ASTAR"):ToAllIf(self.Debug)
+  
+  local Tstart=UTILS.GetOSTime()
 
   while #openset > 0 do
   
     local current=self:_LowestFscore(openset, f_score)
     
     -- Check if we are at the end node.
-    if current==goal then
+    if current.id==goal.id then
     
       local path=self:_UnwindPath({}, came_from, goal)
       
@@ -634,8 +664,20 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
         table.remove(path, 1)
       end
       
+      local Tstop=UTILS.GetOSTime()
+      
+      local dT=nil
+      if Tstart and Tstop then
+        dT=Tstop-Tstart
+      end
+      
       -- Debug message.
-      local text=string.format("Found path with %d nodes", #path)
+      local text=string.format("Found path with %d nodes (%d total nodes)", #path, #self.nodes)
+      if dT then
+        text=text..string.format(". OS Time %.6f seconds", dT)
+      end
+      text=text..string.format("\nNvalid = %d  %d cached", self.nvalid, self.nvalidcache)
+      text=text..string.format("\nNcost  = %d  %d cached", self.ncost, self.ncostcache)
       self:I(self.lid..text)
       MESSAGE:New(text, 60, "ASTAR"):ToAllIf(self.Debug)
       
@@ -689,11 +731,34 @@ end
 -- @return #number "Cost" to go from node A to node B.
 function ASTAR:_HeuristicCost(nodeA, nodeB)
 
-  if self.CostFunc then
+  if self.ncost then
-    return self.CostFunc(nodeA, nodeB, unpack(self.CostArg))
+    self.ncost=self.ncost+1
   else
-    return self:_DistNodes(nodeA, nodeB)
+    self.ncost=1
   end
+
+  -- Get chached cost if available.
+  local cost=nodeA.cost[nodeB.id]
+  if cost~=nil then
+    if self.ncostcache then
+      self.ncostcache=self.ncostcache+1
+    else
+      self.ncostcache=1
+    end
+    return cost
+  end
+
+  local cost=nil
+  if self.CostFunc then
+    cost=self.CostFunc(nodeA, nodeB, unpack(self.CostArg))
+  else
+    cost=self:_DistNodes(nodeA, nodeB)
+  end
+  
+  nodeA.cost[nodeB.id]=cost
+  nodeB.cost[nodeA.id]=cost  -- Symmetric problem. 
+  
+  return cost
 end
 
 --- Check if going from a node to a neighbour is possible.
@@ -703,14 +768,34 @@ end
 -- @return #boolean If true, transition between nodes is possible.
 function ASTAR:_IsValidNeighbour(node, neighbor)
 
-  if self.ValidNeighbourFunc then
+  if self.nvalid then
-  
+    self.nvalid=self.nvalid+1
-    return self.ValidNeighbourFunc(node, neighbor, unpack(self.ValidNeighbourArg))
-  
   else
-    return true
+    self.nvalid=1
+  end
+  
+  local valid=node.valid[neighbor.id]
+  if valid~=nil then
+    --env.info(string.format("Node %d has valid=%s neighbour %d", node.id, tostring(valid), neighbor.id))
+    if self.nvalidcache then
+      self.nvalidcache=self.nvalidcache+1
+    else
+      self.nvalidcache=1
+    end
+    return valid
   end
 
+  local valid=nil
+  if self.ValidNeighbourFunc then
+    valid=self.ValidNeighbourFunc(node, neighbor, unpack(self.ValidNeighbourArg))  
+  else
+    valid=true
+  end
+
+  node.valid[neighbor.id]=valid
+  neighbor.valid[node.id]=valid  -- Symmetric problem. 
+
+  return valid
 end
 
 --- Calculate 2D distance between two nodes.
@@ -727,7 +812,7 @@ end
 -- @param #table set The set of nodes.
 -- @param #number f_score F score.
 -- @return #ASTAR.Node Best node.
-function ASTAR:_LowestFscore(set, f_score)
+function ASTAR:_LowestFscore2(set, f_score)
 
   local lowest, bestNode = ASTAR.INF, nil
   
@@ -743,6 +828,25 @@ function ASTAR:_LowestFscore(set, f_score)
   return bestNode
 end
 
+--- Function that calculates the lowest F score.
+-- @param #ASTAR self
+-- @param #table set The set of nodes.
+-- @param #number f_score F score.
+-- @return #ASTAR.Node Best node.
+function ASTAR:_LowestFscore(set, f_score)
+
+  local function sort(A, B)
+    local a=A --#ASTAR.Node
+    local b=B --#ASTAR.Node
+    return f_score[a]<f_score[b]
+  end
+
+  table.sort(set, sort)
+  
+  return set[1]
+end
+
+
 --- Function to get valid neighbours of a node.
 -- @param #ASTAR self
 -- @param #ASTAR.Node theNode The node.
@@ -751,9 +855,9 @@ end
 function ASTAR:_NeighbourNodes(theNode, nodes)
 
   local neighbors = {}
-  for _, node in ipairs ( nodes ) do
+  for _, node in pairs ( nodes ) do
   
-    if theNode~=node then
+    if theNode.id~=node.id then
     
       local isvalid=self:_IsValidNeighbour(theNode, node)
     
@@ -775,8 +879,8 @@ end
 -- @return #boolean If true, the node is not in the set.
 function ASTAR:_NotIn(set, theNode)
 
-  for _, node in ipairs ( set ) do
+  for _, node in pairs ( set ) do
-    if node == theNode then
+    if node.id == theNode.id then
       return false
     end
   end
@@ -790,8 +894,9 @@ end
 -- @param #ASTAR.Node theNode The node to check.
 function ASTAR:_RemoveNode(set, theNode)
 
-  for i, node in ipairs ( set ) do
+  for i, node in pairs ( set ) do
-    if node == theNode then 
+    if node.id == theNode.id then
+      --table.remove(set, i)
       set [ i ] = set [ #set ]
       set [ #set ] = nil
       break

Moose Development/Moose/Core/Point.lua

@@ -1524,27 +1524,8 @@ do -- COORDINATE
         local coord=COORDINATE:NewFromVec2(_vec2)
         
         Path[#Path+1]=coord
-        
-        if MarkPath then
-          coord:MarkToAll(string.format("Path segment %d.", _i))
-        end
-        if SmokePath then
-          coord:SmokeGreen()
-        end
       end
-            
+                              
-      -- Mark/smoke endpoints
-      if IncludeEndpoints then
-        if MarkPath then
-          COORDINATE:NewFromVec2(path[1]):MarkToAll("Path Initinal Point")
-          COORDINATE:NewFromVec2(path[1]):MarkToAll("Path Final Point")        
-        end
-        if SmokePath then
-          COORDINATE:NewFromVec2(path[1]):SmokeBlue()
-          COORDINATE:NewFromVec2(path[#path]):SmokeBlue()
-        end
-      end
-            
     else
       self:E("Path is nil. No valid path on road could be found.")
       GotPath=false
@@ -1555,6 +1536,23 @@ do -- COORDINATE
       Path[#Path+1]=ToCoord
     end
     
+    -- Mark or smoke.
+    if MarkPath or SmokePath then
+      for i,c in pairs(Path) do
+        local coord=c --#COORDINATE
+        if MarkPath then
+          coord:MarkToAll(string.format("Path segment %d", i))
+        end
+        if SmokePath then
+          if i==1 or i==#Path then          
+            coord:SmokeBlue()
+          else
+            coord:SmokeGreen()
+          end
+        end
+      end
+    end
+    
     -- Sum up distances.
     if #Path>=2 then
       for i=1,#Path-1 do
@@ -1562,7 +1560,7 @@ do -- COORDINATE
       end
     else
       -- There are cases where no path on road can be found.
-      return nil,nil
+      return nil,nil,false
     end 
         
     return Path, Way, GotPath

Moose Development/Moose/Ops/NavyGroup.lua

@@ -1535,12 +1535,6 @@ function NAVYGROUP:_FindPathToNextWaypoint()
       
       for i,_node in ipairs(path) do
         local node=_node --Core.Astar#ASTAR.Node
-                
-        -- Waypoint index.
-        local wpindex=self:GetWaypointIndexCurrent()+i
-        
-        -- ID of current waypoint.
-        local uid=self:GetWaypointCurrent().uid
           
         -- Add waypoints along detour path to next waypoint.
         local wp=self:AddWaypoint(node.coordinate, speed, uid)

Moose Development/Moose/Utilities/Utils.lua

@@ -1400,4 +1400,14 @@ function UTILS.GetSunset(Day, Month, Year, Latitude, Longitude, Tlocal)
   local DayOfYear=UTILS.GetDayOfYear(Year, Month, Day)
 
   return UTILS.GetSunRiseAndSet(DayOfYear, Latitude, Longitude, false, Tlocal)
+end
+
+--- Get OS time. Needs os to be desanitized!
+-- @return #number Os time in seconds.
+function UTILS.GetOSTime()
+  if os then
+    return os.clock()
+  end
+
+  return nil
 end