tb/HX_MapEditor
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
master
HX_MapEditor/Assets/Scripts/PathFinding/PathFinderFast.cs

479 lines
13 KiB
C#
Raw Permalink Normal View History

第一次提交
2025-06-14 13:46:24 +08:00
using System;
using System.Collections.Generic;
using static HxGame.PathFinding.IPathFinder;
namespace HxGame.PathFinding
{
internal struct PathFinderNodeFast
{
public float F;
// f = gone + heuristic
public float G;
public ushort PX;
// Parent
public ushort PY;
public byte Status;
public int Steps;
}
public class PathFinderFast : IPathFinder
{
private CellNode[] mGrid = null; //<2F><><EFBFBD>нڵ<D0BD>
private PriorityQueueB<int> mOpen = null;
private List<PathFinderNode> mClose = new List<PathFinderNode>();
private HeuristicFormula mFormula = HeuristicFormula.Manhattan; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private bool mDiagonals = true; //<2F>Ƿ<EFBFBD><C7B7><EFBFBD>б<EFBFBD><D0B1><EFBFBD><EFBFBD>
private bool mHexagonalGrid = false; //<2F>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>θ<EFBFBD><CEB8><EFBFBD>
private float mHEstimate = 1; //<2F><><EFBFBD><EFBFBD>Ԥ<EFBFBD><D4A4>
private float mHeavyDiagonalsCost = 1.4f; //б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private int mMaxSteps = 2000; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private float mMaxSearchCost = 100000; //<2F><><EFBFBD><EFBFBD>Ѱ·<D1B0><C2B7><EFBFBD><EFBFBD>
private PathFinderNodeFast[] mCalcGrid = null;
private byte mOpenNodeValue = 1;
private byte mCloseNodeValue = 2;
private OnCellCross mOnCellCross = null;
private float mH = 0;
private int mLocation = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private int mNewLocation = 0; //<2F>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private ushort mLocationX = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private ushort mLocationY = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private ushort mNewLocationX = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private ushort mNewLocationY = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private ushort mGridX = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private ushort mGridY = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private ushort mGridXMinus1 = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-1
private ushort mGridYLog2 = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>
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; //<2F>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private float mNewG = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>gֵ
private int mCellGroupMask = -1;
private bool mIgnoreCanCrossCheck; //<2F><><EFBFBD><EFBFBD><EFBFBD>赲
private bool mIgnoreCellCost; //<2F><><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
private bool mIncludeInvisibleCells;
public void SetCalcMatrix(CellNode[] grid)
{
if (grid == null)
throw new Exception("Grid cannot be null");
if (grid.Length != mGrid.Length)
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 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 OnCellCross OnCellCross
{
get { return mOnCellCross; }
set { mOnCellCross = value; }
}
public PathFinderFast(CellNode[] grid, int gridWidth, int gridHeight)
{
if (grid == null)
throw new Exception("Grid cannot be null");
mGrid = grid;
mGridX = (ushort)gridWidth;
mGridY = (ushort)gridHeight;
mGridXMinus1 = (ushort)(mGridX - 1);
mGridYLog2 = (ushort)Math.Log(mGridX, 2);
// This should be done at the constructor, for now we leave it here.
if (Math.Log(mGridX, 2) != (int)Math.Log(mGridX, 2))
throw new Exception("Invalid Grid, size in X must be power of 2");
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)
{
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; //<2F>ܱ߸<DCB1><DFB8><EFBFBD><EFBFBD><EFBFBD>
if (mHexagonalGrid)
maxi = 6;
else
maxi = mDiagonals ? 8 : 4;
//mLocation = (startCell.row << mGridYLog2) + startCell.column;
//mEndLocation = (endCell.row << mGridYLog2) + endCell.column;
mLocation = (startCell.Y << mGridYLog2) + startCell.X;
mEndLocation = (endCell.Y << mGridYLog2) + endCell.X;
mCalcGrid[mLocation].G = 0;
mCalcGrid[mLocation].F = mHEstimate;
//mCalcGrid[mLocation].PX = (ushort)startCell.column;
//mCalcGrid[mLocation].PY = (ushort)startCell.row;
mCalcGrid[mLocation].PX = (ushort)startCell.X;
mCalcGrid[mLocation].PY = (ushort)startCell.Y;
mCalcGrid[mLocation].Status = mOpenNodeValue;
mCalcGrid[mLocation].Steps = 0;
mOpen.Push(mLocation);
while (mOpen.Count > 0)
{
mLocation = mOpen.Pop();
//<2F>Ƿ<EFBFBD><C7B7>ڷ<EFBFBD><DAB7><EFBFBD><EFBFBD>б<EFBFBD><D0B1>У<EFBFBD><D0A3><EFBFBD>ʾ<EFBFBD>Ѵ<EFBFBD><D1B4><EFBFBD><EFBFBD>˽ڵ<CBBD>
if (mCalcGrid[mLocation].Status == mCloseNodeValue)
continue;
if (mLocation == mEndLocation)
{
mCalcGrid[mLocation].Status = mCloseNodeValue;
mFound = true;
break;
}
mLocationX = (ushort)(mLocation & mGridXMinus1);
mLocationY = (ushort)(mLocation >> mGridYLog2);
//<2F><><EFBFBD><EFBFBD><EFBFBD>ܱ<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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 || mNewLocationX >= mGridX)
continue;
mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX;
CellNode nextCell = mGrid[mNewLocation];
格子划分正常
2025-06-15 20:14:45 +08:00
if (nextCell == null || (nextCell.cellType == CellType.Obstacle && !mIgnoreCanCrossCheck))
第一次提交
2025-06-14 13:46:24 +08:00
continue;
//if (!mIncludeInvisibleCells && !mGrid[mNewLocation].visible)
// continue;
float gridValue = (nextCell.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 - endCell.column);
int dist = Math.Abs(mNewLocationX - endCell.X);
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
//mH = mHEstimate * (dist + Math.Abs(mNewLocationY - endCell.row));
mH = mHEstimate * (dist + Math.Abs(mNewLocationY - endCell.Y));
break;
case HeuristicFormula.MaxDXDY:
//mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - endCell.row)));
mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - endCell.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
//float h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - endCell.row));
//float h_straight = (dist + Math.Abs(mNewLocationY - endCell.row));
float h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - endCell.Y));
float h_straight = (dist + Math.Abs(mNewLocationY - endCell.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 - endCell.row), 2)));
mH = mHEstimate * (float)(Math.Sqrt(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - endCell.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
//mH = mHEstimate * (float)(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - endCell.row), 2));
mH = mHEstimate * (float)(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - endCell.Y), 2));
break;
}
mCalcGrid[mNewLocation].F = mNewG + mH;
mOpen.Push(mNewLocation);
mCalcGrid[mNewLocation].Status = mOpenNodeValue;
}
mCalcGrid[mLocation].Status = mCloseNodeValue;
}
if (mFound)
{
mClose.Clear();
//int posX = endCell.column;
//int posY = endCell.row;
int posX = endCell.X;
int posY = endCell.Y;
//PathFinderNodeFast fNodeTmp = mCalcGrid[(endCell.row << mGridYLog2) + endCell.column];
PathFinderNodeFast fNodeTmp = mCalcGrid[(endCell.Y << mGridYLog2) + endCell.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 = endCell.column;
//fNode.Y = endCell.row;
fNode.X = endCell.X;
fNode.Y = endCell.Y;
while (fNode.X != fNode.PX || fNode.Y != fNode.PY)
{
mClose.Add(fNode);
posX = fNode.PX;
posY = fNode.PY;
fNodeTmp = mCalcGrid[(posY << mGridYLog2) + 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;
}
}
}
}
Reference in New Issue Copy Permalink