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