public static void Narrowphase(Voxel m, Voxel.Coord start, Voxel.Box target, Stack <Voxel.Coord> result)
{
Voxel.Box currentBox = m.GetBox(start);
Vector3 targetPos = target.GetCenter();
NarrowphaseEntry startEntry = new NarrowphaseEntry
{
Parent = null,
Coord = start,
G = 0,
F = (targetPos - m.GetRelativePosition(start)).Length(),
};
narrowphaseQueue.Push(startEntry);
narrowphaseQueueLookup[start] = startEntry;
NarrowphaseEntry closestEntry = null;
float closestHeuristic = float.MaxValue;
int iterations = 0;
while (narrowphaseQueue.Count > 0 && iterations < 80)
{
iterations++;
NarrowphaseEntry entry = narrowphaseQueue.Pop();
if (m.GetBox(entry.Coord) == target)
{
closestEntry = entry;
break;
}
narrowphaseQueueLookup.Remove(entry.Coord);
narrowphaseClosed[entry.Coord] = entry.G;
for (int i = 0; i < 6; i++)
{
Voxel.Coord adjacent = entry.Coord.Move(DirectionExtensions.Directions[i]);
if (!currentBox.Contains(adjacent) && !target.Contains(adjacent))
{
continue;
}
int tentativeGScore = entry.G + 1;
int previousGScore;
bool hasPreviousGScore = narrowphaseClosed.TryGetValue(adjacent, out previousGScore);
if (hasPreviousGScore && tentativeGScore > previousGScore)
{
continue;
}
NarrowphaseEntry alreadyInQueue;
narrowphaseQueueLookup.TryGetValue(adjacent, out alreadyInQueue);
if (alreadyInQueue == null || tentativeGScore < previousGScore)
{
NarrowphaseEntry newEntry = alreadyInQueue != null ? alreadyInQueue : new NarrowphaseEntry();
newEntry.Parent = entry;
newEntry.G = tentativeGScore;
float heuristic = (targetPos - m.GetRelativePosition(adjacent)).Length();
newEntry.F = tentativeGScore + heuristic;
if (heuristic < closestHeuristic)
{
closestEntry = newEntry;
closestHeuristic = heuristic;
}
if (alreadyInQueue == null)
{
newEntry.Coord = adjacent;
narrowphaseQueue.Push(newEntry);
narrowphaseQueueLookup[adjacent] = newEntry;
}
}
}
}
narrowphaseClosed.Clear();
narrowphaseQueue.Clear();
narrowphaseQueueLookup.Clear();
if (closestEntry != null)
{
VoxelAStar.reconstructNarrowphasePath(closestEntry, result);
}
}
