diff --git a/server/core/src/main/java/dev/slimevr/autobone/AutoBone.kt b/server/core/src/main/java/dev/slimevr/autobone/AutoBone.kt index faf714f2d2..a0602aa75b 100644 --- a/server/core/src/main/java/dev/slimevr/autobone/AutoBone.kt +++ b/server/core/src/main/java/dev/slimevr/autobone/AutoBone.kt @@ -4,7 +4,8 @@ import dev.slimevr.SLIMEVR_IDENTIFIER import dev.slimevr.VRServer import dev.slimevr.autobone.errors.* import dev.slimevr.config.AutoBoneConfig -import dev.slimevr.poseframeformat.PoseFrameIO +import dev.slimevr.poseframeformat.PfrIO +import dev.slimevr.poseframeformat.PfsIO import dev.slimevr.poseframeformat.PoseFrames import dev.slimevr.tracking.processor.BoneType import dev.slimevr.tracking.processor.HumanPoseManager @@ -713,7 +714,7 @@ class AutoBone(server: VRServer) { if (saveDir.isDirectory || saveDir.mkdirs()) { LogManager .info("[AutoBone] Exporting frames to \"${recordingFile.path}\"...") - if (PoseFrameIO.tryWriteToFile(recordingFile, frames)) { + if (PfsIO.tryWriteToFile(recordingFile, frames)) { LogManager .info( "[AutoBone] Done exporting! Recording can be found at \"${recordingFile.path}\".", @@ -740,29 +741,35 @@ class AutoBone(server: VRServer) { var recordingFile: File var recordingIndex = 1 do { - recordingFile = File(saveDir, "ABRecording${recordingIndex++}.pfr") + recordingFile = File(saveDir, "ABRecording${recordingIndex++}.pfs") } while (recordingFile.exists()) saveRecording(frames, recordingFile) } fun loadRecordings(): FastList> { val recordings = FastList>() - if (!loadDir.isDirectory) return recordings - val files = loadDir.listFiles() ?: return recordings - for (file in files) { - if (!file.isFile || !file.name.endsWith(".pfr", ignoreCase = true)) continue - LogManager - .info( - "[AutoBone] Detected recording at \"${file.path}\", loading frames...", - ) - val frames = PoseFrameIO.tryReadFromFile(file) + loadDir.listFiles()?.forEach { file -> + if (!file.isFile) return@forEach + + val frames = if (file.name.endsWith(".pfs", ignoreCase = true)) { + LogManager.info("[AutoBone] Loading PFS recording from \"${file.path}\"...") + PfsIO.tryReadFromFile(file) + } else if (file.name.endsWith(".pfr", ignoreCase = true)) { + LogManager.info("[AutoBone] Loading PFR recording from \"${file.path}\"...") + PfrIO.tryReadFromFile(file) + } else { + return@forEach + } + if (frames == null) { - LogManager.severe("Reading frames from \"${file.path}\" failed...") + LogManager.severe("[AutoBone] Failed to load recording from \"${file.path}\".") } else { recordings.add(Pair.of(file.name, frames)) + LogManager.info("[AutoBone] Loaded recording from \"${file.path}\".") } } + return recordings } diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/PoseFrameIO.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/PfrIO.kt similarity index 60% rename from server/core/src/main/java/dev/slimevr/poseframeformat/PoseFrameIO.kt rename to server/core/src/main/java/dev/slimevr/poseframeformat/PfrIO.kt index 706abfba58..e30c6ebc0f 100644 --- a/server/core/src/main/java/dev/slimevr/poseframeformat/PoseFrameIO.kt +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/PfrIO.kt @@ -18,7 +18,7 @@ import java.io.FileInputStream import java.io.FileOutputStream import java.io.IOException -object PoseFrameIO { +object PfrIO { @Throws(IOException::class) private fun writeVector3f(outputStream: DataOutputStream, vector: Vector3) { outputStream.writeFloat(vector.x) @@ -34,41 +34,46 @@ object PoseFrameIO { outputStream.writeFloat(quaternion.w) } + fun writeFrame(outputStream: DataOutputStream, trackerFrame: TrackerFrame?) { + if (trackerFrame == null) { + outputStream.writeInt(0) + return + } + + var dataFlags = trackerFrame.dataFlags + + // Don't write destination strings anymore, replace with + // the enum + if (trackerFrame.hasData(TrackerFrameData.DESIGNATION_STRING)) { + dataFlags = TrackerFrameData.TRACKER_POSITION_ENUM + .add(TrackerFrameData.DESIGNATION_STRING.remove(dataFlags)) + } + outputStream.writeInt(dataFlags) + if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { + writeQuaternion(outputStream, trackerFrame.rotation!!) + } + if (trackerFrame.hasData(TrackerFrameData.POSITION)) { + writeVector3f(outputStream, trackerFrame.position!!) + } + if (TrackerFrameData.TRACKER_POSITION_ENUM.check(dataFlags)) { + // ID is offset by 1 for historical reasons + outputStream.writeInt(trackerFrame.trackerPosition!!.id - 1) + } + if (trackerFrame.hasData(TrackerFrameData.ACCELERATION)) { + writeVector3f(outputStream, trackerFrame.acceleration!!) + } + if (trackerFrame.hasData(TrackerFrameData.RAW_ROTATION)) { + writeQuaternion(outputStream, trackerFrame.rawRotation!!) + } + } + fun writeFrames(outputStream: DataOutputStream, frames: PoseFrames) { outputStream.writeInt(frames.frameHolders.size) for (tracker in frames.frameHolders) { outputStream.writeUTF(tracker.name) outputStream.writeInt(tracker.frames.size) for (i in 0 until tracker.frames.size) { - val trackerFrame = tracker.tryGetFrame(i) - if (trackerFrame == null) { - outputStream.writeInt(0) - continue - } - var dataFlags = trackerFrame.dataFlags - - // Don't write destination strings anymore, replace with - // the enum - if (trackerFrame.hasData(TrackerFrameData.DESIGNATION_STRING)) { - dataFlags = TrackerFrameData.TRACKER_POSITION_ENUM - .add(TrackerFrameData.DESIGNATION_STRING.remove(dataFlags)) - } - outputStream.writeInt(dataFlags) - if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { - writeQuaternion(outputStream, trackerFrame.rotation!!) - } - if (trackerFrame.hasData(TrackerFrameData.POSITION)) { - writeVector3f(outputStream, trackerFrame.position!!) - } - if (TrackerFrameData.TRACKER_POSITION_ENUM.check(dataFlags)) { - outputStream.writeInt(trackerFrame.trackerPosition!!.ordinal) - } - if (trackerFrame.hasData(TrackerFrameData.ACCELERATION)) { - writeVector3f(outputStream, trackerFrame.acceleration!!) - } - if (trackerFrame.hasData(TrackerFrameData.RAW_ROTATION)) { - writeQuaternion(outputStream, trackerFrame.rawRotation!!) - } + writeFrame(outputStream, tracker.tryGetFrame(i)) } } } @@ -112,6 +117,43 @@ object PoseFrameIO { return Quaternion(w, x, y, z) } + fun readFrame(inputStream: DataInputStream): TrackerFrame { + val dataFlags = inputStream.readInt() + + var designation: TrackerPosition? = null + if (TrackerFrameData.DESIGNATION_STRING.check(dataFlags)) { + designation = getByDesignation(inputStream.readUTF()) + } + var rotation: Quaternion? = null + if (TrackerFrameData.ROTATION.check(dataFlags)) { + rotation = readQuaternion(inputStream) + } + var position: Vector3? = null + if (TrackerFrameData.POSITION.check(dataFlags)) { + position = readVector3f(inputStream) + } + if (TrackerFrameData.TRACKER_POSITION_ENUM.check(dataFlags)) { + // ID is offset by 1 for historical reasons + designation = TrackerPosition.getById(inputStream.readInt() + 1) + } + var acceleration: Vector3? = null + if (TrackerFrameData.ACCELERATION.check(dataFlags)) { + acceleration = readVector3f(inputStream) + } + var rawRotation: Quaternion? = null + if (TrackerFrameData.RAW_ROTATION.check(dataFlags)) { + rawRotation = readQuaternion(inputStream) + } + + return TrackerFrame( + designation, + rotation, + position, + acceleration, + rawRotation, + ) + } + fun readFrames(inputStream: DataInputStream): PoseFrames { val trackerCount = inputStream.readInt() val trackers = FastList(trackerCount) @@ -122,40 +164,7 @@ object PoseFrameIO { trackerFrameCount, ) for (j in 0 until trackerFrameCount) { - val dataFlags = inputStream.readInt() - var designation: TrackerPosition? = null - if (TrackerFrameData.DESIGNATION_STRING.check(dataFlags)) { - designation = getByDesignation(inputStream.readUTF()) - } - var rotation: Quaternion? = null - if (TrackerFrameData.ROTATION.check(dataFlags)) { - rotation = readQuaternion(inputStream) - } - var position: Vector3? = null - if (TrackerFrameData.POSITION.check(dataFlags)) { - position = readVector3f(inputStream) - } - if (TrackerFrameData.TRACKER_POSITION_ENUM.check(dataFlags)) { - designation = TrackerPosition.values()[inputStream.readInt()] - } - var acceleration: Vector3? = null - if (TrackerFrameData.ACCELERATION.check(dataFlags)) { - acceleration = readVector3f(inputStream) - } - var rawRotation: Quaternion? = null - if (TrackerFrameData.RAW_ROTATION.check(dataFlags)) { - rawRotation = readQuaternion(inputStream) - } - trackerFrames - .add( - TrackerFrame( - designation, - rotation, - position, - acceleration, - rawRotation, - ), - ) + trackerFrames.add(readFrame(inputStream)) } trackers.add(TrackerFrames(name, trackerFrames)) } diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/PfsIO.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/PfsIO.kt new file mode 100644 index 0000000000..203589f938 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/PfsIO.kt @@ -0,0 +1,194 @@ +package dev.slimevr.poseframeformat + +import dev.slimevr.config.AutoBoneConfig +import dev.slimevr.config.SkeletonConfig +import dev.slimevr.poseframeformat.trackerdata.TrackerFrame +import dev.slimevr.poseframeformat.trackerdata.TrackerFrames +import io.eiren.util.logging.LogManager +import java.io.BufferedInputStream +import java.io.BufferedOutputStream +import java.io.DataInputStream +import java.io.DataOutputStream +import java.io.EOFException +import java.io.File +import java.io.FileInputStream +import java.io.FileOutputStream +import java.io.IOException + +/** + * PoseFrameStream File IO, designed to handle the internal PoseFrames format with + * the new file format for streaming and storing additional debugging info + */ +object PfsIO { + private fun writeRecordingDef(stream: DataOutputStream, frameInterval: Float) { + stream.writeByte(PfsPackets.RECORDING_DEFINITION.id) + stream.writeFloat(frameInterval) + } + + private fun writeTrackerDef(stream: DataOutputStream, id: Int, name: String) { + stream.writeByte(PfsPackets.TRACKER_DEFINITION.id) + stream.writeByte(id) + stream.writeUTF(name) + } + + private fun writeTrackerFrame(stream: DataOutputStream, id: Int, frameIndex: Int, frame: TrackerFrame) { + stream.writeByte(PfsPackets.TRACKER_FRAME.id) + stream.writeByte(id) + stream.writeInt(frameIndex) + // Write frame data (same format as PFR) + PfrIO.writeFrame(stream, frame) + } + + private fun writeBodyProportions(stream: DataOutputStream, abConfig: AutoBoneConfig, skeletonConfig: SkeletonConfig) { + stream.writeByte(PfsPackets.PROPORTIONS_CONFIG.id) + // HMD height will be moved to SkeletonConfig in the future + stream.writeFloat(abConfig.targetHmdHeight) + // Floor height not yet implemented + stream.writeFloat(0f) + // Write config map + stream.writeShort(skeletonConfig.offsets.size) + for ((key, value) in skeletonConfig.offsets) { + stream.writeUTF(key) + stream.writeFloat(value) + } + } + + fun writeFrames(stream: DataOutputStream, frames: PoseFrames) { + // Give trackers IDs (max 255) + val trackers = frames.frameHolders.mapIndexed { i, t -> i to t } + + // Write recording definition + writeRecordingDef(stream, frames.frameInterval) + + // Write tracker definitions + for (tracker in trackers) { + writeTrackerDef(stream, tracker.first, tracker.second.name) + } + + // Write tracker frames + for (i in 0 until frames.maxFrameCount) { + for (tracker in trackers) { + // If the tracker has a frame at the index + val frame = tracker.second.tryGetFrame(i) + if (frame != null) { + writeTrackerFrame(stream, tracker.first, i, frame) + } + } + } + } + + fun tryWriteFrames(stream: DataOutputStream, frames: PoseFrames): Boolean = try { + writeFrames(stream, frames) + true + } catch (e: Exception) { + LogManager.severe("[PfsIO] Error writing frame to stream.", e) + false + } + + fun writeToFile(file: File, frames: PoseFrames) { + DataOutputStream( + BufferedOutputStream(FileOutputStream(file)), + ).use { writeFrames(it, frames) } + } + + fun tryWriteToFile(file: File, frames: PoseFrames): Boolean = try { + writeToFile(file, frames) + true + } catch (e: Exception) { + LogManager.severe("[PfsIO] Error writing frames to file.", e) + false + } + + fun readFrame(stream: DataInputStream, poseFrames: PoseFrames, trackers: MutableMap) { + val packetId = stream.readUnsignedByte() + val packetType = PfsPackets.byId[packetId] + + when (packetType) { + null -> { + throw IOException("Encountered unknown packet ID ($packetId) while deserializing PFS stream.") + } + + PfsPackets.RECORDING_DEFINITION -> { + // Unused, useful for debugging + val frameInterval = stream.readFloat() + poseFrames.frameInterval = frameInterval + LogManager.debug("[PfsIO] Frame interval: $frameInterval s") + } + + PfsPackets.TRACKER_DEFINITION -> { + val trackerId = stream.readUnsignedByte() + val name = stream.readUTF() + + // Get or make tracker and set its name + trackers.getOrPut(trackerId) { + TrackerFrames(name) + }.name = name + } + + PfsPackets.TRACKER_FRAME -> { + val trackerId = stream.readUnsignedByte() + val tracker = trackers.getOrPut(trackerId) { + // If tracker doesn't exist yet, make one + TrackerFrames() + } + val frameNum = stream.readInt() + val frame = PfrIO.readFrame(stream) + + tracker.frames.add(frameNum, frame) + } + + PfsPackets.PROPORTIONS_CONFIG -> { + // Unused, useful for debugging + // Currently just prints JSON format config to console + val configCount = stream.readUnsignedShort() + val sb = StringBuilder("[PfsIO] Body proportion configs ($configCount): {") + for (i in 0 until configCount) { + if (i > 0) { + sb.append(", ") + } + sb.append(stream.readUTF()) + sb.append(": ") + sb.append(stream.readFloat()) + } + sb.append('}') + + LogManager.debug(sb.toString()) + } + } + } + + fun readFrames(stream: DataInputStream): PoseFrames { + val poseFrames = PoseFrames() + val trackers = mutableMapOf() + + while (true) { + try { + readFrame(stream, poseFrames, trackers) + } catch (_: EOFException) { + // Reached end of stream, stop reading and return the recording + // LogManager.debug("[PfsIO] Reached end of PFS stream.", e) + break + } + } + + poseFrames.frameHolders.addAll(trackers.values) + return poseFrames + } + + fun tryReadFrames(stream: DataInputStream): PoseFrames? = try { + readFrames(stream) + } catch (e: Exception) { + LogManager.severe("[PfsIO] Error reading frames from stream.", e) + null + } + + fun readFromFile(file: File): PoseFrames = + DataInputStream(BufferedInputStream(FileInputStream(file))).use { readFrames(it) } + + fun tryReadFromFile(file: File): PoseFrames? = try { + readFromFile(file) + } catch (e: Exception) { + LogManager.severe("[PfsIO] Error reading frames from file.", e) + null + } +} diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/PfsPackets.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/PfsPackets.kt new file mode 100644 index 0000000000..f0c6d0da76 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/PfsPackets.kt @@ -0,0 +1,40 @@ +package dev.slimevr.poseframeformat + +/** + * Packet ID ([UByte]), + * Packet data (see [PfsPackets], implemented in [PfsIO]) + */ +enum class PfsPackets(val id: Int) { + /** + * Frame interval ([Float] seconds) + */ + RECORDING_DEFINITION(0), + + /** + * Tracker ID ([UByte]), + * Tracker name (UTF-8 [String]) + */ + TRACKER_DEFINITION(1), + + /** + * Tracker ID ([UByte]), + * Frame number ([UInt]), + * PFR frame data (see [PfrIO.writeFrame] & [PfrIO.readFrame]) + */ + TRACKER_FRAME(2), + + /** + * Hmd height ([Float]), + * Floor height ([Float]), + * Body proportion configs (Count ([UShort]) x (Key (UTF-8 [String]), Value ([Float])) + */ + PROPORTIONS_CONFIG(3), + ; + + val byteId = id.toUByte() + + companion object { + val byId = entries.associateBy { it.id } + val byByteId = entries.associateBy { it.byteId } + } +} diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/PoseFrames.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/PoseFrames.kt index 7b9ab1ffa0..8665834972 100644 --- a/server/core/src/main/java/dev/slimevr/poseframeformat/PoseFrames.kt +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/PoseFrames.kt @@ -8,6 +8,11 @@ import io.eiren.util.collections.FastList class PoseFrames : Iterable> { val frameHolders: FastList + /** + * Frame interval in seconds + */ + var frameInterval: Float = 0.02f + /** * Creates a [PoseFrames] object with the provided list of * [TrackerFrames]s as the internal [TrackerFrames] list. diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/PoseRecorder.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/PoseRecorder.kt index c7cd59f327..7d2fc14095 100644 --- a/server/core/src/main/java/dev/slimevr/poseframeformat/PoseRecorder.kt +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/PoseRecorder.kt @@ -88,6 +88,7 @@ class PoseRecorder(private val server: VRServer) { require(trackers.isNotEmpty()) { "trackers must have at least one entry." } cancelFrameRecording() val poseFrame = PoseFrames(trackers.size) + poseFrame.frameInterval = intervalMs / 1000f // Update tracker list this.trackers.ensureCapacity(trackers.size) diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrameData.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrameData.kt index f988820c52..f6ea7e2a54 100644 --- a/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrameData.kt +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrameData.kt @@ -1,6 +1,6 @@ package dev.slimevr.poseframeformat.trackerdata -enum class TrackerFrameData(id: Int) { +enum class TrackerFrameData(val id: Int) { DESIGNATION_STRING(0), ROTATION(1), POSITION(2), @@ -9,11 +9,7 @@ enum class TrackerFrameData(id: Int) { RAW_ROTATION(5), ; - val flag: Int - - init { - flag = 1 shl id - } + val flag: Int = 1 shl id /* * Inline is fine for these, there's no negative to inlining them as they'll never diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrames.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrames.kt index d81c2dc09e..49021ea593 100644 --- a/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrames.kt +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrames.kt @@ -6,7 +6,7 @@ import dev.slimevr.tracking.trackers.Tracker import dev.slimevr.tracking.trackers.TrackerStatus import io.eiren.util.collections.FastList -data class TrackerFrames(val name: String = "", val frames: FastList) { +data class TrackerFrames(var name: String = "", val frames: FastList) { constructor(name: String = "", initialCapacity: Int = 5) : this(name, FastList(initialCapacity)) constructor(baseTracker: Tracker, frames: FastList) : this(baseTracker.name, frames) diff --git a/server/core/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.kt b/server/core/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.kt index 1a874ead32..28860cac99 100644 --- a/server/core/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.kt +++ b/server/core/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.kt @@ -1,6 +1,6 @@ package dev.slimevr.posestreamer -import dev.slimevr.poseframeformat.PoseFrameIO.readFromFile +import dev.slimevr.poseframeformat.PfrIO.readFromFile import dev.slimevr.poseframeformat.PoseFrames import dev.slimevr.poseframeformat.player.TrackerFramesPlayer import dev.slimevr.tracking.processor.HumanPoseManager