shrink UDPPacket23IMUFrame

SlimeVR · Apr 21, 2024 · f870482 · f870482
1 parent 24b8568
commit f870482
Showing 1 changed file with 8 additions and 2 deletions.
diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt
@@ -337,8 +337,14 @@ data class UDPPacket23IMUFrame(
 	override var sensorId: Int = 0
 	override fun readData(buf: ByteBuffer) {
 		sensorId = buf.get().toInt() and 0xFF
-		rotation = UDPUtils.getSafeBufferQuaternion(buf)
-		acceleration = Vector3(UDPUtils.getSafeBufferFloat(buf), UDPUtils.getSafeBufferFloat(buf), UDPUtils.getSafeBufferFloat(buf))
+		val scaleR = (1 shl 15).toFloat() // Q15: 1 is represented as 0x7FFF and -1 as 0x8000
+		val x = buf.short / scaleR
+		val y = buf.short / scaleR
+		val z = buf.short / scaleR
+		val w = buf.short / scaleR
+		rotation = Quaternion(w, x, y, z).unit()
+		val scaleA = (1 shl 7).toFloat() // The same as the HID scale
+		acceleration = Vector3(buf.short / scaleA, buf.short / scaleA, buf.short / scaleA)
 	}
 }