443 lines
15 KiB
C#
443 lines
15 KiB
C#
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using UnityEngine;
|
|||
|
|
using static HxGame.PathFinding.PathFinderFast;
|
|||
|
|
|
|||
|
|
namespace HxGame.PathFinding
|
|||
|
|
{
|
|||
|
|
public class PathFinderFastNonSQR : IPathFinder
|
|||
|
|
{
|
|||
|
|
private CellNode[] mGrid = null;
|
|||
|
|
private PriorityQueueB<int> mOpen = null;
|
|||
|
|
private List<PathFinderNode> mClose = new List<PathFinderNode>();
|
|||
|
|
private HeuristicFormula mFormula = HeuristicFormula.Manhattan;
|
|||
|
|
private bool mDiagonals = true;
|
|||
|
|
private bool mHexagonalGrid = false;
|
|||
|
|
private float mHEstimate = 1;
|
|||
|
|
private float mHeavyDiagonalsCost = 1.4f;
|
|||
|
|
private int mMaxSteps = 2000;
|
|||
|
|
private float mMaxSearchCost = 100000;
|
|||
|
|
private PathFinderNodeFast[] mCalcGrid = null;
|
|||
|
|
private byte mOpenNodeValue = 1;
|
|||
|
|
private byte mCloseNodeValue = 2;
|
|||
|
|
private OnCellCross mOnCellCross = null;
|
|||
|
|
|
|||
|
|
private float mH = 0;
|
|||
|
|
private int mLocation = 0;
|
|||
|
|
private int mNewLocation = 0;
|
|||
|
|
private ushort mLocationX = 0;
|
|||
|
|
private ushort mLocationY = 0;
|
|||
|
|
private ushort mNewLocationX = 0;
|
|||
|
|
private ushort mNewLocationY = 0;
|
|||
|
|
private ushort mGridX = 0;
|
|||
|
|
private ushort mGridY = 0;
|
|||
|
|
private bool mFound = false;
|
|||
|
|
|
|||
|
|
private sbyte[,] mDirection = new sbyte[8, 2]
|
|||
|
|
{
|
|||
|
|
{ 0, -1}, //bottom
|
|||
|
|
{ 1, 0}, //right
|
|||
|
|
{ 0, 1}, //top
|
|||
|
|
{ -1, 0}, //left
|
|||
|
|
{ 1, -1}, //right-bottom
|
|||
|
|
{ 1, 1}, //right-top
|
|||
|
|
{ -1, 1}, //left-top
|
|||
|
|
{ -1, -1}, //left-bottom
|
|||
|
|
};
|
|||
|
|
private readonly sbyte[,] mDirectionHex0 = new sbyte[6, 2]
|
|||
|
|
{
|
|||
|
|
{ 0, -1 }, //top
|
|||
|
|
{ 1, 0 }, //right
|
|||
|
|
{ 0, 1 }, //bottom
|
|||
|
|
{ -1, 0 }, //left
|
|||
|
|
{ 1, 1}, //right-bottom
|
|||
|
|
{ -1,1 } //left-bottom
|
|||
|
|
};
|
|||
|
|
private readonly sbyte[,] mDirectionHex1 = new sbyte[6, 2]
|
|||
|
|
{
|
|||
|
|
{ 0, -1 }, //top
|
|||
|
|
{ 1, 0 }, //right
|
|||
|
|
{ 0, 1 }, //bottom
|
|||
|
|
{ -1, 0 }, //left
|
|||
|
|
{ -1, -1 }, //left-top
|
|||
|
|
{ 1, -1} //right-top
|
|||
|
|
};
|
|||
|
|
private readonly int[] mCellSide0 = new int[6] {
|
|||
|
|
(int)CELL_SIDE.Bottom,
|
|||
|
|
(int)CELL_SIDE.BottomRight,
|
|||
|
|
(int)CELL_SIDE.Top,
|
|||
|
|
(int)CELL_SIDE.BottomLeft,
|
|||
|
|
(int)CELL_SIDE.TopRight,
|
|||
|
|
(int)CELL_SIDE.TopLeft
|
|||
|
|
};
|
|||
|
|
private readonly int[] mCellSide1 = new int[6] {
|
|||
|
|
(int)CELL_SIDE.Bottom,
|
|||
|
|
(int)CELL_SIDE.TopRight,
|
|||
|
|
(int)CELL_SIDE.Top,
|
|||
|
|
(int)CELL_SIDE.TopLeft,
|
|||
|
|
(int)CELL_SIDE.BottomLeft,
|
|||
|
|
(int)CELL_SIDE.BottomRight
|
|||
|
|
};
|
|||
|
|
private readonly int[] mCellBoxSides = new int[8] {
|
|||
|
|
(int)CELL_SIDE.Bottom,
|
|||
|
|
(int)CELL_SIDE.Right,
|
|||
|
|
(int)CELL_SIDE.Top,
|
|||
|
|
(int)CELL_SIDE.Left,
|
|||
|
|
(int)CELL_SIDE.BottomRight,
|
|||
|
|
(int)CELL_SIDE.TopRight,
|
|||
|
|
(int)CELL_SIDE.TopLeft,
|
|||
|
|
(int)CELL_SIDE.BottomLeft
|
|||
|
|
};
|
|||
|
|
private int mEndLocation = 0;
|
|||
|
|
private float mNewG = 0;
|
|||
|
|
private int mCellGroupMask = -1;
|
|||
|
|
private bool mIgnoreCanCrossCheck;
|
|||
|
|
private bool mIgnoreCellCost;
|
|||
|
|
private bool mIncludeInvisibleCells;
|
|||
|
|
|
|||
|
|
public void SetCalcMatrix(CellNode[] grid)
|
|||
|
|
{
|
|||
|
|
if (grid == null)
|
|||
|
|
throw new Exception("Grid cannot be null");
|
|||
|
|
if (grid.Length != mGrid.Length) // mGridX != (ushort) (mGrid.GetUpperBound(0) + 1) || mGridY != (ushort) (mGrid.GetUpperBound(1) + 1))
|
|||
|
|
throw new Exception("SetCalcMatrix called with matrix with different dimensions. Call constructor instead.");
|
|||
|
|
mGrid = grid;
|
|||
|
|
|
|||
|
|
Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
|
|||
|
|
ComparePFNodeMatrix comparer = (ComparePFNodeMatrix)mOpen.comparer;
|
|||
|
|
comparer.SetMatrix(mCalcGrid);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public HeuristicFormula Formula
|
|||
|
|
{
|
|||
|
|
get { return mFormula; }
|
|||
|
|
set { mFormula = value; }
|
|||
|
|
}
|
|||
|
|
public bool Diagonals
|
|||
|
|
{
|
|||
|
|
get { return mDiagonals; }
|
|||
|
|
set
|
|||
|
|
{
|
|||
|
|
mDiagonals = value;
|
|||
|
|
if (mDiagonals)
|
|||
|
|
mDirection = new sbyte[8, 2] {
|
|||
|
|
{ 0, -1 },
|
|||
|
|
{ 1, 0 },
|
|||
|
|
{ 0, 1 },
|
|||
|
|
{ -1, 0 },
|
|||
|
|
{ 1, -1 },
|
|||
|
|
{ 1, 1 },
|
|||
|
|
{ -1, 1 },
|
|||
|
|
{ -1, -1 }
|
|||
|
|
};
|
|||
|
|
else
|
|||
|
|
mDirection = new sbyte[4, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public float HeavyDiagonalsCost
|
|||
|
|
{
|
|||
|
|
get { return mHeavyDiagonalsCost; }
|
|||
|
|
set { mHeavyDiagonalsCost = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public bool HexagonalGrid
|
|||
|
|
{
|
|||
|
|
get { return mHexagonalGrid; }
|
|||
|
|
set { mHexagonalGrid = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public float HeuristicEstimate
|
|||
|
|
{
|
|||
|
|
get { return mHEstimate; }
|
|||
|
|
set { mHEstimate = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public float MaxSearchCost
|
|||
|
|
{
|
|||
|
|
get { return mMaxSearchCost; }
|
|||
|
|
set { mMaxSearchCost = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public int MaxSteps
|
|||
|
|
{
|
|||
|
|
get { return mMaxSteps; }
|
|||
|
|
set { mMaxSteps = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public OnCellCross OnCellCross
|
|||
|
|
{
|
|||
|
|
get { return mOnCellCross; }
|
|||
|
|
set { mOnCellCross = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public int CellGroupMask
|
|||
|
|
{
|
|||
|
|
get { return mCellGroupMask; }
|
|||
|
|
set { mCellGroupMask = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public bool IgnoreCanCrossCheck
|
|||
|
|
{
|
|||
|
|
get { return mIgnoreCanCrossCheck; }
|
|||
|
|
set { mIgnoreCanCrossCheck = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public bool IgnoreCellCost
|
|||
|
|
{
|
|||
|
|
get { return mIgnoreCellCost; }
|
|||
|
|
set { mIgnoreCellCost = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public bool IncludeInvisibleCells
|
|||
|
|
{
|
|||
|
|
get { return mIncludeInvisibleCells; }
|
|||
|
|
set { mIncludeInvisibleCells = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public PathFinderFastNonSQR(CellNode[] grid, int gridWidth, int gridHeight)
|
|||
|
|
{
|
|||
|
|
if (grid == null)
|
|||
|
|
throw new Exception("Grid cannot be null");
|
|||
|
|
|
|||
|
|
mGrid = grid;
|
|||
|
|
mGridX = (ushort)gridWidth;
|
|||
|
|
mGridY = (ushort)gridHeight;
|
|||
|
|
|
|||
|
|
if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY))
|
|||
|
|
mCalcGrid = new PathFinderNodeFast[mGridX * mGridY];
|
|||
|
|
|
|||
|
|
mOpen = new PriorityQueueB<int>(new ComparePFNodeMatrix(mCalcGrid));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public List<PathFinderNode> FindPath(CellNode startCell, CellNode endCell, out float totalCost, bool evenLayout)
|
|||
|
|
{
|
|||
|
|
PathFindingPoint start = new PathFindingPoint(startCell.X, startCell.Y);
|
|||
|
|
PathFindingPoint end = new PathFindingPoint(endCell.X, endCell.Y);
|
|||
|
|
|
|||
|
|
totalCost = 0;
|
|||
|
|
mFound = false;
|
|||
|
|
int evenLayoutValue = evenLayout ? 1 : 0;
|
|||
|
|
if (mOpenNodeValue > 250)
|
|||
|
|
{
|
|||
|
|
Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
|
|||
|
|
mOpenNodeValue = 1;
|
|||
|
|
mCloseNodeValue = 2;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
mOpenNodeValue += 2;
|
|||
|
|
mCloseNodeValue += 2;
|
|||
|
|
}
|
|||
|
|
mOpen.Clear();
|
|||
|
|
mClose.Clear();
|
|||
|
|
int maxi;
|
|||
|
|
if (mHexagonalGrid)
|
|||
|
|
maxi = 6;
|
|||
|
|
else
|
|||
|
|
maxi = mDiagonals ? 8 : 4;
|
|||
|
|
|
|||
|
|
mLocation = (start.y * mGridX) + start.x;
|
|||
|
|
mEndLocation = (end.y * mGridX) + end.x;
|
|||
|
|
mCalcGrid[mLocation].G = 0;
|
|||
|
|
mCalcGrid[mLocation].F = mHEstimate;
|
|||
|
|
mCalcGrid[mLocation].PX = (ushort)start.x;
|
|||
|
|
mCalcGrid[mLocation].PY = (ushort)start.y;
|
|||
|
|
mCalcGrid[mLocation].Status = mOpenNodeValue;
|
|||
|
|
mCalcGrid[mLocation].Steps = 0;
|
|||
|
|
|
|||
|
|
mOpen.Push(mLocation);
|
|||
|
|
while (mOpen.Count > 0)
|
|||
|
|
{
|
|||
|
|
mLocation = mOpen.Pop();
|
|||
|
|
|
|||
|
|
if (mCalcGrid[mLocation].Status == mCloseNodeValue)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
if (mLocation == mEndLocation)
|
|||
|
|
{
|
|||
|
|
mCalcGrid[mLocation].Status = mCloseNodeValue;
|
|||
|
|
mFound = true;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
mLocationX = (ushort)(mLocation % mGridX);
|
|||
|
|
mLocationY = (ushort)(mLocation / mGridX);
|
|||
|
|
|
|||
|
|
bool hasSideCosts = false;
|
|||
|
|
float[] sideCosts = mGrid[mLocation].crossCost;
|
|||
|
|
if (!mIgnoreCellCost && sideCosts != null)
|
|||
|
|
hasSideCosts = true;
|
|||
|
|
|
|||
|
|
for (int i = 0; i < maxi; i++)
|
|||
|
|
{
|
|||
|
|
int cellSide;
|
|||
|
|
if (mHexagonalGrid)
|
|||
|
|
{
|
|||
|
|
if (mLocationX % 2 == evenLayoutValue)
|
|||
|
|
{
|
|||
|
|
mNewLocationX = (ushort)(mLocationX + mDirectionHex0[i, 0]);
|
|||
|
|
mNewLocationY = (ushort)(mLocationY + mDirectionHex0[i, 1]);
|
|||
|
|
cellSide = mCellSide0[i];
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
mNewLocationX = (ushort)(mLocationX + mDirectionHex1[i, 0]);
|
|||
|
|
mNewLocationY = (ushort)(mLocationY + mDirectionHex1[i, 1]);
|
|||
|
|
cellSide = mCellSide1[i];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]);
|
|||
|
|
mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]);
|
|||
|
|
cellSide = mCellBoxSides[i];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (mNewLocationY >= mGridY)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
if (mNewLocationX >= mGridX)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
mNewLocation = (mNewLocationY * mGridX) + mNewLocationX;
|
|||
|
|
if (mGrid[mNewLocation].cellType == MapManager.CellType.Obstacle && !mIgnoreCanCrossCheck)
|
|||
|
|
{
|
|||
|
|
Debug.Log($"{mNewLocation}<7D><><EFBFBD>赲<EFBFBD><E8B5B2>");
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//if (!mIncludeInvisibleCells && !mGrid[mNewLocation].visible)
|
|||
|
|
// continue;
|
|||
|
|
float gridValue = (mGrid[mNewLocation].group & mCellGroupMask) != 0 ? 1 : 0;
|
|||
|
|
if (gridValue == 0)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
if (hasSideCosts)
|
|||
|
|
{
|
|||
|
|
gridValue = sideCosts[cellSide];
|
|||
|
|
if (gridValue <= 0)
|
|||
|
|
gridValue = 1;
|
|||
|
|
}
|
|||
|
|
if (mOnCellCross != null)
|
|||
|
|
{
|
|||
|
|
gridValue += mOnCellCross(mNewLocation);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!mHexagonalGrid && i > 3)
|
|||
|
|
mNewG = mCalcGrid[mLocation].G + gridValue * mHeavyDiagonalsCost;
|
|||
|
|
else
|
|||
|
|
mNewG = mCalcGrid[mLocation].G + gridValue;
|
|||
|
|
|
|||
|
|
if (mNewG > mMaxSearchCost || mCalcGrid[mLocation].Steps >= mMaxSteps)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue)
|
|||
|
|
{
|
|||
|
|
if (mCalcGrid[mNewLocation].G <= mNewG)
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
mCalcGrid[mNewLocation].PX = mLocationX;
|
|||
|
|
mCalcGrid[mNewLocation].PY = mLocationY;
|
|||
|
|
mCalcGrid[mNewLocation].G = mNewG;
|
|||
|
|
mCalcGrid[mNewLocation].Steps = mCalcGrid[mLocation].Steps + 1;
|
|||
|
|
|
|||
|
|
int dist = Math.Abs(mNewLocationX - end.x);
|
|||
|
|
switch (mFormula)
|
|||
|
|
{
|
|||
|
|
default:
|
|||
|
|
case HeuristicFormula.Manhattan:
|
|||
|
|
mH = mHEstimate * (dist + Math.Abs(mNewLocationY - end.y));
|
|||
|
|
break;
|
|||
|
|
case HeuristicFormula.MaxDXDY:
|
|||
|
|
mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - end.y)));
|
|||
|
|
break;
|
|||
|
|
case HeuristicFormula.DiagonalShortCut:
|
|||
|
|
int h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - end.y));
|
|||
|
|
int h_straight = (dist + Math.Abs(mNewLocationY - end.y));
|
|||
|
|
mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
|
|||
|
|
break;
|
|||
|
|
case HeuristicFormula.Euclidean:
|
|||
|
|
mH = mHEstimate * (float)(Math.Sqrt(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - end.y), 2)));
|
|||
|
|
break;
|
|||
|
|
case HeuristicFormula.EuclideanNoSQR:
|
|||
|
|
mH = mHEstimate * (float)(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - end.y), 2));
|
|||
|
|
break;
|
|||
|
|
case HeuristicFormula.Custom1:
|
|||
|
|
PathFindingPoint dxy = new PathFindingPoint(dist, Math.Abs(end.y - mNewLocationY));
|
|||
|
|
float Orthogonal = Math.Abs(dxy.x - dxy.y);
|
|||
|
|
float Diagonal = Math.Abs(((dxy.x + dxy.y) - Orthogonal) / 2);
|
|||
|
|
mH = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
mCalcGrid[mNewLocation].F = mNewG + mH;
|
|||
|
|
|
|||
|
|
mOpen.Push(mNewLocation);
|
|||
|
|
mCalcGrid[mNewLocation].Status = mOpenNodeValue;
|
|||
|
|
}
|
|||
|
|
mCalcGrid[mLocation].Status = mCloseNodeValue;
|
|||
|
|
}
|
|||
|
|
if (mFound)
|
|||
|
|
{
|
|||
|
|
mClose.Clear();
|
|||
|
|
|
|||
|
|
PathFinderNodeFast fNodeTmp = mCalcGrid[(end.y * mGridX) + end.x];
|
|||
|
|
totalCost = fNodeTmp.G;
|
|||
|
|
PathFinderNode fNode;
|
|||
|
|
fNode.F = fNodeTmp.F;
|
|||
|
|
fNode.G = fNodeTmp.G;
|
|||
|
|
fNode.H = 0;
|
|||
|
|
fNode.PX = fNodeTmp.PX;
|
|||
|
|
fNode.PY = fNodeTmp.PY;
|
|||
|
|
fNode.X = end.x;
|
|||
|
|
fNode.Y = end.y;
|
|||
|
|
|
|||
|
|
while (fNode.X != fNode.PX || fNode.Y != fNode.PY)
|
|||
|
|
{
|
|||
|
|
mClose.Add(fNode);
|
|||
|
|
int posX = fNode.PX;
|
|||
|
|
int posY = fNode.PY;
|
|||
|
|
fNodeTmp = mCalcGrid[(posY * mGridX) + posX];
|
|||
|
|
fNode.F = fNodeTmp.F;
|
|||
|
|
fNode.G = fNodeTmp.G;
|
|||
|
|
fNode.H = 0;
|
|||
|
|
fNode.PX = fNodeTmp.PX;
|
|||
|
|
fNode.PY = fNodeTmp.PY;
|
|||
|
|
fNode.X = posX;
|
|||
|
|
fNode.Y = posY;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return mClose;
|
|||
|
|
}
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
internal class ComparePFNodeMatrix : IComparer<int>
|
|||
|
|
{
|
|||
|
|
protected PathFinderNodeFast[] mMatrix;
|
|||
|
|
|
|||
|
|
public ComparePFNodeMatrix(PathFinderNodeFast[] matrix)
|
|||
|
|
{
|
|||
|
|
mMatrix = matrix;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public int Compare(int a, int b)
|
|||
|
|
{
|
|||
|
|
if (mMatrix[a].F > mMatrix[b].F)
|
|||
|
|
return 1;
|
|||
|
|
else if (mMatrix[a].F < mMatrix[b].F)
|
|||
|
|
return -1;
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void SetMatrix(PathFinderNodeFast[] matrix)
|
|||
|
|
{
|
|||
|
|
mMatrix = matrix;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|