示例#1
0
        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);
        }
示例#2
0
        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);
            }
        }