diff --git a/internal/network/entity_packets_test.go b/internal/network/entity_packets_test.go index a140516..d65417a 100644 --- a/internal/network/entity_packets_test.go +++ b/internal/network/entity_packets_test.go @@ -3,6 +3,7 @@ package network import ( "bufio" "bytes" + "math" "net" "testing" @@ -112,18 +113,15 @@ func TestSendAddEntityLayout(t *testing.T) { uuid := [16]byte{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x10} h := &handler{conn: NewConn(serverSide)} ent := &world.Entity{ - ID: 43, - UUID: uuid, - TypeID: 77, - X: 10.5, - Y: 66.25, - Z: -20.75, - Pitch: 45, - Yaw: 180, - HeadYaw: 90, - VelocityX: 123, - VelocityY: -456, - VelocityZ: 789, + ID: 43, + UUID: uuid, + TypeID: 77, + X: 10.5, + Y: 66.25, + Z: -20.75, + Pitch: 45, + Yaw: 180, + HeadYaw: 90, } 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) } diff --git a/internal/network/play.go b/internal/network/play.go index f841240..69c30ce 100644 --- a/internal/network/play.go +++ b/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) } diff --git a/internal/protocol/lpvec3.go b/internal/protocol/lpvec3.go new file mode 100644 index 0000000..29e06fc --- /dev/null +++ b/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)) +} diff --git a/internal/protocol/lpvec3_test.go b/internal/protocol/lpvec3_test.go new file mode 100644 index 0000000..755254c --- /dev/null +++ b/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()) + } + }) + } +}