public List <PathFinderNode> FindPath(Point start, Point end)
{
Stopwatch sw = Stopwatch.StartNew();
PathFinderNode parentNode;
bool found = false;
int gridX = mGrid.GetUpperBound(0);
int gridY = mGrid.GetUpperBound(1);
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
}
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
}
#endif
sbyte[,] direction;
if (mDiagonals)
{
direction = new sbyte[8, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
}
}
;
else
{
direction = new sbyte[4, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
}
};
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.X = start.X;
parentNode.Y = start.Y;
parentNode.PX = parentNode.X;
parentNode.PY = parentNode.Y;
mOpen.Push(parentNode);
while (mOpen.Count > 0 && !mStop)
{
parentNode = mOpen.Pop();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
}
//Console.WriteLine("{0},{1}",parentNode.X,parentNode.Y);
#endif
if (parentNode.X == end.X && parentNode.Y == end.Y)
{
mClose.Add(parentNode);
found = true;
Console.WriteLine("Endpoint found.");
break;
}
if (mClose.Count > mSearchLimit)
{
mStopped = true;
Console.WriteLine("Too many nodes, aborting.");
found = true;
mLastPoint = new Point(parentNode.X, parentNode.Y);
break;
}
if (mPunishChangeDirection)
{
mHoriz = (parentNode.X - parentNode.PX);
}
//Lets calculate each successors
for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
{
PathFinderNode newNode;
newNode.X = parentNode.X + (direction[i, 0] * mDist);
newNode.Y = parentNode.Y + (direction[i, 1] * mDist);
if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
{
continue;
}
int newG;
if (mHeavyDiagonals && i > 3)
{
newG = parentNode.G + (int)(Convert.ToSingle(mGrid[newNode.X, newNode.Y]) * 512f);
}
else
{
newG = parentNode.G + (int)(Convert.ToSingle(mGrid[newNode.X, newNode.Y]) * 128f);
}
if (newG == parentNode.G)
{
//Unbrekeable
//Console.Write("s");
continue;
}
if (mClose.Count % 1000 == 0)
{
Console.CursorLeft = 0;
Console.Write(" Current Point is ({0},{1}) with a cost of {2}.", newNode.X, newNode.Y, newG);
}
if (mPunishChangeDirection)
{
if ((newNode.X - parentNode.X) != 0)
{
if (mHoriz == 0)
{
newG += 20;
}
}
if ((newNode.Y - parentNode.Y) != 0)
{
if (mHoriz != 0)
{
newG += 20;
}
}
}
int foundInOpenIndex = -1;
for (int j = 0; j < mOpen.Count; j++)
{
if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < mClose.Count; j++)
{
if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
{
continue;
}
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.G = newG;
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
break;
case HeuristicFormula.MaxDXDY:
newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
Point dxy = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
//case HeuristicFormula.River:
// Need to figure this out
//break;
}
if (mTieBreaker)
{
int dx1 = parentNode.X - end.X;
int dy1 = parentNode.Y - end.Y;
int dx2 = start.X - end.X;
int dy2 = start.Y - end.Y;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
newNode.H = (int)(newNode.H + cross * 0.001);
}
newNode.F = newNode.G + newNode.H;
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
}
#endif
//It is faster if we leave the open node in the priority queue
//When it is removed, all nodes around will be closed, it will be ignored automatically
//if (foundInOpenIndex != -1)
// mOpen.RemoveAt(foundInOpenIndex);
//if (foundInOpenIndex == -1)
mOpen.Push(newNode);
}
mClose.Add(parentNode);
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
}
#endif
}
sw.Stop();
mCompletedTime = (double)sw.Elapsed.TotalMilliseconds;
if (found)
{
Console.WriteLine("Cleaning up...");
PathFinderNode fNode = mClose[mClose.Count - 1];
for (int i = mClose.Count - 1; i >= 0; i--)
{
if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
{
#if DEBUGON
if (mDebugFoundPath && PathFinderDebug != null)
{
PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
}
#endif
fNode = mClose[i];
Console.Write(".");
}
else
{
mClose.RemoveAt(i);
Console.Write("-");
}
}
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
public List <PathFinderNode> FindPath(Vector start, Vector end)
{
// HighResolutionTime.Start();
PathFinderNode parentNode;
bool found = false;
int gridX = mGrid.GetUpperBound(0);
int gridY = mGrid.GetUpperBound(1);
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, start.x, start.y, PathFinderNodeType.Start, -1, -1);
}
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, end.x, end.y, PathFinderNodeType.End, -1, -1);
}
#endif
sbyte[,] direction;
if (mDiagonals)
{
direction = new sbyte[8, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
}
}
;
else
{
direction = new sbyte[4, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
}
};
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.X = start.x;
parentNode.Y = start.y;
parentNode.PX = parentNode.X;
parentNode.PY = parentNode.Y;
mOpen.Push(parentNode);
while (mOpen.Count > 0 && !mStop)
{
parentNode = mOpen.Pop();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
}
#endif
if (parentNode.X == end.x && parentNode.Y == end.y)
{
mClose.Add(parentNode);
found = true;
break;
}
if (mClose.Count > mSearchLimit)
{
mStopped = true;
return(null);
}
if (mPunishChangeDirection)
{
mHoriz = (parentNode.X - parentNode.PX);
}
//Lets calculate each successors
for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
{
PathFinderNode newNode;
newNode.X = parentNode.X + direction[i, 0];
newNode.Y = parentNode.Y + direction[i, 1];
if (newNode.X < 0 || newNode.Y < 0 || newNode.X > gridX || newNode.Y > gridY)
{
continue;
}
int newG;
if (mHeavyDiagonals && i > 3)
{
newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y] * 2.41);
}
else
{
newG = parentNode.G + mGrid[newNode.X, newNode.Y];
}
if (newG == parentNode.G)
{
//Unbrekeable
continue;
}
if (mPunishChangeDirection)
{
if ((newNode.X - parentNode.X) != 0)
{
if (mHoriz == 0)
{
newG += 20;
}
}
if ((newNode.Y - parentNode.Y) != 0)
{
if (mHoriz != 0)
{
newG += 20;
}
}
}
int foundInOpenIndex = -1;
for (int j = 0; j < mOpen.Count; j++)
{
if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < mClose.Count; j++)
{
if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
{
continue;
}
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.G = newG;
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.x) + Math.Abs(newNode.Y - end.y));
break;
case HeuristicFormula.MaxDXDY:
newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.x), Math.Abs(newNode.Y - end.y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newNode.X - end.x), Math.Abs(newNode.Y - end.y));
int h_straight = (Math.Abs(newNode.X - end.x) + Math.Abs(newNode.Y - end.y));
newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.x), 2) + Math.Pow((newNode.Y - end.y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.x), 2) + Math.Pow((newNode.Y - end.y), 2)));
break;
case HeuristicFormula.Custom1:
Vector dxy = new Vector(Math.Abs(end.x - newNode.X), Math.Abs(end.y - newNode.Y));
int Orthogonal = Math.Abs(dxy.x - dxy.y);
int Diagonal = Math.Abs(((dxy.x + dxy.y) - Orthogonal) / 2);
newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
break;
}
if (mTieBreaker)
{
int dx1 = parentNode.X - end.x;
int dy1 = parentNode.Y - end.y;
int dx2 = start.x - end.x;
int dy2 = start.y - end.y;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
newNode.H = (int)(newNode.H + cross * 0.001);
}
newNode.F = newNode.G + newNode.H;
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
}
#endif
//It is faster if we leave the open node in the priority queue
//When it is removed, all nodes around will be closed, it will be ignored automatically
//if (foundInOpenIndex != -1)
// mOpen.RemoveAt(foundInOpenIndex);
//if (foundInOpenIndex == -1)
mOpen.Push(newNode);
}
mClose.Add(parentNode);
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
}
#endif
}
// mCompletedTime = HighResolutionTime.GetTime();
if (found)
{
PathFinderNode fNode = mClose[mClose.Count - 1];
for (int i = mClose.Count - 1; i >= 0; i--)
{
if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
{
#if DEBUGON
if (mDebugFoundPath && PathFinderDebug != null)
{
PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
}
#endif
fNode = mClose[i];
}
else
{
mClose.RemoveAt(i);
}
}
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
public List <Microsoft.Xna.Framework.Point> FindPath(Microsoft.Xna.Framework.Point start, Microsoft.Xna.Framework.Point end)
{
PathFinderNode parentNode;
bool found = false;
int gridX = mGrid.GetUpperBound(0);
int gridY = mGrid.GetUpperBound(1);
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
}
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
}
#endif
sbyte[,] direction;
if (mDiagonals)
{
direction = new sbyte[8, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
}
}
;
else
{
direction = new sbyte[4, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
}
};
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.X = start.X;
parentNode.Y = start.Y;
parentNode.PX = parentNode.X;
parentNode.PY = parentNode.Y;
mOpen.Push(parentNode);
while (mOpen.Count > 0 && !mStop)
{
parentNode = mOpen.Pop();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
}
#endif
if (parentNode.X == end.X && parentNode.Y == end.Y)
{
mClose.Add(parentNode);
found = true;
break;
}
if (mClose.Count > mSearchLimit)
{
mStopped = true;
return(null);
}
if (mPunishChangeDirection)
{
mHoriz = (parentNode.X - parentNode.PX);
}
for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
{
PathFinderNode newNode;
newNode.X = parentNode.X + direction[i, 0];
newNode.Y = parentNode.Y + direction[i, 1];
if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
{
continue;
}
int newG;
if (mHeavyDiagonals && i > 3)
{
newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y] * 2.41);
}
else
{
newG = parentNode.G + mGrid[newNode.X, newNode.Y];
}
if (newG == parentNode.G)
{
continue;
}
if (mPunishChangeDirection)
{
if ((newNode.X - parentNode.X) != 0)
{
if (mHoriz == 0)
{
newG += 20;
}
}
if ((newNode.Y - parentNode.Y) != 0)
{
if (mHoriz != 0)
{
newG += 20;
}
}
}
int foundInOpenIndex = -1;
for (int j = 0; j < mOpen.Count; j++)
{
if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < mClose.Count; j++)
{
if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
{
continue;
}
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.G = newG;
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
break;
case HeuristicFormula.MaxDXDY:
newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
Point dxy = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
if (mTieBreaker)
{
int dx1 = parentNode.X - end.X;
int dy1 = parentNode.Y - end.Y;
int dx2 = start.X - end.X;
int dy2 = start.Y - end.Y;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
newNode.H = (int)(newNode.H + cross * 0.001);
}
newNode.F = newNode.G + newNode.H;
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
}
#endif
mOpen.Push(newNode);
}
mClose.Add(parentNode);
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
}
#endif
}
if (found)
{
PathFinderNode fNode = mClose[mClose.Count - 1];
for (int i = mClose.Count - 1; i >= 0; i--)
{
if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
{
#if DEBUGON
if (mDebugFoundPath && PathFinderDebug != null)
{
PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
}
#endif
fNode = mClose[i];
}
else
{
mClose.RemoveAt(i);
}
}
mStopped = true;
List <Microsoft.Xna.Framework.Point> copy = new List <Microsoft.Xna.Framework.Point>();
foreach (PathFinderNode node in mClose)
{
copy.Add(new Microsoft.Xna.Framework.Point(node.X, node.Y));
}
return(copy);
}
mStopped = true;
return(null);
}
public List <PathFinderNode> FindPath(Point start, Point end)
{
HighResolutionTime.Start();
PathFinderNode parentNode;
bool found = false;
int gridX = mGrid.GetUpperBound(0);
int gridY = mGrid.GetUpperBound(1);
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
}
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
}
#endif
sbyte[,] direction;
if (mDiagonals)
{
direction = new sbyte[8, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
}
}
;
else
{
direction = new sbyte[4, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
}
};
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.X = start.X;
parentNode.Y = start.Y;
parentNode.PX = parentNode.X;
parentNode.PY = parentNode.Y;
mOpen.Push(parentNode);
while (mOpen.Count > 0 && !mStop)
{
parentNode = mOpen.Pop();
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
}
#endif
if (parentNode.X == end.X && parentNode.Y == end.Y) //找到end
{
mClose.Add(parentNode);
found = true;
break;
}
if (mClose.Count > mSearchLimit) // 超出计算限制
{
mStopped = true;
mCompletedTime = HighResolutionTime.GetTime();
return(null);
}
if (mPunishChangeDirection) // 改变方向是否惩罚
{
mHoriz = (parentNode.X - parentNode.PX);
}
//Lets calculate each successors
//搜索子节点
for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
{
PathFinderNode newNode;
newNode.X = parentNode.X + direction[i, 0];
newNode.Y = parentNode.Y + direction[i, 1];
if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
{
continue;
}
#region G
int newG;
if (mHeavyDiagonals && i > 3)
{
newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y] * 2.41);
}
else
{
newG = parentNode.G + mGrid[newNode.X, newNode.Y];
}
if (newG == parentNode.G) // when weight == X = 0
{
//Unbrekeable
continue;
}
if (mPunishChangeDirection)
{
if ((newNode.X - parentNode.X) != 0) // x 方向变化
{
if (mHoriz == 0)
{
newG += 20;
}
}
if ((newNode.Y - parentNode.Y) != 0) // y 方向变化
{
if (mHoriz != 0)
{
newG += 20;
}
}
}
// 负权路径会反复来回，形成死循环
// 因此 权重需大于0
// 因此 newG 必然大于第一次加入open或close时的 G ，避免了回溯
// 因此 每个节点只会被计算一次
int foundInOpenIndex = -1;
for (int j = 0; j < mOpen.Count; j++)
{
if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG) // 条件必然成立
{
continue; // 如果 newNode 在 open 集合并且不小于G，则跳过 newNode
}
int foundInCloseIndex = -1;
for (int j = 0; j < mClose.Count; j++)
{
if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG) // 条件必然成立
{
continue; // 如果 newNode 在 close 集合并且不小于G，则跳过 newNode
}
// 更新 newNode 节点
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.G = newG;
#endregion
#region H
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
break;
case HeuristicFormula.MaxDXDY:
newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
Point dxy = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
if (mTieBreaker)
{
int dx1 = parentNode.X - end.X;
int dy1 = parentNode.Y - end.Y;
int dx2 = start.X - end.X;
int dy2 = start.Y - end.Y;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
newNode.H = (int)(newNode.H + cross * 0.001);
}
#endregion
newNode.F = newNode.G + newNode.H;
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
}
#endif
//It is faster if we leave the open node in the priority queue
//When it is removed, all nodes around will be closed, it will be ignored automatically
//if (foundInOpenIndex != -1)
// mOpen.RemoveAt(foundInOpenIndex);
//if (foundInOpenIndex == -1)
mOpen.Push(newNode); // 添加 open 节点
}
mClose.Add(parentNode); // 已经搜索过的 open 节点加入 close
#if DEBUGON
if (mDebugProgress && PathFinderDebug != null)
{
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
}
#endif
}
mCompletedTime = HighResolutionTime.GetTime();
if (found)
{
// 倒推路径节点
PathFinderNode fNode = mClose[mClose.Count - 1];
for (int i = mClose.Count - 1; i >= 0; i--)
{
if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
{
#if DEBUGON
if (mDebugFoundPath && PathFinderDebug != null)
{
PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
}
#endif
fNode = mClose[i];
}
else
{
mClose.RemoveAt(i);
}
}
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
