Fix add entity velocity encoding

internal/network/entity_packets_test.go

@@ -3,6 +3,7 @@ package network
 import (
 	"bufio"
 	"bytes"
+	"math"
 	"net"
 	"testing"
 
@@ -121,9 +122,6 @@ func TestSendAddEntityLayout(t *testing.T) {
 		Pitch:   45,
 		Yaw:     180,
 		HeadYaw: 90,
-		VelocityX: 123,
-		VelocityY: -456,
-		VelocityZ: 789,
 	}
 
 	errc := make(chan error, 1)
@@ -161,6 +159,20 @@ func TestSendAddEntityLayout(t *testing.T) {
 			t.Fatalf("%s = %v, %v; want %v", tc.name, got, err, tc.want)
 		}
 	}
+	vx, vy, vz, err := r.LPVec3()
+	if err != nil {
+		t.Fatalf("velocity: %v", err)
+	}
+	wantVelocity := []float64{
+		float64(ent.VelocityX) / 8000.0,
+		float64(ent.VelocityY) / 8000.0,
+		float64(ent.VelocityZ) / 8000.0,
+	}
+	for i, got := range []float64{vx, vy, vz} {
+		if math.Abs(got-wantVelocity[i]) > 1.0/16383.0 {
+			t.Fatalf("velocity[%d] = %v, want %v", i, got, wantVelocity[i])
+		}
+	}
 	angles := []struct {
 		name string
 		want byte
@@ -178,20 +190,6 @@ func TestSendAddEntityLayout(t *testing.T) {
 	if data, err := r.VarInt(); err != nil || data != 0 {
 		t.Fatalf("data = %d, %v; want 0", data, err)
 	}
-	encodedVelocities := []struct {
-		name string
-		want uint16
-	}{
-		{"velocityX", uint16(ent.VelocityX)},
-		{"velocityY", uint16(ent.VelocityY)},
-		{"velocityZ", uint16(ent.VelocityZ)},
-	}
-	for _, tc := range encodedVelocities {
-		got, err := r.Uint16()
-		if err != nil || got != tc.want {
-			t.Fatalf("%s = %d, %v; want %d", tc.name, got, err, tc.want)
-		}
-	}
 	if rem := r.Remaining(); rem != 0 {
 		t.Fatalf("remaining bytes = %d, want 0", rem)
 	}

internal/network/play.go

@@ -364,13 +364,15 @@ func (h *handler) sendAddEntity(e *world.Entity) error {
 	w.UUID(e.UUID)
 	w.VarInt(int32(e.TypeID))
 	w.Float64(e.X).Float64(e.Y).Float64(e.Z)
+	w.LPVec3(
+		float64(e.VelocityX)/8000.0,
+		float64(e.VelocityY)/8000.0,
+		float64(e.VelocityZ)/8000.0,
+	)
 	w.Byte(byte(e.Pitch * 256.0 / 360.0))
 	w.Byte(byte(e.Yaw * 256.0 / 360.0))
 	w.Byte(byte(e.HeadYaw * 256.0 / 360.0))
 	w.VarInt(0) // Data
-	w.Uint16(uint16(e.VelocityX))
-	w.Uint16(uint16(e.VelocityY))
-	w.Uint16(uint16(e.VelocityZ))
 	return h.conn.SendWriter(protocol.PlayAddEntity, w)
 }
 

internal/protocol/lpvec3.go

@@ -0,0 +1,85 @@
+package protocol
+
+import (
+	"encoding/binary"
+	"math"
+)
+
+const (
+	lpVec3DataMask = uint64(1<<15 - 1)
+	lpVec3MaxValue = 17179869183.0
+	lpVec3MinValue = 1.0 / 32766.0
+)
+
+// LPVec3 reads Minecraft's variable-length low-precision vector encoding.
+func (r *Reader) LPVec3() (x, y, z float64, err error) {
+	first, err := r.ReadByte()
+	if err != nil {
+		return 0, 0, 0, err
+	}
+	if first == 0 {
+		return 0, 0, 0, nil
+	}
+	second, err := r.ReadByte()
+	if err != nil {
+		return 0, 0, 0, err
+	}
+	upper, err := r.readN(4)
+	if err != nil {
+		return 0, 0, 0, err
+	}
+
+	packed := uint64(binary.BigEndian.Uint32(upper))<<16 | uint64(second)<<8 | uint64(first)
+	scale := uint64(first & 3)
+	if first&4 != 0 {
+		continuation, err := r.VarInt()
+		if err != nil {
+			return 0, 0, 0, err
+		}
+		scale |= uint64(uint32(continuation)) << 2
+	}
+
+	unpack := func(shift uint) float64 {
+		value := math.Min(float64((packed>>shift)&lpVec3DataMask), 32766.0)
+		return (value*2.0/32766.0 - 1.0) * float64(scale)
+	}
+	return unpack(3), unpack(18), unpack(33), nil
+}
+
+// LPVec3 appends Minecraft's variable-length low-precision vector encoding.
+func (w *Writer) LPVec3(x, y, z float64) *Writer {
+	x = sanitizeLPVec3(x)
+	y = sanitizeLPVec3(y)
+	z = sanitizeLPVec3(z)
+	maxAbs := math.Max(math.Abs(x), math.Max(math.Abs(y), math.Abs(z)))
+	if maxAbs < lpVec3MinValue {
+		return w.Byte(0)
+	}
+
+	scale := uint64(math.Ceil(maxAbs))
+	header := scale
+	continuation := scale > 3
+	if continuation {
+		header = scale&3 | 4
+	}
+	pack := func(value float64) uint64 {
+		normalized := value / float64(scale)
+		return uint64(math.Floor((normalized*0.5+0.5)*32766.0 + 0.5))
+	}
+	packed := header | pack(x)<<3 | pack(y)<<18 | pack(z)<<33
+
+	w.Byte(byte(packed))
+	w.Byte(byte(packed >> 8))
+	w.Int32(int32(packed >> 16))
+	if continuation {
+		w.VarInt(int32(scale >> 2))
+	}
+	return w
+}
+
+func sanitizeLPVec3(value float64) float64 {
+	if math.IsNaN(value) {
+		return 0
+	}
+	return math.Max(-lpVec3MaxValue, math.Min(lpVec3MaxValue, value))
+}

internal/protocol/lpvec3_test.go

@@ -0,0 +1,48 @@
+package protocol
+
+import (
+	"bytes"
+	"math"
+	"testing"
+)
+
+func TestLPVec3VanillaFixtures(t *testing.T) {
+	tests := []struct {
+		name      string
+		x, y, z   float64
+		encoded   []byte
+		tolerance float64
+	}{
+		{name: "zero", encoded: []byte{0x00}},
+		{
+			name: "entity velocity", x: 123.0 / 8000.0, y: -456.0 / 8000.0, z: 789.0 / 8000.0,
+			encoded: []byte{0xd9, 0x07, 0x8c, 0x9e, 0xf1, 0x66}, tolerance: 1.0 / 16383.0,
+		},
+		{
+			name: "continuation scale", x: 4.095875,
+			encoded: []byte{0x65, 0xa3, 0x7f, 0xfe, 0xff, 0xff, 0x01}, tolerance: 5.0 / 16383.0,
+		},
+	}
+
+	for _, tc := range tests {
+		t.Run(tc.name, func(t *testing.T) {
+			w := NewWriter(len(tc.encoded))
+			w.LPVec3(tc.x, tc.y, tc.z)
+			if !bytes.Equal(w.Bytes(), tc.encoded) {
+				t.Fatalf("encoded = % x, want % x", w.Bytes(), tc.encoded)
+			}
+
+			r := NewReader(tc.encoded)
+			x, y, z, err := r.LPVec3()
+			if err != nil {
+				t.Fatal(err)
+			}
+			if math.Abs(x-tc.x) > tc.tolerance || math.Abs(y-tc.y) > tc.tolerance || math.Abs(z-tc.z) > tc.tolerance {
+				t.Fatalf("decoded = (%v, %v, %v), want (%v, %v, %v)", x, y, z, tc.x, tc.y, tc.z)
+			}
+			if r.Remaining() != 0 {
+				t.Fatalf("remaining bytes = %d, want 0", r.Remaining())
+			}
+		})
+	}
+}