internal static Path FindPath(NodeHandle startNode, NodeHandle targetNode, ConcurrentSet <NodeHandle> closedSet, uint maxMs, CancellationToken cancelToken, uint clearance, Action <NodeHandle> progress, bool debug = false)
{
var s = new Stopwatch();
findPathTimer.Start();
try {
Vector3 targetNodePos = Position(targetNode);
if (startNode == 0)
{
return(Fail($"[{targetNode}] FindPath failed: startNode is zero."));
}
if (targetNode == 0)
{
return(Fail($"[{targetNode}] FindPath failed: targetNode is zero."));
}
closedSetTimer.Start();
closedSet.Remove(startNode);
if (closedSet.Contains(targetNode))
{
return(Fail($"[{targetNode}] FindPath failed: targetNode is blocked"));
}
closedSetTimer.Stop();
// TODO: it would be best if we could combine fScore and openSet
// fScore should be a heap that re-heaps when a value updates
// isOpen(node) becomes fScore.Contains(node)
// var fScore = new Dictionary<NodeHandle, float>();
fScoreTimer.Start();
var fScore = new Heap <NodeHandle>();
fScore.Add(startNode, Estimate(startNode, targetNode));
fScoreTimer.Stop();
var cameFrom = new Dictionary <NodeHandle, NodeHandle>();
var gScore = new Dictionary <NodeHandle, float>();
gScore.TryAdd(startNode, 0);
float GScore(NodeHandle n) => gScore.ContainsKey(n) ? gScore[n] : float.MaxValue;
s.Start();
fScoreTimer.Start();
while (fScore.TryPop(out NodeHandle best))
{
fScoreTimer.Stop();
if (cancelToken.IsCancellationRequested)
{
return(Fail($"[{targetNode}] Cancelled."));
}
if (s.ElapsedMilliseconds > maxMs)
{
return(Fail($"[{targetNode}] Searching for too long, ({closedSet.Count} nodes in {s.ElapsedMilliseconds}ms."));
}
// close this node we are just about to visit
closedSetTimer.Start();
closedSet.Add(best);
closedSetTimer.Stop();
// update the progress callback
progress(best);
Vector3 curPos = Position(best);
float dist = (curPos - targetNodePos).LengthSquared();
// Log($"dist = {dist:F2}");
if (dist <= .5f)
{
var ret = new Path(UnrollPath(cameFrom, best, debug));
Log($"[{targetNode}] Found a path of {ret.Count()} steps ({closedSet.Count} searched in {s.ElapsedMilliseconds}ms)");
return(ret);
}
foreach (NodeHandle e in Edges(best))
{
closedSetTimer.Start();
bool closed = closedSet.Contains(e);
closedSetTimer.Stop();
if (!closed && Clearance(e) >= clearance)
{
gScoreTimer.Start();
Vector3 ePos = Position(e);
float scoreOfNewPath = GScore(best) + (curPos - ePos).Length();
float scoreOfOldPath = GScore(e);
gScoreTimer.Stop();
if (scoreOfNewPath < scoreOfOldPath)
{
cameFrom[e] = best;
gScore[e] = scoreOfNewPath;
fScoreTimer.Start();
fScore.AddOrUpdate(e,
gScore[e] // best path to e so far
+ Estimate(ePos, targetNodePos) // plus standard A* estimate
+ Abs(ePos.Z - curPos.Z) // plus a penalty for going vertical
+ ((15 - Clearance(e)) * .3f) // plus a penalty for low clearance
);
fScoreTimer.Stop();
}
}
}
fScoreTimer.Start();
}
return(Fail($"[{targetNode}] Searched all reachable nodes ({closedSet.Count} nodes in {s.ElapsedMilliseconds}ms)."));
} finally {
findPathTimer.Stop();
fScoreTimer.Stop();
}
}
