Some improvements with path smoothing and interactions.

components/NavigationComponent.cs

@@ -99,6 +99,23 @@ public class NavigationComponent : Spatial
         Debug.Assert(TileWorld != null);
     }
     
+    public void PlanSmoothedPath(KinematicBody body, Transform fromTransformWorld, NavigationPoint navigationPoint)
+    {
+        if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
+            && navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
+        {
+            FindPath(body, fromTransformWorld.origin, navigationPoint);
+        }
+        else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
+        {
+            FindPath(body, fromTransformWorld.origin, navigationPoint.WorldPosition);
+        }
+        else
+        {
+            throw new NotImplementedException();
+        }
+    }
+
     public void FindPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
     {
         HexCell fromCell = TileWorld.HexGrid.GetHexAt(new Vector2(fromPositionWorld.x, fromPositionWorld.z));
@@ -125,25 +142,33 @@ public class NavigationComponent : Spatial
         
         List<HexCell> path = TileWorld.HexGrid.FindPath(fromCell, toCell);
 
-//        GD.Print("Planning path and have " + TileWorld.HexGrid.Obstacles.Count + " obstacles:");
+        // Generate grid navigation points
-//        GD.Print(TileWorld.HexGrid.Obstacles);
-//        foreach (Vector2 coord in TileWorld.HexGrid.Obstacles.Keys)
-//        {
-//            HexCell cell = new HexCell(coord);
-//            GD.Print("  " + cell.OffsetCoords);
-//        }
-        
         _planningPathWorldNavigationPoints = new List<NavigationPoint>();
         foreach (int index in Enumerable.Range(0, path.Count))
         {
-//            GD.Print("Step " + index + ": " + path[index].OffsetCoords);
             _planningPathWorldNavigationPoints.Add(
                 new NavigationPoint(TileWorld.GetHexCenterFromOffset(path[index].OffsetCoords)));
         }
 
+        // Ensure the last point coincides with the target position
+        if ((_planningPathWorldNavigationPoints.Last().WorldPosition - toPositionWorld).LengthSquared() <
+            0.5f * 0.5f)
+        {
+            _planningPathWorldNavigationPoints[_planningPathWorldNavigationPoints.Count - 1].WorldPosition = toPositionWorld;
+        }
+        
+        // Perform smoothing
         _planningPathSmoothedWorldNavigationPoints = SmoothPath(body, _planningPathWorldNavigationPoints);
     }
 
+    public void FindPath(KinematicBody body, Vector3 fromPositionWorld, NavigationPoint navigationPoint)
+    {
+        FindPath(body, fromPositionWorld, navigationPoint.WorldPosition);
+
+        _planningPathWorldNavigationPoints[_planningPathWorldNavigationPoints.Count - 1] = navigationPoint;
+        _planningPathSmoothedWorldNavigationPoints[_planningPathSmoothedWorldNavigationPoints.Count - 1] = navigationPoint;
+    }
+    
     public void PlanGridPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
     {
         Vector2 fromPositionOffset = TileWorld.WorldToOffsetCoords(fromPositionWorld);

entities/Player.cs

@@ -36,6 +36,7 @@ public class Player : Entity, IInteractionInterface
     private AnimationTree _animationTree;
     private DebugGeometry _debugGeometry;
     private InteractionComponent _interactionComponent;
+
     public InteractionComponent InteractionComponent
     {
         get => _interactionComponent;
@@ -76,7 +77,8 @@ public class Player : Entity, IInteractionInterface
         Debug.Assert(_playerAnimationPlayer != null);
         _animationTree = GetNode<AnimationTree>("Geometry/AnimationTree");
         Debug.Assert(_animationTree != null);
-        AnimationNodeStateMachinePlayback stateMachine = (AnimationNodeStateMachinePlayback)_animationTree.Get("parameters/playback");
+        AnimationNodeStateMachinePlayback stateMachine =
+            (AnimationNodeStateMachinePlayback)_animationTree.Get("parameters/playback");
         stateMachine.Start("Idle");
 
         _toolAttachement = (BoneAttachment)FindNode("ToolAttachement");
@@ -109,13 +111,15 @@ public class Player : Entity, IInteractionInterface
                 if (navigationTask != null && navigationTask.PlanningComplete == false)
                 {
                     // _navigationComponent.PlanGridPath(this, GlobalTransform, navigationTask.NavigationPoint);
-                    _navigationComponent.FindPath(this, GlobalTranslation, navigationTask.NavigationPoint.WorldPosition);
+                    _navigationComponent.PlanSmoothedPath(this, GlobalTransform, navigationTask.NavigationPoint);
+
                     _navigationComponent.ActivatePlannedPath();
                     navigationTask.PlanningComplete = true;
                 }
 
                 _navigationComponent.UpdateCurrentGoal(GlobalTransform);
-                _groundMotion.PhysicsProcess(delta, this, _navigationComponent.CurrentGoalPositionWorld, _navigationComponent.CurrentGoalOrientationWorld, _worldInfo.TileWorld);
+                _groundMotion.PhysicsProcess(delta, this, _navigationComponent.CurrentGoalPositionWorld,
+                    _navigationComponent.CurrentGoalOrientationWorld, _worldInfo.TileWorld);
             }
         }
     }
@@ -155,7 +159,6 @@ public class Player : Entity, IInteractionInterface
             _navigationComponent.DebugDraw(this, _debugGeometry);
             return;
         }
-
     }
 
     public void OnItemAttractorBodyEntered(Node node)
@@ -173,6 +176,7 @@ public class Player : Entity, IInteractionInterface
             GoldBar bar = (GoldBar)node;
             bar.UnsetTarget();
         }
+
         _attractedItemList.Remove(node);
     }
 
@@ -200,7 +204,8 @@ public class Player : Entity, IInteractionInterface
         if (toolName == "Axe")
         {
             _toolAttachement.Visible = true;
-        } else if (toolName == "")
+        }
+        else if (toolName == "")
         {
             _toolAttachement.Visible = false;
         }
@@ -208,14 +213,16 @@ public class Player : Entity, IInteractionInterface
 
     public void OnInteractionStart()
     {
-        AnimationNodeStateMachinePlayback stateMachine = (AnimationNodeStateMachinePlayback)_animationTree.Get("parameters/playback");
+        AnimationNodeStateMachinePlayback stateMachine =
+            (AnimationNodeStateMachinePlayback)_animationTree.Get("parameters/playback");
         Debug.Assert(stateMachine != null);
 
         if (_interactionComponent.TargetEntity is Chest)
         {
             GD.Print("Player Opening Box");
             stateMachine.Travel("Interaction");
-        } else if (_interactionComponent.TargetEntity is Tree)
+        }
+        else if (_interactionComponent.TargetEntity is Tree)
         {
             GD.Print("Player Chopping Tree");
             stateMachine.Travel("Hit");
@@ -225,7 +232,8 @@ public class Player : Entity, IInteractionInterface
     public void OnInteractionEnd()
     {
         GD.Print("Player Stopping Interaction");
-        AnimationNodeStateMachinePlayback stateMachine = (AnimationNodeStateMachinePlayback)_animationTree.Get("parameters/playback");
+        AnimationNodeStateMachinePlayback stateMachine =
+            (AnimationNodeStateMachinePlayback)_animationTree.Get("parameters/playback");
         Debug.Assert(stateMachine != null);
         stateMachine.Travel("Idle");
     }