Add default mounting orientation for certain body parts (#898)

SlimeVR · Nov 19, 2023 · d2a9a21 · d2a9a21
1 parent 2dbc25b
commit d2a9a21
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 0 deletions.
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
@@ -110,6 +110,9 @@ class Tracker @JvmOverloads constructor(
 		if (old == new) return@observable
 
 		if (!isInternal) {
+			// Set default mounting position for that body part
+			new?.let { resetsHandler.mountingOrientation = it.defaultMounting() }
+
 			checkReportRequireReset()
 		}
 	}

diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerPosition.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerPosition.kt
@@ -1,5 +1,6 @@
 package dev.slimevr.tracking.trackers
 
+import io.github.axisangles.ktmath.Quaternion
 import solarxr_protocol.datatypes.BodyPart
 
 /**
@@ -35,6 +36,17 @@ enum class TrackerPosition(
 	RIGHT_SHOULDER("body:right_shoulder", TrackerRole.RIGHT_SHOULDER, BodyPart.RIGHT_SHOULDER),
 	;
 
+	/**
+	 * Returns the default mounting position for the body part
+	 */
+	fun defaultMounting(): Quaternion = when (this) {
+		LEFT_LOWER_ARM, LEFT_HAND -> Quaternion.SLIMEVR.LEFT
+		RIGHT_LOWER_ARM, RIGHT_HAND -> Quaternion.SLIMEVR.RIGHT
+		LEFT_UPPER_ARM, LEFT_LOWER_LEG -> Quaternion.SLIMEVR.FRONT_LEFT
+		RIGHT_UPPER_ARM, RIGHT_LOWER_LEG -> Quaternion.SLIMEVR.FRONT_RIGHT
+		else -> Quaternion.SLIMEVR.FRONT
+	}
+
 	companion object {
 		/** Indexed by `BodyPart` int value. EFFICIENCY FTW  */
 		private val byBodyPart: Array<out TrackerPosition?> = arrayOfNulls<TrackerPosition>(BodyPart.names.size).apply {

diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt
@@ -12,6 +12,11 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
 		val J = Quaternion(0f, 0f, 1f, 0f)
 		val K = Quaternion(0f, 0f, 0f, 1f)
 
+		/**
+		 * SlimeVR-specific constants and utils
+		 */
+		val SLIMEVR: SlimeVR = SlimeVR
+
 		/**
 		 * creates a new quaternion representing the rotation about v's axis
 		 * by an angle of v's length
@@ -45,6 +50,20 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
 
 			return (D + D.len()).unit()
 		}
+
+		/**
+		 * SlimeVR-specific constants and utils
+		 */
+		object SlimeVR {
+			val FRONT = Quaternion(0f, 0f, 1f, 0f)
+			val FRONT_LEFT = Quaternion(0.383f, 0f, 0.924f, 0f)
+			val LEFT = Quaternion(0.707f, 0f, 0.707f, 0f)
+			val BACK_LEFT = Quaternion(0.924f, 0f, 0.383f, 0f)
+			val FRONT_RIGHT = Quaternion(0.383f, 0f, -0.924f, 0f)
+			val RIGHT = Quaternion(0.707f, 0f, -0.707f, 0f)
+			val BACK_RIGHT = Quaternion(0.924f, 0f, -0.383f, 0f)
+			val BACK = Quaternion(1f, 0f, 0f, 0f)
+		}
 	}
 
 	/**