public void swap(int i, int j)
{
AStarHeapNode temp = heap[i];
heap[i] = heap[j];
heap[j] = temp;
}
protected List <MeshNode> pathFrom(MeshNode start, MeshNode target, List <MeshNode> ignore)
{
if (ignore == null)
{
ignore = new List <MeshNode>();
}
List <MeshNode> path = new List <MeshNode>();
AStarHeap q = new AStarHeap();
//{node: (path length, previous node)}
Dictionary <MeshNode, KeyValuePair <float, MeshNode> > distances = new Dictionary <MeshNode, KeyValuePair <float, MeshNode> >();
q.add(new AStarHeapNode(start, 0, Vector3.Distance(start.position, target.position)));
distances.Add(start, new KeyValuePair <float, MeshNode>(0, null));
while (!q.empty())
{
AStarHeapNode n = q.pop();
if (n.node == target)
{
break;
}
//try and expand from this node to shortest paths
foreach (MeshNode m in n.node.neighbours)
{
if (ignore.Contains(m))
{
continue;
}
float pathLength = n.pathLength + Vector3.Distance(m.position, n.node.position);
//see if we don't already have a shorter path to this node
if (distances.ContainsKey(m) && distances[m].Key <= pathLength)
{
continue;
}
//otherwise this is promising path, so add it to the queue
q.add(new AStarHeapNode(m, pathLength, pathLength + Vector3.Distance(m.position, target.position)));
distances.Remove(m);
distances.Add(m, new KeyValuePair <float, MeshNode>(pathLength, n.node));
}
}
//if we've found a path
if (distances.ContainsKey(target))
{
//backtrack the entire path and add it to the path list
MeshNode m = target;
while (m != null)
{
path.Insert(0, m);
m = distances[m].Value;
}
}
return(path);
}
public void add(AStarHeapNode a)
{
heap.Add(a);
int i = heap.Count - 1;
while (i > 0)
{
int parentI = (i - 1) / 2;
if (heap[parentI].cost > heap[i].cost)
{
swap(parentI, i);
}
else
{
break;
}
i = parentI;
}
}
public AStarHeapNode pop()
{
if (heap.Count == 0)
{
return(null);
}
AStarHeapNode root = heap[0];
heap.RemoveAt(0);
if (heap.Count <= 1)
{
return(root);
}
heap.Insert(0, heap[heap.Count - 1]);
heap.RemoveAt(heap.Count - 1);
int i = 0;
while (i < heap.Count)
{
int minChild = -1, left = i * 2 + 1, right = i * 2 + 2;
if (left < heap.Count)
{
minChild = left;
}
if (right < heap.Count && (minChild == -1 || heap[right].cost < heap[minChild].cost))
{
minChild = right;
}
if (minChild == -1)
{
break;
}
swap(i, minChild);
i = minChild;
}
return(root);
}
public void add(AStarHeapNode a)
{
heap.Add(a);
int i = heap.Count - 1;
while (i > 0) {
int parentI = (i - 1) / 2;
if (heap[parentI].cost > heap[i].cost)
swap(parentI, i);
else
break;
i = parentI;
}
}