/// <summary> /// Calculates the distance to a second node. /// </summary> public double Distance(PathFindNode other) { int x = Internal.X - other.Internal.X; int y = Internal.Y - other.Internal.Y; int z = Internal.Z - other.Internal.Z; return(Math.Sqrt(x * x + y * y + z * z)); }
List <Location> Reconstruct(PathFindNode node) { List <Location> locs = new List <Location>(); while (node != null) { locs.Add(node.Internal); node = node.Parent; } locs.Reverse(); return(locs); }
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); }
/// <summary> /// Calculates the distance to a second node. /// </summary> public double Distance(PathFindNode other) { return(Internal.Distance(other.Internal)); }
// Thanks to fullwall for the reference sources this was built off public List <Location> FindPath(Location startloc, Location endloc, double maxRadius, double goaldist, bool isAsync = false) { // TODO: 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); } PathFindNode start = new PathFindNode() { Internal = startloc, F = 0, G = 0 }; PathFindNode end = new PathFindNode() { Internal = endloc, F = 0, G = 0 }; SimplePriorityQueue <PathFindNode> open = new SimplePriorityQueue <PathFindNode>(); HashSet <Location> closed = new HashSet <Location>(); HashSet <Location> openset = new HashSet <Location>(); open.Enqueue(start, start.F); openset.Add(start.Internal); while (open.Count > 0) { PathFindNode next = open.Dequeue(); openset.Remove(next.Internal); if (next.Internal.DistanceSquared(end.Internal) < gosq) { return(Reconstruct(next)); } closed.Add(next.Internal); foreach (Location neighbor in PathFindNode.Neighbors) { Location neighb = next.Internal + neighbor; if (closed.Contains(neighb)) { continue; } if (startloc.DistanceSquared(neighb) > mrsq) { continue; } // TODO: Check solidity from entities too! if (GetBlockMaterial(neighb).GetSolidity() != MaterialSolidity.NONSOLID) // TODO: Better solidity check { continue; } if (GetBlockMaterial(neighb + new Location(0, 0, -1)).GetSolidity() == MaterialSolidity.NONSOLID && GetBlockMaterial(neighb + new Location(0, 0, -2)).GetSolidity() == MaterialSolidity.NONSOLID && GetBlockMaterial(next.Internal + new Location(0, 0, -1)).GetSolidity() == MaterialSolidity.NONSOLID && GetBlockMaterial(next.Internal + new Location(0, 0, -2)).GetSolidity() == MaterialSolidity.NONSOLID) { continue; } PathFindNode node = new PathFindNode() { Internal = neighb }; node.G = next.G + 1; // Note: Distance beween 'node' and 'next' is 1. node.F = node.G + node.Distance(end); node.Parent = next; if (openset.Contains(node.Internal)) { continue; } open.Enqueue(node, node.F); openset.Add(node.Internal); } } return(null); }