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;
}
