Add StreamBuffer functions for reading / writing vectors

stream_buffer.gd

@@ -77,6 +77,12 @@ func write_float16(value: float) -> void: ensure_capacity(2); buffer.encode_half
 func write_float32(value: float) -> void: ensure_capacity(4); buffer.encode_float(size, value); size += 4; bit = 0
 func write_float64(value: float) -> void: ensure_capacity(8); buffer.encode_double(size, value); size += 8; bit = 0
 
+func write_vector16(value: Vector3) -> void: write_float16(value.x); write_float16(value.y); write_float16(value.z)
+func write_vector32(value: Vector3) -> void: write_float32(value.x); write_float32(value.y); write_float32(value.z)
+
+func write_transform16(value: Transform3D) -> void: write_vector16(value.basis.x); write_vector16(value.basis.y); write_vector16(value.basis.z); write_vector16(value.origin)
+func write_transform32(value: Transform3D) -> void: write_vector32(value.basis.x); write_vector32(value.basis.y); write_vector32(value.basis.z); write_vector32(value.origin)
+
 
 func read_int8() -> int: assert(remaining_bytes() >= 1); var result := buffer.decode_s8(cursor); cursor += 1; bit = 0; return result
 func read_int16() -> int: assert(remaining_bytes() >= 2); var result := buffer.decode_s16(cursor); cursor += 2; bit = 0; return result
@@ -93,19 +99,20 @@ func read_float16() -> float: assert(remaining_bytes() >= 2); var result := buff
 func read_float32() -> float: assert(remaining_bytes() >= 4); var result := buffer.decode_float(cursor); cursor += 4; bit = 0; return result
 func read_float64() -> float: assert(remaining_bytes() >= 8); var result := buffer.decode_double(cursor); cursor += 8; bit = 0; return result
 
+func read_vector16() -> Vector3: return Vector3(read_float16(), read_float16(), read_float16())
+func read_vector32() -> Vector3: return Vector3(read_float32(), read_float32(), read_float32())
+
+func read_transform16() -> Transform3D: return Transform3D(Basis(read_vector16(), read_vector16(), read_vector16()), read_vector16())
+func read_transform32() -> Transform3D: return Transform3D(Basis(read_vector32(), read_vector32(), read_vector32()), read_vector32())
+
 
 func write_bit(value: bool) -> void:
-	if bit == 0:
+	if bit == 0: ensure_capacity(1); buffer[size] = 0; size += 1
-		ensure_capacity(1)
-		buffer[size] = 0
-		size += 1
 	buffer[size - 1] = buffer[size - 1] | (int(value) << bit)
 	bit = (bit + 1) % 8
 
 func read_bit() -> bool:
-	if bit == 0:
+	if bit == 0: assert(remaining_bytes() >= 1); cursor += 1
-		assert(remaining_bytes() >= 1)
-		cursor += 1
 	var result := bool((buffer[cursor - 1] >> bit) & 1)
 	bit = (bit + 1) % 8
 	return result
@@ -138,21 +145,6 @@ func read_string() -> String:
 	return bytes.get_string_from_utf8()
 
 
-func write_transform32(value: Transform3D) -> void:
-	write_float32(value.basis.x.x); write_float32(value.basis.x.y); write_float32(value.basis.x.z)
-	write_float32(value.basis.y.x); write_float32(value.basis.y.y); write_float32(value.basis.y.z)
-	write_float32(value.basis.z.x); write_float32(value.basis.z.y); write_float32(value.basis.z.z)
-	write_float32(value.origin.x);  write_float32(value.origin.y);  write_float32(value.origin.z)
-
-func read_transform32() -> Transform3D:
-	var result := Transform3D.IDENTITY
-	result.basis.x = Vector3(read_float32(), read_float32(), read_float32())
-	result.basis.y = Vector3(read_float32(), read_float32(), read_float32())
-	result.basis.z = Vector3(read_float32(), read_float32(), read_float32())
-	result.origin  = Vector3(read_float32(), read_float32(), read_float32())
-	return result
-
-
 # Optimized way to write a bone transform, since bones are likely to not contain offset or scale.
 
 func write_bone_pose(value: Transform3D) -> void:
@@ -167,22 +159,12 @@ func write_bone_pose(value: Transform3D) -> void:
 	write_bit(has_rot)
 	write_bit(has_scale)
 	write_bit(has_scale3)
-	if has_pos:
+	if has_pos: write_vector16(pos)
-		write_float16(pos.x)
+	# TODO: Could optimize this by using 16-bit fixed-point values.
-		write_float16(pos.y)
+	#       Since these values can only be in the range 0 to 1.
-		write_float16(pos.z)
+	if has_rot: write_vector16(rot)
-	if has_rot:
+	if has_scale3: write_vector16(scale)
-		# TODO: Could optimize this by using 16-bit fixed-point values.
+	elif has_scale: write_float16((scale.x + scale.y + scale.z) / 3)
-		#       Since these values can only be in the range 0 to 1.
-		write_float16(rot.x)
-		write_float16(rot.y)
-		write_float16(rot.z)
-	if has_scale3:
-		write_float16(scale.x)
-		write_float16(scale.y)
-		write_float16(scale.z)
-	elif has_scale:
-		write_float16((scale.x + scale.y + scale.z) / 3)
 
 func read_bone_pose() -> Transform3D:
 	var pos   := Vector3.ZERO
@@ -192,11 +174,9 @@ func read_bone_pose() -> Transform3D:
 	var has_rot    := read_bit()
 	var has_scale  := read_bit()
 	var has_scale3 := read_bit()
-	if has_pos: pos = Vector3(read_float16(), read_float16(), read_float16())
+	if has_pos: pos = read_vector16()
-	if has_rot: rot = Vector3(read_float16(), read_float16(), read_float16())
+	if has_rot: rot = read_vector16()
-	if has_scale3: scale = Vector3(read_float16(), read_float16(), read_float16())
+	if has_scale3: scale = read_vector16()
-	elif has_scale:
+	elif has_scale: var s := read_float16(); scale = Vector3(s, s, s)
-		var s := read_float16()
-		scale = Vector3(s, s, s)
 	var basis := Basis.from_scale(scale) * Basis.from_euler(rot)
 	return Transform3D(basis, pos)