FlightControl-Master/MOOSE
1
0
Fork 0
mirror of https://github.com/FlightControl-Master/MOOSE.git synced 2025-10-29 16:58:06 +00:00
Code Issues Packages Projects Releases Wiki Activity

ASTAR Pathlines

Browse Source
This commit is contained in:
Frank 2023-09-14 17:40:02 +02:00
parent 39f626390a
commit b03905154d
2 changed files with 42 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Moose Development/Moose/Core/Astar.lua
View File

@ -778,9 +778,7 @@ function ASTAR:FindClosestPathline(Coordinate)
S=s
end
end
end
end
if pathline then
@ -799,8 +797,11 @@ function ASTAR:FindStartNode()
-- Find the closest pathline to the
local pathline, dist, vec3, s=self:FindClosestPathline(self.startCoord)
-- Find the closest node to the given start coordinate.
local node, dist2=self:FindClosestNode(self.startCoord)
if pathline and vec3 and dist and dist>10 then
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))

32
Moose Development/Moose/Utilities/Utils.lua
View File

@ -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.