void PushClosedList(openNode node)
{
while (mOpenList.Contains(node))
{
mOpenList.Remove(node);
}
if (!mClosedList.ContainsKey(node.index) || node.f < mClosedList[node.index].f)
{
mClosedList[node.index] = node;
}
}
public override bool Init()
{
mResults.Clear();
mOpenList.Clear();
mClosedList.Clear();
openNode node = new openNode(mStart.index);
node.fromIndex = mStart.index;
node.g = 0;
node.h = (mEnd.GetPosNormalized() - mStart.GetPosNormalized()).magnitude;
mOpenList.AddFirst(node);
return(true);
}
bool BuildPath()
{
uint index = mEnd.index;
while (mClosedList.ContainsKey(index))
{
openNode node = mClosedList[index];
if (index == mStart.index)
{
mResults.Reverse();
return(true);
}
mResults.Add(index);
index = node.fromIndex;
}
return(false);
}
public override bool Step()
{
if (!Finished)
{
// sort nodes by f value
openNode nodeData = mOpenList.OrderBy(x => x.f).First <openNode>();
mOpenList.Remove(nodeData);
Coords node = new Coords();
node.Set(nodeData.index);
PushClosedList(nodeData);
if (node == mEnd)
{
Finished = true;
BuildPath();
mOpenList.Clear();
}
else
{
// add neighbors if not in open list or results
foreach (int d in node.NeighborDirs)
{
Coords neighbor = node.GetNeighbor(d) as Coords;
float weight;
if (IsTransitionValid(node, neighbor, out weight))
{
//bool add = true;
openNode neighborData = new openNode(neighbor.index);
neighborData.fromIndex = nodeData.index;
neighborData.g = nodeData.g + (2 * 2) + weight;
neighborData.h = (mEnd.GetPosNormalized() - neighbor.GetPosNormalized()).magnitude;
PushOpenList(neighborData);
}
}
Finished = mOpenList.Count <= 0;
}
}
return(!Finished);
}
void PushOpenList(openNode node)
{
if (mClosedList.ContainsKey(node.index))
{
if (node.f < mClosedList[node.index].f)
{
mClosedList.Remove(node.index);
mOpenList.AddFirst(node);
}
}
else if (mOpenList.Contains(node))
{
LinkedListNode <openNode> listNode = mOpenList.Find(node);
if (node.f < listNode.Value.f)
{
listNode.Value = node;
}
}
else
{
mOpenList.AddFirst(node);
}
}
