Example #1
0
		public static void Narrowphase(Voxel m, Voxel.Coord start, Voxel.Box target, Stack<Voxel.Coord> result)
		{
			Voxel.Box currentBox = m.GetBox(start);
			if (currentBox == null)
				return;
			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);
		}
Example #2
0
		private static void reconstructNarrowphasePath(NarrowphaseEntry entry, Stack<Voxel.Coord> result)
		{
			while (entry != null)
			{
				result.Push(entry.Coord);
				entry = entry.Parent;
			}
		}