Navigation2D.GetSimplePath(from, to) returns unexpected path results;

Question

I'm trying to use Navigation2D.GetSimplePath for my enemy to chase the player once discovered.

using this bit of code I'd expect my enemy to get a Vector2 array containing path info to nav to the player but, my enemy goes in a completely different direction than what I'd expect.

I've tried this:

var from = Enemy.Position;
var to = PlayerRef.Position;
//Nav is my Navigation2d
var paths = Nav.GetSimplePath(from, to); 
Enemy.Status.NavPath = new Stack(paths);

for my from and to and but, I've also attempted a lot of conversions

My guess was that my locals need to be converted to the local of the Navigation2d so I tried this:

//Nav is my Navigation2d
var from = Nav.ToLocal(Enemy.GlobalPosition);
var to = Nav.ToLocal(PlayerRef.GlobalPosition);

Since then I've been just bashing my head against the way with converting different global positions to locals of others and using those for the from and to values with similarly off results. I've been looking at this for a really long time (multiple days in my spare time) and I think I'm overlooking something obvious. If possible could anyone provide a second set of eyes and tell me what I've been missing.

Additional note: This issue only started occurring after my attempt at refactoring. I had this logic all smashed together in a single enemy class till it got to be a pain to maintain. I can provide the original as well.

Here is the full listing of the current file.

using Godot;
using ThemedHorrorJam5.Scripts.Enum;
using ThemedHorrorJam5.Scripts.Patterns.StateMachine;
using ThemedHorrorJam5.Scripts.GDUtils;
using System.Collections.Generic;
using System.Linq;

namespace ThemedHorrorJam5.Entities
{

    public class ChaseEnemyState : State
    {

        private Navigation2D? GetLevelNavigation()
        {
            var nodeTuples = Enemy.GetTree().GetNavigation2dNodes();
            if (nodeTuples.Item1) return nodeTuples.Item2[0];
            return null;
        }

        private Navigation2D Nav { get; set; }
        private EnemyV4 Enemy { get; set; }
        private PlayerV2 PlayerRef { get; set; }

        public ChaseEnemyState(EnemyV4 enemy)
        {
            this.Name = EnemyBehaviorStates.ChasePlayer.GetDescription();
            Enemy = enemy;
            (var hasPlayer, PlayerRef) = Enemy.GetTree().GetPlayerNode();
            if (!hasPlayer)
            {
                Logger.Error("Player ref not found on scene tree");
            }

            (var hasNav, var navNodes) = Enemy.GetTree().GetNavigation2dNodes();
            if (hasNav && navNodes != null)
            {
                Nav = navNodes[0];
            }

            this.OnEnter += () => this.Logger.Debug("ChaseEnemyState OnEnter called");
            this.OnExit += () => this.Logger.Debug("ChaseEnemyState Exit called");
            this.OnFrame += ChasePlayer;
        }

