private RegionPath RetracePath(RegionItem end)
{
RegionPath path = new RegionPath();
RegionItem item = end;
List <RegionItem> res = new List <RegionItem>();
while (item != null)
{
res.Add(item);
item = item.parent;
}
res.Reverse();
foreach (RegionItem i in res)
{
path.Add(new PathItem(Position + i.LocalCoordinate, i.Layer));
}
return(path);
}
/// <summary>
/// Поиск пути из точки в точку
/// </summary>
/// <param name="from">Start local point</param>
/// <param name="to">End local point</param>
/// <param name="comparer">Point comparer</param>
/// <returns>Возвращает путь в глобальных координатах</returns>
public RegionPath GetPath(Vector2i from, Vector2i to, IRegion2DItemComparer comparer, IHeuristic2D heuristic, LayerMask walkable, LayersValues weights, LayerMask priority)
{
RegionPath path = null;
RegionItem start = GetItem(from);
RegionItem end = GetItem(to);
if (start == null || end == null ||
!walkable.Contains(start.Layer) || !walkable.Contains(end.Layer))
{
return(null);
}
if (comparer == null)
{
comparer = new OptimalRegion2DItemsComparer();
}
if (heuristic == null)
{
heuristic = new Manhatan2D();
}
byte closedvalue = IncrementCurrentIteration;
sbyte[,] direction = new sbyte[8, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
};
MyTreeQueue <RegionItem> opened = new MyTreeQueue <RegionItem>(comparer);
//TreeQueue<RegionItem> opened = new TreeQueue<RegionItem>(comparer);
RegionItem parent = null;
start.g = 0;
start.f = heuristic.GetHeuristic(from, to);
start.parent = null;
opened.Push(start);
int nx;
int ny;
while (opened.Count > 0)
{
parent = opened.Pop(); // Извлекаем очередной наименьший блок (на первом шаге это стартовый)
parent.closed = closedvalue; // Отмечаем его как просмотренный ранее
parent.opened = 0; // Изьять из списка кандидатов на просмотр
if (parent.LocalCoordinate.x == end.LocalCoordinate.x && parent.LocalCoordinate.y == end.LocalCoordinate.y)
{
path = RetracePath(end);
break;
}
for (int i = 0; i < 8; i++) // Просматриваем всех соседей
{
nx = parent.LocalCoordinate.x + direction[i, 0];
ny = parent.LocalCoordinate.y + direction[i, 1];
if (nx < 0 || ny < 0 || nx >= xScale || ny >= yScale)
{
continue;
}
RegionItem newnode = GetItem(new Vector2i(nx, ny));
// Если блок был просмотрен ранее или поверхность не проходима
if (newnode.closed == closedvalue || !walkable.Contains(newnode.Layer))
{
continue;
}
// Получаем базовый штраф поверхности (если приоритетная поверхность, то штраф минимальный)
int penalti = (priority.Contains(newnode.Layer)) ? weights.MinValue : weights.GetValue(newnode.Layer);
// Вычисляем пройденный путь в штрафах до соседа
int g = parent.g + penalti;
// Если в списке кандидатов на просмотр и новый вес больше старого - пропускаем
if (newnode.opened == closedvalue && newnode.g <= g)
{
continue;
}
// Получаем оставшееся расстояние
double f = heuristic.GetHeuristic(new Vector2i(newnode.LocalCoordinate.x, newnode.LocalCoordinate.y), new Vector2i(end.LocalCoordinate.x, end.LocalCoordinate.y));
newnode.g = g;
newnode.f = f;
newnode.parent = parent;
// Поместить в список кандидатов на просмотр
if (newnode.opened != closedvalue)
{
opened.Push(newnode);
newnode.opened = closedvalue;
}
}
}
return(path);
}