git-svn-id: svn://svn.ijw.co.nz/svn/OpenRa@1297 993157c7-ee19-0410-b2c4-bb4e9862e678

OpenRa.DataStructures/PriorityQueue.cs

@@ -0,0 +1,133 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.Diagnostics;
+
+namespace OpenRa.DataStructures
+{
+	class PriorityQueue<T>
+		where T : IComparable<T>
+	{
+		List<T[]> items = new List<T[]>();
+		int level;
+		int index;
+
+		public PriorityQueue()
+		{
+			items.Add( new T[ 1 ] );
+		}
+
+		public void Add( T item )
+		{
+			int addLevel = level;
+			int addIndex = index;
+
+			while( addLevel >= 1 && Above( addLevel, addIndex ).CompareTo( item ) > 0 )
+			{
+				items[ addLevel ][ addIndex ] = Above( addLevel, addIndex );
+				--addLevel;
+				addIndex >>= 1;
+			}
+
+			items[ addLevel ][ addIndex ] = item;
+
+			++index;
+			if( index >= ( 1 << level ) )
+			{
+				index = 0;
+				++level;
+				items.Add( new T[ 1 << level ] );
+			}
+		}
+
+		T At( int level, int index )
+		{
+			return items[ level ][ index ];
+		}
+
+		T Above( int level, int index )
+		{
+			return items[ level - 1 ][ index >> 1 ];
+		}
+
+		T Last()
+		{
+			int lastLevel = level;
+			int lastIndex = index;
+			--lastIndex;
+			if( lastIndex < 0 )
+			{
+				--lastLevel;
+				lastIndex = ( 1 << lastLevel ) - 1;
+			}
+			return At( lastLevel, lastIndex );
+		}
+
+		public T Pop()
+		{
+			if( level == 0 && index == 0 )
+				throw new InvalidOperationException( "Attempting to pop empty PriorityQueue" );
+
+			T ret = At( 0, 0 );
+			BubbleInto( 0, 0, Last() );
+			--index;
+			if( index < 0 )
+			{
+				--level;
+				index = ( 1 << level ) - 1;
+			}
+
+			return ret;
+		}
+
+		public void BubbleInto( int intoLevel, int intoIndex, T val )
+		{
+			int downLevel = intoLevel + 1;
+			int downIndex = intoIndex << 1;
+
+			if( downLevel > level || ( downLevel == level && downIndex >= index ))
+			{
+				items[ intoLevel ][ intoIndex ] = val;
+				return;
+			}
+
+			if( downLevel == level && downIndex == index - 1 )
+			{
+				//Log.Write( "one-option bubble" );
+			}
+			else if( At( downLevel, downIndex ).CompareTo( At( downLevel, downIndex + 1 ) ) < 0 )
+			{
+				//Log.Write( "left bubble" );
+			}
+			else
+			{
+				//Log.Write( "right bubble" );
+				++downIndex;
+			}
+
+			if( val.CompareTo( At( downLevel, downIndex ) ) <= 0 )
+			{
+				items[ intoLevel ][ intoIndex ] = val;
+				return;
+			}
+
+			items[ intoLevel ][ intoIndex ] = At( downLevel, downIndex );
+			BubbleInto( downLevel, downIndex, val );
+		}
+
+		//void Invariant()
+		//{
+		//      for( int i = 1 ; i < level ; i++ )
+		//            for( int j = 0 ; j < RowLength( i ) ; j++ )
+		//                  if( At( i, j ).CompareTo( Above( i, j ) ) < 0 )
+		//                        System.Diagnostics.Debug.Assert( At( i, j ).CompareTo( Above( i, j ) ) < 0, "At( i, j ) > Above( i, j )" );
+		//}
+
+		private int RowLength( int i )
+		{
+			if( i == level )
+				return index;
+			return ( 1 << i );
+		}
+	}
+}