Navigation with plange angle targets now functional again.

Globals.cs

@@ -1,9 +1,14 @@
-using System;
+using Godot;
 
 public static class Globals
 {
     public const float EpsPosition = 0.01f;
     public const float EpsPositionSquared = EpsPosition * EpsPosition;
-    public const float EpsRadians = 0.1f * Godot.Mathf.Pi / 180f;
+    public const float EpsRadians = 0.1f * Mathf.Pi / 180f;
     public const float EpsRadiansSquared = EpsRadians * EpsRadians;
+
+    public static float CalcPlaneAngle(Transform worldTransform)
+    {
+        return worldTransform.basis.x.SignedAngleTo(Vector3.Right.Rotated(Vector3.Up, Mathf.Pi * 0.5f), -Vector3.Up);
+    }
 }

components/GroundMotionComponent.cs

@@ -14,7 +14,7 @@ public class GroundMotionComponent : Component
     public float TargetAngle;
     public float TargetDeltaAngle;
 
-    private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
+    private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, float targetAngle)
     {
         float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle);
         if (deltaAngleAbsolute > 0.001)
@@ -104,7 +104,7 @@ public class GroundMotionComponent : Component
         entity.Velocity = entityVelocity;
     }
 
-    public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation,
+    public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, float targetAngle,
         World world)
     {
         DirectionToTarget = targetPosition - entity.GlobalTranslation;
@@ -119,16 +119,28 @@ public class GroundMotionComponent : Component
         }
         else
         {
-            GD.Print("Target Position reached!");
             DirectionToTarget = Vector3.Right;
-            TargetAngle = entity.PlaneAngle;
-            TargetDeltaAngle = 0;
             entity.GlobalTranslation = targetPosition;
             entity.Velocity = new Vector3(0, entity.Velocity.y, 0);
+
+            if (entity.PlaneAngle != targetAngle)
+            {
+                Vector3 directionToTarget = Vector3.Right.Rotated(Vector3.Up, targetAngle);
+                TargetAngle = targetAngle;
+                TargetDeltaAngle = entity.CalcShortestPlaneRotationToTargetDirection(directionToTarget);
+
+                if (Mathf.Abs(TargetDeltaAngle) < Globals.EpsRadians)
+                {
+                    TargetAngle = entity.PlaneAngle;
+                    TargetDeltaAngle = 0;
+
                     return;
                 }
+            }
+        }
+
+        CalcAndApplyOrientation(delta, entity, targetPosition, targetAngle);
 
-        CalcAndApplyOrientation(delta, entity, targetPosition, targetOrientation);
         CalcPlaneVelocity(delta, entity, targetPosition);
 //        CalcVerticalVelocity(delta, entity, tileWorld);
 

components/NavigationComponent.cs

@@ -11,8 +11,7 @@ public class NavigationComponent : Spatial
 {
     public World World { set; get; }
     public Vector3 CurrentGoalPositionWorld { get; private set; } = Vector3.Zero;
-    public float CurrentGoalAngleWorld { get; } = 0;
+    public float CurrentGoalAngleWorld { get; private set; }
-    public Quat CurrentGoalOrientationWorld { get; private set; } = Quat.Identity;
 
     private NavigationPoint _currentGoal;
     private HexCell[] _path;
@@ -342,11 +341,7 @@ public class NavigationComponent : Spatial
 
         _currentGoal = _pathWorldNavigationPoints[0];
         CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
-        //_currentGoalOrientationWorld = Vector3.Right.SignedAngleTo(_pathWorldNavigationPoints[0].WorldOrientation);
+        CurrentGoalAngleWorld = _pathWorldNavigationPoints[0].WorldAngle;
-
-//        GD.Print("Navigation Goal: pos " + _currentGoal.WorldPosition + " " + " rot: " + _currentGoal.WorldOrientation +
-//                 " flags: " + _currentGoal.Flags + " path length: " +
-//                 _pathWorldNavigationPoints.Count);
     }
 
     private void ApplyExistingTransform(Transform worldTransform)
@@ -357,7 +352,7 @@ public class NavigationComponent : Spatial
         }
         else if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Position)
         {
-            CurrentGoalOrientationWorld = worldTransform.basis.Quat();
+            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(worldTransform);
         }
     }
 
@@ -371,7 +366,7 @@ public class NavigationComponent : Spatial
 
         if (_pathWorldNavigationPoints.Count == 0)
         {
-            CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat();
+            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
             CurrentGoalPositionWorld = currentTransformWorld.origin;
             return;
         }
