WriteByte() fixed

Lidgren.Network/NetBitWriter.cs

@@ -97,44 +97,54 @@ namespace Lidgren.Network
 			return;
 		}
 
-		/// <summary>
-		/// Write a byte consisting of 1-8 bits to a buffer; assumes buffer is previously allocated
-		/// </summary>
 		public static void WriteByte(byte source, int numberOfBits, byte[] destination, int destBitOffset)
 		{
-			NetException.Assert(((numberOfBits >= 1) && (numberOfBits <= 8)), "Must write between 1 and 8 bits!");
+			if (numberOfBits == 0)
+				return;
 
-			// mask out unwanted bits in the source
+			NetException.Assert(((numberOfBits >= 0) && (numberOfBits <= 8)), "Must write between 0 and 8 bits!");
-			byte isrc = (byte)(source & (0x000000FF >> (8 - numberOfBits)));
 
-			int bytePtr = destBitOffset >> 3;
+			// Mask out all the bits we dont want
+			source = (byte)(source & (0xFF >> (8 - numberOfBits)));
 
-			int localBitLen = (destBitOffset % 8);
+			int p = destBitOffset >> 3;
-			if (localBitLen == 0)
+			int bitsUsed = destBitOffset & 0x7; // mod 8
+			int bitsFree = 8 - bitsUsed;
+			int bitsLeft = bitsFree - numberOfBits;
+
+			// Fast path, everything fits in the first byte
+			if (bitsLeft >= 0)
 			{
-				destination[bytePtr] = (byte)isrc;
+				int mask = (0xFF >> bitsFree) | (0xFF << (8 - bitsLeft));
+
+				destination[p] = (byte)(
+					// Mask out lower and upper bits
+					(destination[p] & mask) |
+
+					// Insert new bits
+					(source << bitsUsed)
+				);
+
 				return;
 			}
 
-			//destination[bytePtr] &= (byte)(255 >> (8 - localBitLen)); // clear before writing
+			destination[p] = (byte)(
-			//destination[bytePtr] |= (byte)(isrc << localBitLen); // write first half
+				// Mask out upper bits
-			destination[bytePtr] = (byte)(
+				(destination[p] & (0xFF >> bitsFree)) |
-				(uint)(destination[bytePtr] & (255 >> (8 - localBitLen))) |
+
-				(uint)(isrc << localBitLen)
+				// Write the lower bits to the upper bits in the first byte
+				(source << bitsUsed)
 			);
 
-			// need write into next byte?
+			p += 1;
-			if (localBitLen + numberOfBits > 8)
-			{
-				//destination[bytePtr + 1] &= (byte)(255 << localBitLen); // clear before writing
-				//destination[bytePtr + 1] |= (byte)(isrc >> (8 - localBitLen)); // write second half
-				destination[bytePtr + 1] = (byte)(
-					(uint)(destination[bytePtr + 1] & (255 << localBitLen)) |
-					(uint)(isrc >> (8 - localBitLen))
-				);
-			}
 
-			return;
+			destination[p] = (byte)(
+				// Mask out lower bits
+				(destination[p] & (0xFF << (numberOfBits - bitsFree))) |
+
+				// Write the upper bits to the lower bits of the second byte
+				(source >> bitsFree)
+			);
 		}
 
 		/// <summary>
@@ -287,35 +297,32 @@ namespace Lidgren.Network
 		//public static ulong ReadUInt64(byte[] fromBuffer, int numberOfBits, int readBitOffset)
 
 		/// <summary>
-		/// Writes un unsigned 16 bit integer
+		/// Writes an unsigned 16 bit integer
 		/// </summary>
 		[CLSCompliant(false)]
-		public static int WriteUInt16(ushort source, int numberOfBits, byte[] destination, int destinationBitOffset)
+		public static void WriteUInt16(ushort source, int numberOfBits, byte[] destination, int destinationBitOffset)
 		{
+			if (numberOfBits == 0)
+				return;
+
+			NetException.Assert((numberOfBits >= 0 && numberOfBits <= 16), "numberOfBits must be between 0 and 16");
 #if BIGENDIAN
 			// reorder bytes
 			uint intSource = source;
 			intSource = ((intSource & 0x0000ff00) >> 8) | ((intSource & 0x000000ff) << 8);
 			source = (ushort)intSource;
 #endif
-
-			int returnValue = destinationBitOffset + numberOfBits;
 			if (numberOfBits <= 8)
 			{
 				NetBitWriter.WriteByte((byte)source, numberOfBits, destination, destinationBitOffset);
-				return returnValue;
+				return;
 			}
+
 			NetBitWriter.WriteByte((byte)source, 8, destination, destinationBitOffset);
-			destinationBitOffset += 8;
+
 			numberOfBits -= 8;
-
+			if (numberOfBits > 0)
-			if (numberOfBits <= 8)
+				NetBitWriter.WriteByte((byte)(source >> 8), numberOfBits, destination, destinationBitOffset + 8);
-			{
-				NetBitWriter.WriteByte((byte)(source >> 8), numberOfBits, destination, destinationBitOffset);
-				return returnValue;
-			}
-
-			return returnValue;
 		}
 
 		/// <summary>

UnitTests/ReadWriteTests.cs

@@ -52,6 +52,7 @@ namespace UnitTests
 			msg.Write(567845.0f);
 			msg.WriteVariableInt32(2115998022);
 			msg.Write(46.0);
+			msg.Write((ushort)14, 9);
 			bcnt += msg.WriteVariableInt32(-47);
 			msg.WriteVariableInt32(470000);
 			msg.WriteVariableUInt32(48);
@@ -89,12 +90,13 @@ namespace UnitTests
 			bdr.Append(inc.ReadSingle());
 			bdr.Append(inc.ReadVariableInt32());
 			bdr.Append(inc.ReadDouble());
+			bdr.Append(inc.ReadUInt32(9));
 			bdr.Append(inc.ReadVariableInt32());
 			bdr.Append(inc.ReadVariableInt32());
 			bdr.Append(inc.ReadVariableUInt32());
 			bdr.Append(inc.ReadVariableInt64());
 
-			if (bdr.ToString().Equals("False-342duke of earl4344True567845211599802246-4747000048-49"))
+			if (bdr.ToString().Equals("False-342duke of earl4344True56784521159980224614-4747000048-49"))
 				Console.WriteLine("Read/write tests OK");
 			else
 				throw new NetException("Read/write tests FAILED!");