Main/Assets/Plugins/Code/UniScene/PathGrid/Define/PathWorld.cs

using System;
using System.Collections.Generic;

using System.Text;
using SimpleAI;
using SimpleAI.Navigation;
using UnityEngine;
using Thousandto.Core.Base;

using PathEditor.Proxy.Plugin;

namespace Thousandto.Plugins.PathGrid
{
    public class PathWorld : SimpleAI.Grid, IPathTerrain
    {
        const float WeightScale = 24;

        public enum eNeighborDirection
        {
            kNoNeighbor = -1,
            kLeft,
            kTop,
            kRight,
            kBottom,
            kNumNeighbors
        };

        PathGirdData m_gridData;
        float m_weightRadiusSq = 0;

        public float weightRadiusSq
        {
            get
            {
                return m_weightRadiusSq;
            }
        }

        public PathWorld(PathGirdData data)
        {
            m_gridData = data;
            m_weightRadiusSq = data.WeightRadius * data.WeightRadius;
        }

        public void Awake()
        {
            base.Awake(m_gridData.Position, m_gridData.NumberOfRows, m_gridData.NumberOfColumns, m_gridData.CellSize, false);
        }

        public bool HasHeightMap()
        {
            return true;
        }

        public int GetMaxNeighborNumber()
        {
            return (int)eNeighborDirection.kNumNeighbors;
        }

        public int GetNeighbors(int index, ref int[] neighbors)
        {
            for (int i = 0; i < (int)eNeighborDirection.kNumNeighbors; i++)
            {
                neighbors[i] = GetNeighbor(index, (eNeighborDirection)i);
            }

            return (int)eNeighborDirection.kNumNeighbors;
        }

        public int GetNumNodes()
        {
            return m_gridData.NumberOfRows * m_gridData.NumberOfColumns;
        }

        public byte GetWeight_Unsafe(int col, int row)
        {
            return m_gridData.MergeWeightData[row, col];
        }

        public byte GetWeight(int col, int row)
        {
            if (!IsInBounds(col, row))
            {
                return byte.MaxValue;
            }
            return m_gridData.MergeWeightData[row, col];
        }

        public byte GetWeight(int index)
        {
            int row = GetRow(index);
            int col = GetColumn(index);
            if (!IsInBounds(col, row))
            {
                return byte.MaxValue;
            }
            return m_gridData.MergeWeightData[row, col];
        }

        public PathGridType GetCellType(int index)
        {
            int row = GetRow(index);
            int col = GetColumn(index);
            if (!IsInBounds(col, row))
            {
                return PathGridType.Block;
            }
            return m_gridData.GetCellType(col, row);
        }

        public PathGridType GetCellType(int col, int row)
        {
            if (!IsInBounds(col, row))
            {
                return PathGridType.Block;
            }
            return m_gridData.GetCellType(col, row);
        }

        public bool IsJump(ref Vector3 pos)
        {
            int cellIndex = GetCellIndex(ref pos);
            bool bInBounds = IsInBounds(cellIndex);
            if (!bInBounds)
            {
                return true;
            }

            int col = GetColumn(cellIndex);
            int row = GetRow(cellIndex);
            return m_gridData.IsJump(col, row);
        }

        public bool IsJump(int index)
        {
            int row = GetRow(index);
            int col = GetColumn(index);
            if (!IsInBounds(col, row))
            {
                return true;
            }
            return m_gridData.IsJump(col, row);
        }

        public bool IsJump(int col, int row)
        {
            if (!IsInBounds(col, row))
            {
                return true;
            }
            return m_gridData.IsJump(col, row);
        }

        public bool CanJump(int index, float height)
        {
            int row = GetRow(index);
            int col = GetColumn(index);
            if (!IsInBounds(col, row))
            {
                return true;
            }
            return m_gridData.CanJump(col, row, height);
        }

        // Determine if the position is blocked by collision
        public bool IsBlocked(ref Vector3 pos)
        {
            int cellIndex = GetCellIndex(ref pos);
            bool bInBounds = IsInBounds(cellIndex);
            if (!bInBounds)
            {
                return true;
            }

            int col = GetColumn(cellIndex);
            int row = GetRow(cellIndex);
            return m_gridData.IsBlock(col, row);
        }


        public bool IsBlocked(int index)
        {
            int row = GetRow(index);
            int col = GetColumn(index);
            if (!IsInBounds(col, row))
            {
                return true;
            }
            return m_gridData.IsBlock(col, row);
        }

        public bool IsSafe(int index)
        {
            int row = GetRow(index);
            int col = GetColumn(index);
            if (!IsInBounds(col, row))
            {
                return true;
            }
            return m_gridData.IsSafe(col, row);
        }

        public bool IsBlocked(int col, int row)
        {
            if (!IsInBounds(col, row))
            {
                return true;
            }
            return m_gridData.IsBlock(col, row);
        }

        public float GetGCost(int startIndex, int goalIndex)
        {
            Vector3 startPos = GetPathNodePos(startIndex);
            Vector3 goalPos = GetPathNodePos(goalIndex);
            float cost = MathLib.GetDistanceSq_XOZ(ref startPos, ref goalPos);
            return cost;
        }

