List <Vector2> FindPath(Vector2 From)
{
Node startNode = InitPF(From);
while (OpenList.Count != 0)
{
Node current = FindLowestF(OpenList);
if (current.position == _goal)
{
//Return goal
PathNode = current;
return(ExtractPath());
}
OpenList.Remove(current);
ClosedList.Add(current.position);
Visited[(int)current.position.X, (int)current.position.Y] = true;
for (int X = -1; X <= 1; X++)
{
for (int Y = -1; Y <= 1; Y++)
{
if (X != 0 || Y != 0)
{
Vector2 newPos = new Vector2(current.position.X + X, current.position.Y + Y);
Vector2 correctedNewPos = WorldHelp.CorrectWorldOverflow(newPos);
if (Visited[(int)correctedNewPos.X, (int)correctedNewPos.Y])
{
continue;
}
else if (OpenList.Any(a => a.position == correctedNewPos))
{
continue;
}
else if (TilePalette.IsTileSailable((int)correctedNewPos.X, (int)correctedNewPos.Y, _elementSize))
{
Node newNode = new Node()
{
parent = current,
position = correctedNewPos,
G = current.G + ((X == 0 || Y == 0) ? 1 : Sqrt2)
};
OpenList.Add(newNode);
}
}
}
}
}
return(new List <Vector2>());
}
