GodotComponentTest/components/NavigationComponent.cs

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

/// <summary>
/// </summary>
public class NavigationComponent : Spatial
{
    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 NavigationPoint(Transform worldTransform)
        {
            WorldPosition = worldTransform.origin;
            WorldOrientation = worldTransform.basis.Quat();
            Flags = NavigationFlags.Position | NavigationFlags.Orientation;
        }

        public bool IsReached(Transform worldTransform)
        {
            bool goalReached = false;
            float positionErrorSquared = (worldTransform.origin - WorldPosition).LengthSquared();
            Quat own_orientation = worldTransform.basis.Quat();
            float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));

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

            return goalReached;
        }
    }

    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 Area _pathCollisionQueryVolume;
    private CollisionShape _pathCollisionQueryShape;

    private List<NavigationPoint> _pathWorldNavigationPoints;
    private HexCell[] _path;

    public override void _Ready()
    {
        base._Ready();
        _pathWorldNavigationPoints = new List<NavigationPoint>();
        
        _pathCollisionQueryVolume = (Area)FindNode("PathCollisionQueryVolume");
        Debug.Assert(_pathCollisionQueryVolume != null);
        _pathCollisionQueryShape = (CollisionShape)_pathCollisionQueryVolume.FindNode("CollisionShape");
        Debug.Assert(_pathCollisionQueryShape != null);

    }

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


    public void PlanGridPath(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.GetHexCenterFromOffset(_path[index].OffsetCoords)));
        }

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

        _pathWorldNavigationPoints.Add(new NavigationPoint(toPositionWorld));

        UpdateCurrentGoal();
    }


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

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


    public void PlanGridPath(Transform fromTransformWorld, NavigationPoint navigationPoint)
    {
        if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
            && navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
        {
            PlanGridPath(fromTransformWorld.origin, navigationPoint.WorldPosition, navigationPoint.WorldOrientation);
        }
        else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
        {
            PlanGridPath(fromTransformWorld.origin, navigationPoint.WorldPosition);
        }
        else
        {
            throw new NotImplementedException();
        }
        
        CheckPathCollision(fromTransformWorld.origin, navigationPoint.WorldPosition);
    }


    public void PlanDirectPath(Vector3 fromPositionWorld, Vector3 toPositionWorld)
    {
        _pathWorldNavigationPoints.Clear();
        _pathWorldNavigationPoints.Add(new NavigationPoint(toPositionWorld));

        UpdateCurrentGoal();
    }


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

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


    bool CheckPathCollision(Vector3 fromPositionWorld, Vector3 toPositionWorld)
    {
        Vector3 fromPositionLocal = GlobalTransform.XformInv(fromPositionWorld);
        Vector3 toPositionLocal = GlobalTransform.XformInv(toPositionWorld);
        float distance = (toPositionLocal - fromPositionLocal).Length(); 
        Vector3 direction = (toPositionLocal - fromPositionLocal) / distance;
        Vector3 side = Vector3.Up.Cross(direction);
        Basis orientation = new Basis(side, Vector3.Up, direction);

        _pathCollisionQueryVolume.Transform =
            new Transform(orientation, 0.5f * toPositionLocal + 0.5f * fromPositionLocal).Scaled(new Vector3(0.5f, 0.5f, distance));
        
        var collisionBodies = _pathCollisionQueryVolume.GetOverlappingBodies();

        if (collisionBodies.Count > 0)
        {
            GD.Print("There is a body: " + collisionBodies[0]);
            return true;
        }
        
        if ((fromPositionWorld - toPositionWorld).LengthSquared() < 0.001)
        {
            return false;
        }
        
        // TODO: Complete Implementation
        Debug.Assert(false);
        return true;
    }

    public List<NavigationPoint> SmoothPath(List<NavigationPoint> navigationPoints)
    {
        List<NavigationPoint> smoothedPath = new List<NavigationPoint>(); 
            
        int startIndex = 0;
        smoothedPath.Add(navigationPoints[startIndex]);
        
        int endIndex = navigationPoints.Count > 1 ? 1 : 0;

        while (endIndex != navigationPoints.Count - 1)
        {
            Vector3 startPoint = navigationPoints[startIndex].WorldPosition;
            Vector3 endPoint = navigationPoints[endIndex].WorldPosition;

            if (CheckPathCollision(startPoint, endPoint))
            {
                smoothedPath.Add(navigationPoints[endIndex-1]);
                smoothedPath.Add(navigationPoints[endIndex]);
                startIndex = endIndex;
                endIndex += 1;
            }
        }

        smoothedPath.Add(navigationPoints[endIndex]);

        return smoothedPath;
    }

    public void PlanDirectPath(Transform fromTransformWorld, NavigationPoint navigationPoint)
    {
        if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
            && navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
        {
            PlanDirectPath(fromTransformWorld.origin, navigationPoint.WorldPosition, navigationPoint.WorldOrientation);
        }
        else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
        {
            PlanDirectPath(fromTransformWorld.origin, navigationPoint.WorldPosition);
        }
        else
        {
            throw new NotImplementedException();
        }
    }


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

        _currentGoal = _pathWorldNavigationPoints[0];
        _currentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
        _currentGoalOrientationWorld = _pathWorldNavigationPoints[0].WorldOrientation;

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

    private void ApplyExistingTransform(Transform worldTransform)
    {
        if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Orientation)
        {
            _currentGoalPositionWorld = worldTransform.origin;
        }
        else if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Position)
        {
            _currentGoalOrientationWorld = worldTransform.basis.Quat();
        }
    }


    public void UpdateCurrentGoal(Transform currentTransformWorld)
    {
        if (_pathWorldNavigationPoints.Count == 0)
        {
            _currentGoalOrientationWorld = currentTransformWorld.basis.Quat();
            _currentGoalPositionWorld = currentTransformWorld.origin;
            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();
        }

        if (_currentGoal.IsReached(currentTransformWorld))
        {
            _pathWorldNavigationPoints.RemoveAt(0);

            UpdateCurrentGoal();
            ApplyExistingTransform(currentTransformWorld);
        }

        if (_pathWorldNavigationPoints.Count == 0)
        {
            _currentGoalOrientationWorld = currentTransformWorld.basis.Quat();
            _currentGoalPositionWorld = currentTransformWorld.origin;
        }
    }

    public void DebugDraw(Spatial parentNode, DebugGeometry debugGeometry)
    {
        
        Vector3 yOffset = Vector3.Up * 0.1f;

        debugGeometry.GlobalTransform = Transform.Identity;
        debugGeometry.Begin(Mesh.PrimitiveType.Lines);
        debugGeometry.SetColor(Colors.Pink);
        debugGeometry.AddVertex(parentNode.GlobalTranslation + yOffset);
        debugGeometry.SetColor(Colors.Pink);
        debugGeometry.AddVertex(CurrentGoalPositionWorld + yOffset);
        debugGeometry.SetColor(Colors.Pink);

        debugGeometry.PushTranslated(CurrentGoalPositionWorld);
        debugGeometry.AddBox(Vector3.One * 1);
        debugGeometry.PopTransform();

        Vector3 previousPoint = parentNode.GlobalTranslation;
        foreach (NavigationPoint point in _pathWorldNavigationPoints)
        {
            debugGeometry.AddVertex(previousPoint + yOffset);
            debugGeometry.AddVertex(point.WorldPosition + yOffset);

            previousPoint = point.WorldPosition;
        }
        
        debugGeometry.End();
    }
}