public void Find(NodePosition from, NodePosition to, Path <T> path)
{
UnityEngine.Profiling.Profiler.BeginSample("PathFinder.Find()");
path.Clear();
if (!m_Nodes[from.Row * m_ColumnSize + from.Column].Walkable)
{
return;
}
path.Lock();
if (m_Version == byte.MaxValue)
{
m_Version = 0;
}
else
{
m_Version++;
}
m_OpenSet.FastClear();
m_ClosedSet.Clear();
var startCell = m_Cells[from.Row * m_ColumnSize + from.Column];
var targetCell = m_Cells[to.Row * m_ColumnSize + to.Column];
startCell.Version = m_Version;
startCell.Parent = null;
startCell.Level = 1;
startCell.GCost = 0;
startCell.HCost = GetDistance(ref startCell, ref targetCell);
startCell.FCost = 0;
m_OpenSet.Add(startCell);
CellData closestToTarget = startCell;
CellData currentCell = null;
bool success = false;
while (m_OpenSet.Count > 0)
{
currentCell = m_OpenSet.RemoveFirst();
m_ClosedSet.Add(currentCell);
currentCell.IsClosed = true;
if ((currentCell == targetCell))
{
success = true;
break;
}
else if (currentCell.Level > path.Capacity)
{
break;
}
ProcessAllNeighbours(ref currentCell, ref targetCell, ref closestToTarget);
}
if (success)
{
GeneratePath(path, targetCell);
}
else
{
GeneratePath(path, closestToTarget);
}
path.UnLock();
UnityEngine.Profiling.Profiler.EndSample();
}
void ProcessNeighbour(ref CellData currentCell, ref CellData targetCell, ref CellData closestToTarget, ref CellData neighbourCell)
{
if (!m_Nodes[neighbourCell.Position.Row * m_ColumnSize + neighbourCell.Position.Column].Walkable)
{
return;
}
if (neighbourCell.Version == m_Version && neighbourCell.IsClosed)
{
return;
}
var newGCostToNeighbour = currentCell.GCost + GetDistance(ref currentCell, ref neighbourCell);
if (newGCostToNeighbour < neighbourCell.GCost || neighbourCell.Version != m_Version)
{
neighbourCell.GCost = newGCostToNeighbour;
neighbourCell.HCost = GetDistance(ref neighbourCell, ref targetCell);
neighbourCell.FCost = neighbourCell.GCost + neighbourCell.HCost;
neighbourCell.Parent = currentCell;
var level = currentCell.Level;
neighbourCell.Level = (++level);
if (neighbourCell.HCost < closestToTarget.HCost)
{
closestToTarget = neighbourCell;
}
if (neighbourCell.Version != m_Version)
{
m_OpenSet.Add(neighbourCell);
neighbourCell.Version = m_Version;
neighbourCell.IsClosed = false;
}
else
{
m_OpenSet.UpdateItem(neighbourCell);
}
}
}
