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