// Basic A* implementation.
public List<IVector3> Path(IVector3 start, IVector3 end)
{
Debug.Assert(start.BoundedBy(IVector3.Zero, Dimensions));
Debug.Assert(end.BoundedBy(IVector3.Zero, Dimensions));
List<IVector3> result = new List<IVector3>();
HashSet<IVector3> closed = new HashSet<IVector3>();
HashSet<IVector3> open = new HashSet<IVector3>();
open.Add(start);
var cameFrom = new Dictionary<IVector3, IVector3>();
var goalScore = new Dictionary<IVector3, double>();
goalScore.Add(start, 0);
var guessScore = new Dictionary<IVector3, double>();
guessScore.Add(start, Cost(start, end));
var finalScore = new Dictionary<IVector3, double>();
finalScore.Add(start, guessScore[start]);
while(open.Count != 0)
{
double minscore = finalScore[open.First()];
IVector3 node = open.First();
foreach (var n in open)
{
if (finalScore[n] < minscore)
{
minscore = finalScore[n];
node = n;
}
}
if (node == end)
{
Reconstruct(cameFrom, cameFrom[end], result);
result.Add(end);
return result;
}
open.Remove(node);
closed.Add(node);
foreach (var t in PassableNodes(node))
{
if (closed.Contains(t))
{
continue;
}
bool isBetter = false;
double score = goalScore[node] + IVector3.Distance(node, t);
if (!open.Contains(t))
{
open.Add(t);
isBetter = true;
}
else if (score < goalScore[t])
{
isBetter = true;
}
else
{
isBetter = false;
}
if (isBetter)
{
cameFrom[t] = node;
goalScore[t] = score;
guessScore[t] = Cost(t, end);
finalScore[t] = score + Cost(t, end);
}
}
}
//No path!
return null;
}
