2 files changed, 112 insertions, 21 deletions

diff --git a/OpenSim/Framework/PriorityQueue.cs b/OpenSim/Framework/PriorityQueue.cs
index ea718c4..8eeafd1 100644
--- a/OpenSim/Framework/PriorityQueue.cs
+++ b/OpenSim/Framework/PriorityQueue.cs
@@ -42,22 +42,40 @@ namespace OpenSim.Framework
 
         public delegate bool UpdatePriorityHandler(ref uint priority, ISceneEntity entity);
 
-        // Heap[0] for self updates
-        // Heap[1..12] for entity updates
-
+        /// <summary>
+        /// Total number of queues (priorities) available
+        /// </summary>
         public const uint NumberOfQueues = 12;
-        public const uint ImmediateQueue = 0;
+
+        /// <summary>
+        /// Number of queuest (priorities) that are processed immediately
+        /// </summary.
+        public const uint NumberOfImmediateQueues = 2;
 
         private MinHeap<MinHeapItem>[] m_heaps = new MinHeap<MinHeapItem>[NumberOfQueues];
         private Dictionary<uint, LookupItem> m_lookupTable;
+
+        // internal state used to ensure the deqeues are spread across the priority
+        // queues "fairly". queuecounts is the amount to pull from each queue in 
+        // each pass. weighted towards the higher priority queues
         private uint m_nextQueue = 0;
+        private uint m_countFromQueue = 0;
+        private uint[] m_queueCounts = { 8, 4, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1 };
+
+        // next request is a counter of the number of updates queued, it provides
+        // a total ordering on the updates coming through the queue and is more
+        // lightweight (and more discriminating) than tick count
         private UInt64 m_nextRequest = 0;
 
+        /// <summary>
+        /// Lock for enqueue and dequeue operations on the priority queue
+        /// </summary>
         private object m_syncRoot = new object();
         public object SyncRoot {
             get { return this.m_syncRoot; }
         }
 
+#region constructor
         public PriorityQueue() : this(MinHeap<MinHeapItem>.DEFAULT_CAPACITY) { }
 
         public PriorityQueue(int capacity)
@@ -66,8 +84,16 @@ namespace OpenSim.Framework
 
             for (int i = 0; i < m_heaps.Length; ++i)
                 m_heaps[i] = new MinHeap<MinHeapItem>(capacity);
+
+            m_nextQueue = NumberOfImmediateQueues;
+            m_countFromQueue = m_queueCounts[m_nextQueue];
         }
+#endregion Constructor
 
+#region PublicMethods
+        /// <summary>
+        /// Return the number of items in the queues
+        /// </summary>
         public int Count
         {
             get
@@ -79,6 +105,9 @@ namespace OpenSim.Framework
             }
         }
 
+        /// <summary>
+        /// Enqueue an item into the specified priority queue
+        /// </summary>
         public bool Enqueue(uint pqueue, IEntityUpdate value)
         {
             LookupItem lookup;
@@ -100,32 +129,62 @@ namespace OpenSim.Framework
             return true;
         }
 
