Simplify reset code (again)

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

@@ -93,6 +93,9 @@ class Tracker @JvmOverloads constructor(
 	private val timer = BufferedTimer(1f)
 	private var timeAtLastUpdate: Long = System.currentTimeMillis()
 	private var _rotation = Quaternion.IDENTITY
+
+	// IMU: +z forward, +x left, +y up
+	// SlimeVR: +z backward, +x right, +y up
 	private var _acceleration = Vector3.NULL
 	private var _magVector = Vector3.NULL
 	var position = Vector3.NULL

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

@@ -27,6 +27,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		Math.PI.toFloat(),
 		0f,
 	).toQuaternion()
+	private val QuarterPitch = Quaternion.rotationAroundXAxis(FastMath.HALF_PI)
+
 	private var driftAmount = 0f
 	private var averagedDriftQuat = Quaternion.IDENTITY
 	private var rotationSinceReset = Quaternion.IDENTITY
@@ -56,16 +58,6 @@ class TrackerResetsHandler(val tracker: Tracker) {
 
 	// Reference adjustment quats
 
-	/**
-	 * Gyro fix is set by full reset. This sets the current y rotation to 0, correcting
-	 * for initial yaw rotation and the rotation incurred by mounting orientation. This
-	 * is a local offset in rotation and does not affect the axes of rotation.
-	 *
-	 * This rotation is only used to compute [attachmentFix], otherwise [yawFix] would
-	 * correct for the same rotation.
-	 */
-	private var gyroFix = Quaternion.IDENTITY
-
 	/**
 	 * Attachment fix is set by full reset. This sets the current x and z rotations to
 	 * 0, correcting for initial pitch and roll rotation. This is a global offset in
@@ -191,9 +183,11 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	fun getIdentityAdjustedDriftRotationFrom(rotation: Quaternion): Quaternion = adjustToDrift(adjustToIdentity(rotation))
 
 	/**
-	 * Get the adjusted accel from yawFixZeroReference
+	 * Get the reference adjusted accel.
 	 */
-	fun getReferenceAdjustedAccel(rawRot: Quaternion, accel: Vector3): Vector3 = (adjustToReference(rawRot) / yawFix).sandwich(accel)
+	// All IMU axis corrections are inverse to undo `adjustToReference` after local yaw offsets are added
+	// Order is VERY important here! Please be extremely careful! >~>
+	fun getReferenceAdjustedAccel(rawRot: Quaternion, accel: Vector3): Vector3 = (adjustToReference(rawRot) * (attachmentFix * mountingOrientation * mountRotFix * tposeDownFix).inv()).sandwich(accel)
 
 	/**
 	 * Converts raw or filtered rotation into reference- and
@@ -202,13 +196,17 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	 */
 	private fun adjustToReference(rotation: Quaternion): Quaternion {
 		var rot = rotation
+		// Correct for global pitch/roll offset
+		rot *= attachmentFix
+		// Correct for global yaw offset without affecting local yaw so we can change this
+		// later without invalidating local yaw offset corrections
 		if (!tracker.isHmd || tracker.trackerPosition != TrackerPosition.HEAD) {
-			rot *= mountingOrientation
+			rot = mountingOrientation.inv() * rot * mountingOrientation
 		}
-		rot = gyroFix * rot
-		rot *= attachmentFix
-		rot = mountRotFix.inv() * (rot * mountRotFix)
+		rot = mountRotFix.inv() * rot * mountRotFix
+		// T-pose global correction
 		rot *= tposeDownFix
+		// Align local yaw with reference
 		rot = yawFix * rot
 		rot = constraintFix * rot
 		return rot
@@ -272,15 +270,23 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		lastResetQuaternion = oldRot
 
 		// Adjust raw rotation to mountingOrientation
-		val mountingAdjustedRotation = tracker.getRawRotation() * mountingOrientation
+		val rotation = tracker.getRawRotation()
 
 		// Gyrofix
-		if (tracker.needsMounting || (tracker.trackerPosition == TrackerPosition.HEAD && !tracker.isHmd)) {
-			gyroFix = if (tracker.isComputed) {
-				fixGyroscope(tracker.getRawRotation())
+		val gyroFix = if (tracker.needsMounting || (tracker.trackerPosition == TrackerPosition.HEAD && !tracker.isHmd)) {
+			if (tracker.isComputed) {
+				fixGyroscope(rotation)
 			} else {
-				fixGyroscope(mountingAdjustedRotation * tposeDownFix)
+				if (tracker.trackerPosition.isFoot()) {
+					// Feet are rotated by 90 deg pitch, this means we're relying on IMU rotation
+					//  to be set correctly here.
+					fixGyroscope(rotation * tposeDownFix * QuarterPitch)
+				} else {
+					fixGyroscope(rotation * tposeDownFix)
+				}
 			}
+		} else {
+			Quaternion.IDENTITY
 		}
 
 		// Mounting for computed trackers
@@ -295,15 +301,15 @@ class TrackerResetsHandler(val tracker: Tracker) {
 			if (resetHmdPitch) {
 				// Reset the HMD's pitch if it's assigned to head and resetHmdPitch is true
 				// Get rotation without yaw (make sure to use the raw rotation directly!)
-				val rotBuf = getYawQuaternion(tracker.getRawRotation()).inv() * tracker.getRawRotation()
+				val rotBuf = getYawQuaternion(rotation).inv() * rotation
 				// Isolate pitch
 				Quaternion(rotBuf.w, -rotBuf.x, 0f, 0f).unit()
 			} else {
 				// Don't reset the HMD at all
 				Quaternion.IDENTITY
 			}
 		} else {
-			fixAttachment(mountingAdjustedRotation)
+			(gyroFix * rotation).inv()
 		}
 
 		// Rotate attachmentFix by 180 degrees as a workaround for t-pose (down)
@@ -315,7 +321,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 
 		// Don't adjust yaw if head and computed
 		if (tracker.trackerPosition != TrackerPosition.HEAD || !tracker.isComputed) {
-			yawFix = fixYaw(mountingAdjustedRotation, reference)
+			yawFix = gyroFix * reference.project(Vector3.POS_Y).unit()
 			tracker.yawResetSmoothing.reset()
 		}
 
@@ -360,7 +366,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		lastResetQuaternion = oldRot
 
 		val yawFixOld = yawFix
-		yawFix = fixYaw(tracker.getRawRotation() * mountingOrientation, reference)
+		yawFix = fixYaw(tracker.getRawRotation(), reference)
 		tracker.yawResetSmoothing.reset()
 
 		makeIdentityAdjustmentQuatsYaw()
@@ -410,9 +416,9 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		constraintFix = Quaternion.IDENTITY
 
 		// Get the current calibrated rotation
-		var rotBuf = adjustToDrift(tracker.getRawRotation() * mountingOrientation)
-		rotBuf = gyroFix * rotBuf
+		var rotBuf = adjustToDrift(tracker.getRawRotation())
 		rotBuf *= attachmentFix
+		rotBuf = mountingOrientation.inv() * rotBuf * mountingOrientation
 		rotBuf = yawFix * rotBuf
 
 		// Adjust buffer to reference
@@ -468,14 +474,22 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		mountRotFix = Quaternion.IDENTITY
 	}
 
-	private fun fixGyroscope(sensorRotation: Quaternion): Quaternion = getYawQuaternion(sensorRotation).inv()
-
-	private fun fixAttachment(sensorRotation: Quaternion): Quaternion = (gyroFix * sensorRotation).inv()
+	// EulerOrder.YXZ is actually better for gyroscope fix, as it can get yaw at any roll.
+	//  Consequentially, instead of the roll being limited, the pitch is limited to
+	//  90 degrees from the yaw plane. This means trackers may be mounted upside down
+	//  or with incorrectly configured IMU rotation, but we will need to compensate for
+	//  the pitch.
+	private fun fixGyroscope(sensorRotation: Quaternion): Quaternion = getYawQuaternion(sensorRotation, EulerOrder.YXZ).inv()
 
 	private fun fixYaw(sensorRotation: Quaternion, reference: Quaternion): Quaternion {
-		var rot = gyroFix * sensorRotation
-		rot *= attachmentFix
-		rot = mountRotFix.inv() * (rot * mountRotFix)
+		var rot = sensorRotation * attachmentFix
+		// We need to fix the global yaw offset for the euler yaw calculation
+		if (!tracker.isHmd || tracker.trackerPosition != TrackerPosition.HEAD) {
+			rot = mountingOrientation.inv() * rot * mountingOrientation
+		}
+		rot = mountRotFix.inv() * rot * mountRotFix
+		// TODO: Get diff from ref to rot, use euler angle (YZX) yaw as output.
+		//  This prevents pitch and roll from affecting the alignment.
 		rot = getYawQuaternion(rot)
 		return rot.inv() * reference.project(Vector3.POS_Y).unit()
 	}
@@ -486,7 +500,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	// In both cases, the isolated yaw value changes
 	// with the tracker's roll when pointing forward.
 	// calling twinNearest() makes sure this rotation has the wanted polarity (+-).
-	private fun getYawQuaternion(rot: Quaternion): Quaternion = EulerAngles(EulerOrder.YZX, 0f, rot.toEulerAngles(EulerOrder.YZX).y, 0f).toQuaternion().twinNearest(rot)
+	private fun getYawQuaternion(rot: Quaternion, order: EulerOrder = EulerOrder.YZX): Quaternion = EulerAngles(order, 0f, rot.toEulerAngles(order).y, 0f).toQuaternion().twinNearest(rot)
 
 	private fun makeIdentityAdjustmentQuatsFull() {
 		val sensorRotation = tracker.getRawRotation()

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

@@ -9,8 +9,8 @@ import dev.slimevr.tracking.trackers.*
 import io.eiren.util.Util
 import io.eiren.util.collections.FastList
 import io.eiren.util.logging.LogManager
+import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Quaternion.Companion.fromRotationVector
-import io.github.axisangles.ktmath.Vector3
 import kotlinx.coroutines.*
 import solarxr_protocol.rpc.ResetType
 import java.net.DatagramPacket
@@ -395,8 +395,9 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 				if (tracker == null) return
 				tracker.setRotation(rot)
 				if (packet is UDPPacket23RotationAndAcceleration) {
-					// Switch x and y around to adjust for different axes
-					tracker.setAcceleration(Vector3(packet.acceleration.y, packet.acceleration.x, packet.acceleration.z))
+					// If sensorOffset was applied to accel correctly, the axes will already
+					//  be correct for SlimeVR
+					tracker.setAcceleration(SENSOR_OFFSET_CORRECTION.sandwich(packet.acceleration))
 				}
 				tracker.dataTick()
 			}
@@ -428,8 +429,9 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 			is UDPPacket4Acceleration -> {
 				tracker = connection?.getTracker(packet.sensorId)
 				if (tracker == null) return
-				// Switch x and y around to adjust for different axes
-				tracker.setAcceleration(Vector3(packet.acceleration.y, packet.acceleration.x, packet.acceleration.z))
+				// If sensorOffset was applied to accel correctly, the axes will already
+				//  be correct for SlimeVR
+				tracker.setAcceleration(SENSOR_OFFSET_CORRECTION.sandwich(packet.acceleration))
 			}
 
 			is UDPPacket10PingPong -> {
@@ -614,6 +616,10 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 		 * Change between IMU axes and OpenGL/SteamVR axes
 		 */
 		private val AXES_OFFSET = fromRotationVector(-FastMath.HALF_PI, 0f, 0f)
+
+		// TODO: Set this offset to Quaternion.IDENTITY when the firmware is corrected!
+		// 270 deg (-90 deg) default for officials
+		private val SENSOR_OFFSET_CORRECTION = Quaternion.rotationAroundZAxis(-FastMath.HALF_PI)
 		private const val RESET_SOURCE_NAME = "TrackerServer"
 		private fun packetToString(packet: DatagramPacket?): String {
 			val sb = StringBuilder()

server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt

@@ -35,7 +35,7 @@ class MountingResetTests {
 
 	private fun checkResetMounting(expected: Quaternion, reference: Quaternion) {
 		// Compute the pitch/roll for the expected mounting
-		val trackerRot = (expected * (TrackerTestUtils.frontRot / expected))
+		val trackerRot = expected * TrackerTestUtils.frontRot / expected
 
 		val tracker = Tracker(
 			null,
@@ -120,7 +120,7 @@ class MountingResetTests {
 		val expected = Quaternion.SLIMEVR.RIGHT
 		val reference = EulerAngles(EulerOrder.YZX, FastMath.PI / 8f, FastMath.HALF_PI, 0f).toQuaternion()
 		// Compute the pitch/roll for the expected mounting
-		val trackerRot = (expected * (TrackerTestUtils.frontRot / expected))
+		val trackerRot = expected * TrackerTestUtils.frontRot / expected
 
 		val tracker = Tracker(
 			null,

server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt

@@ -93,13 +93,13 @@ class SkeletonResetTests {
 				TrackerPosition.HIP,
 				TrackerPosition.LEFT_LOWER_LEG,
 				TrackerPosition.RIGHT_LOWER_LEG,
-				-> mountRot * Quaternion.SLIMEVR.FRONT
+				-> mountRot
 
 				TrackerPosition.LEFT_UPPER_LEG,
 				TrackerPosition.RIGHT_UPPER_LEG,
-				-> mountRot
+				-> mountRot * Quaternion.SLIMEVR.FRONT
 
-				else -> mountRot
+				else -> mountRot * Quaternion.SLIMEVR.FRONT
 			}
 			val actualMounting = tracker.resetsHandler.mountRotFix
 
@@ -118,5 +118,5 @@ class SkeletonResetTests {
 		}
 	}
 
-	fun mkTrackMount(rot: Quaternion): Quaternion = rot * (TrackerTestUtils.frontRot / rot)
+	fun mkTrackMount(rot: Quaternion): Quaternion = rot * TrackerTestUtils.frontRot / rot
 }