diff --git a/server/core/src/main/java/dev/slimevr/platform/SteamVRBridge.java b/server/core/src/main/java/dev/slimevr/platform/SteamVRBridge.java index 6713b7f4f..73b9f0ca1 100644 --- a/server/core/src/main/java/dev/slimevr/platform/SteamVRBridge.java +++ b/server/core/src/main/java/dev/slimevr/platform/SteamVRBridge.java @@ -160,7 +160,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.getByTrackerRole(role) ) @@ -171,7 +171,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.WAIST ) @@ -182,7 +182,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.CHEST ) @@ -190,7 +190,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.UPPER_CHEST ) @@ -202,7 +202,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.CHEST ) @@ -213,7 +213,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.WAIST ) @@ -221,7 +221,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.HIP ) @@ -232,7 +232,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.LEFT_LOWER_LEG ) @@ -243,7 +243,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.LEFT_UPPER_LEG ) @@ -254,7 +254,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.RIGHT_LOWER_LEG ) @@ -265,7 +265,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.RIGHT_UPPER_LEG ) @@ -276,7 +276,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.LEFT_LOWER_ARM ) @@ -284,7 +284,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.LEFT_SHOULDER ) @@ -294,7 +294,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.RIGHT_LOWER_ARM ) @@ -302,7 +302,7 @@ public abstract class SteamVRBridge extends ProtobufBridge implements Runnable { batteryTrackers .add( TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( allTrackers, TrackerPosition.RIGHT_SHOULDER ) diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java b/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java index 14b8211c5..030427eb9 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java @@ -245,7 +245,7 @@ public class RPCHandler extends ProtocolHandler TrackerPosition pos = TrackerPosition.getByBodyPart(req.bodyPosition()); Tracker previousTracker = pos != null ? TrackerUtils - .getNonInternalTrackerForBodyPosition(this.api.server.getAllTrackers(), pos) + .getTrackerForSkeleton(this.api.server.getAllTrackers(), pos) : null; if (previousTracker != null) { previousTracker.setTrackerPosition(null); diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java index 980b18032..8dd9b41ed 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java @@ -386,104 +386,103 @@ public class HumanSkeleton { // #region Set trackers inputs protected void setTrackersFromList(List trackers) { - // TODO prioritize IMU over Computed for head headTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.HEAD ); neckTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.NECK ); upperChestTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.UPPER_CHEST ); chestTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.CHEST ); waistTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.WAIST ); hipTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.HIP ); leftUpperLegTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.LEFT_UPPER_LEG ); leftLowerLegTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.LEFT_LOWER_LEG ); leftFootTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.LEFT_FOOT ); rightUpperLegTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.RIGHT_UPPER_LEG ); rightLowerLegTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.RIGHT_LOWER_LEG ); rightFootTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.RIGHT_FOOT ); leftLowerArmTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.LEFT_LOWER_ARM ); rightLowerArmTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.RIGHT_LOWER_ARM ); leftUpperArmTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.LEFT_UPPER_ARM ); rightUpperArmTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.RIGHT_UPPER_ARM ); leftHandTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.LEFT_HAND ); rightHandTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.RIGHT_HAND ); leftShoulderTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.LEFT_SHOULDER ); rightShoulderTracker = TrackerUtils - .getNonInternalTrackerForBodyPosition( + .getTrackerForSkeleton( trackers, TrackerPosition.RIGHT_SHOULDER ); diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerUtils.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerUtils.kt index 37cd22bc5..d04f3a8e7 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerUtils.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerUtils.kt @@ -2,14 +2,33 @@ package dev.slimevr.tracking.trackers object TrackerUtils { + /** + * Finds a suitable tracker for use in the SlimeVR skeleton + * in allTrackers matching the position. + * This won't return disconnected, errored or internal trackers. + * + * @return A tracker for use in the SlimeVR skeleton + */ + @JvmStatic + fun getTrackerForSkeleton( + allTrackers: List, + position: TrackerPosition, + ): Tracker? { + if (position == TrackerPosition.HEAD) { + // Prioritize non-computed + val head = getNonInternalNonComputedTrackerForBodyPosition(allTrackers, position) + if (head != null) return head + } + return getNonInternalTrackerForBodyPosition(allTrackers, position) + } + /** * Finds the first non-internal tracker from allTrackers * matching the position, that is not DISCONNECTED * * @return A non-internal tracker */ - @JvmStatic - fun getNonInternalTrackerForBodyPosition( + private fun getNonInternalTrackerForBodyPosition( allTrackers: List, position: TrackerPosition, ): Tracker? = allTrackers.firstOrNull { @@ -19,6 +38,23 @@ object TrackerUtils { it.status != TrackerStatus.ERROR } + /** + * Finds the first non-internal non-computed tracker from allTrackers + * matching the position, that is not DISCONNECTED. + * + * @return A non-internal non-computed tracker + */ + private fun getNonInternalNonComputedTrackerForBodyPosition( + allTrackers: List, + position: TrackerPosition, + ): Tracker? = allTrackers.firstOrNull { + it.trackerPosition === position && + !it.isComputed && + !it.isInternal && + it.status != TrackerStatus.DISCONNECTED && + it.status != TrackerStatus.ERROR + } + /** * Returns the first tracker that isn't null out of the 2 trackers passed as * arguments.