Add compact IMU frame (rot+acc) (#1001)

Author: 9il

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt

@@ -321,6 +321,9 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 				tracker = connection?.getTracker(packet.sensorId)
 				if (tracker == null) return
 				tracker.setRotation(rot)
+				if (packet is UDPPacket23RotationAndAcceleration) {
+					tracker.setAcceleration(packet.acceleration)
+				}
 				tracker.dataTick()
 			}
 

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt

@@ -329,6 +329,27 @@ class UDPPacket22FeatureFlags(
 	}
 }
 
+data class UDPPacket23RotationAndAcceleration(
+	override var rotation: Quaternion = Quaternion.IDENTITY,
+	var acceleration: Vector3 = Vector3.NULL,
+) : UDPPacket(23),
+	RotationPacket {
+	override var sensorId: Int = 0
+	override fun readData(buf: ByteBuffer) {
+		// s16 s16 s16 s16 s16 s16 s16
+		// qX  qY  qZ  qW  aX  aY  aZ
+		sensorId = buf.get().toInt() and 0xFF
+		val scaleR = 1 / (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 / (1 shl 7).toFloat() // The same as the HID scale
+		acceleration = Vector3(buf.short * scaleA, buf.short * scaleA, buf.short * scaleA)
+	}
+}
+
 data class UDPPacket200ProtocolChange(
 	var targetProtocol: Int = 0,
 	var targetProtocolVersion: Int = 0,

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt

@@ -127,6 +127,7 @@ class UDPProtocolParser {
 		const val PACKET_TEMPERATURE = 20
 		const val PACKET_USER_ACTION = 21
 		const val PACKET_FEATURE_FLAGS = 22
+		const val PACKET_ROTATION_AND_ACCELERATION = 23
 		const val PACKET_BUNDLE = 100
 		const val PACKET_PROTOCOL_CHANGE = 200
 		private val HANDSHAKE_BUFFER = ByteArray(64)