+        /// <summary>
+        /// Remove an item from one of the queues. Specifically, it removes the
+        /// oldest item from the next queue in order to provide fair access to
+        /// all of the queues
+        /// </summary>
         public bool TryDequeue(out IEntityUpdate value, out Int32 timeinqueue)
         {
             // If there is anything in priority queue 0, return it first no
             // matter what else. Breaks fairness. But very useful.
-            if (m_heaps[ImmediateQueue].Count > 0)
+            for (int iq = 0; iq < NumberOfImmediateQueues; iq++)
+            {
+                if (m_heaps[iq].Count > 0)
+                {
+                    MinHeapItem item = m_heaps[iq].RemoveMin();
+                    m_lookupTable.Remove(item.Value.Entity.LocalId);
+                    timeinqueue = Util.EnvironmentTickCountSubtract(item.EntryTime);
+                    value = item.Value;
+
+                    return true;
+                }
+            }
+            
+            // To get the fair queing, we cycle through each of the
+            // queues when finding an element to dequeue. 
+            // We pull (NumberOfQueues - QueueIndex) items from each queue in order
+            // to give lower numbered queues a higher priority and higher percentage
+            // of the bandwidth. 
+            
+            // Check for more items to be pulled from the current queue
+            if (m_heaps[m_nextQueue].Count > 0 && m_countFromQueue > 0)
             {
-                MinHeapItem item = m_heaps[ImmediateQueue].RemoveMin();
+                m_countFromQueue--;
+                
+                MinHeapItem item = m_heaps[m_nextQueue].RemoveMin();
                 m_lookupTable.Remove(item.Value.Entity.LocalId);
                 timeinqueue = Util.EnvironmentTickCountSubtract(item.EntryTime);
                 value = item.Value;
-
+                
                 return true;
             }
-
-            for (int i = 0; i < NumberOfQueues; ++i)
+            
+            // Find the next non-immediate queue with updates in it
+            for (int i = 1; i < NumberOfQueues; ++i)
             {
-                // To get the fair queing, we cycle through each of the
-                // queues when finding an element to dequeue, this code
-                // assumes that the distribution of updates in the queues
-                // is polynomial, probably quadractic (eg distance of PI * R^2)
-                uint h = (uint)((m_nextQueue + i) % NumberOfQueues);
-                if (m_heaps[h].Count > 0)
+                m_nextQueue = (uint)((m_nextQueue + i) % NumberOfQueues);
+                m_countFromQueue = m_queueCounts[m_nextQueue];
+
+                // if this is one of the immediate queues, just skip it
+                if (m_nextQueue < NumberOfImmediateQueues)
+                    continue;
+                
+                if (m_heaps[m_nextQueue].Count > 0)
                 {
-                    m_nextQueue = (uint)((h + 1) % NumberOfQueues);
+                    m_countFromQueue--;
 
-                    MinHeapItem item = m_heaps[h].RemoveMin();
+                    MinHeapItem item = m_heaps[m_nextQueue].RemoveMin();
                     m_lookupTable.Remove(item.Value.Entity.LocalId);
                     timeinqueue = Util.EnvironmentTickCountSubtract(item.EntryTime);
                     value = item.Value;
@@ -139,6 +198,10 @@ namespace OpenSim.Framework
             return false;
         }
 
+        /// <summary>
+        /// Reapply the prioritization function to each of the updates currently
+        /// stored in the priority queues. 
+        /// </summary
         public void Reprioritize(UpdatePriorityHandler handler)
         {
             MinHeapItem item;
@@ -184,6 +247,8 @@ namespace OpenSim.Framework
             return s;
         }
 
+#endregion PublicMethods
+
 #region MinHeapItem
         private struct MinHeapItem : IComparable<MinHeapItem>
         {
diff --git a/OpenSim/Region/Framework/Scenes/Prioritizer.cs b/OpenSim/Region/Framework/Scenes/Prioritizer.cs
index 2e80156..a7637c0 100644
--- a/OpenSim/Region/Framework/Scenes/Prioritizer.cs
+++ b/OpenSim/Region/Framework/Scenes/Prioritizer.cs
@@ -88,7 +88,7 @@ namespace OpenSim.Region.Framework.Scenes
 
             // If this is an update for our own avatar give it the highest priority
             if (client.AgentId == entity.UUID)
-                return PriorityQueue.ImmediateQueue;
+                return 0;
 
             uint priority;
             
@@ -119,16 +119,40 @@ namespace OpenSim.Region.Framework.Scenes
 
         private uint GetPriorityByTime(IClientAPI client, ISceneEntity entity)
         {
-            return 1;
+            // And anything attached to this avatar gets top priority as well
+            if (entity is SceneObjectPart)
+            {
+                SceneObjectPart sop = (SceneObjectPart)entity;
+                if (sop.ParentGroup.RootPart.IsAttachment && client.AgentId == sop.ParentGroup.RootPart.AttachedAvatar)
+                    return 1;
+            }
+
+            return PriorityQueue.NumberOfImmediateQueues; // first queue past the immediate queues
         }
 
         private uint GetPriorityByDistance(IClientAPI client, ISceneEntity entity)
         {
+            // And anything attached to this avatar gets top priority as well
+            if (entity is SceneObjectPart)
+            {
+                SceneObjectPart sop = (SceneObjectPart)entity;
+                if (sop.ParentGroup.RootPart.IsAttachment && client.AgentId == sop.ParentGroup.RootPart.AttachedAvatar)
+                    return 1;
+            }
+
             return ComputeDistancePriority(client,entity,false);
         }
         
         private uint GetPriorityByFrontBack(IClientAPI client, ISceneEntity entity)
         {
+            // And anything attached to this avatar gets top priority as well
+            if (entity is SceneObjectPart)
+            {
+                SceneObjectPart sop = (SceneObjectPart)entity;
+                if (sop.ParentGroup.RootPart.IsAttachment && client.AgentId == sop.ParentGroup.RootPart.AttachedAvatar)
+                    return 1;
+            }
+
             return ComputeDistancePriority(client,entity,true);
         }
 
@@ -197,8 +221,10 @@ namespace OpenSim.Region.Framework.Scenes
 
             // And convert the distance to a priority queue, this computation gives queues
             // at 10, 20, 40, 80, 160, 320, 640, and 1280m
-            uint pqueue = 1;
-            for (int i = 0; i < 8; i++)
+            uint pqueue = PriorityQueue.NumberOfImmediateQueues;
+            uint queues = PriorityQueue.NumberOfQueues - PriorityQueue.NumberOfImmediateQueues;
+            
+            for (int i = 0; i < queues - 1; i++)
             {
                 if (distance < 10 * Math.Pow(2.0,i))
                     break;