GodotComponentTest/components/NavigationComponent.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Numerics;
using Godot;
using Vector2 = Godot.Vector2;
using Vector3 = Godot.Vector3;
using GoDotLog;

/// <summary>
/// </summary>
public class NavigationComponent : Node
{
    public TileWorld TileWorld { set; get; }

    public Vector3 CurrentGoalWorld => _currentGoalWorld;

    private Vector3 _currentGoalWorld = Vector3.Zero;
    private Vector2 _currentGoalPlane = Vector2.Zero;
    private Vector2 _currentGoalOffset = Vector2.Zero;
    private Vector3 _currentPositionWorld = Vector3.Zero;

    private List<Vector2> _pathOffsetCoords;
    private HexCell[] _path;

    public override void _Ready()
    {
        base._Ready();
        _pathOffsetCoords = new List<Vector2>();
    }

    public override void _Process(float delta)
    {
        Debug.Assert(TileWorld != null);
    }


    public void Plan(Vector3 fromPositionWorld, Vector3 toPositionWorld)
    {
        Vector2 fromPositionOffset = TileWorld.WorldToOffsetCoords(fromPositionWorld);
        Vector2 toPositionOffset = TileWorld.WorldToOffsetCoords(toPositionWorld);

        HexCell fromCell = new HexCell();
        fromCell.OffsetCoords = fromPositionOffset;

        HexCell toCell = new HexCell();
        toCell.OffsetCoords = toPositionOffset;

        _path = fromCell.LineTo(toCell);
        Debug.Assert(_path.Length > 0);

        _pathOffsetCoords = new List<Vector2>();
        foreach (int index in Enumerable.Range(1, _path.Length - 1))
        {
            _pathOffsetCoords.Add(_path[index].OffsetCoords);
        }

        UpdateCurrentGoal();
    }

    private void UpdateCurrentGoal()
    {
        if (_pathOffsetCoords.Count == 0)
        {
            return;
        }

        _currentGoalWorld = TileWorld.GetTileWorldCenterFromOffset(_pathOffsetCoords[0]);

        GD.Print("Navigation: at " + _currentGoalWorld + " path length: " +
                 _pathOffsetCoords.Count);
    }


    public void UpdateCurrentGoal(Vector3 currentPositionWorld)
    {
        _currentPositionWorld = currentPositionWorld;

        Vector2 currentPositionPlane = new Vector2(currentPositionWorld.x, currentPositionWorld.z);
        Vector2 currentGoalPlane = new Vector2(_currentGoalWorld.x, _currentGoalWorld.z);
        if (_pathOffsetCoords.Count > 0 &&
            (currentPositionPlane - currentGoalPlane).LengthSquared() < Globals.EpsPositionSquared)
        {
            _pathOffsetCoords.RemoveAt(0);
            UpdateCurrentGoal();
        }
    }
}