Fix acceleration axis

Author: 9il

.gitignore

@@ -46,3 +46,4 @@ local.properties
 
 # Ignore temporary config
 vrconfig.yml.tmp
+*.DS_Store

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

@@ -6,6 +6,7 @@ import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByDesignation
 import dev.slimevr.tracking.trackers.udp.IMUType
 import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
 import dev.slimevr.tracking.trackers.udp.TrackerDataType
+import io.eiren.math.FloatMath.INV_SQRT_TWO
 import io.eiren.util.BufferedTimer
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
@@ -437,4 +438,13 @@ class Tracker @JvmOverloads constructor(
 	fun resetFilteringQuats() {
 		filteringHandler.resetMovingAverage(_rotation)
 	}
+
+	companion object {
+		/**
+		 * Changes from IMU axis to OpenGL/SteamVR axis
+		 */
+		fun axisOffset(v: Vector3): Vector3 = Vector3(v.x, v.z, -v.y)
+		private val rotationOffset = Quaternion(INV_SQRT_TWO, -INV_SQRT_TWO, 0f, 0f)
+		fun axisOffset(q: Quaternion): Quaternion = rotationOffset * q
+	}
 }

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

@@ -1,16 +1,14 @@
 package dev.slimevr.tracking.trackers.udp
 
-import com.jme3.math.FastMath
 import dev.slimevr.NetworkProtocol
 import dev.slimevr.VRServer
 import dev.slimevr.config.config
 import dev.slimevr.protocol.rpc.MAG_TIMEOUT
 import dev.slimevr.tracking.trackers.*
+import dev.slimevr.tracking.trackers.Tracker.Companion.axisOffset
 import io.eiren.util.Util
 import io.eiren.util.collections.FastList
 import io.eiren.util.logging.LogManager
-import io.github.axisangles.ktmath.Quaternion.Companion.fromRotationVector
-import io.github.axisangles.ktmath.Vector3
 import kotlinx.coroutines.*
 import org.apache.commons.lang3.ArrayUtils
 import solarxr_protocol.rpc.ResetType
@@ -387,13 +385,12 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 
 			is RotationPacket -> {
 				var rot = packet.rotation
-				rot = AXES_OFFSET.times(rot)
+				rot = axisOffset(rot)
 				tracker = connection?.getTracker(packet.sensorId)
 				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))
+					tracker.setAcceleration(axisOffset(packet.acceleration))
 				}
 				tracker.dataTick()
 			}
@@ -402,7 +399,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 				tracker = connection?.getTracker(packet.sensorId)
 				if (tracker == null) return
 				var rot17 = packet.rotation
-				rot17 = AXES_OFFSET * rot17
+				rot17 = axisOffset(rot17)
 				when (packet.dataType) {
 					UDPPacket17RotationData.DATA_TYPE_NORMAL -> {
 						tracker.setRotation(rot17)
@@ -425,8 +422,7 @@ 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))
+				tracker.setAcceleration(axisOffset(packet.acceleration))
 			}
 
 			is UDPPacket10PingPong -> {
@@ -600,10 +596,6 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 	}
 
 	companion object {
-		/**
-		 * Change between IMU axes and OpenGL/SteamVR axes
-		 */
-		private val AXES_OFFSET = fromRotationVector(-FastMath.HALF_PI, 0f, 0f)
 		private const val RESET_SOURCE_NAME = "TrackerServer"
 		private fun packetToString(packet: DatagramPacket?): String {
 			val sb = StringBuilder()

server/core/src/main/java/io/eiren/math/FloatMath.kt

@@ -17,7 +17,7 @@ object FloatMath {
 	const val ZERO_TOLERANCE_D: Double = 0.0001
 
 	val SQRT_TWO: Float = kotlin.math.sqrt(2.0).toFloat()
-	val INV_SQRT_TWO: Float = 1f / SQRT_TWO
+	val INV_SQRT_TWO: Float = (1.0 / kotlin.math.sqrt(2.0)).toFloat()
 	val SQRT_THREE: Float = kotlin.math.sqrt(3.0).toFloat()
 	val INV_SQRT_THREE: Float = 1f / SQRT_THREE
 	const val TWO_FPI: Float = PI * 2

server/desktop/src/main/java/dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt

@@ -1,16 +1,15 @@
 package dev.slimevr.desktop.tracking.trackers.hid
 
-import com.jme3.math.FastMath
 import dev.slimevr.VRServer
 import dev.slimevr.tracking.trackers.Device
 import dev.slimevr.tracking.trackers.Tracker
+import dev.slimevr.tracking.trackers.Tracker.Companion.axisOffset
 import dev.slimevr.tracking.trackers.TrackerStatus
 import dev.slimevr.tracking.trackers.udp.BoardType
 import dev.slimevr.tracking.trackers.udp.IMUType
 import dev.slimevr.tracking.trackers.udp.MCUType
 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 org.hid4java.HidDevice
 import org.hid4java.HidException
@@ -389,7 +388,7 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 							// x y z w -> w x y z
 							var rot = Quaternion(q[3].toFloat(), q[0].toFloat(), q[1].toFloat(), q[2].toFloat())
 							val scaleRot = 1 / (1 shl 15).toFloat() // compile time evaluation
-							rot = AXES_OFFSET.times(scaleRot).times(rot) // no division
+							rot = axisOffset(rot * scaleRot) // no division
 							tracker.setRotation(rot)
 						}
 						if (packetType == 2) {
@@ -408,13 +407,14 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 							val s = sin(a)
 							val k = s * invSqrtD
 							var rot = Quaternion(cos(a), k * v[0], k * v[1], k * v[2])
-							rot = AXES_OFFSET.times(rot) // no division
+							rot = axisOffset(rot) // no division
 							tracker.setRotation(rot)
 						}
 						if (packetType == 1 || packetType == 2) {
 							// TODO: Acceleration "was" wrong
 							val scaleAccel = 1 / (1 shl 7).toFloat() // compile time evaluation
 							var acceleration = Vector3(a[0].toFloat(), a[1].toFloat(), a[2].toFloat()).times(scaleAccel) // no division
+							acceleration = axisOffset(acceleration)
 							tracker.setAcceleration(acceleration)
 							tracker.dataTick() // only data tick if there is rotation data
 						}
@@ -481,10 +481,6 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 	}
 
 	companion object {
-		/**
-		 * Change between IMU axes and OpenGL/SteamVR axes
-		 */
-		private val AXES_OFFSET = fromRotationVector(-FastMath.HALF_PI, 0f, 0f)
 		private const val resetSourceName = "TrackerServer"
 	}
 }