Fix acceleration axis and leg tweaks

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

@@ -4,6 +4,7 @@ import dev.slimevr.VRServer
 import dev.slimevr.config.TrackerConfig
 import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByDesignation
 import dev.slimevr.tracking.trackers.udp.IMUType
+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
@@ -382,4 +383,12 @@ class Tracker @JvmOverloads constructor(
 	 */
 	val tps: Float
 		get() = timer.averageFPS
+
+	companion object {
+		/**
+		 * Changes from IMU axis to OpenGL/SteamVR axis
+		 */
+		fun axisOffset(v: Vector3): Vector3 = Vector3(v.x, v.z, -v.y)
+		fun axisOffset(q: Quaternion): Quaternion = Quaternion(INV_SQRT_TWO, -INV_SQRT_TWO, 0f, 0f) * q
+	}
 }

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

@@ -1,15 +1,13 @@
 package dev.slimevr.tracking.trackers.udp
 
-import com.jme3.math.FastMath
 import dev.slimevr.NetworkProtocol
 import dev.slimevr.VRServer
 import dev.slimevr.tracking.trackers.Tracker
+import dev.slimevr.tracking.trackers.Tracker.Companion.axisOffset
 import dev.slimevr.tracking.trackers.TrackerStatus
 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 org.apache.commons.lang3.ArrayUtils
 import solarxr_protocol.rpc.ResetType
 import java.net.DatagramPacket
@@ -317,13 +315,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()
 			}
@@ -332,7 +329,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)
@@ -355,8 +352,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 -> {
@@ -495,10 +491,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,14 +1,13 @@
 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.IMUType
 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.HidManager
@@ -236,13 +235,14 @@ 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)
 						// TODO: I think the acceleration is wrong???
 						// Yes it was. And rotation was wrong too.
 						// At lease we have fixed the fixed point decoding.
 						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()
 						i += 20
@@ -305,10 +305,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"
 	}
 }