diff --git a/Moose Development/Moose/Core/Astar.lua b/Moose Development/Moose/Core/Astar.lua index 51de732d8..443ea1387 100644 --- a/Moose Development/Moose/Core/Astar.lua +++ b/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. @@ -618,28 +625,22 @@ function ASTAR.Pathline(nodeA, nodeB, distmax) end else - --- local dist=nodeA.coordinate:Get2DDistance(nodeB.coordinate) --- if dist 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) - if dist 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,23 +800,24 @@ function ASTAR:FindStartNode() -- Find the closest pathline to the local pathline, dist, vec3, s=self:FindClosestPathline(self.startCoord) - if pathline and vec3 then - - local Coordinate=COORDINATE:NewFromVec3(vec3) - - local node=self:AddNodeFromCoordinate(Coordinate) - - node.pathline=pathline + if pathline and vec3 and dist and dist>10 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)) + -- We also need the pathline point. local point=pathline:AddPointFromVec3(vec3, nil, s.p1) - + + node.pathline=pathline node.pathpoint=point - self.startNode=node - else - -- Find the closest node to the given start coordinate. - self.startNode, dist=self:FindClosestNode(self.startCoord) + self:T2(self.lid..string.format("Added new node=%d, which is closest to start coord. dist=%.1f m", node.id, dist)) 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 - - local Coordinate=COORDINATE:NewFromVec3(vec3) - - local node=self:AddNodeFromCoordinate(Coordinate) - - node.pathline=pathline - + if pathline and vec3 and dist and dist>10 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)) + + -- 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 - else - self.endNode, dist=self:FindClosestNode(self.endCoord, {self.startNode}) + self:T2(self.lid..string.format("Added new node=%d, which is closest to END coord: dist=%.1f m", node.id, dist)) 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) - env.info("FF came_from") - for _neighbour,_current in pairs(came_from) do - local neighbour=_neighbour --#ASTAR.Node - local current=_current --#ASTAR.Node - env.info(string.format("neighbour=%d --> %d=current", neighbour.id, current.id)) - end - --self:I(came_from) - - local text="Path:" - local path=self:_UnwindPath({}, came_from, current) - for i,_node in pairs(path) do - local node=_node --#ASTAR.Node - text=text..string.format("\n[%d] Node=%d", i, node.id) - end - env.info(text) + -- Debug +-- self:I("FF current node="..current.id) +-- self:I("FF came_from") +-- for _neighbour,_current in pairs(came_from) do +-- local neighbour=_neighbour --#ASTAR.Node +-- local current=_current --#ASTAR.Node +-- self:I(string.format("neighbour=%d --> %d=current", neighbour.id, current.id)) +-- end +-- local text="Path:" +-- local path=self:_UnwindPath({}, came_from, current) +-- for i,_node in pairs(path) do +-- local node=_node --#ASTAR.Node +-- text=text..string.format("\n[%d] Node=%d", i, node.id) +-- end +-- self:I(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") - for _,_node in pairs(neighbors) do - 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") - end - self:T(self.lid..text) +-- 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 +-- 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") +-- end +-- 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 - local note=_note --#ASTAR.Node + for _,_node in pairs(nodes) do + local node=_node --#ASTAR.Node - local point=pathline:AddPointFromVec3(note.coordinate) + local point=pathline:AddPointFromVec3(node.coordinate) + point.name=node.pathline.name end @@ -1074,11 +1081,7 @@ function ASTAR:GetPathlinesFromNodes(Nodes) end 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 diff --git a/Moose Development/Moose/Core/Pathline.lua b/Moose Development/Moose/Core/Pathline.lua index 66e18c3c4..5254e759d 100644 --- a/Moose Development/Moose/Core/Pathline.lua +++ b/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 List of points. function PATHLINE:GetPoints() return self.points end +--- Get segments of pathline. +-- @param #PATHLINE self +-- @return 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 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