IEnumerator Search(Vector3 beginning, Vector3 end)
{
HeapArray <State> openSet = new HeapArray <State>(gridMap.LargestSize);
State goal = gridMap.RetrieveState(end);
State start = gridMap.RetrieveState(beginning);
openSet.Enqueue(start);
HashSet <State> cameFrom = new HashSet <State>();
Vector3[] intermediates = new Vector3[0];
bool foundPath = false;
while (openSet.Count > 0)
{
State present = openSet.Dequeue();
cameFrom.Add(present);
if (present == goal)
{
foundPath = true;
intermediates = ReconstructPath(start, goal);
break;
}
else
{
List <State> adjacents = gridMap.RetrieveAdjacentStates(present);
foreach (State s in adjacents)
{
if (cameFrom.Contains(s) || !(s.unblocked))
{
continue;
}
else
{
int yWeight = Mathf.Abs(present.yCoordinate - s.yCoordinate);
int xWeight = Mathf.Abs(present.xCoordinate - s.xCoordinate);
int edgeWeight = 0;
if (xWeight < yWeight)
{
edgeWeight = 10 * (yWeight - xWeight) + 14 * xWeight;
}
else
{
edgeWeight = 10 * (xWeight - yWeight) + 14 * yWeight;
}
int tentativeCost = present.gOfN + edgeWeight;
if (!(openSet.Contains(s)) || tentativeCost < s.gOfN)
{
s.parent = present;
s.gOfN = tentativeCost;
yWeight = Mathf.Abs(s.yCoordinate - goal.yCoordinate);
xWeight = Mathf.Abs(s.xCoordinate - goal.xCoordinate);
if (xWeight < yWeight)
{
s.hOfN = 10 * (yWeight - xWeight) + 14 * xWeight;
}
else
{
s.hOfN = 10 * (xWeight - yWeight) + 14 * yWeight;
}
if (!openSet.Contains(s))
{
openSet.Enqueue(s);
}
else
{
openSet.UpdateState(s);
}
}
}
}
}
}
yield return(null);
searchHandler.FinishedPresentSearch(intermediates, foundPath);
}
