Temporary waypoint once again takes direction from next waypoint. Way…

…points above or below once again gets a locked direction.

src/flight/FlightCore.lua

@@ -109,6 +109,12 @@ function FlightCore.New(routeController, flightFSM)
 
         previousWaypoint = currentWaypoint
         currentWaypoint = FlightCore.CreateWPFromPoint(nextPoint, route.LastPointReached())
+
+        -- When the next waypoint is nearly above or below us, lock yaw
+        local dir = (currentWaypoint.Destination() - previousWaypoint.Destination()):NormalizeInPlace()
+        if abs(dir:Dot(plane.Up())) > 0.9 then
+            currentWaypoint.LockYawTo(plane.Forward(), false)
+        end
     end
 
     function s.WaitForGate()

src/flight/Waypoint.lua

@@ -34,6 +34,7 @@ WPReachMode = {
 ---@field SetLastInRoute fun(lastInRoute:boolean)
 ---@field WithinMargin fun(mode:WPReachMode):boolean
 ---@field Yaw fun(prev:Waypoint):Vec3|nil
+---@field LockedYawDirection fun():Vec3|nil
 
 
 local Waypoint = {}
@@ -147,6 +148,10 @@ function Waypoint.New(destination, finalSpeed, maxSpeed, margin)
         return yawLockDir ~= nil
     end
 
+    function s.LockedYawDirection()
+        return yawLockDir
+    end
+
     ---Gets the vertical up reference to use
     ---@param prev Waypoint Previous waypoint
     ---@return Vec3
@@ -204,17 +209,12 @@ function Waypoint.New(destination, finalSpeed, maxSpeed, margin)
 
         if yawLockDir then
             dir = yawLockDir
+        elseif s.DistanceTo() > 50 then
+            -- Point towards the next point. Use the previous point as a reference when we get close to prevent spinning.
+            dir = s.DirectionTo()
         else
-            -- When the next waypoint is nearly above or below us, lock yaw
-            local dirSelf = (s.Destination() - prev.Destination()):NormalizeInPlace()
-            if abs(dirSelf:Dot(plane.Up())) > 0.9 then -- <= calc.AngleToDot(25) then
-                dir = plane.Forward()
-            elseif s.DistanceTo() > 100 then           -- Point towards the next point. Use the previous point as a reference when we get close to prevent spinning.
-                dir = s.DirectionTo()
-            else
-                dir = s.Destination() - prev.Destination()
-                dir:NormalizeInPlace()
-            end
+            dir = s.Destination() - prev.Destination()
+            dir:NormalizeInPlace()
         end
 
         dir = dir:ProjectOnPlane(s.GetVerticalUpReference(prev))

src/flight/state/ReturnToPath.lua

@@ -1,4 +1,5 @@
 local Waypoint = require("flight/Waypoint")
+local Plane = require("math/Plane")
 
 ---@class ReturnToPath
 ---@field New fun(fsm:FlightFSM, returnPoint:Vec3):FlightState
@@ -35,6 +36,7 @@ function ReturnToPath.New(fsm, returnPoint)
     function s.Flush(deltaTime, next, previous, nearestPointOnPath)
         if not temporaryWP then
             temporaryWP = Waypoint.New(returnPoint, 0, 0, next.Margin())
+            temporaryWP.LockYawTo(next.LockedYawDirection() or Plane.NewByVertialReference().Forward())
             fsm.SetTemporaryWaypoint(temporaryWP)
         end