private static void reconstructBroadphasePath(BroadphaseEntry entry, Stack <Voxel.Box> result)
{
while (entry != null)
{
result.Push(entry.Box);
entry = entry.Parent;
}
}
private static void reconstructBroadphasePath(BroadphaseEntry entry, Stack<Voxel.Box> result)
{
while (entry != null)
{
result.Push(entry.Box);
entry = entry.Parent;
}
}
public static bool Broadphase(Voxel m, Voxel.Box start, Voxel.Coord target, Func <Voxel.State, bool> filter, Stack <Voxel.Box> result, int maxIterations = 50)
{
BroadphaseEntry closestEntry = null;
bool found = false;
lock (m.MutationLock)
{
Vector3 targetPos = m.GetRelativePosition(target);
BroadphaseEntry startEntry = new BroadphaseEntry
{
Parent = null,
Box = start,
G = 0,
F = (targetPos - start.GetCenter()).Length(),
BoxSize = Math.Max(start.Width, Math.Max(start.Height, start.Depth)),
};
broadphaseQueue.Push(startEntry);
broadphaseQueueLookup[start] = startEntry;
float closestHeuristic = float.MaxValue;
int iterations = 0;
while (broadphaseQueue.Count > 0 && iterations < maxIterations)
{
iterations++;
BroadphaseEntry entry = broadphaseQueue.Pop();
if (entry.Box.Contains(target))
{
closestEntry = entry;
found = true;
break;
}
broadphaseQueueLookup.Remove(entry.Box);
broadphaseClosed[entry.Box] = entry.G;
lock (entry.Box.Adjacent)
{
for (int i = 0; i < entry.Box.Adjacent.Count; i++)
{
Voxel.Box adjacent = entry.Box.Adjacent[i];
if (adjacent == null || !filter(adjacent.Type))
{
continue;
}
int boxSize = (int)((adjacent.Width + adjacent.Height + adjacent.Depth) / 3.0f);
int tentativeGScore = entry.G + boxSize;
int previousGScore;
bool hasPreviousGScore = broadphaseClosed.TryGetValue(adjacent, out previousGScore);
if (hasPreviousGScore && tentativeGScore > previousGScore)
{
continue;
}
BroadphaseEntry alreadyInQueue;
broadphaseQueueLookup.TryGetValue(adjacent, out alreadyInQueue);
if (alreadyInQueue == null || tentativeGScore < previousGScore)
{
BroadphaseEntry newEntry = alreadyInQueue != null ? alreadyInQueue : new BroadphaseEntry();
newEntry.Parent = entry;
newEntry.G = tentativeGScore;
float heuristic = (targetPos - adjacent.GetCenter()).Length();
newEntry.F = tentativeGScore + heuristic;
if (heuristic < closestHeuristic)
{
closestEntry = newEntry;
closestHeuristic = heuristic;
}
if (alreadyInQueue == null)
{
newEntry.Box = adjacent;
newEntry.BoxSize = boxSize;
broadphaseQueue.Push(newEntry);
broadphaseQueueLookup[adjacent] = newEntry;
}
}
}
}
}
}
broadphaseClosed.Clear();
broadphaseQueue.Clear();
broadphaseQueueLookup.Clear();
if (closestEntry != null)
{
VoxelAStar.reconstructBroadphasePath(closestEntry, result);
}
return(found);
}
public static bool Broadphase(Voxel m, Voxel.Box start, Voxel.Coord target, Func<Voxel.State, bool> filter, Stack<Voxel.Box> result, int maxIterations = 50)
{
BroadphaseEntry closestEntry = null;
bool found = false;
lock (m.MutationLock)
{
Vector3 targetPos = m.GetRelativePosition(target);
BroadphaseEntry startEntry = new BroadphaseEntry
{
Parent = null,
Box = start,
G = 0,
F = (targetPos - start.GetCenter()).Length(),
BoxSize = Math.Max(start.Width, Math.Max(start.Height, start.Depth)),
};
broadphaseQueue.Push(startEntry);
broadphaseQueueLookup[start] = startEntry;
float closestHeuristic = float.MaxValue;
int iterations = 0;
while (broadphaseQueue.Count > 0 && iterations < maxIterations)
{
iterations++;
BroadphaseEntry entry = broadphaseQueue.Pop();
if (entry.Box.Contains(target))
{
closestEntry = entry;
found = true;
break;
}
broadphaseQueueLookup.Remove(entry.Box);
broadphaseClosed[entry.Box] = entry.G;
lock (entry.Box.Adjacent)
{
for (int i = 0; i < entry.Box.Adjacent.Count; i++)
{
Voxel.Box adjacent = entry.Box.Adjacent[i];
if (adjacent == null || !filter(adjacent.Type))
continue;
int boxSize = (int)((adjacent.Width + adjacent.Height + adjacent.Depth) / 3.0f);
int tentativeGScore = entry.G + boxSize;
int previousGScore;
bool hasPreviousGScore = broadphaseClosed.TryGetValue(adjacent, out previousGScore);
if (hasPreviousGScore && tentativeGScore > previousGScore)
continue;
BroadphaseEntry alreadyInQueue;
broadphaseQueueLookup.TryGetValue(adjacent, out alreadyInQueue);
if (alreadyInQueue == null || tentativeGScore < previousGScore)
{
BroadphaseEntry newEntry = alreadyInQueue != null ? alreadyInQueue : new BroadphaseEntry();
newEntry.Parent = entry;
newEntry.G = tentativeGScore;
float heuristic = (targetPos - adjacent.GetCenter()).Length();
newEntry.F = tentativeGScore + heuristic;
if (heuristic < closestHeuristic)
{
closestEntry = newEntry;
closestHeuristic = heuristic;
}
if (alreadyInQueue == null)
{
newEntry.Box = adjacent;
newEntry.BoxSize = boxSize;
broadphaseQueue.Push(newEntry);
broadphaseQueueLookup[adjacent] = newEntry;
}
}
}
}
}
}
broadphaseClosed.Clear();
broadphaseQueue.Clear();
broadphaseQueueLookup.Clear();
if (closestEntry != null)
VoxelAStar.reconstructBroadphasePath(closestEntry, result);
return found;
}
