private static IEnumerable <Tuple <Location, float> > AdvancedNeighbours(Location root, PathfinderContext context) { yield return(Tuple.Create(context.CreatePoint(root.X + 1, root.Y, root.Z), context.GetBlockWeight(root.X + 1, root.Y, root.Z) + context.GetBlockWeight(root.X + 1, root.Y + 1, root.Z))); yield return(Tuple.Create(context.CreatePoint(root.X - 1, root.Y, root.Z), context.GetBlockWeight(root.X - 1, root.Y, root.Z) + context.GetBlockWeight(root.X - 1, root.Y + 1, root.Z))); yield return(Tuple.Create(context.CreatePoint(root.X, root.Y, root.Z + 1), context.GetBlockWeight(root.X, root.Y, root.Z + 1) + context.GetBlockWeight(root.X, root.Y + 1, root.Z + 1))); yield return(Tuple.Create(context.CreatePoint(root.X, root.Y, root.Z - 1), context.GetBlockWeight(root.X, root.Y, root.Z - 1) + context.GetBlockWeight(root.X, root.Y + 1, root.Z - 1))); yield return(Tuple.Create(context.CreatePoint(root.X, root.Y + 1, root.Z), context.GetBlockWeight(root.X, root.Y + 2, root.Z))); if (root.Y > 0) { yield return(Tuple.Create(context.CreatePoint(root.X, root.Y - 1, root.Z), context.GetBlockWeight(root.X, root.Y - 1, root.Z))); } }
public static Path Shovel(Location start, Location end, bool includeLast = true, bool digBlocs = true, float maxDistance = 64f, PathfinderContext context = null) { if (context == null) { context = Context; } try { var nodes = new FastPriorityQueue <Location>((int)maxDistance * (int)maxDistance * 255); var cost = new Dictionary <Location, float>(); var succeed = false; nodes.Enqueue(start, 0); start.Prev = null; cost.Add(start, 0f); var endCost = context.GetBlockWeight(end.X, end.Y, end.Z) + context.GetBlockWeight(end.X, end.Y + 1, end.Z); if (endCost < 0) { return(null); } while (nodes.Count > 0) { var current = nodes.Dequeue(); if (current.Equals(end)) { end = current; succeed = true; break; } foreach (var node in AdvancedNeighbours(current, context)) { var next = node.Item1; if (next.Equals(end) && !includeLast) { end = current; succeed = true; break; } if (node.Item2 < 0) { continue; } if (node.Item1.DistanceTo(start) > maxDistance) { continue; } if (!digBlocs && node.Item2 > 0) { continue; } var nodeCost = cost[current] + 1f + node.Item2; if (!cost.TryGetValue(next, out float nextCost)) { cost.Add(next, nodeCost); nodes.Enqueue(next, nodeCost + next.DistanceTo(end)); next.Prev = current; } else if (nextCost > nodeCost) { cost[next] = nodeCost; nodes.UpdatePriority(next, nodeCost + next.DistanceTo(end)); next.Prev = current; } } if (succeed) { break; } } if (!succeed) { return(null); } var pathfrom = new List <Location>(); while (end.Prev != null) { pathfrom.Add(end); end = end.Prev; } pathfrom.Reverse(); return(new Path(pathfrom)); } catch (Exception e) { Log.Error($"Cant create path! Error : {e}"); return(null); } finally { context.ResetBuffers(); GC.Collect(); } }