int GetDistance(AstarObject A, AstarObject B)
{
int dstX = UnityEngine.Mathf.Abs(A.x - B.x);
int dstY = UnityEngine.Mathf.Abs(A.y - B.y);
if (dstX > dstY)
{
return(14 * dstY + 10 * dstX);
}
return(14 * dstX + 10 * (dstY - dstX));
}
List <Vector2> RetracePath(AstarObject Start, AstarObject End)
{
List <Vector2> path = new List <Vector2>();
AstarObject CurrentA = End;
while (CurrentA != Start)
{
path.Add(CurrentA.ToVector2());
CurrentA = CurrentA.parent;
}
path.Reverse();
return(path);
}
List <AstarObject> GetNeighbours(AstarObject A, ref AstarObject[,] Set)
{
List <AstarObject> Neighbours = new List <AstarObject>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
{
continue;
}
int CheckX = A.x + x;
int CheckY = A.y + y;
if (CheckX >= LeftBottomCoordinate.x && CheckX < LeftBottomCoordinate.x + Size.x && CheckY >= LeftBottomCoordinate.y && CheckY < LeftBottomCoordinate.y + Size.y)
{
Neighbours.Add(Set[CheckX - LeftBottomCoordinate.x, CheckY - LeftBottomCoordinate.y]);
}
}
}
return(Neighbours);
}
List <Vector2> FindPath(Vector2 StartPosition, Vector2 EndPosition, TileType T)
{
AstarObject[,] Set = new AstarObject[Size.x, Size.y];
for (int x = 0; x < Size.x; x++)
{
for (int y = 0; y < Size.y; y++)
{
Set[x, y] = new AstarObject(LeftBottomCoordinate.x + x, LeftBottomCoordinate.y + y, world);
}
}
//List<AstarObject> OpenSet = new List<AstarObject>();
Heap <AstarObject> OpenSet = new Heap <AstarObject>(Size.x * Size.y);
HashSet <AstarObject> ClosedSet = new HashSet <AstarObject>();
AstarObject Start = Set[StartPosition.x, StartPosition.y];
AstarObject End = Set[EndPosition.x, EndPosition.y];
OpenSet.Add(Start);
while (OpenSet.Count > 0)
{
/*
* AstarObject CurrentLocation = OpenSet[0];
* for (int i = 1; i < OpenSet.Count; i++)
* {
* if (OpenSet[i].fCost < CurrentLocation.fCost || OpenSet[i].fCost == CurrentLocation.fCost && OpenSet[i].hCost < CurrentLocation.hCost)
* {
* CurrentLocation = OpenSet[i];
* }
* }
* OpenSet.Remove(CurrentLocation);
*/
AstarObject CurrentLocation = OpenSet.RemoveFirst();
ClosedSet.Add(CurrentLocation);
if (CurrentLocation == End)
{
return(RetracePath(Start, End));
//Retracepath and stuff.
}
List <AstarObject> Neighbours = GetNeighbours(CurrentLocation, ref Set);
foreach (AstarObject neighbour in Neighbours)
{
if (neighbour.TType != T || ClosedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = CurrentLocation.gCost + GetDistance(CurrentLocation, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !OpenSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, End);
neighbour.parent = CurrentLocation;
if (!OpenSet.Contains(neighbour))
{
OpenSet.Add(neighbour);
}
else
{
OpenSet.UpdateItem(neighbour);
}
}
}
}
return(new List <Vector2>());
}