diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/player/PlayerTracker.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/player/PlayerTracker.kt index c70b7aee7..7d685fd0c 100644 --- a/server/core/src/main/java/dev/slimevr/poseframeformat/player/PlayerTracker.kt +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/player/PlayerTracker.kt @@ -59,7 +59,7 @@ class PlayerTracker(val trackerFrames: TrackerFrames, val tracker: Tracker, priv val acceleration = frame.tryGetAcceleration() if (acceleration != null) { - tracker.acceleration = acceleration + tracker.setAcceleration(acceleration) } } } diff --git a/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrame.kt b/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrame.kt index 4bee6540a..cdb3c5173 100644 --- a/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrame.kt +++ b/server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrame.kt @@ -117,7 +117,7 @@ data class TrackerFrame( val rotation: Quaternion? = if (tracker.hasRotation) tracker.getRotation() else null val position: Vector3? = if (tracker.hasPosition) tracker.position else null - val acceleration: Vector3? = if (tracker.hasAcceleration) tracker.acceleration else null + val acceleration: Vector3? = if (tracker.hasAcceleration) tracker.getAcceleration() else null var rawRotation: Quaternion? = if (tracker.hasAdjustedRotation) tracker.getRawRotation() else null // If the rawRotation is the same as rotation, there's no point in saving it, set it back to null diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt index 5de22cba8..b37d359b9 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt @@ -45,8 +45,8 @@ class Tracker @JvmOverloads constructor( private val timer = BufferedTimer(1f) private var timeAtLastUpdate: Long = 0 private var rotation = Quaternion.IDENTITY + private var acceleration = Vector3.NULL var position = Vector3.NULL - var acceleration = Vector3.NULL val resetsHandler: TrackerResetsHandler = TrackerResetsHandler(this) val filteringHandler: TrackerFilteringHandler = TrackerFilteringHandler() var batteryVoltage: Float? = null @@ -264,6 +264,18 @@ class Tracker @JvmOverloads constructor( return rot } + /** + * Gets the adjusted tracker acceleration after mounting corrections. + */ + fun getAcceleration(): Vector3 { + var vec = acceleration + if (needsReset) { + vec = resetsHandler.getMountingAdjustedRotationFrom(rotation).sandwich(vec) + } + + return vec + } + /** * Gets the identity-adjusted tracker rotation after some corrections * (filtering, identity reset and identity mounting). @@ -301,6 +313,13 @@ class Tracker @JvmOverloads constructor( this.rotation = rotation } + /** + * Sets the raw (unadjusted) acceleration of the tracker. + */ + fun setAcceleration(vec: Vector3) { + this.acceleration = vec + } + fun isImu(): Boolean { return imuType != null } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index 391e14f62..46d48a11a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -96,6 +96,13 @@ class TrackerResetsHandler(val tracker: Tracker) { return adjustToIdentity(rotation) } + /** + * Takes a rotation and adjusts it to mounting + */ + fun getMountingAdjustedRotationFrom(rotation: Quaternion): Quaternion { + return rotation * mountingOrientation * mountRotFix + } + /** * Converts raw or filtered rotation into reference- and * mounting-reset-adjusted by applying quaternions produced after diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt index 84ffc49d7..17c5fbc63 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt @@ -292,8 +292,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker is UDPPacket4Acceleration -> { tracker = connection?.getTracker(packet.sensorId) if (tracker == null) return - val acceleration = tracker.getRotation().sandwich(packet.acceleration) - tracker.acceleration = acceleration + tracker.setAcceleration(packet.acceleration) } is UDPPacket10PingPong -> { if (connection == null) return