Beispiel #1
0
        /// <summary>
        /// Add a node to the heap which will be sorted.
        /// </summary>
        /// <param name="node"> The node to add. </param>
        /// <exception cref="IndexOutOfRangeException"> Throws if capacity reached. </exception>
        public void Push(MinHeapNode node)
        {
#if ENABLE_UNITY_COLLECTIONS_CHECKS
            if (this.length == this.capacity)
            {
                throw new IndexOutOfRangeException("Capacity Reached");
            }

            AtomicSafetyHandle.CheckReadAndThrow(this.m_Safety);
#endif
            if (this.head < 0)
            {
                this.head = this.length;
            }
            else if (node.ExpectedCost < this.Get(this.head).ExpectedCost)
            {
                node.Next = this.head;
                this.head = this.length;
            }
            else
            {
                var currentPtr = this.head;
                var current    = this.Get(currentPtr);

                while (current.Next >= 0 && this.Get(current.Next).ExpectedCost <= node.ExpectedCost)
                {
                    currentPtr = current.Next;
                    current    = this.Get(current.Next);
                }

                node.Next    = current.Next;
                current.Next = this.length;

                UnsafeUtility.WriteArrayElement(this.buffer, currentPtr, current);
            }

            UnsafeUtility.WriteArrayElement(this.buffer, this.length, node);
            this.length += 1;
        }
Beispiel #2
0
            bool ProcessPath(ref InstanceStash stash)
            {
                // Push the start to NativeMinHeap openSet

                float       hh   = H(stash.Start, stash.Goal);
                MinHeapNode head = new MinHeapNode(stash.Start, hh, hh);

                stash.OpenSet.Push(head);

                int iterations = this.Iterations;

                MinHeapNode closest = head;

                // While we still have potential nodes to explore
                while (stash.OpenSet.HasNext())
                {
                    MinHeapNode current = stash.OpenSet.Pop();

                    if (current.DistanceToGoal < closest.DistanceToGoal)
                    {
                        closest = current;
                    }

                    // Found our goal
                    if (current.Position.Equals(stash.Goal))
                    {
                        return(true);
                    }

                    // Path might still be obtainable but we've run out of allowed iterations
                    if (iterations == 0)
                    {
                        if (stash.Request.NavigateToBestIfIncomplete)
                        {
                            // Return the best result we've found so far
                            // Need to update goal so we can reconstruct the shorter path
                            stash.Goal = closest.Position;
                            return(true);
                        }
                        return(false);
                    }

                    iterations--;

                    var initialCost = stash.CostSoFar[this.GetIndex(current.Position)];

                    var fromIndex = this.GetIndex(current.Position);

                    // Loop our potential cells - generally neighbours but could include portals
                    for (var i = 0; i < this.Neighbors.Length; i++)
                    {
                        var neighbour = this.Neighbors[i];
                        var position  = current.Position + neighbour.Offset;

                        // Make sure the node isn't outside our grid
                        if (position.x < 0 || position.x >= this.DimX || position.y < 0 || position.y >= this.DimY)
                        {
                            continue;
                        }

                        var index = this.GetIndex(position);

                        // Get the cost of going to this cell
                        var cellCost = this.GetCellCost(stash.Grid, stash.Capability, fromIndex, index, neighbour, true);

                        // Infinity means the cell is un-walkable, skip it
                        if (float.IsInfinity(cellCost))
                        {
                            continue;
                        }

                        var newCost = initialCost + (neighbour.Distance * cellCost);
                        var oldCost = stash.CostSoFar[index];

                        // If we've explored this cell before and it was a better path, ignore this route
                        if (!(oldCost <= 0) && !(newCost < oldCost))
                        {
                            continue;
                        }

                        // Update the costing and best path
                        stash.CostSoFar[index] = newCost;
                        stash.CameFrom[index]  = current.Position;

                        // Push the node onto our heap
                        var h            = H(position, stash.Goal);
                        var expectedCost = newCost + h;
                        stash.OpenSet.Push(new MinHeapNode(position, expectedCost, h));
                    }
                }

                if (stash.Request.NavigateToNearestIfBlocked)
                {
                    stash.Goal = closest.Position;
                    return(true);
                }

                // All routes have been explored without finding a route to destination
                return(false);
            }