|
1 | 1 | using System; |
2 | 2 | using System.Runtime.CompilerServices; |
3 | 3 | using Unity.Mathematics; |
4 | | -using static BasisNetworkPrimitiveCompression; |
| 4 | + |
5 | 5 | namespace Basis.Scripts.Networking.Compression |
6 | 6 | { |
7 | 7 | public static class BasisUnityBitPackerExtensionsUnsafe |
8 | 8 | { |
9 | 9 | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
10 | | - public static void WriteUShort(ushort value, ref byte[] bytes, ref int offset) |
| 10 | + private static bool EnsureSpace(byte[] bytes, int offset, int size) |
11 | 11 | { |
12 | | - bytes[offset++] = (byte)value; |
13 | | - bytes[offset++] = (byte)(value >> 8); |
| 12 | + return (uint)offset <= (uint)bytes.Length && offset + size <= bytes.Length; |
14 | 13 | } |
15 | 14 |
|
16 | 15 | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
17 | | - public static ushort ReadUShort(ref byte[] bytes, ref int offset) |
| 16 | + private static bool IsFinite(float v) => math.isfinite(v); |
| 17 | + |
| 18 | + [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| 19 | + private static bool IsFinite3(float a, float b, float c) => |
| 20 | + IsFinite(a) & IsFinite(b) & IsFinite(c); |
| 21 | + |
| 22 | + [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| 23 | + private static bool IsFinite4(float a, float b, float c, float d) => |
| 24 | + IsFinite(a) & IsFinite(b) & IsFinite(c) & IsFinite(d); |
| 25 | + |
| 26 | + /// <summary> |
| 27 | + /// Optional stricter validation: quaternions should be close-ish to unit length. |
| 28 | + /// Networking compression often expects normalized rotations. |
| 29 | + /// </summary> |
| 30 | + [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| 31 | + private static bool IsReasonableQuaternion(float x, float y, float z, float w, float tolerance) |
18 | 32 | { |
19 | | - ushort result = (ushort)(bytes[offset] | (bytes[offset + 1] << 8)); |
| 33 | + // length^2 should be near 1 |
| 34 | + float lenSq = x * x + y * y + z * z + w * w; |
| 35 | + // Reject zeros / nonsense |
| 36 | + if (!(lenSq > 0f) || !IsFinite(lenSq)) return false; |
| 37 | + |
| 38 | + // Accept within tolerance (e.g. 0.01..0.05 depending on how noisy your source can be) |
| 39 | + return math.abs(lenSq - 1f) <= tolerance; |
| 40 | + } |
| 41 | + |
| 42 | + [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| 43 | + public static bool TryReadUShort(ref byte[] bytes, ref int offset, out ushort value) |
| 44 | + { |
| 45 | + value = default; |
| 46 | + if (!EnsureSpace(bytes, offset, 2)) return false; |
| 47 | + |
| 48 | + value = (ushort)(bytes[offset] | (bytes[offset + 1] << 8)); |
20 | 49 | offset += 2; |
21 | | - return result; |
| 50 | + return true; |
22 | 51 | } |
23 | | - public unsafe static void WriteQuaternionToBytes(quaternion q,ref byte[] bytes,ref int offset) |
| 52 | + |
| 53 | + |
| 54 | + public unsafe static bool TryReadQuaternionFromBytes( |
| 55 | + ref byte[] bytes, |
| 56 | + ref int offset, |
| 57 | + out quaternion q, |
| 58 | + float unitLengthTolerance = 0.02f, |
| 59 | + bool requireUnitLength = true) |
24 | 60 | { |
| 61 | + q = default; |
| 62 | + if (!EnsureSpace(bytes, offset, 16)) return false; |
| 63 | + |
| 64 | + float x, y, z, w; |
25 | 65 | fixed (byte* ptr = &bytes[offset]) |
26 | 66 | { |
27 | 67 | float* f = (float*)ptr; |
28 | | - f[0] = float.IsNaN(q.value.x) ? 0f : q.value.x; |
29 | | - f[1] = float.IsNaN(q.value.y) ? 0f : q.value.y; |
30 | | - f[2] = float.IsNaN(q.value.z) ? 0f : q.value.z; |
31 | | - f[3] = float.IsNaN(q.value.w) ? 1f : q.value.w; |
| 68 | + x = f[0]; |
| 69 | + y = f[1]; |
| 70 | + z = f[2]; |
| 71 | + w = f[3]; |
| 72 | + } |
| 73 | + |
| 74 | + // Validate without repairing |
| 75 | + if (!IsFinite4(x, y, z, w)) |
| 76 | + { |
| 77 | + return false; |
| 78 | + } |
| 79 | + |
| 80 | + if (requireUnitLength && !IsReasonableQuaternion(x, y, z, w, unitLengthTolerance)) |
| 81 | + { |
| 82 | + return false; |
32 | 83 | } |
| 84 | + |
33 | 85 | offset += 16; |
| 86 | + q = new quaternion(x, y, z, w); |
| 87 | + return true; |
34 | 88 | } |
35 | 89 |
|
36 | | - public unsafe static quaternion ReadQuaternionFromBytes(ref byte[] bytes, ref int offset) |
| 90 | + [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| 91 | + public static bool TryReadPosition(ref byte[] buffer, ref int offset, out Unity.Mathematics.float3 position) |
37 | 92 | { |
38 | | - float x, y, z, w; |
39 | | - fixed (byte* ptr = &bytes[offset]) |
| 93 | + position = default; |
| 94 | + if (!EnsureSpace(buffer, offset, 12)) return false; |
| 95 | + |
| 96 | + float x, y, z; |
| 97 | + unsafe |
40 | 98 | { |
41 | | - x = *((float*)ptr); |
42 | | - y = *((float*)(ptr + 4)); |
43 | | - z = *((float*)(ptr + 8)); |
44 | | - w = *((float*)(ptr + 12)); |
| 99 | + fixed (byte* src = &buffer[offset]) |
| 100 | + { |
| 101 | + float* fSrc = (float*)src; |
| 102 | + x = fSrc[0]; |
| 103 | + y = fSrc[1]; |
| 104 | + z = fSrc[2]; |
| 105 | + } |
45 | 106 | } |
46 | | - offset += 16; |
47 | 107 |
|
48 | | - if (float.IsNaN(x)) x = 0f; |
49 | | - if (float.IsNaN(y)) y = 0f; |
50 | | - if (float.IsNaN(z)) z = 0f; |
51 | | - if (float.IsNaN(w)) w = 1f; |
| 108 | + if (!IsFinite3(x, y, z)) return false; |
52 | 109 |
|
53 | | - return new quaternion(x, y, z, w); |
| 110 | + offset += 12; |
| 111 | + position = new Unity.Mathematics.float3(x, y, z); |
| 112 | + return true; |
54 | 113 | } |
55 | | - static void WriteULongLE(ulong v, ref byte[] bytes, ref int offset) |
| 114 | + [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| 115 | + private static float Sanitize(float v, float fallback) |
56 | 116 | { |
57 | | - // little-endian |
58 | | - bytes[offset++] = (byte)(v); |
59 | | - bytes[offset++] = (byte)(v >> 8); |
60 | | - bytes[offset++] = (byte)(v >> 16); |
61 | | - bytes[offset++] = (byte)(v >> 24); |
62 | | - bytes[offset++] = (byte)(v >> 32); |
63 | | - bytes[offset++] = (byte)(v >> 40); |
64 | | - bytes[offset++] = (byte)(v >> 48); |
65 | | - bytes[offset++] = (byte)(v >> 56); |
| 117 | + // math.isfinite catches both NaN and ±Infinity. |
| 118 | + return math.isfinite(v) ? v : fallback; |
66 | 119 | } |
67 | | - |
68 | | - static ulong ReadULongLE(ref byte[] bytes, ref int offset) |
| 120 | + [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| 121 | + public static void WriteUShort(ushort value, ref byte[] bytes, ref int offset) |
69 | 122 | { |
70 | | - ulong v = |
71 | | - (ulong)bytes[offset] | |
72 | | - ((ulong)bytes[offset + 1] << 8) | |
73 | | - ((ulong)bytes[offset + 2] << 16) | |
74 | | - ((ulong)bytes[offset + 3] << 24) | |
75 | | - ((ulong)bytes[offset + 4] << 32) | |
76 | | - ((ulong)bytes[offset + 5] << 40) | |
77 | | - ((ulong)bytes[offset + 6] << 48) | |
78 | | - ((ulong)bytes[offset + 7] << 56); |
79 | | - |
80 | | - offset += 8; |
81 | | - return v; |
| 123 | + EnsureSpace(bytes, offset, 2); bytes[offset++] = (byte)value; bytes[offset++] = (byte)(value >> 8); |
82 | 124 | } |
83 | 125 | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
84 | | - public static void WritePosition(UnityEngine.Vector3 position, ref byte[] buffer, ref int offset) |
| 126 | + public static ushort ReadUShort(ref byte[] bytes, ref int offset) |
85 | 127 | { |
86 | | - unsafe |
| 128 | + EnsureSpace(bytes, offset, 2); |
| 129 | + ushort result = (ushort)(bytes[offset] | (bytes[offset + 1] << 8)); |
| 130 | + offset += 2; |
| 131 | + return result; |
| 132 | + } |
| 133 | + public unsafe static void WriteQuaternionToBytes(quaternion q, ref byte[] bytes, ref int offset) |
| 134 | + { |
| 135 | + EnsureSpace(bytes, offset, 16); fixed (byte* ptr = &bytes[offset]) |
87 | 136 | { |
88 | | - fixed (byte* dst = &buffer[offset]) |
89 | | - { |
90 | | - float* fDst = (float*)dst; |
91 | | - fDst[0] = position.x; |
92 | | - fDst[1] = position.y; |
93 | | - fDst[2] = position.z; |
94 | | - } |
| 137 | + float* f = (float*)ptr; f[0] = Sanitize(q.value.x, 0f); |
| 138 | + f[1] = Sanitize(q.value.y, 0f); |
| 139 | + f[2] = Sanitize(q.value.z, 0f); |
| 140 | + f[3] = Sanitize(q.value.w, 1f); |
95 | 141 | } |
96 | | - |
97 | | - offset += 12; |
| 142 | + offset += 16; |
98 | 143 | } |
99 | | - |
100 | 144 | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
101 | | - public static UnityEngine.Vector3 ReadPosition(ref byte[] buffer, ref int offset) |
| 145 | + public static void WritePosition(UnityEngine.Vector3 position, ref byte[] buffer, ref int offset) |
102 | 146 | { |
103 | | - UnityEngine.Vector3 result; |
| 147 | + EnsureSpace(buffer, offset, 12); |
104 | 148 | unsafe |
105 | 149 | { |
106 | | - fixed (byte* src = &buffer[offset]) |
| 150 | + fixed (byte* dst = &buffer[offset]) |
107 | 151 | { |
108 | | - float* fSrc = (float*)src; |
109 | | - result.x = float.IsNaN(fSrc[0]) ? 0f : fSrc[0]; |
110 | | - result.y = float.IsNaN(fSrc[1]) ? 0f : fSrc[1]; |
111 | | - result.z = float.IsNaN(fSrc[2]) ? 0f : fSrc[2]; |
| 152 | + float* fDst = (float*)dst; |
| 153 | + fDst[0] = Sanitize(position.x, 0f); |
| 154 | + fDst[1] = Sanitize(position.y, 0f); |
| 155 | + fDst[2] = Sanitize(position.z, 0f); |
112 | 156 | } |
113 | 157 | } |
114 | | - |
115 | 158 | offset += 12; |
116 | | - return result; |
117 | 159 | } |
118 | 160 | } |
119 | 161 | } |
0 commit comments