From 5816289d646fd7d9eebf7ceb975a1350dbe8b8a9 Mon Sep 17 00:00:00 2001 From: Ilia Ki Date: Tue, 30 Apr 2024 04:13:52 +0700 Subject: [PATCH 1/3] Fix acceleration axis --- .../dev/slimevr/tracking/trackers/Tracker.kt | 9 +++++++++ .../tracking/trackers/udp/TrackersUDPServer.kt | 18 +++++------------- .../src/main/java/io/eiren/math/FloatMath.kt | 2 +- .../tracking/trackers/hid/TrackersHID.kt | 11 ++++------- 4 files changed, 19 insertions(+), 21 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 index 6a3f6f9d97..94231b13cc 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 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 + } } 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..8a5a56b2cf 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.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() 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..fe0734b398 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.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,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 = axisOffset(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 = axisOffset(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" } } From 46f01a4fb5af334f579e61c143cad27bd185a9d2 Mon Sep 17 00:00:00 2001 From: Ilia Ki Date: Sat, 11 May 2024 18:01:23 +0700 Subject: [PATCH 2/3] Apply suggestions from code review Co-authored-by: Uriel --- .../dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 fe0734b398..b4697ce7e4 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 @@ -235,7 +235,7 @@ 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 = rot.times(scaleRot) // no division + rot *= scaleRot // no division rot = axisOffset(rot) tracker.setRotation(rot) // TODO: I think the acceleration is wrong??? From 22c433f4454b56bdcab47eba5c7416ce91706e23 Mon Sep 17 00:00:00 2001 From: Ilia Ki Date: Wed, 22 May 2024 22:00:46 +0700 Subject: [PATCH 3/3] Update server/desktop/src/main/java/dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt Co-authored-by: Butterscotch! --- .../dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 b4697ce7e4..b509aed08a 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 @@ -235,8 +235,7 @@ 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 *= scaleRot // no division - rot = axisOffset(rot) + rot = axisOffset(rot * scaleRot) // no division tracker.setRotation(rot) // TODO: I think the acceleration is wrong??? // Yes it was. And rotation was wrong too.