static List <AstarNode> NearNodes(AstarNode center, IAstar ia)
{
List <AstarNode> list = new List <AstarNode>();
KeyValuePair <int, int> loc;
if (Inside(loc = new KeyValuePair <int, int>(center.Loc.Key, center.Loc.Value - 1)))
{
list.Add(new AstarNode(loc, ia.GetWeight(center.Loc.Key, center.Loc.Value - 1), center.Cost, center.Deep));
}
if (Inside(loc = new KeyValuePair <int, int>(center.Loc.Key - 1, center.Loc.Value)))
{
list.Add(new AstarNode(loc, ia.GetWeight(center.Loc.Key - 1, center.Loc.Value), center.Cost, center.Deep));
}
if (Inside(loc = new KeyValuePair <int, int>(center.Loc.Key + 1, center.Loc.Value)))
{
list.Add(new AstarNode(loc, ia.GetWeight(center.Loc.Key + 1, center.Loc.Value), center.Cost, center.Deep));
}
if (Inside(loc = new KeyValuePair <int, int>(center.Loc.Key, center.Loc.Value + 1)))
{
list.Add(new AstarNode(loc, ia.GetWeight(center.Loc.Key, center.Loc.Value + 1), center.Cost, center.Deep));
}
return(list);
}
static public List <KeyValuePair <int, int> > Search(KeyValuePair <int, int> start, KeyValuePair <int, int> goal, IAstar ia)
{
openHeap = new AntHill.NET.Heap.Heap <AstarNode>();
closedHeap = new AntHill.NET.Heap.Heap <AstarNode>();
AstarNode StartNode = new AstarNode(start, ia.GetWeight(start.Key, start.Value), 0, 0);
StartNode.Parent = null;
StartNode.Calc(goal);
dict = new Dictionary <KeyValuePair <int, int>, AstarNode>();
int idx;
openHeap.Insert(StartNode);
dict.Add(StartNode.Loc, StartNode);
bool inClosed;
bool flag;
List <AstarNode> near;
List <AstarNode> .Enumerator e;
AstarNode other;
while (openHeap.Count != 0)
{
AstarNode node = openHeap.DeleteMax();
dict.Remove(node.Loc);
if (node.Loc.Equals(goal) || node.Deep == max_depth)
{
return(CreatePath(node));
}
near = NearNodes(node, ia);
e = near.GetEnumerator();
while (e.MoveNext())
{
other = e.Current;
if (other.IsObstacle)
{
closedHeap.Insert(other); /*Closed.Add(other)*/; continue;
}
inClosed = false;
other.Calc(goal);
flag = false;
if (dict.ContainsKey(other.Loc))
{
if (dict[other.Loc].TotalCost < other.TotalCost)
{
continue;
}
flag = true;
}
else if ((idx = closedHeap.Contains(other)) != -1)
{
inClosed = true;
if (closedHeap[idx].TotalCost < other.TotalCost)
{
continue;
}
}
other.Parent = node;
if (inClosed)
{
closedHeap.Remove(other);
}
else
{
if (flag)
{
openHeap.Remove(dict[other.Loc]);
dict.Remove(other.Loc);
}
openHeap.Insert(other);
dict.Add(other.Loc, other);
}
}//foreach
closedHeap.Insert(node);
}
List <KeyValuePair <int, int> > l = new List <KeyValuePair <int, int> >();
return(l);
}