public int GetNode(PathFindNodeSet nodes, ref int nloc, Vector3i loc, double f, double g, int parent)
{
int id = nloc++;
if (nloc == nodes.Nodes.Length)
{
PathFindNode[] new_nodes = new PathFindNode[nodes.Nodes.Length * 2];
Array.Copy(nodes.Nodes, 0, new_nodes, 0, nodes.Nodes.Length);
nodes.Nodes = new_nodes;
}
PathFindNode pf;
pf.Parent = parent;
pf.Internal = loc;
pf.F = f;
pf.G = g;
nodes.Nodes[id] = pf;
return(id);
}
/// <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="goaldist">The maximum distance from the goal allowed.</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, double goaldist, 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 = goaldist * goaldist;
if (startloc.DistanceSquared(endloc) > mrsq)
{
cts.Cancel();
return(null);
}
PathFindNodeSet nodes;
PriorityQueue <PFEntry> open;
Dictionary <Vector3i, Chunk> map;
lock (PFNodeSetLock)
{
if (PFNodeSet.Count == 0)
{
nodes = null;
open = null;
map = null;
}
else
{
nodes = PFNodeSet.Pop();
open = PFQueueSet.Pop();
map = PFMapSet.Pop();
}
}
if (nodes == null)
{
nodes = new PathFindNodeSet()
{
Nodes = new PathFindNode[8192]
};
open = new PriorityQueue <PFEntry>(8192);
map = new Dictionary <Vector3i, Chunk>(1024);
}
int nloc = 0;
int start = GetNode(nodes, ref nloc, startloc.ToVec3i(), 0.0, 0.0, -1);
// TODO: Grab these from a stack too?
Dictionary <Vector3i, PathFindNode> closed = new Dictionary <Vector3i, PathFindNode>();
Dictionary <Vector3i, PathFindNode> openset = new Dictionary <Vector3i, PathFindNode>();
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();
lock (PFNodeSetLock)
{
PFNodeSet.Push(nodes);
PFQueueSet.Push(open);
PFMapSet.Push(map);
}
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)
{
continue;
}
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();
lock (PFNodeSetLock)
{
PFNodeSet.Push(nodes);
PFQueueSet.Push(open);
PFMapSet.Push(map);
}
cts.Cancel();
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.
/// Questionably 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="goaldist">The maximum distance from the goal allowed.</param>
/// <returns>The shortest path, as a list of blocks to travel through.</returns>
public List <Location> FindPath(Location startloc, Location endloc, double maxRadius, double goaldist)
{
// 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 = goaldist * goaldist;
if (startloc.DistanceSquared(endloc) > mrsq)
{
return(null);
}
PathFindNodeSet nodes;
PriorityQueue <PFEntry> open;
Dictionary <Vector3i, Chunk> map;
lock (PFNodeSetLock)
{
if (PFNodeSet.Count == 0)
{
nodes = null;
open = null;
map = null;
}
else
{
nodes = PFNodeSet.Pop();
open = PFQueueSet.Pop();
map = PFMapSet.Pop();
}
}
if (nodes == null)
{
nodes = new PathFindNodeSet()
{
Nodes = new PathFindNode[8192]
};
open = new PriorityQueue <PFEntry>(8192);
map = new Dictionary <Vector3i, Chunk>(1024);
}
int nloc = 0;
int start = GetNode(nodes, ref nloc, startloc, 0.0, 0.0, -1);
HashSet <Location> closed = new HashSet <Location>();
HashSet <Location> openset = new HashSet <Location>();
PFEntry pfet;
pfet.Nodes = nodes;
pfet.ID = start;
open.Enqueue(ref pfet, 0.0);
openset.Add(startloc);
// TODO: relevant chunk map, to shorten the block solidity lookup time!
while (open.Count > 0)
{
int nextid = open.Dequeue().ID;
PathFindNode next = nodes.Nodes[nextid];
openset.Remove(next.Internal);
if (next.Internal.DistanceSquared(endloc) < gosq)
{
open.Clear();
map.Clear();
lock (PFNodeSetLock)
{
PFNodeSet.Push(nodes);
PFQueueSet.Push(open);
PFMapSet.Push(map);
}
return(Reconstruct(nodes.Nodes, nextid));
}
closed.Add(next.Internal);
foreach (Location neighbor in PathFindNode.Neighbors)
{
Location neighb = next.Internal + neighbor;
if (startloc.DistanceSquared(neighb) > mrsq)
{
continue;
}
if (closed.Contains(neighb))
{
continue;
}
if (openset.Contains(neighb))
{
continue;
}
// TODO: Check solidity from entities too!
if (GetBlockMaterial(map, neighb).GetSolidity() != MaterialSolidity.NONSOLID) // TODO: Better solidity check
{
continue;
}
if (GetBlockMaterial(map, neighb + new Location(0, 0, -1)).GetSolidity() == MaterialSolidity.NONSOLID &&
GetBlockMaterial(map, neighb + new Location(0, 0, -2)).GetSolidity() == MaterialSolidity.NONSOLID &&
GetBlockMaterial(map, next.Internal + new Location(0, 0, -1)).GetSolidity() == MaterialSolidity.NONSOLID &&
GetBlockMaterial(map, next.Internal + new Location(0, 0, -2)).GetSolidity() == MaterialSolidity.NONSOLID)
{
continue;
}
int node = GetNode(nodes, ref nloc, neighb, next.G + 1.0 + neighb.Distance(endloc), next.G + 1.0, nextid);
PFEntry tpfet;
tpfet.Nodes = nodes;
tpfet.ID = node;
open.Enqueue(ref tpfet, nodes.Nodes[node].F);
openset.Add(nodes.Nodes[node].Internal);
}
}
open.Clear();
map.Clear();
lock (PFNodeSetLock)
{
PFNodeSet.Push(nodes);
PFQueueSet.Push(open);
PFMapSet.Push(map);
}
return(null);
}