public ICollection<Helper.Point> Star( Helper.Point posIni, Helper.Point posFinal, out int totalCost)
{
var heapBorder = new Heap<Elem>();
// Console.WriteLine("cheguei no astar");
List<Elem> explored = new List<Elem>();
/* Array to verify if a position was explored */
var hasExpl = new bool[qtdNodes,qtdNodes];
var inBorder = new bool[qtdNodes,qtdNodes];
hasExpl.Initialize();
inBorder.Initialize();
Elem father = new Elem(0, posIni);
heapBorder.HeapAdd( h(posIni,posFinal), father );
while (heapBorder.HeapSize() > 0 )
{
father = heapBorder.HeapExtractMin().Item3 ;
inBorder[father.pos.x, father.pos.y] = false;
if( father.pos.Equals(posFinal) )
break;
explored.Insert(0, father);
hasExpl[father.pos.x, father.pos.y] = true;
foreach (var child in father.pos.Neighborhood( posFinal) )
{
int accChild = 0;
accChild = father.accCost + 1;
if (hasExpl[child.x, child.y] && accChild >= father.accCost)
continue;
if (inBorder[child.x, child.y] == false || accChild < father.accCost)
{
heapBorder.HeapAdd(h(child, posFinal) + accChild, new Elem(accChild, child, father.pos));
inBorder[child.x, child.y] = true;
}
}
}
var pathReturn = new List<Helper.Point>();
pathReturn.Insert(0, father.pos );
totalCost = father.accCost;
if (!father.parent.HasValue)
return pathReturn;
var currParent = father.parent.Value ;
for (int i = 0 , j = 1; i < explored.Count; i++)
{
if (explored[i].pos.Equals(currParent) )
{
pathReturn.Insert(j,explored[i].pos);
j++;
currParent = explored[i].parent.HasValue ? explored[i].parent.Value : posIni ;
//Debug.WriteLine("custo "+explored[i].accCost);
}
}
pathReturn.Reverse();
return pathReturn.Skip(1).ToList();
}
public void TestHeapMinEmptySimple()
{
var testHeap = new Heap<string>();
Random rd = new Random();
int size = 1000000;
//Heap comeca vazio
int heapTreeSizeBefore = 0;
for (int i = 1; i <= size; i++)
{
testHeap.HeapAdd(rd.Next(1, int.MaxValue), i.ToString());
Assert.IsTrue(testHeap.HeapSize() > heapTreeSizeBefore);
heapTreeSizeBefore = testHeap.HeapSize();
}
int minElementBefore = -1;
heapTreeSizeBefore = size;
for (int i = 1; i <= size; i++)
{
var minElement = testHeap.HeapExtractMin();
Assert.IsTrue(minElementBefore <= minElement.Item1);
Assert.IsTrue(testHeap.HeapSize() < heapTreeSizeBefore);
heapTreeSizeBefore = testHeap.HeapSize();
}
}