Acceleration alignment fix (#737)

Co-authored-by: Erimel <[email protected]>
Co-authored-by: Butterscotch! <[email protected]>

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)
 		}
 	}
 }

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

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
 	}

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

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