/// <summary>
/// Finds a path from the start to the end, if one exists.
/// Current implementation is A-Star (A*).
/// Possibly safe for Async usage.
/// Runs two searches simultaneously (using async) from either end, and returns the shorter path (either one failing = both fail immediately!).
/// </summary>
/// <param name="startloc">The starting location.</param>
/// <param name="endloc">The ending location.</param>
/// <param name="maxRadius">The maximum radius to search through.</param>
/// <param name="pfopts">Any pathfinder options.</param>
/// <returns>The shortest path, as a list of blocks to travel through.</returns>
public List <Location> FindPathAsyncDouble(Location startloc, Location endloc, double maxRadius, PathfinderOptions pfopts)
{
CancellationTokenSource cts = new CancellationTokenSource();
List <Location> a = new List <Location>();
List <Location> b = new List <Location>();
Task one = TheWorld.Schedule.StartAsyncTask(() =>
{
List <Location> f = FindPath(startloc, endloc, maxRadius, pfopts, cts);
if (f != null)
{
a.AddRange(f);
}
}).Created;
Task two = TheWorld.Schedule.StartAsyncTask(() =>
{
List <Location> f = FindPath(endloc, startloc, maxRadius, pfopts, cts);
if (f != null)
{
b.AddRange(f);
}
}).Created;
one.Wait();
two.Wait();
if (a.Count > 0 && b.Count > 0)
{
if (a.Count < b.Count)
{
return(a);
}
else
{
b.Reverse();
return(b);
}
}
return(null);
}
/// <summary>
/// Finds a path from the start to the end, if one exists.
/// Current implementation is A-Star (A*).
/// Thanks to fullwall for the reference sources this was originally built from.
/// Possibly safe for Async usage.
/// </summary>
/// <param name="startloc">The starting location.</param>
/// <param name="endloc">The ending location.</param>
/// <param name="maxRadius">The maximum radius to search through.</param>
/// <param name="pfopts">Any pathfinder options.</param>
/// <param name="cts">A cancellation token, if any.</param>
/// <returns>The shortest path, as a list of blocks to travel through.</returns>
public List <Location> FindPath(Location startloc, Location endloc, double maxRadius, PathfinderOptions pfopts, CancellationTokenSource cts = null)
{
// TODO: Improve async safety!
startloc = startloc.GetBlockLocation() + new Location(0.5, 0.5, 1.0);
endloc = endloc.GetBlockLocation() + new Location(0.5, 0.5, 1.0);
double mrsq = maxRadius * maxRadius;
double gosq = pfopts.GoalDist * pfopts.GoalDist;
if (startloc.DistanceSquared(endloc) > mrsq)
{
cts.Cancel();
return(null);
}
PathFindNodeSet nodes;
PriorityQueue <PFEntry> open;
Dictionary <Vector3i, Chunk> map;
Dictionary <Vector3i, PathFindNode> closed;
Dictionary <Vector3i, PathFindNode> openset;
lock (PFNodeSetLock)
{
if (PFNodeSet.Count == 0)
{
nodes = null;
open = null;
map = null;
closed = null;
openset = null;
}
else
{
nodes = PFNodeSet.Pop();
open = PFQueueSet.Pop();
map = PFMapSet.Pop();
closed = PFClosedSet.Pop();
openset = PFOpenSet.Pop();
}
}
if (nodes == null)
{
nodes = new PathFindNodeSet()
{
Nodes = new PathFindNode[8192]
};
open = new PriorityQueue <PFEntry>(8192);
map = new Dictionary <Vector3i, Chunk>(1024);
closed = new Dictionary <Vector3i, PathFindNode>(1024);
openset = new Dictionary <Vector3i, PathFindNode>(1024);
}
int nloc = 0;
int start = GetNode(nodes, ref nloc, startloc.ToVec3i(), 0.0, 0.0, -1);
PFEntry pfet;
pfet.Nodes = nodes;
pfet.ID = start;
open.Enqueue(ref pfet, 0.0);
openset[startloc.ToVec3i()] = nodes.Nodes[start];
while (open.Count > 0)
{
if (cts.IsCancellationRequested)
{
return(null);
}
int nextid = open.Dequeue().ID;
PathFindNode next = nodes.Nodes[nextid];
if (openset.TryGetValue(next.Internal, out PathFindNode pano) && pano.F < next.F)
{
continue;
}
openset.Remove(next.Internal);
if (next.Internal.ToLocation().DistanceSquared(endloc) < gosq)
{
open.Clear();
map.Clear();
closed.Clear();
openset.Clear();
lock (PFNodeSetLock)
{
PFNodeSet.Push(nodes);
PFQueueSet.Push(open);
PFMapSet.Push(map);
PFClosedSet.Push(closed);
PFOpenSet.Push(openset);
}
return(Reconstruct(nodes.Nodes, nextid));
}
if (closed.TryGetValue(next.Internal, out PathFindNode pfn) && pfn.F < next.F)
{
continue;
}
closed[next.Internal] = next;
foreach (Vector3i neighbor in PathFindNode.Neighbors)
{
Vector3i neighb = next.Internal + neighbor;
if (startloc.DistanceSquared(neighb.ToLocation()) > mrsq)
{
continue;
}
// Note: Add `&& fbv.F <= next.F)` to enhance precision of results... but it makes invalid searches take forever.
if (closed.TryGetValue(neighb, out PathFindNode fbv))
{
continue;
}
// Note: Add `&& pfv.F <= next.F)` to enhance precision of results... but it makes invalid searches take forever.
if (openset.TryGetValue(neighb, out PathFindNode pfv))
{
continue;
}
// TODO: Check solidity from very solid entities too!
if (GetBlockMaterial(map, neighb).GetSolidity() != MaterialSolidity.NONSOLID) // TODO: Better solidity check
{
continue;
}
if (GetBlockMaterial(map, neighb + new Vector3i(0, 0, -1)).GetSolidity() == MaterialSolidity.NONSOLID &&
GetBlockMaterial(map, neighb + new Vector3i(0, 0, -2)).GetSolidity() == MaterialSolidity.NONSOLID &&
GetBlockMaterial(map, next.Internal + new Vector3i(0, 0, -1)).GetSolidity() == MaterialSolidity.NONSOLID &&
GetBlockMaterial(map, next.Internal + new Vector3i(0, 0, -2)).GetSolidity() == MaterialSolidity.NONSOLID)
{
// TODO: Implement CanParkour
continue;
}
// TODO: Implement CanSwim
int node = GetNode(nodes, ref nloc, neighb, next.G + 1.0, next.F + 1.0 + neighb.ToLocation().Distance(endloc), nextid);
PFEntry tpfet;
tpfet.Nodes = nodes;
tpfet.ID = node;
open.Enqueue(ref tpfet, nodes.Nodes[node].F);
openset[neighb] = nodes.Nodes[node];
}
}
open.Clear();
map.Clear();
closed.Clear();
openset.Clear();
lock (PFNodeSetLock)
{
PFNodeSet.Push(nodes);
PFQueueSet.Push(open);
PFMapSet.Push(map);
PFClosedSet.Push(closed);
PFOpenSet.Push(openset);
}
cts.Cancel();
return(null);
}
