From e71ed5cf6c430ec853aa3dcb8016f9d633378859 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 3 Jul 2023 16:43:28 -0700 Subject: [PATCH] Acceleration alignment fix (#737) Co-authored-by: Erimel Co-authored-by: Butterscotch! --- .../poseframeformat/player/PlayerTracker.kt | 2 +- .../trackerdata/TrackerFrame.kt | 2 +- .../dev/slimevr/tracking/trackers/Tracker.kt | 21 ++++++++++++++++++- .../tracking/trackers/TrackerResetsHandler.kt | 7 +++++++ .../trackers/udp/TrackersUDPServer.kt | 3 +-- 5 files changed, 30 insertions(+), 5 deletions(-) diff --git a/server/src/main/java/dev/slimevr/poseframeformat/player/PlayerTracker.kt b/server/src/main/java/dev/slimevr/poseframeformat/player/PlayerTracker.kt index c70b7aee7..7d685fd0c 100644 --- a/server/src/main/java/dev/slimevr/poseframeformat/player/PlayerTracker.kt +++ b/server/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/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrame.kt b/server/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrame.kt index 4bee6540a..cdb3c5173 100644 --- a/server/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrame.kt +++ b/server/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/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt b/server/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt index 5d3778ea8..c5273f1bc 100644 --- a/server/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt +++ b/server/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/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index 12fc7ad8a..4414c2af9 100644 --- a/server/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/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/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt b/server/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt index 648707c7d..0f58a6571 100644 --- a/server/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt +++ b/server/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt @@ -293,8 +293,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