From 1f630ab4909a5f8c2f4bc2a463729d6f74653fc0 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 5 Sep 2023 23:29:27 +0200 Subject: [PATCH] Astar Pathline --- Moose Development/Moose/Core/Astar.lua | 67 +++++++++++++++++++-- Moose Development/Moose/Core/Pathline.lua | 72 +++++++++++++++-------- 2 files changed, 108 insertions(+), 31 deletions(-) diff --git a/Moose Development/Moose/Core/Astar.lua b/Moose Development/Moose/Core/Astar.lua index 1b43bea8a..3cb3fe920 100644 --- a/Moose Development/Moose/Core/Astar.lua +++ b/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 d1000 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 diff --git a/Moose Development/Moose/Core/Pathline.lua b/Moose Development/Moose/Core/Pathline.lua index 435073612..6b9d8456a 100644 --- a/Moose Development/Moose/Core/Pathline.lua +++ b/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