private void StoreNeighborNodes(PathNode inAround, PathNode[] inNeighbors, NeighbourClassification neighbourClassification)
{
int x = inAround.X;
int y = inAround.Y;
if ((x > 0) && (y > 0) && neighbourClassification == NeighbourClassification.ByWallAndCorner)
{
inNeighbors[0] = m_SearchSpace[x - 1, y - 1];
}
else
{
inNeighbors[0] = null;
}
if (y > 0)
{
inNeighbors[1] = m_SearchSpace[x, y - 1];
}
else
{
inNeighbors[1] = null;
}
if ((x < Width - 1) && (y > 0) && neighbourClassification == NeighbourClassification.ByWallAndCorner)
{
inNeighbors[2] = m_SearchSpace[x + 1, y - 1];
}
else
{
inNeighbors[2] = null;
}
if (x > 0)
{
inNeighbors[3] = m_SearchSpace[x - 1, y];
}
else
{
inNeighbors[3] = null;
}
if (x < Width - 1)
{
inNeighbors[4] = m_SearchSpace[x + 1, y];
}
else
{
inNeighbors[4] = null;
}
if ((x > 0) && (y < Height - 1) && neighbourClassification == NeighbourClassification.ByWallAndCorner)
{
inNeighbors[5] = m_SearchSpace[x - 1, y + 1];
}
else
{
inNeighbors[5] = null;
}
if (y < Height - 1)
{
inNeighbors[6] = m_SearchSpace[x, y + 1];
}
else
{
inNeighbors[6] = null;
}
if ((x < Width - 1) && (y < Height - 1) && neighbourClassification == NeighbourClassification.ByWallAndCorner)
{
inNeighbors[7] = m_SearchSpace[x + 1, y + 1];
}
else
{
inNeighbors[7] = null;
}
}
//private List<Int64> elapsed = new List<long>();
/// <summary>
/// Returns null, if no path is found. Start- and End-Node are included in returned path. The user context
/// is passed to IsWalkable().
/// </summary>
///
///
public LinkedList <TPathNode> Search(IPoint inStartNode, IPoint inEndNode, NeighbourClassification neighbourClassification)
{
PathNode startNode = m_SearchSpace[inStartNode.X, inStartNode.Y];
PathNode endNode = m_SearchSpace[inEndNode.X, inEndNode.Y];
//System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
//watch.Start();
if (startNode == endNode)
{
return(new LinkedList <TPathNode>(new TPathNode[] { startNode.UserContext }));
}
PathNode[] neighborNodes = new PathNode[8];
m_ClosedSet.Clear();
m_OpenSet.Clear();
m_RuntimeGrid.Clear();
m_OrderedOpenSet.Clear();
for (int x = 0; x < Width; x++)
{
for (int y = 0; y < Height; y++)
{
m_CameFrom[x, y] = null;
}
}
startNode.G = 0;
startNode.H = Heuristic(startNode, endNode);
startNode.F = startNode.H;
m_OpenSet.Add(startNode);
m_OrderedOpenSet.Push(startNode);
m_RuntimeGrid.Add(startNode);
int nodes = 0;
while (!m_OpenSet.IsEmpty)
{
PathNode x = m_OrderedOpenSet.Pop();
if (x == endNode)
{
// watch.Stop();
//elapsed.Add(watch.ElapsedMilliseconds);
LinkedList <TPathNode> result = ReconstructPath(m_CameFrom, m_CameFrom[endNode.X, endNode.Y]);
result.AddLast(endNode.UserContext);
return(result);
}
m_OpenSet.Remove(x);
m_ClosedSet.Add(x);
StoreNeighborNodes(x, neighborNodes, neighbourClassification);
for (int i = 0; i < neighborNodes.Length; i++)
{
PathNode y = neighborNodes[i];
Boolean tentative_is_better;
if (y == null)
{
continue;
}
if (!y.UserContext.IsWalkable())
{
continue;
}
if (m_ClosedSet.Contains(y))
{
continue;
}
nodes++;
Double tentative_g_score = m_RuntimeGrid[x].G + NeighborDistance(x, y);
Boolean wasAdded = false;
if (!m_OpenSet.Contains(y))
{
m_OpenSet.Add(y);
tentative_is_better = true;
wasAdded = true;
}
else if (tentative_g_score < m_RuntimeGrid[y].G)
{
tentative_is_better = true;
}
else
{
tentative_is_better = false;
}
if (tentative_is_better)
{
m_CameFrom[y.X, y.Y] = x;
if (!m_RuntimeGrid.Contains(y))
{
m_RuntimeGrid.Add(y);
}
m_RuntimeGrid[y].G = tentative_g_score;
m_RuntimeGrid[y].H = Heuristic(y, endNode);
m_RuntimeGrid[y].F = m_RuntimeGrid[y].G + m_RuntimeGrid[y].H;
if (wasAdded)
{
m_OrderedOpenSet.Push(y);
}
else
{
m_OrderedOpenSet.Update(y);
}
}
}
}
return(null);
}
