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 class NavigationPoint
    {
        [Flags]
        public enum NavigationFlags
        {
            Position = 1,
            Orientation = 2
        }

        public Vector3 WorldPosition = Vector3.Zero;
        public Quat WorldOrientation = Quat.Identity;
        public NavigationFlags Flags = NavigationFlags.Position | NavigationFlags.Orientation;

        public NavigationPoint(Vector3 worldPosition)
        {
            WorldPosition = worldPosition;
            Flags = NavigationFlags.Position;
        }

        public NavigationPoint(Quat worldOrientation)
        {
            WorldOrientation = worldOrientation;
            Flags = NavigationFlags.Orientation;
        }
    }

    public TileWorld TileWorld { set; get; }

    public Vector3 CurrentGoalPositionWorld => _currentGoalPositionWorld;
    public Quat CurrentGoalOrientationWorld => _currentGoalOrientationWorld;

    private NavigationPoint _currentGoal;
    private Vector3 _currentGoalPositionWorld = Vector3.Zero;
    private Quat _currentGoalOrientationWorld = Quat.Identity;
    
    private List<NavigationPoint> _pathWorldNavigationPoints;
    private HexCell[] _path;

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

    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);

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

        if ((toPositionWorld - TileWorld.GetTileWorldCenterFromOffset(toCell.OffsetCoords)).LengthSquared() >
            Globals.EpsPositionSquared)
        {
            // Remove the last one, because it is only the position rounded to HexGrid coordinates.
            if (_pathWorldNavigationPoints.Count > 1)
            {
                _pathWorldNavigationPoints.RemoveAt(_pathWorldNavigationPoints.Count - 1);
            }
            _pathWorldNavigationPoints.Add(new NavigationPoint(toPositionWorld));
        }

        UpdateCurrentGoal();
    }


    public void Plan(Vector3 fromPositionWorld, Vector3 toPositionWorld, Quat toWorldOrientation)
    {
        Plan(fromPositionWorld, toPositionWorld);

        _pathWorldNavigationPoints.Add(new NavigationPoint(toWorldOrientation));
    }


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

        _currentGoal = _pathWorldNavigationPoints[0];

        GD.Print("Navigation Goal: pos " + _currentGoal.WorldPosition + " " + " rot: " + _currentGoal.WorldOrientation +
                 " flags: " + _currentGoal.Flags + " path length: " +
                 _pathWorldNavigationPoints.Count);
    }


    public void UpdateCurrentGoal(Transform currentTransformWorld)
    {
        if (_pathWorldNavigationPoints.Count == 0)
        {
            return;
        }

        if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
        {
            _currentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;            
        }
        else
        {
            _currentGoalPositionWorld = currentTransformWorld.origin;
        }

        if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
        {
            _currentGoalOrientationWorld = _currentGoal.WorldOrientation;
        }
        else
        {
            _currentGoalOrientationWorld = currentTransformWorld.basis.Quat();
        }
        
        Vector3 currentPositionWorld = currentTransformWorld.origin;
        Quat currentOrientationWorld = currentTransformWorld.basis.Quat();

        Vector2 currentPositionPlane = new Vector2(currentPositionWorld.x, currentPositionWorld.z);
        Vector2 currentGoalPlane = new Vector2(_currentGoal.WorldPosition.x, _currentGoal.WorldPosition.z);

        bool goalReached = false;
        float positionErrorSquared = (currentPositionPlane - currentGoalPlane).LengthSquared();
        float orientationError = _currentGoalOrientationWorld.AngleTo(currentOrientationWorld);

        if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
            && _currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation)
            && positionErrorSquared < Globals.EpsPositionSquared
            && orientationError < Globals.EpsRadians)
        {
            goalReached = true;
        }
        else if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Position) &&
                 positionErrorSquared < Globals.EpsPositionSquared)
        {
            goalReached = true;
        }
        else if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation) &&
                 orientationError < Globals.EpsRadians)

        {
            goalReached = true;
        }

        if (goalReached)
        {
            _pathWorldNavigationPoints.RemoveAt(0);
            UpdateCurrentGoal();
        }
    }
}