GodotComponentTest/components/NavigationComponent.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using Godot;
using GodotComponentTest.utils;

/// <summary>
/// </summary>
public class NavigationComponent : Spatial {
    [Export] public bool PathSmoothing = false;
    
    public World World { set; get; }
    public Vector3 CurrentGoalPositionWorld { get; private set; } = Vector3.Zero;
    public float CurrentGoalAngleWorld { get; private set; }

    private NavigationPoint _currentGoal;
    private HexCell[] _path;
    private List<NavigationPoint> _pathWorldNavigationPoints = new();
    private List<NavigationPoint> _activePathWorldNavigationPoints = new();

    private List<NavigationPoint> _planningPathWorldNavigationPoints = new();
    private List<NavigationPoint> _smoothedPathWorldNavigationPoints = new();

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

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

    public void PlanSmoothedPath(Entity 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(Entity entity, Vector3 fromPositionWorld, Vector3 toPositionWorld) {
        HexCell fromCell = World.HexGrid.GetHexAt(new Vector2(fromPositionWorld.x, fromPositionWorld.z));
        if (World.HexGrid.GetHexCost(fromCell) == 0) {
            GD.Print("Invalid starting point for FindPath(): returning empty path.");
            _planningPathWorldNavigationPoints = new List<NavigationPoint>();
            _planningPathWorldNavigationPoints.Add(new NavigationPoint(fromPositionWorld));
            _activePathWorldNavigationPoints = _planningPathWorldNavigationPoints;
            return;
        }

        HexCell toCell = World.HexGrid.GetHexAt(new Vector2(toPositionWorld.x, toPositionWorld.z));
        toCell = World.HexGrid.GetClosestWalkableCell(fromCell, toCell);

        if (World.HexGrid.GetHexCost(toCell) == 0) {
            GD.Print("Invalid target point for FindPath(): returning empty path.");
            _planningPathWorldNavigationPoints = new List<NavigationPoint>();
            _planningPathWorldNavigationPoints.Add(new NavigationPoint(fromPositionWorld));
            _activePathWorldNavigationPoints = _planningPathWorldNavigationPoints;
            return;
        }

        List<HexCell> path = World.FindPath(entity, fromCell, toCell);

        // Generate grid navigation points
        _planningPathWorldNavigationPoints = new List<NavigationPoint>();
        foreach (int index in Enumerable.Range(0, path.Count)) {
            _planningPathWorldNavigationPoints.Add(
                new NavigationPoint(World.HexGrid.GetHexCenterVec3FromOffset(path[index].OffsetCoords)));
        }

        // Ensure the last point coincides with the target position
        if (_planningPathWorldNavigationPoints.Count > 0 &&
            (_planningPathWorldNavigationPoints.Last().WorldPosition - toPositionWorld).LengthSquared() <
            0.5f * 0.5f) {
            _planningPathWorldNavigationPoints[_planningPathWorldNavigationPoints.Count - 1].WorldPosition =
                toPositionWorld;
        }

        // Perform smoothing
        if (PathSmoothing)
        {
            _activePathWorldNavigationPoints = World.SmoothPath(entity, _planningPathWorldNavigationPoints);    
        }
        else
        {
            _activePathWorldNavigationPoints = _planningPathWorldNavigationPoints;
        }
        
        
        // Ensure starting point is the current position
        if (_activePathWorldNavigationPoints.Count > 0) {
            _activePathWorldNavigationPoints[0] = new NavigationPoint(fromPositionWorld);
        }
    }

    public void FindPath(Entity entity, Vector3 fromPositionWorld, NavigationPoint navigationPoint) {
        FindPath(entity, fromPositionWorld, navigationPoint.WorldPosition);

        _planningPathWorldNavigationPoints[_planningPathWorldNavigationPoints.Count - 1] = navigationPoint;
        _activePathWorldNavigationPoints[_activePathWorldNavigationPoints.Count - 1] =
            navigationPoint;
    }

    public void PlanGridPath(Entity entity, Vector3 fromPositionWorld, Vector3 toPositionWorld) {
        Vector2 fromPositionOffset = World.WorldToOffsetCoords(fromPositionWorld);
        Vector2 toPositionOffset = World.WorldToOffsetCoords(toPositionWorld);

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

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

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

        _pathWorldNavigationPoints = new List<NavigationPoint>();
        _pathWorldNavigationPoints.Add(
            new NavigationPoint(World.HexGrid.GetHexCenterVec3FromOffset(fromPositionOffset)));

        foreach (int index in Enumerable.Range(1, _path.Length - 1)) {
            _pathWorldNavigationPoints.Add(
                new NavigationPoint(World.GetHexCenterFromOffset(_path[index].OffsetCoords)));
        }

        if ((fromPositionWorld - World.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));
        if (_pathWorldNavigationPoints.Count > 2) {
            _smoothedPathWorldNavigationPoints = SmoothPath(entity, _pathWorldNavigationPoints);
            _pathWorldNavigationPoints = _smoothedPathWorldNavigationPoints;
        }

        UpdateCurrentGoal();
    }


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

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


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


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

        UpdateCurrentGoal();
    }


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

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


    public bool HasPathCollision(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld) {
        Vector3 fromPositionLocal = GlobalTransform.XformInv(fromPositionWorld);
        Vector3 toPositionLocal = GlobalTransform.XformInv(toPositionWorld);
        Vector3 relativeVelocity = GlobalTransform.basis.Xform(toPositionLocal - fromPositionLocal);

        KinematicCollision moveCollision = body.MoveAndCollide(relativeVelocity, true, true, true);
        if (moveCollision != null) {
            Spatial colliderSpatial = moveCollision.Collider as Spatial;
            // GD.Print("Found collision: " + moveCollision.Collider + " (" + colliderSpatial.Name + ")");
            return true;
        }

        return false;
    }