        private void ChasePlayer(float delta)
        {
            if (Enemy.IsDebugging && Enemy.HasNode("Line2D"))
            {
                Enemy.Status.Line = (Line2D)Enemy.GetNode("Line2D");

            }
            if (Nav!=null)
            {
                //Enemy.Status.Navigation2D = GetLevelNavigation();
                //var nav = (Navigation2D)Enemy.Owner.GetNode("Navigation2D");
                //Enemy.Status.Navigation2D = (Navigation2D)Enemy.Owner.GetNode("Navigation2D");


                //var from = Enemy.Position;
                var from = Nav.ToLocal(Enemy.GlobalPosition);
                //var from = Enemy.GlobalPosition;

                var to = Nav.ToLocal(PlayerRef.GlobalPosition);
                //var to = PlayerRef.Position;
                //var to = Enemy.Status.Target.Position;
                //var to = Enemy.Status.Target.ToLocal(Enemy.Status.Target.Position);

                //Enemy.DrawLine(from, to, new Color(255, 255, 255), 3);

                //var paths = Enemy.Status.Navigation2D.GetSimplePath(from, to);

                var paths = Nav.GetSimplePath(from, to);

                Enemy.Status.NavPath = new Stack(paths);

                if (Enemy.Status.Line != null)
                {
                    Enemy.Status.Line.Points = Enemy.Status.NavPath.ToArray();
                }

                var distance_to_walk = Enemy.MoveSpeed * delta;

                while (distance_to_walk > 0f && Enemy.Status.NavPath.Count > 0f)
                {
                    var distance_to_next_point = Enemy.Position.DistanceTo(Enemy.Status.NavPath.Peek());
                    if (distance_to_walk <= distance_to_next_point)
                    {
                        var newPosition = Enemy.Position.DirectionTo(Enemy.Status.NavPath.Peek()) * distance_to_walk;
                        Enemy.Status.VisionManager.UpdateFacingDirection(newPosition.Normalized());
                        Enemy.Position += newPosition;
                    }
                    else
                    {
                        var newPosition = Enemy.Status.NavPath.Pop();
                        Enemy.Status.VisionManager.UpdateFacingDirection(newPosition.Normalized());
                        if (Enemy.GetSlideCount() > 0)
                        {
                            Enemy.HandleMovableObstacleCollision(newPosition);
                        }
                        Enemy.Position = newPosition;
                    }
                    distance_to_walk -= distance_to_next_point;
                }
                if (Enemy.IsDebugging)
                {
                    Enemy.Status.DebugLabel.Text =
                   @$"
                    |-----------------------------------------------------------
                    | Enemy Global Position: {Enemy.GlobalPosition}
                    | Enemy Local Position: {Enemy.Position}
                    |----------------------------------------------------------
                    | Target Global Position: {Enemy.Status.Target.GlobalPosition}
                    | Target Local Position: {Enemy.Status.Target.Position}
                    |-----------------------------------------------------------
                    | From {from}
                    | To {to}
                    |-----------------------------------------------------------";
                }
            }
            else
            {
                Logger.Error("Navigation2D not found");
            }
            if (Enemy.Status.CurrentCoolDownCounter > 0)
            {
                Enemy.Status.CurrentCoolDownCounter -= delta;
            }
        }
    }
}

Some images to help show what I'm seeing. Player is green and the enemy is blue. From and To correspond to the values of the variables. The yellow is the sight cone for the enemy and is pointing out in the direction the enemy is walking. Target is an alias for player and the red line is a Line2d I've been using to draw the enemy's path.

//Nav is my Navigation2d
var from = Enemy.Position;
var to = PlayerRef.lPosition;

Second attempt results with :

//Nav is my Navigation2d
var from = Nav.ToLocal(Enemy.GlobalPosition);
var to = Nav.ToLocal(PlayerRef.GlobalPosition);

Any insight at all would be helpful as to what I'm doing wrong.

user_402022 · Answer

I can only guess this began failing due to the nodes being moved around. Either the order in the scene tree, or the Navigation2D position. I'm guessing that because the code was previously only using local positions. Meaning that their local coordinates used to match, but they no longer do.

Anyway, this code is correct:

var from = Nav.ToLocal(Enemy.GlobalPosition);
var to = Nav.ToLocal(PlayerRef.GlobalPosition);

And this is progress. You had fragile code before (it depended on the coincidence that the local coordinates matched). So this will ultimately be good.

I believe the issue you are facing comes from here:

Enemy.Position.DistanceTo(Enemy.Status.NavPath.Peek());

Which would be be correct if their local coordinates matched, but they don't. So we need to convert the result we get here.

If you are going to work on the enemy local coordinates, you need to convert again:

Enemy.Position.DistanceTo(Enemy.ToLocal(Nav.ToGlobal(Enemy.Status.NavPath.Peek())));

However, I'd suggest to work in global coordinates instead (since you can write to Enemy.GlobalPosition instead of Enemy.Position):

Enemy.GlobalPosition.DistanceTo(Nav.ToGlobal(Enemy.Status.NavPath.Peek()));

You would have to do the same change in other places in your code.

Let us see here:

var newPosition = Enemy.Position.DirectionTo(Enemy.Status.NavPath.Peek()) * distance_to_walk;
Enemy.Status.VisionManager.UpdateFacingDirection(newPosition.Normalized());

Wait, that isn't a position is it? That's a displacement. Don't mix things up.

Well, the question is what coordinates does UpdateFacingDirection expect.

You also have this code:

var newPosition = Enemy.Status.NavPath.Pop();
Enemy.Status.VisionManager.UpdateFacingDirection(newPosition.Normalized());

