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 index 6a3f6f9d97..3d1ed10899 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt +++ b/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 axisOffet(v: Vector3): Vector3 = Vector3(v.x, v.z, -v.y) + fun axisOffet(q: Quaternion): Quaternion = Quaternion(INV_SQRT_TWO, -INV_SQRT_TWO, 0f, 0f) * q + } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt index ea19fb6f22..45f79af213 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt +++ b/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.axisOffet 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 = axisOffet(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(axisOffet(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 = axisOffet(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(axisOffet(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() diff --git a/server/core/src/main/java/io/eiren/math/FloatMath.kt b/server/core/src/main/java/io/eiren/math/FloatMath.kt index 1442bf19e3..bfcada158f 100644 --- a/server/core/src/main/java/io/eiren/math/FloatMath.kt +++ b/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 diff --git a/server/desktop/src/main/java/dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt b/server/desktop/src/main/java/dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt index d5f0cf9049..284432fa1f 100644 --- a/server/desktop/src/main/java/dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt +++ b/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.axisOffet 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,15 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer) // 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 = rot.times(scaleRot) // no division + rot = axisOffet(rot) 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 = axisOffet(acceleration) tracker.setAcceleration(acceleration) tracker.dataTick() i += 20 @@ -305,10 +306,6 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer) } 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" } }