JisolGame/JNFrame/Assets/HPJ Pathfinding Pro/Scripts/1.Presentation/3.Obstacles/NavigationObstacleMesh.cs

using HPJ.Simulation;
using HPJ.Simulation.Enums;
using HPJ.Simulation.Map;
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace HPJ.Presentation.Obstacle
{
    /// <summary>
    /// A Navigation Obstacle that adjusts the Map based on the given mesh, position and rotation 
    /// </summary>
    public class NavigationObstacleMesh : NavigationObstacle
    {
        #region Unity

        /// <summary>
        /// Mesh Renderer we want to use for the obstacle
        /// </summary>
        [SerializeField]
        protected MeshRenderer ObstacleRenderer;
        /// <summary>
        /// Mesh we want to use for the obstacle
        /// </summary>
        [SerializeField]
        protected MeshFilter ObstacleFilter;

        private void Awake()
        {
            // Generate all the connection points that will need to be tested for collision
            Vertices.Clear();
            VertexConnections.Clear();

            VertexCount = 0;
            for (int i = 0; i < ObstacleFilter.mesh.vertices.Length; i++)
            {
                if (!Points.ContainsKey(ObstacleFilter.mesh.vertices[i]))
                {
                    VertexPoints NewVertex = new VertexPoints(ObstacleFilter.mesh.vertices[i], VertexCount);
                    Points.Add(ObstacleFilter.mesh.vertices[i], NewVertex);
                    Vertices.Add(NewVertex);
                    VertexCount++;
                }

                VertexPoints Vertex = Points[ObstacleFilter.mesh.vertices[i]];
                Vertex.SharedPoints.Add(i);
            }

            int count = 0;
            Vector3Int CurrentTriangle = new Vector3Int();
            foreach (int vertexIndex in ObstacleFilter.mesh.triangles)
            {
                count++;
                if (count == 1)
                {
                    CurrentTriangle.x = vertexIndex;
                }
                else if (count == 2)
                {
                    CurrentTriangle.y = vertexIndex;
                }
                else if (count == 3)
                {
                    CurrentTriangle.z = vertexIndex;

                    foreach (VertexPoints Vertex in Points.Values)
                    {
                        if (Vertex.ContainsTriangeIndex(CurrentTriangle))
                        {
                            if (!Vertex.ContainsTriange(CurrentTriangle))
                            {
                                Vertex.Triangles.Add(CurrentTriangle);
                            }
                        }
                    }

                    Triangles.Add(CurrentTriangle);
                    count = 0;
                }
            }

            foreach (VertexPoints Vertex in Points.Values)
            {
                foreach (Vector3Int Triange in Vertex.Triangles)
                {
                    Vector3 PositionX = ObstacleFilter.mesh.vertices[Triange.x];
                    if (PositionX != Vertex.VertexPosition)
                    {
                        VertexPoints OtherVertex = Points[PositionX];
                        if (!Vertex.ConnectedVertex.Contains(OtherVertex))
                        {
                            Vertex.ConnectedVertex.Add(OtherVertex);
                        }
                    }

                    Vector3 PositionY = ObstacleFilter.mesh.vertices[Triange.y];
                    if (PositionY != Vertex.VertexPosition)
                    {
                        VertexPoints OtherVertex = Points[PositionY];
                        if (!Vertex.ConnectedVertex.Contains(OtherVertex))
                        {
                            Vertex.ConnectedVertex.Add(OtherVertex);
                        }
                    }

                    Vector3 PositionZ = ObstacleFilter.mesh.vertices[Triange.z];
                    if (PositionZ != Vertex.VertexPosition)
                    {
                        VertexPoints OtherVertex = Points[PositionZ];
                        if (!Vertex.ConnectedVertex.Contains(OtherVertex))
                        {
                            Vertex.ConnectedVertex.Add(OtherVertex);
                        }
                    }
                }
            }

            foreach (VertexPoints Vertex in Points.Values)
            {
                foreach (VertexPoints ConnectedVertex in Vertex.ConnectedVertex)
                {
                    Vector2Int IndexKey = new Vector2Int();
                    if (Vertex.VertexIndex < ConnectedVertex.VertexIndex)
                    {
                        IndexKey.x = Vertex.VertexIndex;
                        IndexKey.y = ConnectedVertex.VertexIndex;
                    }
                    else
                    {
                        IndexKey.x = ConnectedVertex.VertexIndex;
                        IndexKey.y = Vertex.VertexIndex;
                    }

                    if (!Connections.ContainsKey(IndexKey))
                    {
                        VertexConnection NewConnection = new VertexConnection(Vertex, ConnectedVertex);
                        Connections.Add(IndexKey, NewConnection);
                        VertexConnections.Add(NewConnection);
                    }
                }
            }
        }

        // Start is called before the first frame update
        public override void Start()
        {
            MapTileRelationships = new Dictionary<MapSet, MapObstacleRelationshipData>();
            OrientationChanged(transform.rotation, transform.position);
            base.Start();
        }

        public virtual void OnDrawGizmosSelected()
        {
            Gizmos.color = Color.red;
            foreach (VertexConnection Connection in VertexConnections)
            {
                Vector3 A = ObstacleRenderer.transform.localToWorldMatrix.MultiplyPoint3x4(Connection.PointA.VertexPosition);
                Vector3 B = ObstacleRenderer.transform.localToWorldMatrix.MultiplyPoint3x4(Connection.PointB.VertexPosition);

                Gizmos.DrawLine(A, B);
            }

            Gizmos.color = Color.blue;
            foreach (Vector3Int Triange in Triangles)
            {
                Vector3 A = ObstacleRenderer.transform.localToWorldMatrix.MultiplyPoint3x4(ObstacleFilter.mesh.vertices[Triange.x]);
                A.y = transform.position.y;
                Vector3 B = ObstacleRenderer.transform.localToWorldMatrix.MultiplyPoint3x4(ObstacleFilter.mesh.vertices[Triange.y]);
                B.y = transform.position.y;
                Vector3 C = ObstacleRenderer.transform.localToWorldMatrix.MultiplyPoint3x4(ObstacleFilter.mesh.vertices[Triange.z]);
                C.y = transform.position.y;

                Gizmos.DrawLine(A, B);
                Gizmos.DrawLine(A, C);
                Gizmos.DrawLine(C, B);
            }
        }

        #endregion

        #region Obstacle

        /// <summary>
        /// Type of Collision we want to use for this obstacle
        /// </summary>
        public ObstacleCollisionCheckType CollisionCheckMode = ObstacleCollisionCheckType.Both;

        /// <summary>
        /// A Dictionary of the Vertex points based on their physical position relative to the default mesh
        /// </summary>
        public Dictionary<Vector3, VertexPoints> Points = new Dictionary<Vector3, VertexPoints>();
        /// <summary>
        /// A List of the Vertex points for this mesh
        /// </summary>
        public List<VertexPoints> Vertices = new List<VertexPoints>();
        /// <summary>
        /// A Dictionary of the connection based on index of the smallest vertex point index as Vector2Int.x and the largest vertex point index as Vector2Int.y
        /// </summary>
        public Dictionary<Vector2Int, VertexConnection> Connections = new Dictionary<Vector2Int, VertexConnection>();
        /// <summary>
        /// A List of the Vertex connections for this mesh
        /// </summary>
        public List<VertexConnection> VertexConnections = new List<VertexConnection>();
        /// <summary>
        /// A List of the traingles for the mesh
        /// </summary>
        public List<Vector3Int> Triangles = new List<Vector3Int>();

        /// <summary>
        /// The number of Vertex on this Mesh
        /// </summary>
        public int VertexCount { get; protected set; }

        protected Vector3 _previousPosition = new Vector3();
        /// <summary>
        /// Checks to see if the new position changes and if so it changes the mesh to block out the new corresponding tiles
        /// </summary>
        /// <param name="NewPosition"></param>
        /// <param name="ForceCheck"></param>
        public override void UpdatePosition(Vector3 NewPosition, bool ForceCheck = false)
        {
            if (NewPosition != _previousPosition || _previousRotation != transform.rotation)
            {
                OrientationChanged(transform.rotation, NewPosition);
                transform.position = NewPosition;
                _previousPosition = NewPosition;
                _previousRotation = transform.rotation;
            }
            else
            {
                RevalidateTiles();
            }
        }

        /// <summary>
        /// Revalidate tiles
        /// </summary>
        public override void RevalidateTiles()
        {
            foreach (MapObstacleRelationshipData Data in MapTileRelationships.Values)
            {
                Data.Revalidate();
            }
        }

        /// <summary>
        /// What happens the the obstacle mesh position changes
        /// </summary>
        /// <param name="NewPosition"></param>
        /// <returns></returns>
        protected override bool PositionChanged(Vector3 NewPosition)
        {
            return false;
        }

        protected Quaternion _previousRotation = Quaternion.identity;
        /// <summary>
        /// Rotates the obstacle based on the desired Rotation. Basic NavigationObstacle will only swap the object size x and y values. Inherit to ovverride this behaviour
        /// </summary>
        /// <param name="Rotation"></param>
        public override void Rotate(Quaternion Rotation)
        {
            if (_previousPosition != transform.position || transform.rotation != Rotation)
            {
                OrientationChanged(Rotation, transform.position);
                transform.rotation = Rotation;
            }
        }

        protected PositionSquareRect OrientationRect;
        /// <summary>
        /// Changes the mesh to block out the new corresponding tiles
        /// </summary>
        /// <param name="Rotation"></param>
        /// <param name="NewPosition"></param>
        protected virtual void OrientationChanged(Quaternion Rotation, Vector3 NewPosition)
        {
            ResizeBoundsEstimate();

            foreach (MapObstacleRelationshipData Data in MapTileRelationships.Values)
            {
                Data.UnvalidateTiles();
            }

            foreach (MapSet Set in NavigationManager.Instance.MapSets)
            {
                if (!NavigationManager.Instance.RectOnMap(Set, OrientationRect, _checkMapHeight))
                {
                    continue;
                }

                MapObstacleRelationshipData RelationshipData;
                if (MapTileRelationships.ContainsKey(Set))
                {
                    RelationshipData = MapTileRelationships[Set];
                }
                else
                {
                    RelationshipData = new MapObstacleRelationshipData(Set, this, _obstacleTileType);
                    MapTileRelationships.Add(Set, RelationshipData);
                }

                IntVector2 HigherBound = Set.GetMapTileIndex(NavigationManager.Instance.GetTileWorldPosition(OrientationRect.Corners[0]));
                IntVector2 LowerBound = Set.GetMapTileIndex(NavigationManager.Instance.GetTileWorldPosition(OrientationRect.Corners[2]));
                for (int x = LowerBound.x; x <= HigherBound.x; x++)
                {
                    for (int y = LowerBound.y; y <= HigherBound.y; y++)
                    {
                        if (Set.GetTileType(x, y) != TileTypes.OutOfBounds)
                        {
                            //Debug.Log("Change Tile");
                            TileBounds Bounds = NavigationManager.Instance.GetTileBoundsForMap(x, y, Set);
                            if (ObstacleCollidesWithBounds(Bounds, CollisionCheckMode))
                            {
                                IntVector3 TileKey = new IntVector3(x, y, Set.InstanceID);
                                RelationshipData.AddOrValidateTile(TileKey);
                            }
                        }
                    }
                }
            }

            foreach (MapObstacleRelationshipData Data in MapTileRelationships.Values)
            {
                ChangeMapJob GeneratedMapChangeJob = Data.GenerateMapChangeJob();
                if (GeneratedMapChangeJob != null)
                {
                    //Debug.Log("Change Map From Job");
                    Data.Map.ChangeMap(GeneratedMapChangeJob);
                }
                Data.SortRelatedTileData();
            }
        }

        /// <summary>
        /// Collision Check for the obstacle and the maps Tiles
        /// </summary>
        /// <param name="Bounds"></param>
        /// <param name="CheckMode"></param>
        /// <returns></returns>
        public bool ObstacleCollidesWithBounds(TileBounds Bounds, ObstacleCollisionCheckType CheckMode = ObstacleCollisionCheckType.Both)
        {
            if (CheckMode == ObstacleCollisionCheckType.LineCollisions)
            {
                return ObstacleLinesCollide(Bounds);
            }
            else if (CheckMode == ObstacleCollisionCheckType.AreaCollisions)
            {
                return ObstacleAreaCollide(Bounds);
            }
            else if (CheckMode == ObstacleCollisionCheckType.Both)
            {
                return ObstacleAreaCollide(Bounds) || ObstacleLinesCollide(Bounds);
            }

            return OrientationRect.InBounds(Bounds.BottomRight) || OrientationRect.InBounds(Bounds.BottomLeft) || OrientationRect.InBounds(Bounds.TopLeft) || OrientationRect.InBounds(Bounds.TopRight) || Bounds.InBounds(OrientationRect.Corners[0]) || Bounds.InBounds(OrientationRect.Corners[1]) || Bounds.InBounds(OrientationRect.Corners[2]) || Bounds.InBounds(OrientationRect.Corners[3]);
        }

        /// <summary>
        /// Line Line Collision between all the vertex points and the map tiles
        /// </summary>
        /// <param name="Bounds"></param>
        /// <returns></returns>
        protected bool ObstacleLinesCollide(TileBounds Bounds)
        {
            foreach (VertexConnection connection in VertexConnections)
            {
                if (Bounds.IntersectsBounds(GetVertexPosition(connection.PointB.VertexPosition), GetVertexPosition(connection.PointA.VertexPosition)))
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>
        /// Area Collision between all the vertex triangles and the map tiles
        /// </summary>
        /// <param name="Bounds"></param>
        /// <returns></returns>
        protected bool ObstacleAreaCollide(TileBounds Bounds)
        {
            foreach (Vector3Int Triange in Triangles)
            {
                Vector3 TriangePoint1 = GetVertexPosition(ObstacleFilter.mesh.vertices[Triange.x]);
                Vector3 TriangePoint2 = GetVertexPosition(ObstacleFilter.mesh.vertices[Triange.y]);
                Vector3 TriangePoint3 = GetVertexPosition(ObstacleFilter.mesh.vertices[Triange.z]);

                if (IsInsideTriange(TriangePoint1, TriangePoint2, TriangePoint3, Bounds))
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>
        /// Calculates the Area of the triange with the given points
        /// </summary>
        /// <param name="point1"></param>
        /// <param name="point2"></param>
        /// <param name="point3"></param>
        /// <returns></returns>
        public float TriangeArea(Vector3 point1, Vector3 point2, Vector3 point3)
        {
            return Mathf.Abs((point1.x * (point2.z - point3.z) + point2.x * (point3.z - point1.z) + point3.x * (point1.z - point2.z)) / 2.0f);
        }

        /// <summary>
        /// Checks to see if a given point is withen a triangle
        /// </summary>
        /// <param name="TriangePoint1"></param>
        /// <param name="TriangePoint2"></param>
        /// <param name="TriangePoint3"></param>
        /// <param name="CheckingPoint"></param>
        /// <returns></returns>
        public bool IsInsideTriange(Vector3 TriangePoint1, Vector3 TriangePoint2, Vector3 TriangePoint3, Vector3 CheckingPoint)
        {
            // Calculate area of triangle ABC
            float A = TriangeArea(TriangePoint1, TriangePoint2, TriangePoint3);

            // Calculate area of triangle PBC
            float A1 = TriangeArea(CheckingPoint, TriangePoint2, TriangePoint3);

            // Calculate area of triangle APC
            float A2 = TriangeArea(TriangePoint1, CheckingPoint, TriangePoint3);

            //Calculate area of triangle ABP
            float A3 = TriangeArea(TriangePoint1, TriangePoint2, CheckingPoint);

            // Check if sum of A1, A2 and A3 is same as A
            int AInt = Mathf.RoundToInt(A * 100);
            int SumA123 = Mathf.RoundToInt((A1 + A2 + A3) * 100);

            return AInt == SumA123;
        }

        /// <summary>
        /// Checks to see if a given tile is withen a triangle
        /// </summary>
        /// <param name="TriangePoint1"></param>
        /// <param name="TriangePoint2"></param>
        /// <param name="TriangePoint3"></param>
        /// <param name="CheckingPoint"></param>
        /// <returns></returns>
        public bool IsInsideTriange(Vector3 TriangePoint1, Vector3 TriangePoint2, Vector3 TriangePoint3, TileBounds TileBounds)
        {
            return IsInsideTriange(TriangePoint1, TriangePoint2, TriangePoint3, TileBounds.BottomLeft) ||
                IsInsideTriange(TriangePoint1, TriangePoint2, TriangePoint3, TileBounds.TopRight) ||
                IsInsideTriange(TriangePoint1, TriangePoint2, TriangePoint3, TileBounds.BottomRight) ||
                IsInsideTriange(TriangePoint1, TriangePoint2, TriangePoint3, TileBounds.TopLeft) ||
                IsInsideTriange(TriangePoint1, TriangePoint2, TriangePoint3, TileBounds.Center);
        }

        /// <summary>
        /// Converts a position relative to the local transforms roation and scale
        /// </summary>
        /// <param name="Position"></param>
        /// <returns></returns>
        public Vector3 GetVertexPosition(Vector3 Position)
        {
            return ObstacleRenderer.transform.localToWorldMatrix.MultiplyPoint3x4(Position);
        }

        /// <summary>
        /// Calculates the bounds of the mesh
        /// </summary>
        public void ResizeBoundsEstimate()
        {
            if (Vertices.Count <= 0)
            {
                return;
            }

            Vector3 LowerBounds = GetVertexPosition(Vertices[0].VertexPosition);
            Vector3 HigherBounds = LowerBounds;

            for(int i = 1; i < Vertices.Count; i++)
            {
                Vector3 VertexWorldPosition = GetVertexPosition(Vertices[i].VertexPosition);

                if (VertexWorldPosition.x < LowerBounds.x)
                {
                    LowerBounds.x = VertexWorldPosition.x;
                }
                else if (VertexWorldPosition.x > HigherBounds.x)
                {
                    HigherBounds.x = VertexWorldPosition.x;
                }

                if (VertexWorldPosition.y < LowerBounds.y)
                {
                    LowerBounds.y = VertexWorldPosition.y;
                }
                else if (VertexWorldPosition.y > HigherBounds.y)
                {
                    HigherBounds.y = VertexWorldPosition.y;
                }

                if (VertexWorldPosition.z < LowerBounds.z)
                {
                    LowerBounds.z = VertexWorldPosition.z;
                }
                else if (VertexWorldPosition.z > HigherBounds.z)
                {
                    HigherBounds.z = VertexWorldPosition.z;
                }
            }

            OrientationRect = new PositionSquareRect(transform.position, LowerBounds, HigherBounds);
        }

        #endregion

        /// <summary>
        /// A Vertex Point based on the obstacle mesh
        /// </summary>
        [Serializable]
        public class VertexPoints
        {
            public int VertexIndex;
            public Vector3 VertexPosition;
            public List<int> SharedPoints = new List<int>();
            public List<Vector3Int> Triangles = new List<Vector3Int>();

            [NonSerialized] public List<VertexPoints> ConnectedVertex = new List<VertexPoints>();

            public VertexPoints(Vector3 Position, int vertexIndex)
            {
                VertexPosition = Position;
                VertexIndex = vertexIndex;
            }

            /// <summary>
            /// Checks to see if this triange shares a mesh index with this vertex point
            /// </summary>
            /// <param name="currentTriangle"></param>
            /// <returns></returns>
            public bool ContainsTriangeIndex(Vector3Int currentTriangle)
            {
                if (SharedPoints.Contains(currentTriangle.x) || SharedPoints.Contains(currentTriangle.y) || SharedPoints.Contains(currentTriangle.z))
                {
                    return true;
                }

                return false;
            }

            /// <summary>
            /// Checks to see if this triange shares has this specific triange
            /// </summary>
            /// <param name="currentTriangle"></param>
            /// <returns></returns>
            public bool ContainsTriange(Vector3Int currentTriangle)
            {
                return Triangles.Contains(currentTriangle);
            }
        }

        /// <summary>
        /// This is a connection between two vertex points
        /// </summary>
        [Serializable]
        public class VertexConnection
        {
            public VertexPoints PointA;
            public VertexPoints PointB;

            public VertexConnection(VertexPoints A, VertexPoints B)
            {
                PointA = A;
                PointB = B;
            }
        }

        /// <summary>
        /// The Type of Collision the Obstacle Mesh attempts
        /// </summary>
        public enum ObstacleCollisionCheckType
        { 
            None,
            LineCollisions,
            AreaCollisions,
            Both,
        }
    }
}