Sure a position normalized is a direction… From the origin. But which origin? In this case it is the origin of the Navigation2D…

So, I have searched for UpdateFacingDirection in your code. I found a couple implementations which look like this:

public void UpdateFacingDirection(Vector2 newVelocity)
{
    this.Rotation = this.Position.AngleToPoint(newVelocity);
}

See? This makes no sense. The code claims to be taking the angle to go from a point to a velocity. I'll believe your instruction and not your naming. In which case UpdateFacingDirection takes a position in local coordinates.

I have also looked for HandleMovableObstacleCollision, and I found this implementation:

public void HandleMovableObstacleCollision(Vector2 motion)
{
    this.PrintCaller();
    motion = motion.Normalized();

    if (GetSlideCollision(0).Collider is PushBlock box && box.CanBePushed)
    {
        box.Push(PushSpeed * motion);
    }
}

So apparently this only cares about the direction of the argument. Now, this is the Push I found:

public void Push(Vector2 velocity)
{
    MoveAndSlide(velocity);
}

So that is global coordinates. Meaning that HandleMovableObstacleCollision takes a direction in global coordinates.

Ok, I think I can rewrite:

var from = Nav.ToLocal(Enemy.GlobalPosition);
var to = Nav.ToLocal(PlayerRef.GlobalPosition);
var paths = Nav.GetSimplePath(from, to);
// …
var distance_to_walk = Enemy.MoveSpeed * delta;
while (distance_to_walk > 0f && paths.Count > 0f)
{
    var next_point = Nav.ToGlobal(paths.Peek());
    var global_direction = Enemy.GlobalPosition.DirectionTo(next_point);
    var global_distance = Enemy.GlobalPosition.DistanceTo(next_point);
    if (distance_to_walk <= global_distance)
    {
        var global_displacement = global_direction * distance_to_walk;
        var global_new_position = Enemy.GlobalPosition + global_displacement;
        var local_new_position = Enemy.ToLocal(global_new_position);
        Enemy.Status.VisionManager.UpdateFacingDirection(local_new_position);
        Enemy.GlobalPosition = global_new_position;
    }
    else
    {
        _ = paths.Pop();
        // var global_displacement = next_point - Enemy.GlobalPosition;
        var global_new_position = next_point;
        var local_new_position = Enemy.ToLocal(global_new_position);
        Enemy.Status.VisionManager.UpdateFacingDirection(local_new_position);
        if (Enemy.GetSlideCount() > 0)
        {
            Enemy.HandleMovableObstacleCollision(global_direction);
        }
        Enemy.GlobalPosition = global_new_position;
    }
    distance_to_walk -= global_distance;
}

I don't know what are you storing Enemy.Status.NavPath for, if you are calling navigation each frame. So I removed that from the code above. If you do need it, add it back. Similarly, I don't know what to make of Enemy.Status.Line.Points. My guess is that is only for debug, and it should be in local coordinates of the Line2D, if you need it, perhaps you can use the global transforms to convert them (with Xform and XformInv).

About the line:

_ = paths.Pop();

Is a discard. We don't really need a discard. You can simply call Pop:

paths.Pop();

The discard - in this case - is just meant to indicate that we intentionally do not use the returned value. We don't need to, because we already got it from Peek.

By the way, in the second branch I added comment with global_displacement, you will see what you would need that line for below.

There is something else that bothers me:

if (Enemy.GetSlideCount() > 0)
{
    Enemy.HandleMovableObstacleCollision(/*…*/);
}

What slide count? In fact, in HandleMovableObstacleCollision I see you use GetSlideCollision, but what slide collisions? If you don't use MoveAndSlide or similar on the Enemy? Instead I see you write Enemy.Position (which I changed to Enemy.GlobalPosition)?

Let us use MoveAndSlide. The catch is that it does not take a displacement, nor a position, it takes a velocity. So we will pass global_displacement / delta (remember that velocity is displacement divided by delta time). So instead of this:

Enemy.GlobalPosition = global_new_position;

Do this:

Enemy.MoveAndSlide(global_displacement / delta);

Also, don't you want to do that before checking Enemy.GetSlideCount()? Well, I don't know. You can finish figuring this out.

Navigation2D.GetSimplePath(from, to) returns unexpected path results;

1 Answers