        public float GetHCost(int startIndex, int goalIndex)
        {
            Vector3 startPos = GetPathNodePos(startIndex);
            Vector3 goalPos = GetPathNodePos(goalIndex);
            float heuristicWeight = 2.0f;
            float disSq = MathLib.GetDistanceSq_XOZ(ref startPos, ref goalPos);
            float cost = heuristicWeight * disSq;
            // Give extra cost to height difference
            if (disSq > weightRadiusSq)
            {
                cost += GetWeight(startIndex) * WeightScale;
            }
            return cost;
        }

        public bool IsNodeBlocked(int index)
        {
            return IsBlocked(index);
        }

        public Vector3 GetPathNodePos(int index)
        {
            // Use the center of the grid cell, as the position of the planning node.
            Vector3 nodePos = GetCellPosition(index);
            nodePos.x += m_halfCellSize;
            nodePos.z += m_halfCellSize;
            return nodePos;
        }

        public int GetPathNodeIndex(Vector3 pos)
        {
            int index = GetCellIndex(ref pos);
            if (!IsInBounds(index))
            {
                index = SimpleAI.Planning.Node.kInvalidIndex;
            }
            return index;
        }

        public int GetPathNodeIndex(Vector2 pos)
        {
            int index = GetCellIndex(ref pos);
            if (!IsInBounds(index))
            {
                index = SimpleAI.Planning.Node.kInvalidIndex;
            }
            return index;
        }

        public int GetPathNodeIndex(ref Vector3 pos)
        {
            int index = GetCellIndex(ref pos);
            if (!IsInBounds(index))
            {
                index = SimpleAI.Planning.Node.kInvalidIndex;
            }
            return index;
        }

        public int GetPathNodeIndex(ref Vector2 pos)
        {
            int index = GetCellIndex(ref pos);
            if (!IsInBounds(index))
            {
                index = SimpleAI.Planning.Node.kInvalidIndex;
            }
            return index;
        }

        public void ComputePortalsForPathSmoothing(Vector3[] roughPath, out Vector3[] aLeftPortalEndPts, out Vector3[] aRightPortalEndPts)
        {
            aLeftPortalEndPts = null;
            aRightPortalEndPts = null;
        }

        public Vector3 GetValidPathFloorPos(Vector3 position)
        {
            // Save this value off, in case we need to use it to search for a valid location further along in this function.
            Vector3 originalPosition = position;

            // Find a position that is within the grid, at the same height as the grid.
            float padding = m_cellSize / 4.0f;
            Bounds gridBounds = GetGridBounds();
            position.x = Mathf.Clamp(position.x, gridBounds.min.x + padding, gridBounds.max.x - padding);
            position.y = Origin.y;
            position.z = Mathf.Clamp(position.z, gridBounds.min.z + padding, gridBounds.max.z - padding);

            // If this position is blocked, then look at all of the neighbors of this cell, and see if one of those cells is
            // unblocked. If one of those neighbors is unblocked, then we return the position of that neighbor, to ensure that 
            // the agent is always pathing to and from valid positions.
            int cellIndex = GetCellIndex(ref position);
            if (IsBlocked(cellIndex))
            {
                // Find the closest unblocked neighbor, if one exists.
                int[] neighbors = new int[GetMaxNeighborNumber()];
                int numNeighbors = GetNeighbors(cellIndex, ref neighbors);
                float closestDistSq = float.MaxValue;
                for (int i = 0; i < numNeighbors; i++)
                {
                    int neighborIndex = neighbors[i];
                    if (!IsBlocked(neighborIndex))
                    {
                        Vector3 neighborPos = GetCellCenter(neighborIndex);
                        float distToCellSq = (neighborPos - originalPosition).sqrMagnitude;
                        if (distToCellSq < closestDistSq)
                        {
                            closestDistSq = distToCellSq;
                            position = neighborPos;
                        }
                    }
                }
            }

            return position;
        }

        public float GetTerrainHeight(ref Vector3 position)
        {
            var index = GetCellIndex(ref position);
            int row = GetRow(index);
            int col = GetColumn(index);
            if (!IsInBounds(col, row))
            {
                return m_gridData.Position.y;
            }
            return m_gridData.HeightMap[row * m_gridData.NumberOfColumns + col] / 100f;
        }

        #region Neighbor Functions
        public eNeighborDirection GetNeighborDirection(int index, int neighborIndex)
        {
            for (int i = 0; i < (int)eNeighborDirection.kNumNeighbors; i++)
            {
                int testNeighborIndex = GetNeighbor(index, (eNeighborDirection)i);
                if (testNeighborIndex == neighborIndex)
                {
                    return (eNeighborDirection)i;
                }
            }

            return eNeighborDirection.kNoNeighbor;
        }

        private int GetNeighbor(int index, eNeighborDirection neighborDirection)
        {
            Vector3 neighborPos = GetCellCenter(index);

            switch (neighborDirection)
            {
                case eNeighborDirection.kLeft:
                    neighborPos.x -= m_cellSize;
                    break;
                case eNeighborDirection.kTop:
                    neighborPos.z += m_cellSize;
                    break;
                case eNeighborDirection.kRight:
                    neighborPos.x += m_cellSize;
                    break;
                case eNeighborDirection.kBottom:
                    neighborPos.z -= m_cellSize;
                    break;
                default:
                    System.Diagnostics.Debug.Assert(false);
                    break;
            };

            int neighborIndex = GetCellIndex(ref neighborPos);
            if (!IsInBounds(neighborIndex))
            {
                neighborIndex = (int)eNeighborDirection.kNoNeighbor;
            }

            return neighborIndex;
        }
        #endregion
    }
}