    public bool CheckSweptTriangleCellCollision(Vector3 startWorld, Vector3 endWorld, float radius) {
        Vector2 startPlane = new(startWorld.x, startWorld.z);
        Vector2 endPlane = new(endWorld.x, endWorld.z);
        Vector2 directionPlane = (endPlane - startPlane).Normalized();
        Vector2 sidePlane = directionPlane.Rotated(Mathf.Pi * 0.5f);

        List<HexCell> cells =
            World.HexGrid.GetCellsForLine(startPlane + directionPlane * radius, endPlane + directionPlane * radius);
        foreach (HexCell cell in cells) {
            if (World.HexGrid.GetHexCost(cell) == 0) {
                return true;
            }
        }

        cells = World.HexGrid.GetCellsForLine(startPlane + sidePlane * radius, endPlane + sidePlane * radius);
        foreach (HexCell cell in cells) {
            if (World.HexGrid.GetHexCost(cell) == 0) {
                return true;
            }
        }

        cells = World.HexGrid.GetCellsForLine(startPlane - sidePlane * radius, endPlane - sidePlane * radius);
        foreach (HexCell cell in cells) {
            if (World.HexGrid.GetHexCost(cell) == 0) {
                return true;
            }
        }

        return false;
    }

    public List<NavigationPoint> SmoothPath(KinematicBody body, List<NavigationPoint> navigationPoints) {
        if (navigationPoints.Count <= 2) {
            return navigationPoints;
        }

        Vector3 bodyGlobalTranslation = body.GlobalTranslation;
        List<NavigationPoint> smoothedPath = new();

        int startIndex = 0;
        int endIndex = navigationPoints.Count > 1 ? 1 : 0;
        smoothedPath.Add(navigationPoints[startIndex]);
        Vector3 startPoint = navigationPoints[startIndex].WorldPosition;

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

            if (CheckSweptTriangleCellCollision(startPoint, endPoint, 0.27f)) {
                if (endIndex - startIndex == 1) {
                    GD.Print("Aborting SmoothPath: input path passes through collision geometry.");
                    body.GlobalTranslation = bodyGlobalTranslation;
                    return smoothedPath;
                }

                smoothedPath.Add(navigationPoints[endIndex - 1]);
                startIndex = endIndex - 1;
                startPoint = navigationPoints[startIndex].WorldPosition;
                body.GlobalTranslation = startPoint;

                continue;
            }

            if (endIndex == navigationPoints.Count - 1) {
                break;
            }

            endIndex += 1;
        }

        smoothedPath.Add(navigationPoints[endIndex]);
        body.GlobalTranslation = bodyGlobalTranslation;

        return smoothedPath;
    }

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

    public void ActivatePlannedPath() {
        _pathWorldNavigationPoints = _activePathWorldNavigationPoints;
        UpdateCurrentGoal();
    }

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

        _currentGoal = _pathWorldNavigationPoints[0];
        CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
        CurrentGoalAngleWorld = _pathWorldNavigationPoints[0].WorldAngle;
    }

    private void ApplyExistingTransform(Transform worldTransform) {
        if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Orientation) {
            CurrentGoalPositionWorld = worldTransform.origin;
        } else if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Position) {
            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(worldTransform);
        }
    }


    public void UpdateCurrentGoal(Transform currentTransformWorld) {
        if (_currentGoal == null) {
            _currentGoal = new NavigationPoint(currentTransformWorld);
        }

        if (_pathWorldNavigationPoints.Count == 0) {
            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
            CurrentGoalPositionWorld = currentTransformWorld.origin;
            return;
        }

        if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)) {
            CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
        } else {
            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
        }

        if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation)) {
            CurrentGoalAngleWorld = _currentGoal.WorldAngle;
        } else {
            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
        }

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

            UpdateCurrentGoal();
            ApplyExistingTransform(currentTransformWorld);
        }

        if (_pathWorldNavigationPoints.Count == 0) {
            CurrentGoalPositionWorld = currentTransformWorld.origin;
            CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
        }
    }

    public bool IsGoalReached() {
        return _pathWorldNavigationPoints.Count == 0;
    }

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

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

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

        previousPoint = parentNode.GlobalTranslation;
        foreach (NavigationPoint point in _smoothedPathWorldNavigationPoints) {
            debugGeometry.SetColor(new Color(0, 0, 1));
            debugGeometry.AddVertex(previousPoint + yOffset);
            debugGeometry.AddVertex(point.WorldPosition + yOffset);

            previousPoint = point.WorldPosition;
        }

        previousPoint = parentNode.GlobalTranslation;
        foreach (NavigationPoint point in _planningPathWorldNavigationPoints) {
            debugGeometry.SetColor(new Color(1, 0, 1));
            debugGeometry.AddVertex(previousPoint + yOffset);
            debugGeometry.AddVertex(point.WorldPosition + yOffset);

            previousPoint = point.WorldPosition;
        }

        previousPoint = parentNode.GlobalTranslation;
        foreach (NavigationPoint point in _activePathWorldNavigationPoints) {
            debugGeometry.SetColor(new Color(1, 1, 0));
            debugGeometry.AddVertex(previousPoint + yOffset);
            debugGeometry.AddVertex(point.WorldPosition + yOffset);

            previousPoint = point.WorldPosition;
        }

        debugGeometry.End();
    }
}