public void Add(int pr, Vector2 po) { AStarEntry entry = new AStarEntry(pr, po); queue.Add(entry); }
// This isn't really A* at all. But whatevs. public List<Coordinate> CustomAStar(Coordinate start, Coordinate end, int iterationLimit = 200) { Dictionary<Coordinate, bool> closed = new Dictionary<Coordinate, bool>(); Dictionary<Coordinate, AStarEntry> queueReverseLookup = new Dictionary<Coordinate, AStarEntry>(); Coordinate? closestStart = this.FindClosestAStarCell(start, 10); Coordinate? closestEnd = this.FindClosestFilledCell(end); if (!closestStart.HasValue || !closestEnd.HasValue) return null; else { start = closestStart.Value; end = closestEnd.Value; } Vector3 endPos = this.GetRelativePosition(end); PriorityQueue<AStarEntry> queue = new PriorityQueue<AStarEntry>(new LambdaComparer<AStarEntry>((x, y) => x.ToGoal.CompareTo(y.ToGoal))); AStarEntry firstEntry = new AStarEntry { Coordinate = start, SoFar = 0, ToGoal = (this.GetRelativePosition(start) - endPos).Length() }; queue.Push(firstEntry); queueReverseLookup.Add(start, firstEntry); int iteration = 0; while (queue.Count > 0) { AStarEntry entry = queue.Pop(); if (iteration == iterationLimit || (Math.Abs(entry.Coordinate.X - end.X) <= 1 && Math.Abs(entry.Coordinate.Y - end.Y) <= 1 && Math.Abs(entry.Coordinate.Z - end.Z) <= 1)) return this.constructPath(entry); queueReverseLookup.Remove(entry.Coordinate); try { closed.Add(entry.Coordinate, true); } catch (ArgumentException) { continue; } foreach (Direction d in DirectionExtensions.Directions) { Coordinate next = entry.Coordinate.Move(d); if ((entry.Parent == null || !next.Equivalent(entry.Parent.Coordinate)) && !closed.ContainsKey(next)) { Map.CellState state = this[next]; if (state.ID == 0) { // This is an empty cell // We can still use it if it's adjacent to a full cell if (this[next.Move(0, 0, 1)].ID == 0 && this[next.Move(0, 1, 0)].ID == 0 && this[next.Move(0, 1, 1)].ID == 0 && this[next.Move(1, 0, 0)].ID == 0 && this[next.Move(1, 0, 1)].ID == 0 && this[next.Move(1, 1, 0)].ID == 0 && this[next.Move(1, 1, 1)].ID == 0 && this[next.Move(0, 0, -1)].ID == 0 && this[next.Move(0, -1, 0)].ID == 0 && this[next.Move(0, -1, -1)].ID == 0 && this[next.Move(-1, 0, 0)].ID == 0 && this[next.Move(-1, 0, 1)].ID == 0 && this[next.Move(-1, -1, 0)].ID == 0 && this[next.Move(-1, -1, -1)].ID == 0) continue; } else if (state.Permanent) continue; float tentativeGScore = entry.SoFar + 1; AStarEntry newEntry; if (queueReverseLookup.TryGetValue(next, out newEntry)) { if (newEntry.SoFar < tentativeGScore) continue; } if (newEntry == null) { newEntry = new AStarEntry { Coordinate = next, Parent = entry, SoFar = tentativeGScore, ToGoal = (this.GetRelativePosition(next) - endPos).Length() }; queue.Push(newEntry); queueReverseLookup.Add(next, newEntry); } else newEntry.SoFar = tentativeGScore; } } iteration++; } return null; }
public static Map.Box AStar(Map m, Map.Box start, Vector3 target, out int pathLength) { Dictionary<Map.Box, int> closed = new Dictionary<Map.Box, int>(); PriorityQueue<AStarEntry> queue = new PriorityQueue<AStarEntry>(new LambdaComparer<AStarEntry>((x, y) => x.F.CompareTo(y.F))); Dictionary<Map.Box, AStarEntry> queueLookup = new Dictionary<Map.Box, AStarEntry>(); AStarEntry startEntry = new AStarEntry { Parent = null, Box = start, G = 0, F = (target - start.GetCenter()).Length(), BoxSize = Math.Max(start.Width, Math.Max(start.Height, start.Depth)), PathIndex = 0, }; queue.Push(startEntry); queueLookup[start] = startEntry; const float thresholdFCoefficient = 0.6f; const int iterationLimit = 10; int iteration = 0; while (queue.Count > 0) { AStarEntry entry = queue.Pop(); if (iteration >= iterationLimit || entry.F < entry.BoxSize * thresholdFCoefficient) return VoxelChaseAI.reconstructPath(entry, out pathLength); iteration++; queueLookup.Remove(entry.Box); closed[entry.Box] = entry.G; foreach (Map.Box adjacent in entry.Box.Adjacent.ToList()) { if (adjacent == null) continue; int boxSize = Math.Max(adjacent.Width, Math.Max(adjacent.Height, adjacent.Depth)); int tentativeGScore = entry.G + boxSize; int previousGScore; bool hasPreviousGScore = closed.TryGetValue(adjacent, out previousGScore); if (hasPreviousGScore && tentativeGScore > previousGScore) continue; AStarEntry alreadyInQueue; bool throwaway = queueLookup.TryGetValue(adjacent, out alreadyInQueue); if (alreadyInQueue == null || tentativeGScore < previousGScore) { AStarEntry newEntry = alreadyInQueue != null ? alreadyInQueue : new AStarEntry(); newEntry.Parent = entry; newEntry.G = tentativeGScore; newEntry.F = tentativeGScore + (target - adjacent.GetCenter()).Length(); newEntry.PathIndex = entry.PathIndex + 1; if (alreadyInQueue == null) { newEntry.Box = adjacent; newEntry.BoxSize = boxSize; queue.Push(newEntry); queueLookup[adjacent] = newEntry; } } } } pathLength = 0; return null; }
private List<Coordinate> constructPath(AStarEntry entry) { List<Coordinate> result = new List<Coordinate>(); result.Add(entry.Coordinate); while (entry.Parent != null) { entry = entry.Parent; result.Insert(0, entry.Coordinate); } return result; }
private static Map.Box reconstructPath(AStarEntry entry, out int length) { length = 0; while (entry.PathIndex > 1) { entry = entry.Parent; length++; } return entry.Box; }