@@ -382,22 +377,18 @@ public class NavigationComponent : Spatial
         }
         else
         {
-            CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat();
+            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
         }
 
         if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
         {
-            CurrentGoalOrientationWorld = _currentGoal.WorldOrientation;
+            CurrentGoalAngleWorld = _currentGoal.WorldAngle;
         }
         else
         {
-            CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat();
+            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
         }
 
-        //            Vector3 currentWorldXAxis = currentTransformWorld.basis.x;
-        //            Debug.Assert(Mathf.Abs(currentWorldXAxis.y) < 0.9);
-        //            float angle = Mathf.Atan2(currentWorldXAxis.y, currentWorldXAxis.x);
-        //            _currentGoalOrientationWorld = Basis.Identity.Rotated(Vector3.Up, angle).Quat();
         if (_currentGoal.IsReached(currentTransformWorld))
         {
             _pathWorldNavigationPoints.RemoveAt(0);
@@ -408,8 +399,8 @@ public class NavigationComponent : Spatial
 
         if (_pathWorldNavigationPoints.Count == 0)
         {
-            CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat();
             CurrentGoalPositionWorld = currentTransformWorld.origin;
+            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
         }
     }
 
@@ -477,62 +468,4 @@ public class NavigationComponent : Spatial
 
         debugGeometry.End();
     }
-
-    public class NavigationPoint
-    {
-        [Flags]
-        public enum NavigationFlags
-        {
-            Position = 1,
-            Orientation = 2
-        }
-
-        public readonly NavigationFlags Flags;
-        public Quat WorldOrientation = Quat.Identity;
-
-        public Vector3 WorldPosition = Vector3.Zero;
-
-        public NavigationPoint(Vector3 worldPosition)
-        {
-            WorldPosition = worldPosition;
-            Flags = NavigationFlags.Position;
-        }
-
-        public NavigationPoint(Quat worldOrientation)
-        {
-            WorldOrientation = worldOrientation;
-            Flags = NavigationFlags.Orientation;
-        }
-
-        public NavigationPoint(Transform worldTransform)
-        {
-            WorldPosition = worldTransform.origin;
-            WorldOrientation = worldTransform.basis.Quat();
-            Flags = NavigationFlags.Position | NavigationFlags.Orientation;
-        }
-
-        public bool IsReached(Transform worldTransform)
-        {
-            var goalReached = false;
-            var positionError = new Vector2(WorldPosition.x - worldTransform.origin.x,
-                WorldPosition.z - worldTransform.origin.z);
-            var positionErrorSquared = positionError.LengthSquared();
-            worldTransform.basis.Quat();
-            var orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));
-
-            if (Flags.HasFlag(NavigationFlags.Position)
-                && Flags.HasFlag(NavigationFlags.Orientation)
-                && positionErrorSquared < Globals.EpsPositionSquared
-                && orientationError < Globals.EpsRadians)
-                goalReached = true;
-            else if (Flags == NavigationFlags.Position &&
-                     positionErrorSquared < Globals.EpsPositionSquared)
-                goalReached = true;
-            else if (Flags == NavigationFlags.Orientation &&
-                     orientationError < Globals.EpsRadians)
-                goalReached = true;
-
-            return goalReached;
-        }
-    }
 }

components/TaskQueueComponent.cs

@@ -51,10 +51,10 @@ public class TaskQueueComponent : Component
     /// </summary>
     public class NavigationTask : Task
     {
-        public NavigationComponent.NavigationPoint NavigationPoint;
+        public NavigationPoint NavigationPoint;
         public bool PlanningComplete = false;
 
-        public NavigationTask(NavigationComponent.NavigationPoint navigationPoint)
+        public NavigationTask(NavigationPoint navigationPoint)
         {
             NavigationPoint = navigationPoint;
         }

entities/Entity.cs

@@ -9,7 +9,7 @@ public class Entity : KinematicBody
     /** Defines the angle in plane coordinates, 0 => pointing to the right/east, pi/2 pointing up/north, range [-pi,pi]. */
     public float PlaneAngle
     {
-        get => GlobalTransform.basis.x.SignedAngleTo(Vector3.Right.Rotated(Vector3.Up, Mathf.Pi * 0.5f), -Vector3.Up);
+        get => Globals.CalcPlaneAngle(GlobalTransform);
         set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation);
     }
    

