Skip to content

Commit 7ef0d59

Browse files
9il9il
committed
Fix acceleration axis
1 parent 046be5f commit 7ef0d59

File tree

8 files changed

+43
-26
lines changed

8 files changed

+43
-26
lines changed

‎server/core/src/main/java/dev/slimevr/VRServer.kt

Lines changed: 16 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -308,12 +308,26 @@ class VRServer @JvmOverloads constructor(
308308
queueTask { humanPoseManager.clearTrackersMounting(resetSourceName) }
309309
}
310310

311+
fun getPauseTracking(): Boolean = humanPoseManager.getPauseTracking()
312+
311313
fun setPauseTracking(pauseTracking: Boolean, sourceName: String?) {
312-
queueTask { humanPoseManager.setPauseTracking(pauseTracking, sourceName) }
314+
queueTask {
315+
humanPoseManager.setPauseTracking(pauseTracking, sourceName)
316+
// Toggle trackers as they don't toggle when tracking is paused
317+
if (this.getVRBridge(ISteamVRBridge::class.java)?.updateShareSettingsAutomatically() == true) {
318+
RPCSettingsHandler.sendSteamVRUpdatedSettings(protocolAPI, protocolAPI.rpcHandler)
319+
}
320+
}
313321
}
314322

315323
fun togglePauseTracking(sourceName: String?) {
316-
queueTask { humanPoseManager.togglePauseTracking(sourceName) }
324+
queueTask {
325+
humanPoseManager.togglePauseTracking(sourceName)
326+
// Toggle trackers as they don't toggle when tracking is paused
327+
if (this.getVRBridge(ISteamVRBridge::class.java)?.updateShareSettingsAutomatically() == true) {
328+
RPCSettingsHandler.sendSteamVRUpdatedSettings(protocolAPI, protocolAPI.rpcHandler)
329+
}
330+
}
317331
}
318332

319333
fun scheduleResetTrackersFull(resetSourceName: String?, delay: Long) {

‎server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -461,7 +461,7 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
461461
val req = messageHeader
462462
.message(SetPauseTrackingRequest()) as? SetPauseTrackingRequest ?: return
463463

464-
api.server.humanPoseManager.setPauseTracking(req.pauseTracking(), RESET_SOURCE_NAME)
464+
api.server.setPauseTracking(req.pauseTracking(), RESET_SOURCE_NAME)
465465
}
466466

