コード例 #1
0
        // Remove a node at a particular position in the queue.
        private PriorityQueueItem <TValue, TPriority> RemoveAt(Int32 index)
        {
            // remove an item from the heap
            PriorityQueueItem <TValue, TPriority> o   = items[index];
            PriorityQueueItem <TValue, TPriority> tmp = items[numItems - 1];

            items[--numItems] = default(PriorityQueueItem <TValue, TPriority>);
            if (numItems > 0)
            {
                int i = index;
                int j = i + 1;
                while (i < Count / 2)
                {
                    if ((j < Count - 1) && (compareFunc(items[j].Priority, items[j + 1].Priority) > 0))
                    {
                        j++;
                    }
                    if (compareFunc(items[j].Priority, tmp.Priority) >= 0)
                    {
                        break;
                    }
                    items[i] = items[j];
                    i        = j;
                    j       *= 2;
                }
                items[i] = tmp;
            }
            return(o);
        }
コード例 #2
0
 /// <summary>
 /// Copies the queue elements to a new array.
 /// </summary>
 /// <returns>A new array containing elements copied from the Queue.</returns>
 public PriorityQueueItem <TValue, TPriority>[] ToArray()
 {
     PriorityQueueItem <TValue, TPriority>[] newItems =
         new PriorityQueueItem <TValue, TPriority> [numItems];
     Array.Copy(items, newItems, numItems);
     return(newItems);
 }
コード例 #3
0
        /// <summary>
        /// Adds an object to the queue, in order by priority.
        /// </summary>
        /// <param name="value">The object to be added.</param>
        /// <param name="priority">Priority of the object to be added.</param>
        public void Enqueue(TValue value, TPriority priority)
        {
            if (numItems == capacity)
            {
                // need to increase capacity
                // grow by 50 percent
                //SetCapacity((3 * Capacity) / 2);
                SetCapacity((int)(Capacity * 1.5));
            }

            // Create the new item
            PriorityQueueItem <TValue, TPriority> newItem = new PriorityQueueItem <TValue, TPriority>(value, priority);
            int i = numItems;

            ++numItems;

            // and insert it into the heap.
            while ((i > 0) && (compareFunc(items[i / 2].Priority, newItem.Priority) > 0))
            {
                items[i] = items[i / 2];
                i       /= 2;
            }
            items[i] = newItem;
        }
コード例 #4
0
ファイル: PathFinder.cs プロジェクト: hakanaku2009/svn-dump
        public List <PathReturnNode> FindPath(Point start, Point end)
        {
            nodeFound = false;
            isStop    = false;
            isStopped = false;

            NodeOpenVal  += 2;
            NodeCloseVal += 2;

            OpenList.Clear();

            Position      = (uint)(start.X + start.Y * GridX);
            startPosition = (uint)(start.X + start.Y * GridX);
            endPosition   = (uint)(end.X + end.Y * GridX);

            //Set first node in info grid:
            PathInfoGrid[Position].Gone         = 0;
            PathInfoGrid[Position].F            = HeuristicEstimateVal;
            PathInfoGrid[Position].Parrent      = Position;
            PathInfoGrid[Position].OpenOrClosed = NodeOpenVal;

            //Enqueue first node
            OpenList.Enqueue(Position, 1);

            while (OpenList.Count > 0 && !isStop)
            {
                //Dequeue node with lowest cost
                PriorityQueueItem <uint, int> item = OpenList.Dequeue();
                Position = item.Value;

                //Node Closed?
                if (PathInfoGrid[Position].OpenOrClosed == NodeCloseVal)
                {
                    continue;
                }

                PathInfoGrid[Position].OpenOrClosed = NodeCloseVal;

                PosX = (ushort)PathInfoGrid[Position].PosX;
                PosY = (ushort)PathInfoGrid[Position].PosY;

                //Found end node - vaild path is found
                if (Position == endPosition)
                {
                    PathInfoGrid[Position].OpenOrClosed = NodeCloseVal;
                    nodeFound = true;
                    break;
                }
                //Foreach direction
                for (int i = 0; i < 8; i += 1)
                {
                    newPosX = (ushort)(PosX + Directions[i, 0]);
                    newPosY = (ushort)(PosY + Directions[i, 1]);

                    newPosition = (uint)(newPosY * GridX + newPosX);

                    //Outside of grid
                    if (newPosX >= GridX || newPosY >= GridY)
                    {
                        continue;
                    }

                    if (PathInfoGrid[newPosition].OpenOrClosed == NodeCloseVal)
                    {
                        continue;
                    }

                    //Solid block
                    if (PathGrid[newPosX, newPosY] == 0)
                    {
                        continue;
                    }

                    if (i > 3)
                    {
                        //Smooth path and do-not allow passing throw nodes with connected corners
                        int iminus4 = i - 4;
                        if (PathGrid[newPosX + SmoothDir[iminus4, 0], newPosY] == 0 || PathGrid[newPosX, newPosY + SmoothDir[iminus4, 1]] == 0)
                        {
                            continue;
                        }
                        //Passing thro diognal cost more:
                        newGone = PathInfoGrid[Position].Gone + (int)(PathGrid[newPosX, newPosY] * 2.41);
                    }
                    else
                    {
                        newGone = PathInfoGrid[Position].Gone + PathGrid[newPosX, newPosY];
                    }

                    if (PathInfoGrid[newPosition].OpenOrClosed == NodeOpenVal)
                    {
                        if (PathInfoGrid[newPosition].Gone < newGone)
                        {
                            continue;
                        }
                    }

                    PathInfoGrid[newPosition].Parrent = Position;
                    PathInfoGrid[newPosition].Gone    = newGone;

                    //Calculate Euclidean heuristic
                    //Heuristic = (int)(HeuristicEstimateVal * (Math.Sqrt(Math.Pow((newPosX - end.X), 2) + Math.Pow((newPosY - end.Y), 2))));
                    //Manhattan
                    Heuristic = HeuristicEstimateVal * (Math.Abs(end.X - newPosX) + Math.Abs(end.Y - newPosY));
                    //Diognal distance
                    //Heuristic = HeuristicEstimateVal * Math.Max(Math.Abs(newPosX - end.X), Math.Abs(newPosX - end.Y));

                    PathInfoGrid[newPosition].F = newGone + Heuristic;

                    OpenList.Enqueue(newPosition, PathInfoGrid[newPosition].F);

                    PathInfoGrid[newPosition].OpenOrClosed = NodeOpenVal;
                }
            }

            //Vaild path is found
            if (nodeFound)
            {
                SolovedList.Clear();

                //Add end node to found list
                PathGridNode   foundNodeEnd = PathInfoGrid[endPosition];
                PathReturnNode foundNode;

                foundNode.Parrent = foundNodeEnd.Parrent;
                foundNode.Pos     = foundNodeEnd.Pos;
                foundNode.PosX    = foundNodeEnd.PosX;
                foundNode.PosY    = foundNodeEnd.PosY;

                SolovedList.Add(foundNode);

                //Add rest of the nodes by parrents
                while (foundNode.Pos != startPosition)
                {
                    foundNodeEnd      = PathInfoGrid[foundNode.Parrent];
                    foundNode.Parrent = foundNodeEnd.Parrent;
                    foundNode.Pos     = foundNodeEnd.Pos;
                    foundNode.PosX    = foundNodeEnd.PosX;
                    foundNode.PosY    = foundNodeEnd.PosY;
                    SolovedList.Add(foundNode);
                }

                isStopped = true;

                return(SolovedList);
            }
            isStopped = true;
            //If no path found -> return null
            return(null);
        }