private void ReconstructPlan(ref AntAIPlan aPlan, List <AntAINode> aClosed, AntAINode aGoal)
{
aPlan.Reset();
int index;
AntAINode current = aGoal;
while (current != null && current.parent != null)
{
aPlan.Insert(current.action);
index = FindEqual(aClosed, current.parent);
current = (index == -1) ? aClosed[0] : aClosed[index];
}
}
/// <summary>
/// Builds the plan.
/// </summary>
/// <param name="aPlan">Reference to the result Plan.</param>
/// <param name="aCurrent">Current World State.</param>
/// <param name="aGoal">Goal World State that we want reach.</param>
public void MakePlan(ref AntAIPlan aPlan, AntAICondition aCurrent, AntAICondition aGoal)
{
#if UNITY_EDITOR
DebugConditions = aCurrent.Clone();
#endif
var opened = new List <AntAINode>();
var closed = new List <AntAINode>();
opened.Add(new AntAINode
{
world = aCurrent,
parent = null,
cost = 0,
heuristic = aCurrent.Heuristic(aGoal),
sum = aCurrent.Heuristic(aGoal),
action = string.Empty
});
AntAINode current = opened[0];
while (opened.Count > 0)
{
// Find lowest rank.
current = opened[0];
for (int i = 1, n = opened.Count; i < n; i++)
{
if (opened[i].sum < current.sum)
{
current = opened[i];
}
}
opened.Remove(current);
if (current.world.Match(aGoal))
{
// Plan is found!
ReconstructPlan(ref aPlan, closed, current);
aPlan.isSuccess = true;
if (EventPlanUpdated != null)
{
EventPlanUpdated(aPlan);
}
return;
}
closed.Add(current);
// Get neighbors.
List <AntAIAction> neighbors = GetPossibleTransitions(current.world);
AntAICondition neighbor;
int openedIndex = -1;
int closedIndex = -1;
int cost = -1;
for (int i = 0, n = neighbors.Count; i < n; i++)
{
cost = current.cost + neighbors[i].cost;
neighbor = current.world.Clone();
neighbor.Act(neighbors[i].post);
openedIndex = FindEqual(opened, neighbor);
closedIndex = FindEqual(closed, neighbor);
if (openedIndex > -1 && cost < opened[openedIndex].cost)
{
opened.RemoveAt(openedIndex);
openedIndex = -1;
}
if (closedIndex > -1 && cost < closed[closedIndex].cost)
{
closed.RemoveAt(closedIndex);
closedIndex = -1;
}
if (openedIndex == -1 && closedIndex == -1)
{
opened.Add(new AntAINode
{
world = neighbor,
cost = cost,
heuristic = neighbor.Heuristic(aGoal),
sum = cost + neighbor.Heuristic(aGoal),
action = neighbors[i].name,
parent = current.world
});
}
}
}
// Failed plan.
ReconstructPlan(ref aPlan, closed, current);
aPlan.isSuccess = false;
if (EventPlanUpdated != null)
{
EventPlanUpdated(aPlan);
}
}