mirror of
https://github.com/FlightControl-Master/MOOSE.git
synced 2025-10-29 16:58:06 +00:00
ASTAR & PATHLINE
This commit is contained in:
Frank
parent
21e5c124c0
commit
47c0006537
@ -157,6 +157,8 @@ ASTAR = {
|
||||
-- @field #number surfacetype Surface type.
|
||||
-- @field #table valid Cached valid/invalid nodes.
|
||||
-- @field #table cost Cached cost.
|
||||
-- @field Core.Pathline#PATHLINE pathline Pathline that node is part of.
|
||||
-- @field Core.Pathline#PATHLINE.Point pathpoint Pathline point.
|
||||
|
||||
--- ASTAR infinity.
|
||||
-- @field #number INF
|
||||
@ -264,21 +266,24 @@ end
|
||||
|
||||
--- Adds nodes to the table of grid nodes from a PATHLINE.
|
||||
-- @param #ASTAR self
|
||||
-- @param #string PathlineName Name of pathline. Has to exist.
|
||||
-- @param Core.Pathline#PATHLINE Pathline Pathline or name of pathline. Has to exist.
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:AddNodeFromPathlineName(PathlineName)
|
||||
function ASTAR:AddNodeFromPathlineName(Pathline)
|
||||
|
||||
local pathline=PATHLINE:FindByName(PathlineName)
|
||||
if type(Pathline)=="string" then
|
||||
Pathline=PATHLINE:FindByName(Pathline)
|
||||
end
|
||||
|
||||
if pathline then
|
||||
if Pathline then
|
||||
|
||||
local coords=pathline:GetCoordinats()
|
||||
|
||||
for _,_coord in pairs(coords) do
|
||||
local c=_coord --Core.Point#COORDINATE
|
||||
env.info("FF 100")
|
||||
self:AddNodeFromCoordinate(c)
|
||||
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)
|
||||
node.pathline=Pathline
|
||||
node.pathpoint=point
|
||||
end
|
||||
|
||||
else
|
||||
env.error("FF error pathline")
|
||||
end
|
||||
@ -364,6 +369,18 @@ function ASTAR:SetValidNeighbourRoad(MaxDistance)
|
||||
return self
|
||||
end
|
||||
|
||||
--- Set valid neighbours to be on the same pathline or not further apart than 10 meters.
|
||||
-- @param #ASTAR self
|
||||
-- @param #number MaxDistance Max distance between nodes in meters. Default is 2000 m.
|
||||
-- @return #ASTAR self
|
||||
function ASTAR:SetValidNeighbourPathline(MaxDistance)
|
||||
|
||||
self:SetValidNeighbourFunction(ASTAR.Pathline, MaxDistance)
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
|
||||
--- Set the function which calculates the "cost" to go from one to another node.
|
||||
-- The first to arguments of this function are always the two nodes under consideration. But you can add optional arguments.
|
||||
-- Very often the distance between nodes is a good measure for the cost.
|
||||
@ -568,6 +585,37 @@ function ASTAR.Road(nodeA, nodeB)
|
||||
|
||||
end
|
||||
|
||||
--- Function to check if two nodes are on the same pathline or if nodes are less than 10 meters apart.
|
||||
-- @param #ASTAR.Node nodeA First node.
|
||||
-- @param #ASTAR.Node nodeB Other node.
|
||||
-- @param #number distmax Max distance in meters. Default is 10 m.
|
||||
-- @return #boolean If true, two nodes are connected.
|
||||
function ASTAR.Pathline(nodeA, nodeB, distmax)
|
||||
|
||||
distmax=distmax or 10
|
||||
|
||||
if nodeA.pathpoint.name==nodeB.pathpoint.name then
|
||||
|
||||
local pathline=nodeA.pathline
|
||||
|
||||
local idxA=pathline:_GetPointIndex(nodeA.pathpoint)
|
||||
local idxB=pathline:_GetPointIndex(nodeB.pathpoint)
|
||||
|
||||
if math.abs(idxA-idxB)<=1 then
|
||||
return true
|
||||
end
|
||||
|
||||
else
|
||||
local dist=nodeA.coordinate:Get2DDistance(nodeB.coordinate)
|
||||
if dist<distmax then
|
||||
return true
|
||||
end
|
||||
end
|
||||
|
||||
return false
|
||||
end
|
||||
|
||||
|
||||
--- Function to check if distance between two nodes is less than a threshold distance.
|
||||
-- @param #ASTAR.Node nodeA First node.
|
||||
-- @param #ASTAR.Node nodeB Other node.
|
||||
@ -591,7 +639,7 @@ end
|
||||
-- @param #ASTAR.Node nodeB Other node.
|
||||
-- @return #number Distance between the two nodes.
|
||||
function ASTAR.Dist2D(nodeA, nodeB)
|
||||
local dist=nodeA.coordinate:Get2DDistance(nodeB)
|
||||
local dist=nodeA.coordinate:Get2DDistance(nodeB.coordinate)
|
||||
return dist
|
||||
end
|
||||
|
||||
@ -744,6 +792,9 @@ function ASTAR:GetPath(ExcludeStartNode, ExcludeEndNode)
|
||||
-- Get current node.
|
||||
local current=self:_LowestFscore(openset, f_score)
|
||||
|
||||
self:I(current)
|
||||
self:I(goal)
|
||||
|
||||
-- Check if we are at the end node.
|
||||
if current.id==goal.id then
|
||||
|
||||
@ -871,6 +922,8 @@ function ASTAR:_HeuristicCost(nodeA, nodeB)
|
||||
cost=self:_DistNodes(nodeA, nodeB)
|
||||
end
|
||||
|
||||
self:T(self.lid..string.format("Cost nodeA=%d --> nodeB=%d = %.1f", nodeA.id, nodeB.id, cost))
|
||||
|
||||
nodeA.cost[nodeB.id]=cost
|
||||
nodeB.cost[nodeA.id]=cost -- Symmetric problem.
|
||||
|
||||
|
||||
@ -21,6 +21,7 @@
|
||||
-- @field #string lid Class id string for output to DCS log file.
|
||||
-- @field #string name Name of the path line.
|
||||
-- @field #table points List of 3D points defining the path.
|
||||
-- @field #number counter Running number counting the point IDs.
|
||||
-- @extends Core.Base#BASE
|
||||
|
||||
--- *The shortest distance between two points is a straight line.* -- Archimedes
|
||||
@ -59,10 +60,13 @@ PATHLINE = {
|
||||
ClassName = "PATHLINE",
|
||||
lid = nil,
|
||||
points = {},
|
||||
counter = 0,
|
||||
}
|
||||
|
||||
--- 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 DCS#Vec3 vec3 3D position.
|
||||
-- @field DCS#Vec2 vec2 2D position.
|
||||
-- @field #number surfaceType Surface type.
|
||||
@ -73,7 +77,7 @@ PATHLINE = {
|
||||
|
||||
--- PATHLINE class version.
|
||||
-- @field #string version
|
||||
PATHLINE.version="0.1.0"
|
||||
PATHLINE.version="0.2.0"
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-- TODO list
|
||||
@ -312,7 +316,7 @@ function PATHLINE:MarkPoints(Switch)
|
||||
if Switch==false then
|
||||
|
||||
if point.markerID then
|
||||
UTILS.RemoveMark(point.markerID, Delay)
|
||||
UTILS.RemoveMark(point.markerID)
|
||||
end
|
||||
|
||||
else
|
||||
@ -323,7 +327,7 @@ function PATHLINE:MarkPoints(Switch)
|
||||
|
||||
point.markerID=UTILS.GetMarkID()
|
||||
|
||||
local text=string.format("Pathline %s: Point #%d\nSurface Type=%d\nHeight=%.1f m\nDepth=%.1f m", self.name, i, point.surfaceType, point.landHeight, point.depth)
|
||||
local text=string.format("Pathline %s: Point #%d [UID=%d]\nSurface Type=%d\nHeight=%.1f m\nDepth=%.1f m", self.name, i, point.uid, point.surfaceType, point.landHeight, point.depth)
|
||||
|
||||
trigger.action.markToAll(point.markerID, text, point.vec3, "")
|
||||
|
||||
@ -344,6 +348,11 @@ function PATHLINE:_CreatePoint(Vec)
|
||||
|
||||
local point={} --#PATHLINE.Point
|
||||
|
||||
self.counter=self.counter+1
|
||||
|
||||
point.uid=self.counter
|
||||
point.name=self.name
|
||||
|
||||
if Vec.z then
|
||||
-- Given vec is 3D
|
||||
point.vec3=UTILS.DeepCopy(Vec)
|
||||
@ -365,6 +374,23 @@ function PATHLINE:_CreatePoint(Vec)
|
||||
return point
|
||||
end
|
||||
|
||||
--- Get index of point in the lua table.
|
||||
-- @param #PATHLINE self
|
||||
-- @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
|
||||
|
||||
if point.uid==Point.uid then
|
||||
return i
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
return nil
|
||||
end
|
||||
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user