467467
fun onHeightRequest(conn: GenericConnection, messageHeader: RpcMessageHeader?) {

‎server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -690,6 +690,8 @@ class HumanPoseManager(val server: VRServer?) {
690690
get() = skeletonConfigManager.userHeightFromOffsets
691691

692692
// #endregion
693+
fun getPauseTracking(): Boolean = skeleton.getPauseTracking()
694+
693695
fun setPauseTracking(pauseTracking: Boolean, sourceName: String?) {
694696
skeleton.setPauseTracking(pauseTracking, sourceName)
695697
}

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

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ import dev.slimevr.VRServer
44
import dev.slimevr.config.TrackerConfig
55
import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByDesignation
66
import dev.slimevr.tracking.trackers.udp.IMUType
7+
import io.eiren.math.FloatMath.INV_SQRT_TWO
78
import io.eiren.util.BufferedTimer
89
import io.github.axisangles.ktmath.Quaternion
910
import io.github.axisangles.ktmath.Vector3
@@ -382,4 +383,12 @@ class Tracker @JvmOverloads constructor(
382383
*/
383384
val tps: Float
384385
get() = timer.averageFPS
386+
387+
companion object {
388+
/**
389+
* Changes from IMU axis to OpenGL/SteamVR axis
390+
*/
391+
fun axisOffet(v: Vector3): Vector3 = Vector3(v.x, v.z, -v.y)
392+
fun axisOffet(q: Quaternion): Quaternion = Quaternion(INV_SQRT_TWO, -INV_SQRT_TWO, 0f, 0f) * q
393+
}
385394
}

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

Lines changed: 5 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,13 @@
11
package dev.slimevr.tracking.trackers.udp
22

3-
import com.jme3.math.FastMath
43
import dev.slimevr.NetworkProtocol
54
import dev.slimevr.VRServer
65
import dev.slimevr.tracking.trackers.Tracker
6+
import dev.slimevr.tracking.trackers.Tracker.Companion.axisOffet
77
import dev.slimevr.tracking.trackers.TrackerStatus
88
import io.eiren.util.Util
99
import io.eiren.util.collections.FastList
1010
import io.eiren.util.logging.LogManager
11-
import io.github.axisangles.ktmath.Quaternion.Companion.fromRotationVector
12-
import io.github.axisangles.ktmath.Vector3
1311
import org.apache.commons.lang3.ArrayUtils
1412
import solarxr_protocol.rpc.ResetType
1513
import java.net.DatagramPacket
@@ -317,13 +315,12 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
317315

318316
is RotationPacket -> {
319317
var rot = packet.rotation
320-
rot = AXES_OFFSET.times(rot)
318+
rot = axisOffet(rot)
321319
tracker = connection?.getTracker(packet.sensorId)
322320
if (tracker == null) return
323321
tracker.setRotation(rot)
324322
if (packet is UDPPacket23RotationAndAcceleration) {
325-
// Switch x and y around to adjust for different axes
326-
tracker.setAcceleration(Vector3(packet.acceleration.y, packet.acceleration.x, packet.acceleration.z))
323+
tracker.setAcceleration(axisOffet(packet.acceleration))
327324
}
328325
tracker.dataTick()
329326
}
@@ -332,7 +329,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
332329
tracker = connection?.getTracker(packet.sensorId)
333330
if (tracker == null) return
334331
var rot17 = packet.rotation
335-
rot17 = AXES_OFFSET * rot17
332+
rot17 = axisOffet(rot17)
336333
when (packet.dataType) {
337334
UDPPacket17RotationData.DATA_TYPE_NORMAL -> {
338335
tracker.setRotation(rot17)
@@ -355,8 +352,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
355352
is UDPPacket4Acceleration -> {
356353
tracker = connection?.getTracker(packet.sensorId)
357354
if (tracker == null) return
358-
// Switch x and y around to adjust for different axes
359-
tracker.setAcceleration(Vector3(packet.acceleration.y, packet.acceleration.x, packet.acceleration.z))
355+
tracker.setAcceleration(axisOffet(packet.acceleration))
360356
}
361357

362358
is UDPPacket10PingPong -> {
@@ -495,10 +491,6 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
495491
}
496492

497493
companion object {
498-
/**
499-
* Change between IMU axes and OpenGL/SteamVR axes
500-
*/
501-
private val AXES_OFFSET = fromRotationVector(-FastMath.HALF_PI, 0f, 0f)
502494
private const val RESET_SOURCE_NAME = "TrackerServer"
503495
private fun packetToString(packet: DatagramPacket?): String {
504496
val sb = StringBuilder()

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ object FloatMath {
1717
const val ZERO_TOLERANCE_D: Double = 0.0001
1818

1919
val SQRT_TWO: Float = kotlin.math.sqrt(2.0).toFloat()
20-
val INV_SQRT_TWO: Float = 1f / SQRT_TWO
20+
val INV_SQRT_TWO: Float = (1.0 / kotlin.math.sqrt(2.0)).toFloat()
2121
val SQRT_THREE: Float = kotlin.math.sqrt(3.0).toFloat()
2222
val INV_SQRT_THREE: Float = 1f / SQRT_THREE
2323
const val TWO_FPI: Float = PI * 2

‎server/desktop/src/main/java/dev/slimevr/desktop/platform/SteamVRBridge.kt

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ import solarxr_protocol.rpc.StatusDataUnion
2020
import solarxr_protocol.rpc.StatusSteamVRDisconnectedT
2121

2222
abstract class SteamVRBridge(
23-
server: VRServer,
23+
protected val server: VRServer,
2424
threadName: String,
2525
bridgeName: String,
2626
protected val bridgeSettingsKey: String,
@@ -53,7 +53,9 @@ abstract class SteamVRBridge(
5353
}
5454

5555
override fun updateShareSettingsAutomatically(): Boolean {
56-
if (!config.automaticSharedTrackersToggling) return false
56+
// Return false if automatic trackers is disabled or if tracking is paused
57+
if (!config.automaticSharedTrackersToggling || server.getPauseTracking()) return false
58+
5759
val skeleton = instance.humanPoseManager.skeleton
5860
val isWaistSteamVr = skeleton.hipTracker?.device?.isOpenVrDevice == true ||
5961
skeleton.waistTracker?.device?.isOpenVrDevice == true
@@ -110,6 +112,7 @@ abstract class SteamVRBridge(
110112
changeShareSettings(TrackerRole.RIGHT_ELBOW, hasRightElbow && !isRightElbowSteamVr)
111113

112114
// Hands aren't touched as they will override the controller's tracking
115+
// Return true to say that trackers were successfully toggled automatically
113116
return true
114117
}
115118

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

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,13 @@
11
package dev.slimevr.desktop.tracking.trackers.hid
22

3-
import com.jme3.math.FastMath
43
import dev.slimevr.VRServer
54
import dev.slimevr.tracking.trackers.Device
65
import dev.slimevr.tracking.trackers.Tracker
6+
import dev.slimevr.tracking.trackers.Tracker.Companion.axisOffet
77
import dev.slimevr.tracking.trackers.TrackerStatus
88
import dev.slimevr.tracking.trackers.udp.IMUType
99
import io.eiren.util.logging.LogManager
1010
import io.github.axisangles.ktmath.Quaternion
11-
import io.github.axisangles.ktmath.Quaternion.Companion.fromRotationVector
1211
import io.github.axisangles.ktmath.Vector3
1312
import org.hid4java.HidDevice
1413
import org.hid4java.HidManager
@@ -236,13 +235,15 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
236235
// x y z w -> w x y z
237236
var rot = Quaternion(q[3].toFloat(), q[0].toFloat(), q[1].toFloat(), q[2].toFloat())
238237
val scaleRot = 1 / (1 shl 15).toFloat() // compile time evaluation
239-
rot = AXES_OFFSET.times(scaleRot).times(rot) // no division
238+
rot = rot.times(scaleRot) // no division
239+
rot = axisOffet(rot)
240240
tracker.setRotation(rot)
241241
// TODO: I think the acceleration is wrong???
242242
// Yes it was. And rotation was wrong too.
243243
// At lease we have fixed the fixed point decoding.
244244
val scaleAccel = 1 / (1 shl 7).toFloat() // compile time evaluation
245245
var acceleration = Vector3(a[0].toFloat(), a[1].toFloat(), a[2].toFloat()).times(scaleAccel) // no division
246+
acceleration = axisOffet(acceleration)
246247
tracker.setAcceleration(acceleration)
247248
tracker.dataTick()
248249
i += 20
@@ -305,10 +306,6 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
305306
}
306307

307308
companion object {
308-
/**
309-
* Change between IMU axes and OpenGL/SteamVR axes
310-
*/
311-
private val AXES_OFFSET = fromRotationVector(-FastMath.HALF_PI, 0f, 0f)
312309
private const val resetSourceName = "TrackerServer"
313310
}
314311
}

0 commit comments

Comments
 (0)