entities/Player.cs

@@ -98,10 +98,10 @@ public class Player : Entity, IInteractionInterface
 
                 NavigationComponent.UpdateCurrentGoal(GlobalTransform);
                 _groundMotion.PhysicsProcess(delta, this, NavigationComponent.CurrentGoalPositionWorld,
-                    NavigationComponent.CurrentGoalOrientationWorld, _worldInfo.World);
+                    NavigationComponent.CurrentGoalAngleWorld, _worldInfo.World);
 
                 if (NavigationComponent.IsGoalReached())
-                    navigationTask.NavigationPoint = new NavigationComponent.NavigationPoint(GlobalTransform);
+                    navigationTask.NavigationPoint = new NavigationPoint(GlobalTransform);
             }
         }
     }
@@ -139,20 +139,16 @@ public class Player : Entity, IInteractionInterface
         if (NavigationComponent != null)
         {
             NavigationComponent.DebugDraw(this, _debugGeometry);
-            return;
         }
     }
 
     public void OnItemAttractorBodyEntered(Node node)
     {
-        GD.Print("Item entered " + node);
         _attractedItemList.Add(node);
     }
 
     public void OnItemAttractorBodyExited(Node node)
     {
-        GD.Print("Item exited " + node);
-
         if (node is GoldBar)
         {
             GoldBar bar = (GoldBar)node;

scenes/Game.cs

@@ -206,7 +206,7 @@ public class Game : Spatial
 
         _player.TaskQueueComponent.Reset();
         _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask(
-            new NavigationComponent.NavigationPoint(tile.GlobalTranslation)));
+            new NavigationPoint(tile.GlobalTranslation)));
     }
 
     public void OnTileHovered(HexTile3D tile)
@@ -229,7 +229,7 @@ public class Game : Spatial
         {
             _player.TaskQueueComponent.Reset();
             _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask(
-                new NavigationComponent.NavigationPoint(mountPoint.GlobalTransform)));
+                new NavigationPoint(mountPoint.GlobalTransform)));
             _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.InteractionTask(entity));
         }
     }

utils/NavigationPoint.cs

@@ -0,0 +1,70 @@
+using System;
+using Godot;
+
+public class NavigationPoint
+{
+    [Flags]
+    public enum NavigationFlags
+    {
+        Position = 1,
+        Orientation = 2
+    }
+
+    public readonly NavigationFlags Flags;
+    public Quat WorldOrientation = Quat.Identity;
+    public float WorldAngle;
+
+    public Vector3 WorldPosition = Vector3.Zero;
+
+    public NavigationPoint(Vector3 worldPosition)
+    {
+        WorldPosition = worldPosition;
+        Flags = NavigationFlags.Position;
+    }
+
+    public NavigationPoint(Quat worldOrientation)
+    {
+        WorldOrientation = worldOrientation;
+        Flags = NavigationFlags.Orientation;
+    }
+
+    public NavigationPoint(Transform worldTransform)
+    {
+        WorldPosition = worldTransform.origin;
+        WorldOrientation = worldTransform.basis.Quat();
+        WorldAngle = Globals.CalcPlaneAngle(worldTransform);
+        
+        Flags = NavigationFlags.Position | NavigationFlags.Orientation;
+    }
+
+    public bool IsReached(Transform worldTransform)
+    {
+        var goalReached = false;
+        var positionError = new Vector2(WorldPosition.x - worldTransform.origin.x,
+            WorldPosition.z - worldTransform.origin.z);
+        var positionErrorSquared = positionError.LengthSquared();
+        worldTransform.basis.Quat();
+        var orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));
+        var angleError = Mathf.Abs(Globals.CalcPlaneAngle(worldTransform) - WorldAngle);
+
+        if (Flags.HasFlag(NavigationFlags.Position)
+            && Flags.HasFlag(NavigationFlags.Orientation)
+            && positionErrorSquared < Globals.EpsPositionSquared
+            && angleError < Globals.EpsRadians)
+        {
+            goalReached = true;
+        }
+        else if (Flags == NavigationFlags.Position &&
+                 positionErrorSquared < Globals.EpsPositionSquared)
+        {
+            goalReached = true;
+        }
+        else if (Flags == NavigationFlags.Orientation &&
+                 angleError < Globals.EpsRadians)
+        {
+            goalReached = true;
+        }
+
+        return goalReached;
+    }
+}