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);
}
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;
}
