diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/BoneType.java b/src/main/java/dev/slimevr/vr/processor/skeleton/BoneType.java index 5f742844b4..85b26165ec 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/BoneType.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/BoneType.java @@ -37,8 +37,8 @@ public enum BoneType { UPPER_ARM, LEFT_UPPER_ARM(BodyPart.LEFT_UPPER_ARM), RIGHT_UPPER_ARM(BodyPart.RIGHT_UPPER_ARM), - LEFT_SHOULDER, - RIGHT_SHOULDER, + LEFT_SHOULDER(BodyPart.LEFT_SHOULDER), + RIGHT_SHOULDER(BodyPart.RIGHT_SHOULDER), LEFT_HAND(BodyPart.LEFT_HAND), RIGHT_HAND(BodyPart.RIGHT_HAND), LEFT_HAND_TRACKER, diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java index 1343d9cfe9..8374060993 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java @@ -26,8 +26,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { protected final TransformNode trackerChestNode = new TransformNode("Chest-Tracker", false); protected final TransformNode waistNode = new TransformNode("Waist", false); protected final TransformNode hipNode = new TransformNode("Hip", false); - // #endregion protected final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false); + // #endregion // #region Lower body nodes (legs) protected final TransformNode leftHipNode = new TransformNode("Left-Hip", false); protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false); @@ -53,6 +53,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { "Right-Foot-Tracker", false ); + // #endregion // #region Arms (from controllers) protected final TransformNode leftControllerNodeContrl = new TransformNode( "Left-Controller-Contrl", @@ -87,7 +88,10 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { "Right-Elbow-Tracker-Contrl", false ); + // #endregion // #region Arms (from HMD) + protected final TransformNode leftNeckNode = new TransformNode("Left-Neck", false); + protected final TransformNode rightNeckNode = new TransformNode("Right-Neck", false); protected final TransformNode leftShoulderNodeHmd = new TransformNode( "Left-Shoulder-Hmd", false @@ -96,7 +100,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { "Right-Shoulder-Hmd", false ); - // #endregion protected final TransformNode leftElbowNodeHmd = new TransformNode("Left-Elbow-Hmd", false); protected final TransformNode rightElbowNodeHmd = new TransformNode("Right-Elbow-Hmd", false); protected final TransformNode trackerLeftElbowNodeHmd = new TransformNode( @@ -147,11 +150,13 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { protected Tracker rightControllerTracker; protected Tracker leftLowerArmTracker; protected Tracker rightLowerArmTracker; - // #endregion protected Tracker leftUpperArmTracker; protected Tracker rightUpperArmTracker; protected Tracker leftHandTracker; protected Tracker rightHandTracker; + protected Tracker leftShoulderTracker; + protected Tracker rightShoulderTracker; + // #endregion // #region Tracker Output protected ComputedHumanPoseTracker computedChestTracker; protected ComputedHumanPoseTracker computedWaistTracker; @@ -217,8 +222,11 @@ protected HumanSkeleton(List computedTracker // #endregion // #region Assemble skeleton arms from neck - neckNode.attachChild(leftShoulderNodeHmd); - neckNode.attachChild(rightShoulderNodeHmd); + neckNode.attachChild(leftNeckNode); + neckNode.attachChild(rightNeckNode); + + leftNeckNode.attachChild(leftShoulderNodeHmd); + rightNeckNode.attachChild(rightShoulderNodeHmd); leftShoulderNodeHmd.attachChild(leftElbowNodeHmd); rightShoulderNodeHmd.attachChild(rightElbowNodeHmd); @@ -456,6 +464,17 @@ public void setTrackersFromList(List trackers, boolean setHmd trackers, TrackerPosition.RIGHT_HAND ); + this.leftShoulderTracker = TrackerUtils + .findNonComputedHumanPoseTrackerForBodyPosition( + trackers, + TrackerPosition.LEFT_SHOULDER + ); + this.rightShoulderTracker = TrackerUtils + .findNonComputedHumanPoseTrackerForBodyPosition( + trackers, + TrackerPosition.RIGHT_SHOULDER + ); + resetBones(); } @@ -687,12 +706,14 @@ protected void updateLocalTransforms() { Tracker leftControllerTracker = trackerPreUpdate(this.leftControllerTracker); Tracker rightControllerTracker = trackerPreUpdate(this.rightControllerTracker); - Tracker rightLowerArmTracker = trackerPreUpdate(this.rightLowerArmTracker); Tracker leftLowerArmTracker = trackerPreUpdate(this.leftLowerArmTracker); - Tracker rightUpperArmTracker = trackerPreUpdate(this.rightUpperArmTracker); + Tracker rightLowerArmTracker = trackerPreUpdate(this.rightLowerArmTracker); Tracker leftUpperArmTracker = trackerPreUpdate(this.leftUpperArmTracker); + Tracker rightUpperArmTracker = trackerPreUpdate(this.rightUpperArmTracker); Tracker leftHandTracker = trackerPreUpdate(this.leftHandTracker); Tracker rightHandTracker = trackerPreUpdate(this.rightHandTracker); + Tracker leftShoulderTracker = trackerPreUpdate(this.leftShoulderTracker); + Tracker rightShoulderTracker = trackerPreUpdate(this.rightShoulderTracker); // #endregion // hasSomething booleans @@ -975,18 +996,35 @@ protected void updateLocalTransforms() { } // Left arm from HMD - if (leftUpperArmTracker != null || leftLowerArmTracker != null) { + if ( + leftShoulderTracker != null + || leftUpperArmTracker != null + || leftLowerArmTracker != null + ) { + if (leftShoulderTracker != null) { + leftShoulderTracker.getRotation(rotBuf1); + } else { + neckNode.localTransform.getRotation(rotBuf1); + } + leftNeckNode.localTransform.setRotation(rotBuf1); + TrackerUtils - .getFirstAvailableTracker(leftUpperArmTracker, leftLowerArmTracker, null) + .getFirstAvailableTracker( + leftUpperArmTracker, + leftShoulderTracker, + leftLowerArmTracker + ) .getRotation(rotBuf1); - leftShoulderNodeHmd.localTransform.setRotation(rotBuf1); trackerLeftElbowNodeHmd.localTransform.setRotation(rotBuf1); TrackerUtils - .getFirstAvailableTracker(leftLowerArmTracker, leftUpperArmTracker, null) + .getFirstAvailableTracker( + leftLowerArmTracker, + leftUpperArmTracker, + leftShoulderTracker + ) .getRotation(rotBuf1); - leftElbowNodeHmd.localTransform.setRotation(rotBuf1); } if (leftHandTracker != null) { @@ -997,18 +1035,35 @@ protected void updateLocalTransforms() { } // Right arm from HMD - if (rightUpperArmTracker != null || rightLowerArmTracker != null) { + if ( + rightShoulderTracker != null + || rightUpperArmTracker != null + || rightLowerArmTracker != null + ) { + if (rightShoulderTracker != null) { + rightShoulderTracker.getRotation(rotBuf1); + } else { + neckNode.localTransform.getRotation(rotBuf1); + } + rightNeckNode.localTransform.setRotation(rotBuf1); + TrackerUtils - .getFirstAvailableTracker(rightUpperArmTracker, rightLowerArmTracker, null) + .getFirstAvailableTracker( + rightUpperArmTracker, + rightShoulderTracker, + rightLowerArmTracker + ) .getRotation(rotBuf1); - rightShoulderNodeHmd.localTransform.setRotation(rotBuf1); trackerRightElbowNodeHmd.localTransform.setRotation(rotBuf1); TrackerUtils - .getFirstAvailableTracker(rightLowerArmTracker, rightUpperArmTracker, null) + .getFirstAvailableTracker( + rightLowerArmTracker, + rightUpperArmTracker, + rightShoulderTracker + ) .getRotation(rotBuf1); - rightElbowNodeHmd.localTransform.setRotation(rotBuf1); } if (rightHandTracker != null) { @@ -1018,7 +1073,7 @@ protected void updateLocalTransforms() { trackerRightHandNodeHmd.localTransform.setRotation(rotBuf1); } - // Left elbow from SteamVR controller + // Left elbow from controller if (leftControllerTracker != null) { leftControllerTracker.getPosition(posBuf); leftControllerTracker.getRotation(rotBuf1); @@ -1041,7 +1096,7 @@ protected void updateLocalTransforms() { } } - // Right elbow from SteamVR controller + // Right elbow from controller if (rightControllerTracker != null) { rightControllerTracker.getPosition(posBuf); rightControllerTracker.getRotation(rotBuf1); @@ -1538,7 +1593,10 @@ protected Tracker[] getTrackersToReset() { trackerPreUpdate(this.leftUpperArmTracker), trackerPreUpdate(this.rightUpperArmTracker), trackerPreUpdate(this.leftHandTracker), - trackerPreUpdate(this.rightHandTracker) }; + trackerPreUpdate(this.rightHandTracker), + trackerPreUpdate(this.leftShoulderTracker), + trackerPreUpdate(this.rightShoulderTracker), + }; } @Override diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java b/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java index 687c70757e..20ff59bde7 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java @@ -36,7 +36,9 @@ public enum TrackerPosition { LEFT_UPPER_ARM("body:left_upper_arm", TrackerRole.LEFT_SHOULDER, BodyPart.LEFT_UPPER_ARM), RIGHT_UPPER_ARM("body:right_upper_arm", TrackerRole.RIGHT_SHOULDER, BodyPart.RIGHT_UPPER_ARM), LEFT_HAND("body:left_hand", TrackerRole.LEFT_HAND, BodyPart.LEFT_HAND), - RIGHT_HAND("body:right_hand", TrackerRole.RIGHT_HAND, BodyPart.RIGHT_HAND),; + RIGHT_HAND("body:right_hand", TrackerRole.RIGHT_HAND, BodyPart.RIGHT_HAND), + LEFT_SHOULDER("body:left_shoulder", TrackerRole.LEFT_SHOULDER, BodyPart.LEFT_SHOULDER), + RIGHT_SHOULDER("body:right_shoulder", TrackerRole.RIGHT_SHOULDER, BodyPart.RIGHT_SHOULDER); // @formatter:on public static final TrackerPosition[] values = values();