From f49b2556ae5a0fa53dd21d95ecd0e7010463a5ef Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Sat, 4 Dec 2021 00:45:37 -0500 Subject: [PATCH] Add full tracker functionality to SimpleSkeleton --- .../java/dev/slimevr/autobone/AutoBone.java | 137 ++-- .../dev/slimevr/autobone/SimpleSkeleton.java | 374 --------- .../poserecorder/PoseFrameSkeleton.java | 71 ++ .../poserecorder/PoseFrameTracker.java | 6 +- .../slimevr/poserecorder/TrackerFrame.java | 6 +- .../slimevr/vr/processor/SimpleSkeleton.java | 726 ++++++++++++++++++ 6 files changed, 883 insertions(+), 437 deletions(-) delete mode 100644 src/main/java/dev/slimevr/autobone/SimpleSkeleton.java create mode 100644 src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java create mode 100644 src/main/java/dev/slimevr/vr/processor/SimpleSkeleton.java diff --git a/src/main/java/dev/slimevr/autobone/AutoBone.java b/src/main/java/dev/slimevr/autobone/AutoBone.java index 2b2f0d9fd..9d484d7c0 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/autobone/AutoBone.java @@ -1,12 +1,16 @@ package dev.slimevr.autobone; import java.util.HashMap; +import java.util.List; import java.util.Map; import java.util.Map.Entry; import java.util.function.Consumer; +import com.jme3.math.FastMath; import com.jme3.math.Vector3f; +import dev.slimevr.poserecorder.PoseFrameSkeleton; +import dev.slimevr.poserecorder.PoseFrameTracker; import dev.slimevr.poserecorder.PoseFrames; import dev.slimevr.poserecorder.TrackerFrame; import dev.slimevr.poserecorder.TrackerFrameData; @@ -18,6 +22,7 @@ import io.eiren.vr.processor.HumanSkeleton; import io.eiren.vr.processor.HumanSkeletonWithLegs; import io.eiren.vr.processor.HumanSkeletonWithWaist; import io.eiren.vr.trackers.TrackerPosition; +import io.eiren.vr.trackers.TrackerRole; import io.eiren.vr.trackers.TrackerUtils; public class AutoBone { @@ -86,21 +91,23 @@ public class AutoBone { reloadConfigValues(null); } - public void reloadConfigValues(TrackerFrame[] frame) { + public void reloadConfigValues(List trackers) { // Load torso configs staticConfigs.put("Head", server.config.getFloat("body.headShift", HumanSkeletonWithWaist.HEAD_SHIFT_DEFAULT)); staticConfigs.put("Neck", server.config.getFloat("body.neckLength", HumanSkeletonWithWaist.NECK_LENGTH_DEFAULT)); configs.put("Torso", server.config.getFloat("body.torsoLength", 0.7f)); - if(server.config.getBoolean("autobone.forceChestTracker", false) || (frame != null && TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.CHEST) != null) || TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerPosition.CHEST) != null) { + if(server.config.getBoolean("autobone.forceChestTracker", false) || (trackers != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.CHEST) != null)) { // If force enabled or has a chest tracker + staticConfigs.remove("Chest"); configs.put("Chest", server.config.getFloat("body.chestDistance", 0.35f)); } else { // Otherwise, make sure it's not used configs.remove("Chest"); staticConfigs.put("Chest", server.config.getFloat("body.chestDistance", 0.35f)); } - if(server.config.getBoolean("autobone.forceHipTracker", false) || (frame != null && TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.HIP) != null && TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.WAIST) != null) || (TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerPosition.HIP) != null && TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerPosition.WAIST) != null)) { + if(server.config.getBoolean("autobone.forceHipTracker", false) || (trackers != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.HIP) != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.WAIST) != null)) { // If force enabled or has a hip tracker and waist tracker + staticConfigs.remove("Waist"); configs.put("Waist", server.config.getFloat("body.waistDistance", 0.1f)); } else { // Otherwise, make sure it's not used @@ -112,6 +119,10 @@ public class AutoBone { staticConfigs.put("Hips width", server.config.getFloat("body.hipsWidth", HumanSkeletonWithLegs.HIPS_WIDTH_DEFAULT)); configs.put("Legs length", server.config.getFloat("body.legsLength", 0.84f)); configs.put("Knee height", server.config.getFloat("body.kneeHeight", 0.42f)); + + // Keep "feet" at ankles + staticConfigs.put("Foot length", 0f); + staticConfigs.put("Foot offset", 0f); } @ThreadSafe @@ -238,15 +249,12 @@ public class AutoBone { public float processFrames(PoseFrames frames, boolean calcInitError, float targetHeight, Consumer epochCallback) { final int frameCount = frames.getMaxFrameCount(); + + List trackers = frames.getTrackers(); + reloadConfigValues(trackers); // Reload configs and detect chest tracker from the first frame - final SimpleSkeleton skeleton1 = new SimpleSkeleton(configs, staticConfigs); - final TrackerFrame[] trackerBuffer1 = new TrackerFrame[frames.getTrackerCount()]; - - frames.getFrames(0, trackerBuffer1); - reloadConfigValues(trackerBuffer1); // Reload configs and detect chest tracker from the first frame - - final SimpleSkeleton skeleton2 = new SimpleSkeleton(configs, staticConfigs); - final TrackerFrame[] trackerBuffer2 = new TrackerFrame[frames.getTrackerCount()]; + final PoseFrameSkeleton skeleton1 = new PoseFrameSkeleton(trackers, null, configs, staticConfigs); + final PoseFrameSkeleton skeleton2 = new PoseFrameSkeleton(trackers, null, configs, staticConfigs); // If target height isn't specified, auto-detect if(targetHeight < 0f) { @@ -270,29 +278,31 @@ public class AutoBone { float sumError = 0f; int errorCount = 0; - float adjustRate = epoch >= 0 ? (float) (initialAdjustRate / Math.pow(adjustRateDecay, epoch)) : 0f; + float adjustRate = epoch >= 0 ? (initialAdjustRate / FastMath.pow(adjustRateDecay, epoch)) : 0f; for(int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) { for(int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) { - frames.getFrames(frameCursor, trackerBuffer1); - frames.getFrames(frameCursor + cursorOffset, trackerBuffer2); - + int frameCursor2 = frameCursor + cursorOffset; + skeleton1.setSkeletonConfigs(configs); skeleton2.setSkeletonConfigs(configs); - skeleton1.setPoseFromFrame(trackerBuffer1); - skeleton2.setPoseFromFrame(trackerBuffer2); + skeleton1.setCursor(frameCursor); + skeleton1.updatePose(); + + skeleton2.setCursor(frameCursor2); + skeleton2.updatePose(); float totalLength = getLengthSum(configs); float curHeight = getHeight(configs, staticConfigs); - float errorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - curHeight); + float errorDeriv = getErrorDeriv(frames, frameCursor, frameCursor2, skeleton1, skeleton2, targetHeight - curHeight); float error = errorFunc(errorDeriv); // In case of fire if(Float.isNaN(error) || Float.isInfinite(error)) { // Extinguish LogManager.log.warning("[AutoBone] Error value is invalid, resetting variables to recover"); - reloadConfigValues(trackerBuffer1); + reloadConfigValues(trackers); // Reset error sum values sumError = 0f; @@ -308,6 +318,11 @@ public class AutoBone { float adjustVal = error * adjustRate; + // If there is no adjustment whatsoever, skip this + if (adjustVal == 0f) { + continue; + } + for(Entry entry : configs.entrySet()) { // Skip adjustment if the epoch is before starting (for logging only) if(epoch < 0) { @@ -333,7 +348,7 @@ public class AutoBone { updateSkeletonBoneLength(skeleton1, skeleton2, entry.getKey(), newLength); float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight; - float newErrorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - newHeight); + float newErrorDeriv = getErrorDeriv(frames, frameCursor, frameCursor2, skeleton1, skeleton2, targetHeight - newHeight); if(newErrorDeriv < minError) { minError = newErrorDeriv; @@ -363,50 +378,50 @@ public class AutoBone { float finalHeight = getHeight(configs, staticConfigs); LogManager.log.info("[AutoBone] Target height: " + targetHeight + " New height: " + finalHeight); - return Math.abs(finalHeight - targetHeight); + return FastMath.abs(finalHeight - targetHeight); } // The change in position of the ankle over time - protected float getSlideErrorDeriv(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { - float slideLeft = skeleton1.getNodePosition(TrackerPosition.LEFT_ANKLE).distance(skeleton2.getNodePosition(TrackerPosition.LEFT_ANKLE)); - float slideRight = skeleton1.getNodePosition(TrackerPosition.RIGHT_ANKLE).distance(skeleton2.getNodePosition(TrackerPosition.RIGHT_ANKLE)); + protected float getSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) { + float slideLeft = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.distance(skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position); + float slideRight = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.distance(skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position); // Divide by 4 to halve and average, it's halved because you want to approach a midpoint, not the other point return (slideLeft + slideRight) / 4f; } // The offset between both feet at one instant and over time - protected float getOffsetErrorDeriv(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { - float skeleton1Left = skeleton1.getNodePosition(TrackerPosition.LEFT_ANKLE).getY(); - float skeleton1Right = skeleton1.getNodePosition(TrackerPosition.RIGHT_ANKLE).getY(); + protected float getOffsetErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) { + float skeleton1Left = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY(); + float skeleton1Right = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY(); - float skeleton2Left = skeleton2.getNodePosition(TrackerPosition.LEFT_ANKLE).getY(); - float skeleton2Right = skeleton2.getNodePosition(TrackerPosition.RIGHT_ANKLE).getY(); + float skeleton2Left = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY(); + float skeleton2Right = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY(); - float dist1 = Math.abs(skeleton1Left - skeleton1Right); - float dist2 = Math.abs(skeleton2Left - skeleton2Right); + float dist1 = FastMath.abs(skeleton1Left - skeleton1Right); + float dist2 = FastMath.abs(skeleton2Left - skeleton2Right); - float dist3 = Math.abs(skeleton1Left - skeleton2Right); - float dist4 = Math.abs(skeleton2Left - skeleton1Right); + float dist3 = FastMath.abs(skeleton1Left - skeleton2Right); + float dist4 = FastMath.abs(skeleton2Left - skeleton1Right); - float dist5 = Math.abs(skeleton1Left - skeleton2Left); - float dist6 = Math.abs(skeleton1Right - skeleton2Right); + float dist5 = FastMath.abs(skeleton1Left - skeleton2Left); + float dist6 = FastMath.abs(skeleton1Right - skeleton2Right); // Divide by 12 to halve and average, it's halved because you want to approach a midpoint, not the other point return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f; } // The distance from average human proportions - protected float getProportionErrorDeriv(SimpleSkeleton skeleton) { + protected float getProportionErrorDeriv(PoseFrameSkeleton skeleton) { Float neckLength = skeleton.getSkeletonConfig("Neck"); Float chestLength = skeleton.getSkeletonConfig("Chest"); Float torsoLength = skeleton.getSkeletonConfig("Torso"); Float legsLength = skeleton.getSkeletonConfig("Legs length"); Float kneeHeight = skeleton.getSkeletonConfig("Knee height"); - - float chestTorso = chestLength != null && torsoLength != null ? Math.abs((chestLength / torsoLength) - chestTorsoRatio) : 0f; - float legBody = legsLength != null && torsoLength != null && neckLength != null ? Math.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio) : 0f; - float kneeLeg = kneeHeight != null && legsLength != null ? Math.abs((kneeHeight / legsLength) - kneeLegRatio) : 0f; + + float chestTorso = chestLength != null && torsoLength != null ? FastMath.abs((chestLength / torsoLength) - chestTorsoRatio) : 0f; + float legBody = legsLength != null && torsoLength != null && neckLength != null ? FastMath.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio) : 0f; + float kneeLeg = kneeHeight != null && legsLength != null ? FastMath.abs((kneeHeight / legsLength) - kneeLegRatio) : 0f; if(legBody <= legBodyRatioRange) { legBody = 0f; @@ -418,18 +433,22 @@ public class AutoBone { } // The distance of any points to the corresponding absolute position - protected float getPositionErrorDeriv(TrackerFrame[] frame, SimpleSkeleton skeleton) { + protected float getPositionErrorDeriv(PoseFrames frames, int cursor, PoseFrameSkeleton skeleton) { float offset = 0f; int offsetCount = 0; - for(TrackerFrame trackerFrame : frame) { + List trackers = frames.getTrackers(); + for (int i = 0; i < trackers.size(); i++) { + PoseFrameTracker tracker = trackers.get(i); + + TrackerFrame trackerFrame = tracker.safeGetFrame(cursor); if(trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) { continue; } - Vector3f nodePos = skeleton.getNodePosition(trackerFrame.designation.designation); + Vector3f nodePos = skeleton.getComputedTracker(trackerFrame.designation.trackerRole).position; if(nodePos != null) { - offset += Math.abs(nodePos.distance(trackerFrame.position)); + offset += FastMath.abs(nodePos.distance(trackerFrame.position)); offsetCount++; } } @@ -438,41 +457,45 @@ public class AutoBone { } // The difference between offset of absolute position and the corresponding point over time - protected float getPositionOffsetErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { + protected float getPositionOffsetErrorDeriv(PoseFrames frames, int cursor1, int cursor2, PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) { float offset = 0f; int offsetCount = 0; - for(TrackerFrame trackerFrame1 : frame1) { + List trackers = frames.getTrackers(); + for (int i = 0; i < trackers.size(); i++) { + PoseFrameTracker tracker = trackers.get(i); + + TrackerFrame trackerFrame1 = tracker.safeGetFrame(cursor1); if(trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) { continue; } - TrackerFrame trackerFrame2 = TrackerUtils.findTrackerForBodyPosition(frame2, trackerFrame1.designation); + TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2); if(trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) { continue; } - Vector3f nodePos1 = skeleton1.getNodePosition(trackerFrame1.designation); + Vector3f nodePos1 = skeleton1.getComputedTracker(trackerFrame1.designation.trackerRole).position; if(nodePos1 == null) { continue; } - Vector3f nodePos2 = skeleton2.getNodePosition(trackerFrame2.designation); + Vector3f nodePos2 = skeleton2.getComputedTracker(trackerFrame2.designation.trackerRole).position; if(nodePos2 == null) { continue; } - float dist1 = Math.abs(nodePos1.distance(trackerFrame1.position)); - float dist2 = Math.abs(nodePos2.distance(trackerFrame2.position)); + float dist1 = FastMath.abs(nodePos1.distance(trackerFrame1.position)); + float dist2 = FastMath.abs(nodePos2.distance(trackerFrame2.position)); - offset += Math.abs(dist2 - dist1); + offset += FastMath.abs(dist2 - dist1); offsetCount++; } return offsetCount > 0 ? offset / offsetCount : 0f; } - protected float getErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, float heightChange) { + protected float getErrorDeriv(PoseFrames frames, int cursor1, int cursor2, PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2, float heightChange) { float totalError = 0f; float sumWeight = 0f; @@ -493,17 +516,17 @@ public class AutoBone { } if(heightErrorFactor > 0f) { - totalError += Math.abs(heightChange) * heightErrorFactor; + totalError += FastMath.abs(heightChange) * heightErrorFactor; sumWeight += heightErrorFactor; } if(positionErrorFactor > 0f) { - totalError += (getPositionErrorDeriv(frame1, skeleton1) + getPositionErrorDeriv(frame2, skeleton2) / 2f) * positionErrorFactor; + totalError += (getPositionErrorDeriv(frames, cursor1, skeleton1) + getPositionErrorDeriv(frames, cursor2, skeleton2) / 2f) * positionErrorFactor; sumWeight += positionErrorFactor; } if(positionOffsetErrorFactor > 0f) { - totalError += getPositionOffsetErrorDeriv(frame1, frame2, skeleton1, skeleton2) * positionOffsetErrorFactor; + totalError += getPositionOffsetErrorDeriv(frames, cursor1, cursor2, skeleton1, skeleton2) * positionOffsetErrorFactor; sumWeight += positionOffsetErrorFactor; } @@ -516,7 +539,7 @@ public class AutoBone { return 0.5f * (errorDeriv * errorDeriv); } - protected void updateSkeletonBoneLength(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, String joint, float newLength) { + protected void updateSkeletonBoneLength(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2, String joint, float newLength) { skeleton1.setSkeletonConfig(joint, newLength, true); skeleton2.setSkeletonConfig(joint, newLength, true); } diff --git a/src/main/java/dev/slimevr/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/autobone/SimpleSkeleton.java deleted file mode 100644 index 3ff1c3706..000000000 --- a/src/main/java/dev/slimevr/autobone/SimpleSkeleton.java +++ /dev/null @@ -1,374 +0,0 @@ -package dev.slimevr.autobone; - -import java.util.HashMap; -import java.util.Map; - -import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; - -import dev.slimevr.poserecorder.TrackerFrame; -import dev.slimevr.poserecorder.TrackerFrameData; -import io.eiren.vr.processor.HumanSkeletonWithLegs; -import io.eiren.vr.processor.HumanSkeletonWithWaist; -import io.eiren.vr.processor.TransformNode; -import io.eiren.vr.trackers.TrackerPosition; -import io.eiren.vr.trackers.TrackerUtils; -import io.eiren.yaml.YamlFile; - -public class SimpleSkeleton { - - // Upper body (torso) - protected final TransformNode hmdNode = new TransformNode("HMD", false); - protected final TransformNode headNode = new TransformNode("Head", false); - protected final TransformNode neckNode = new TransformNode("Neck", false); - protected final TransformNode waistNode = new TransformNode("Waist", false); - protected final TransformNode chestNode = new TransformNode("Chest", false); - protected final TransformNode hipNode = new TransformNode("Hip", false); - - /** - * Distance from shoulders to chest - */ - protected float chestDistance = 0.35f; - /** - * Distance from hip to waist - */ - protected float waistDistance = 0.1f; - /** - * Distance from shoulder to hip - */ - protected float torsoLength = 0.7f; - /** - * Distance from eyes to the base of the neck - */ - protected float neckLength = HumanSkeletonWithWaist.NECK_LENGTH_DEFAULT; - /** - * Distance from eyes to ear - */ - protected float headShift = HumanSkeletonWithWaist.HEAD_SHIFT_DEFAULT; - - // Lower body (legs) - protected final TransformNode leftHipNode = new TransformNode("Left-Hip", false); - protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false); - protected final TransformNode leftAnkleNode = new TransformNode("Left-Ankle", false); - protected final TransformNode rightHipNode = new TransformNode("Right-Hip", false); - protected final TransformNode rightKneeNode = new TransformNode("Right-Knee", false); - protected final TransformNode rightAnkleNode = new TransformNode("Right-Ankle", false); - - /** - * Distance between centers of both hips - */ - protected float hipsWidth = HumanSkeletonWithLegs.HIPS_WIDTH_DEFAULT; - /** - * Length from hip to knees - */ - protected float kneeHeight = 0.42f; - /** - * Distance from hip to ankle - */ - protected float legsLength = 0.84f; - - protected final HashMap nodes = new HashMap(); - - private Quaternion rotBuf1 = new Quaternion(); - private Quaternion rotBuf2 = new Quaternion(); - - public SimpleSkeleton() { - // Assemble skeleton to hip - hmdNode.attachChild(headNode); - headNode.localTransform.setTranslation(0, 0, headShift); - - headNode.attachChild(neckNode); - neckNode.localTransform.setTranslation(0, -neckLength, 0); - - neckNode.attachChild(chestNode); - chestNode.localTransform.setTranslation(0, -chestDistance, 0); - - chestNode.attachChild(waistNode); - waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); - - waistNode.attachChild(hipNode); - hipNode.localTransform.setTranslation(0, -waistDistance, 0); - - // Assemble skeleton to feet - hipNode.attachChild(leftHipNode); - leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0); - - hipNode.attachChild(rightHipNode); - rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0); - - leftHipNode.attachChild(leftKneeNode); - leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); - - rightHipNode.attachChild(rightKneeNode); - rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); - - leftKneeNode.attachChild(leftAnkleNode); - leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0); - - rightKneeNode.attachChild(rightAnkleNode); - rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0); - - // Set up a HashMap to get nodes by name easily - hmdNode.depthFirstTraversal(visitor -> { - nodes.put(visitor.getName(), visitor); - }); - } - - public SimpleSkeleton(Map configs, Map altConfigs) { - // Initialize - this(); - - // Set configs - if(altConfigs != null) { - // Set alts first, so if there's any overlap it doesn't affect the values - setSkeletonConfigs(altConfigs); - } - setSkeletonConfigs(configs); - } - - public SimpleSkeleton(Map configs) { - this(configs, null); - } - - public void setPoseFromFrame(TrackerFrame[] frame) { - - TrackerFrame hmd = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.HMD); - - if(hmd != null) { - if(hmd.hasData(TrackerFrameData.ROTATION)) { - hmdNode.localTransform.setRotation(hmd.rotation); - headNode.localTransform.setRotation(hmd.rotation); - } - - if(hmd.hasData(TrackerFrameData.POSITION)) { - hmdNode.localTransform.setTranslation(hmd.position); - } - } - TrackerFrame chest = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.CHEST, TrackerPosition.WAIST, TrackerPosition.HIP); - setRotation(chest, neckNode); - - TrackerFrame waist = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.WAIST, TrackerPosition.CHEST, TrackerPosition.HIP); - setRotation(waist, chestNode); - - TrackerFrame leftLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.LEFT_LEG); - TrackerFrame rightLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.RIGHT_LEG); - - TrackerFrame hip = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.HIP, TrackerPosition.WAIST, TrackerPosition.CHEST); - averagePelvis(hip, leftLeg, rightLeg); - - setRotation(leftLeg, leftHipNode); - setRotation(rightLeg, rightHipNode); - - TrackerFrame leftAnkle = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.LEFT_ANKLE); - setRotation(leftAnkle, rightKneeNode); - - TrackerFrame rightAnkle = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.RIGHT_ANKLE); - setRotation(rightAnkle, leftKneeNode); - - updatePose(); - } - - public void setRotation(TrackerFrame trackerFrame, TransformNode node) { - if(trackerFrame != null && trackerFrame.hasData(TrackerFrameData.ROTATION)) { - node.localTransform.setRotation(trackerFrame.rotation); - } - } - - public void averagePelvis(TrackerFrame hip, TrackerFrame leftLeg, TrackerFrame rightLeg) { - if((leftLeg == null || rightLeg == null) || (!leftLeg.hasData(TrackerFrameData.ROTATION) || !rightLeg.hasData(TrackerFrameData.ROTATION))) { - setRotation(hip, waistNode); - return; - } - if(hip == null || !hip.hasData(TrackerFrameData.ROTATION)) { - if(leftLeg.hasData(TrackerFrameData.ROTATION) && rightLeg.hasData(TrackerFrameData.ROTATION)) { - leftLeg.getRotation(rotBuf1); - rightLeg.getRotation(rotBuf2); - rotBuf1.nlerp(rotBuf2, 0.5f); - - hipNode.localTransform.setRotation(rotBuf1); - } - - return; - } - - // Average the pelvis with the waist rotation - leftLeg.getRotation(rotBuf1); - rightLeg.getRotation(rotBuf2); - rotBuf1.nlerp(rotBuf2, 0.5f); - hip.getRotation(rotBuf2); - rotBuf1.nlerp(rotBuf2, 0.3333333f); - - hipNode.localTransform.setRotation(rotBuf1); - } - - public void setSkeletonConfigs(Map configs) { - configs.forEach(this::setSkeletonConfig); - } - - public void setSkeletonConfig(String joint, float newLength) { - setSkeletonConfig(joint, newLength, false); - } - - public void setSkeletonConfig(String joint, float newLength, boolean updatePose) { - switch(joint) { - case "Head": - headShift = newLength; - headNode.localTransform.setTranslation(0, 0, headShift); - if(updatePose) { - headNode.update(); - } - break; - case "Neck": - neckLength = newLength; - neckNode.localTransform.setTranslation(0, -neckLength, 0); - if(updatePose) { - neckNode.update(); - } - break; - case "Torso": - torsoLength = newLength; - waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); - if(updatePose) { - hipNode.update(); - } - break; - case "Chest": - chestDistance = newLength; - chestNode.localTransform.setTranslation(0, -chestDistance, 0); - waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); - if(updatePose) { - chestNode.update(); - } - break; - case "Waist": - waistDistance = newLength; - waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); - hipNode.localTransform.setTranslation(0, -waistDistance, 0); - if(updatePose) { - waistNode.update(); - } - break; - case "Hips width": - hipsWidth = newLength; - leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0); - rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0); - if(updatePose) { - leftHipNode.update(); - rightHipNode.update(); - } - break; - case "Knee height": - kneeHeight = newLength; - leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0); - rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0); - leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); - rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); - if(updatePose) { - leftKneeNode.update(); - rightKneeNode.update(); - } - break; - case "Legs length": - legsLength = newLength; - leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); - rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); - if(updatePose) { - leftKneeNode.update(); - rightKneeNode.update(); - } - break; - } - } - - public Float getSkeletonConfig(String joint) { - switch(joint) { - case "Head": - return headShift; - case "Neck": - return neckLength; - case "Torso": - return torsoLength; - case "Waist": - return waistDistance; - case "Chest": - return chestDistance; - case "Hips width": - return hipsWidth; - case "Knee height": - return kneeHeight; - case "Legs length": - return legsLength; - } - - return null; - } - - public void updatePose() { - hmdNode.update(); - } - - public TransformNode getNode(String node) { - return nodes.get(node); - } - - public TransformNode getNode(TrackerPosition bodyPosition) { - return getNode(bodyPosition, false); - } - - public TransformNode getNode(TrackerPosition bodyPosition, boolean rotationNode) { - if(bodyPosition == null) { - return null; - } - - switch(bodyPosition) { - case HMD: - return hmdNode; - case CHEST: - return rotationNode ? neckNode : chestNode; - case WAIST: - return rotationNode ? chestNode : waistNode; - case HIP: - return rotationNode ? waistNode : hipNode; - - case LEFT_LEG: - return rotationNode ? leftHipNode : leftKneeNode; - case RIGHT_LEG: - return rotationNode ? rightHipNode : rightKneeNode; - - case LEFT_ANKLE: - return rotationNode ? leftKneeNode : leftAnkleNode; - case RIGHT_ANKLE: - return rotationNode ? rightKneeNode : rightAnkleNode; - } - - return null; - } - - public Vector3f getNodePosition(String node) { - TransformNode transformNode = getNode(node); - return transformNode != null ? transformNode.worldTransform.getTranslation() : null; - } - - public Vector3f getNodePosition(TrackerPosition bodyPosition) { - TransformNode node = getNode(bodyPosition); - if(node == null) { - return null; - } - - return node.worldTransform.getTranslation(); - } - - public void saveConfigs(YamlFile config) { - // Save waist configs - config.setProperty("body.headShift", headShift); - config.setProperty("body.neckLength", neckLength); - config.setProperty("body.waistDistance", waistDistance); - config.setProperty("body.chestDistance", chestDistance); - config.setProperty("body.torsoLength", torsoLength); - - // Save leg configs - config.setProperty("body.hipsWidth", hipsWidth); - config.setProperty("body.kneeHeight", kneeHeight); - config.setProperty("body.legsLength", legsLength); - } -} diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java new file mode 100644 index 000000000..c2126c18b --- /dev/null +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java @@ -0,0 +1,71 @@ +package dev.slimevr.poserecorder; + +import java.util.List; +import java.util.Map; + +import dev.slimevr.vr.processor.SimpleSkeleton; +import io.eiren.vr.VRServer; +import io.eiren.vr.processor.ComputedHumanPoseTracker; +import io.eiren.vr.trackers.Tracker; + +public class PoseFrameSkeleton extends SimpleSkeleton { + + private int frameCursor = 0; + + protected PoseFrameSkeleton(List computedTrackers) { + super(computedTrackers); + } + + public PoseFrameSkeleton(VRServer server, List computedTrackers) { + super(server, computedTrackers); + } + + public PoseFrameSkeleton(List trackers, List computedTrackers) { + super(trackers, computedTrackers); + } + + public PoseFrameSkeleton(List trackers, List computedTrackers, Map configs, Map altConfigs) { + super(trackers, computedTrackers, configs, altConfigs); + } + + public PoseFrameSkeleton(List trackers, List computedTrackers, Map configs) { + super(trackers, computedTrackers, configs); + } + + private int limitCursor() { + if(frameCursor < 0) { + frameCursor = 0; + } + + return frameCursor; + } + + public int setCursor(int index) { + frameCursor = index; + return limitCursor(); + } + + public int incrementCursor(int increment) { + frameCursor += increment; + return limitCursor(); + } + + public int incrementCursor() { + return incrementCursor(1); + } + + public int getCursor() { + return frameCursor; + } + + // Get tracker for specific frame + @Override + protected Tracker trackerPreUpdate(Tracker tracker) { + if (tracker instanceof PoseFrameTracker) { + // Return frame if available, otherwise return the original tracker + TrackerFrame frame = ((PoseFrameTracker)tracker).safeGetFrame(frameCursor); + return frame == null ? tracker : frame; + } + return tracker; + } +} diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java index 533aeb27f..971fda55d 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java @@ -137,7 +137,7 @@ public class PoseFrameTracker implements Tracker, Iterable { return true; } - store.set(0, 0, 0, 1); + store.set(Quaternion.IDENTITY); return false; } @@ -149,7 +149,7 @@ public class PoseFrameTracker implements Tracker, Iterable { return true; } - store.set(0, 0, 0); + store.set(Vector3f.ZERO); return false; } @@ -175,7 +175,7 @@ public class PoseFrameTracker implements Tracker, Iterable { @Override public float getConfidenceLevel() { - return 0; + return 1f; } @Override diff --git a/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java index 78cb55364..570693690 100644 --- a/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java @@ -86,7 +86,7 @@ public final class TrackerFrame implements Tracker { return true; } - store.set(0, 0, 0, 1); + store.set(Quaternion.IDENTITY); return false; } @@ -97,7 +97,7 @@ public final class TrackerFrame implements Tracker { return true; } - store.set(0, 0, 0); + store.set(Vector3f.ZERO); return false; } @@ -123,7 +123,7 @@ public final class TrackerFrame implements Tracker { @Override public float getConfidenceLevel() { - return 0; + return 1f; } @Override diff --git a/src/main/java/dev/slimevr/vr/processor/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/processor/SimpleSkeleton.java new file mode 100644 index 000000000..29f937040 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/processor/SimpleSkeleton.java @@ -0,0 +1,726 @@ +package dev.slimevr.vr.processor; + +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import io.eiren.util.collections.FastList; +import io.eiren.vr.VRServer; +import io.eiren.vr.processor.ComputedHumanPoseTracker; +import io.eiren.vr.processor.ComputedHumanPoseTrackerPosition; +import io.eiren.vr.processor.HumanSkeletonWithLegs; +import io.eiren.vr.processor.HumanSkeletonWithWaist; +import io.eiren.vr.processor.TransformNode; +import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerPosition; +import io.eiren.vr.trackers.TrackerRole; +import io.eiren.vr.trackers.TrackerStatus; +import io.eiren.vr.trackers.TrackerUtils; +import io.eiren.yaml.YamlFile; + +public class SimpleSkeleton { + + //#region Upper body nodes (torso) + protected final TransformNode hmdNode = new TransformNode("HMD", false); + protected final TransformNode headNode = new TransformNode("Head", false); + protected final TransformNode neckNode = new TransformNode("Neck", false); + protected final TransformNode chestNode = new TransformNode("Chest", false); + protected final TransformNode waistNode = new TransformNode("Waist", false); + protected final TransformNode hipNode = new TransformNode("Hip", false); + protected final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false); + + /** + * Distance from shoulders to chest + */ + protected float chestDistance = 0.35f; + /** + * Distance from hip to waist + */ + protected float waistDistance = 0.1f; + /** + * Distance from shoulder to hip + */ + protected float torsoLength = 0.7f; + /** + * Distance from eyes to hip, defines reported + * tracker position, if you want to move resulting + * tracker up or down from actual hip + */ + protected float hipOffset = 0.0f; + /** + * Distance from eyes to the base of the neck + */ + protected float neckLength = HumanSkeletonWithWaist.NECK_LENGTH_DEFAULT; + /** + * Distance from eyes to ear + */ + protected float headShift = HumanSkeletonWithWaist.HEAD_SHIFT_DEFAULT; + //#endregion + + //#region Lower body nodes (legs) + protected final TransformNode leftHipNode = new TransformNode("Left-Hip", false); + protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false); + protected final TransformNode leftAnkleNode = new TransformNode("Left-Ankle", false); + protected final TransformNode leftFootNode = new TransformNode("Left-Foot", false); + + protected final TransformNode rightHipNode = new TransformNode("Right-Hip", false); + protected final TransformNode rightKneeNode = new TransformNode("Right-Knee", false); + protected final TransformNode rightAnkleNode = new TransformNode("Right-Ankle", false); + protected final TransformNode rightFootNode = new TransformNode("Right-Foot", false); + + /** + * Distance between centers of both hips + */ + protected float hipsWidth = HumanSkeletonWithLegs.HIPS_WIDTH_DEFAULT; + /** + * Length from hip to knees + */ + protected float kneeHeight = 0.42f; + /** + * Distance from hip to ankle + */ + protected float legsLength = 0.84f; + + protected float footLength = HumanSkeletonWithLegs.FOOT_LENGTH_DEFAULT; + protected float footOffset = 0f; //horizontal forward/backwards translation feet offset for avatars with bent knees + + protected float minKneePitch = 0f * FastMath.DEG_TO_RAD; + protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD; + + protected float kneeLerpFactor = 0.5f; + //#endregion + + //#region Tracker Input + protected Tracker hmdTracker; + protected Tracker chestTracker; + protected Tracker waistTracker; + protected Tracker hipTracker; + + protected Tracker leftLegTracker; + protected Tracker leftAnkleTracker; + protected Tracker leftFootTracker; + + protected Tracker rightLegTracker; + protected Tracker rightAnkleTracker; + protected Tracker rightFootTracker; + //#endregion + + //#region Tracker Output + protected ComputedHumanPoseTracker computedChestTracker; + protected ComputedHumanPoseTracker computedWaistTracker; + + protected ComputedHumanPoseTracker computedLeftKneeTracker; + protected ComputedHumanPoseTracker computedLeftFootTracker; + + protected ComputedHumanPoseTracker computedRightKneeTracker; + protected ComputedHumanPoseTracker computedRightFootTracker; + //#endregion + + protected final HashMap nodes = new HashMap(); + + protected boolean extendedPelvisModel = true; + protected boolean extendedKneeModel = false; + + //#region Buffers + private Vector3f posBuf = new Vector3f(); + + private Quaternion rotBuf1 = new Quaternion(); + private Quaternion rotBuf2 = new Quaternion(); + + protected final Vector3f hipVector = new Vector3f(); + protected final Vector3f ankleVector = new Vector3f(); + + protected final Quaternion kneeRotation = new Quaternion(); + //#endregion + + //#region Constructors + protected SimpleSkeleton(List computedTrackers) { + //#region Assemble skeleton to hip + hmdNode.attachChild(headNode); + headNode.localTransform.setTranslation(0, 0, headShift); + + headNode.attachChild(neckNode); + neckNode.localTransform.setTranslation(0, -neckLength, 0); + + neckNode.attachChild(chestNode); + chestNode.localTransform.setTranslation(0, -chestDistance, 0); + + chestNode.attachChild(waistNode); + waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); + + waistNode.attachChild(hipNode); + hipNode.localTransform.setTranslation(0, -waistDistance, 0); + + hipNode.attachChild(trackerWaistNode); + trackerWaistNode.localTransform.setTranslation(0, hipOffset, 0); + //#endregion + + //#region Assemble skeleton to feet + hipNode.attachChild(leftHipNode); + leftHipNode.localTransform.setTranslation(-hipsWidth / 2f, 0, 0); + + hipNode.attachChild(rightHipNode); + rightHipNode.localTransform.setTranslation(hipsWidth / 2f, 0, 0); + + leftHipNode.attachChild(leftKneeNode); + leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); + + rightHipNode.attachChild(rightKneeNode); + rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); + + leftKneeNode.attachChild(leftAnkleNode); + leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset); + + rightKneeNode.attachChild(rightAnkleNode); + rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset); + + leftAnkleNode.attachChild(leftFootNode); + leftFootNode.localTransform.setTranslation(0, 0, -footLength); + + rightAnkleNode.attachChild(rightFootNode); + rightFootNode.localTransform.setTranslation(0, 0, -footLength); + //#endregion + + // Set up a HashMap to get nodes by name easily + hmdNode.depthFirstTraversal(visitor -> { + nodes.put(visitor.getName(), visitor); + }); + + if (computedTrackers != null) { + setComputedTrackers(computedTrackers); + } + fillNullComputedTrackers(true); + } + + public SimpleSkeleton(VRServer server, List computedTrackers) { + this(computedTrackers); + setTrackersFromServer(server); + loadConfigs(server.config); + } + + public SimpleSkeleton(List trackers, List computedTrackers) { + this(computedTrackers); + + if (trackers != null) { + setTrackersFromList(trackers); + } else { + setTrackersFromList(new FastList(0)); + } + } + + public SimpleSkeleton(List trackers, List computedTrackers, Map configs, Map altConfigs) { + // Initialize + this(trackers, computedTrackers); + + // Set configs + if(altConfigs != null) { + // Set alts first, so if there's any overlap it doesn't affect the values + setSkeletonConfigs(altConfigs); + } + setSkeletonConfigs(configs); + } + + public SimpleSkeleton(List trackers, List computedTrackers, Map configs) { + this(trackers, computedTrackers, configs, null); + } + //#endregion + + //#region Set Trackers + public void setTrackersFromList(List trackers, boolean setHmd) { + if (setHmd) { + this.hmdTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.HMD); + } + + this.chestTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.CHEST, TrackerPosition.WAIST, TrackerPosition.HIP); + this.waistTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.WAIST, TrackerPosition.CHEST, TrackerPosition.HIP); + this.hipTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.HIP, TrackerPosition.WAIST, TrackerPosition.CHEST); + + this.leftLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.LEFT_LEG, TrackerPosition.LEFT_ANKLE, null); + this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.LEFT_ANKLE, TrackerPosition.LEFT_LEG, null); + this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_FOOT); + + this.rightLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.RIGHT_LEG, TrackerPosition.RIGHT_ANKLE, null); + this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.RIGHT_ANKLE, TrackerPosition.RIGHT_LEG, null); + this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_FOOT); + } + + public void setTrackersFromList(List trackers) { + setTrackersFromList(trackers, true); + } + + public void setTrackersFromServer(VRServer server) { + this.hmdTracker = server.hmdTracker; + setTrackersFromList(server.getAllTrackers(), false); + } + + public void setComputedTracker(ComputedHumanPoseTracker tracker) { + switch (tracker.getTrackerRole()) { + case CHEST: + computedChestTracker = tracker; + break; + case WAIST: + computedWaistTracker = tracker; + break; + + case LEFT_KNEE: + computedLeftKneeTracker = tracker; + break; + case LEFT_FOOT: + computedLeftFootTracker = tracker; + break; + + case RIGHT_KNEE: + computedRightKneeTracker = tracker; + break; + case RIGHT_FOOT: + computedRightFootTracker = tracker; + break; + } + } + + public void setComputedTrackers(List trackers) { + for(int i = 0; i < trackers.size(); ++i) { + setComputedTracker(trackers.get(i)); + } + } + + public void setComputedTrackersAndFillNull(List trackers, boolean onlyFillWaistAndFeet) { + setComputedTrackers(trackers); + fillNullComputedTrackers(onlyFillWaistAndFeet); + } + + public void fillNullComputedTrackers(boolean onlyFillWaistAndFeet) { + if (computedWaistTracker == null) { + computedWaistTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.WAIST, TrackerRole.WAIST); + computedWaistTracker.setStatus(TrackerStatus.OK); + } + + if (computedLeftFootTracker == null) { + computedLeftFootTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerRole.LEFT_FOOT); + computedLeftFootTracker.setStatus(TrackerStatus.OK); + } + + if (computedRightFootTracker == null) { + computedRightFootTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerRole.RIGHT_FOOT); + computedRightFootTracker.setStatus(TrackerStatus.OK); + } + + if (!onlyFillWaistAndFeet) { + if (computedChestTracker == null) { + computedChestTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.CHEST, TrackerRole.CHEST); + computedChestTracker.setStatus(TrackerStatus.OK); + } + + if (computedLeftKneeTracker == null) { + computedLeftKneeTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_KNEE, TrackerRole.LEFT_KNEE); + computedLeftKneeTracker.setStatus(TrackerStatus.OK); + } + + if (computedRightKneeTracker == null) { + computedRightKneeTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_KNEE, TrackerRole.RIGHT_KNEE); + computedRightKneeTracker.setStatus(TrackerStatus.OK); + } + } + } + //#endregion + + //#region Get Trackers + public ComputedHumanPoseTracker getComputedTracker(TrackerRole trackerRole) { + switch (trackerRole) { + case CHEST: + return computedChestTracker; + case WAIST: + return computedWaistTracker; + + case LEFT_KNEE: + return computedLeftKneeTracker; + case LEFT_FOOT: + return computedLeftFootTracker; + + case RIGHT_KNEE: + return computedRightKneeTracker; + case RIGHT_FOOT: + return computedRightFootTracker; + } + + return null; + } + //#endregion + + //#region Processing + // Useful for sub-classes that need to return a sub-tracker (like PoseFrameTracker -> TrackerFrame) + protected Tracker trackerPreUpdate(Tracker tracker) { + return tracker; + } + + // Updates the pose from tracker positions + public void updatePose() { + updateLocalTransforms(); + hmdNode.update(); + updateComputedTrackers(); + } + + //#region Update the node transforms from the trackers + protected void updateLocalTransforms() { + //#region Pass all trackers through trackerPreUpdate + Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); + + Tracker chestTracker = trackerPreUpdate(this.chestTracker); + Tracker waistTracker = trackerPreUpdate(this.waistTracker); + Tracker hipTracker = trackerPreUpdate(this.hipTracker); + + Tracker leftLegTracker = trackerPreUpdate(this.leftLegTracker); + Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker); + Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker); + + Tracker rightLegTracker = trackerPreUpdate(this.rightLegTracker); + Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker); + Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker); + //#endregion + + if (hmdTracker != null) { + if (hmdTracker.getPosition(posBuf)) { + hmdNode.localTransform.setTranslation(posBuf); + } + if (hmdTracker.getRotation(rotBuf1)) { + hmdNode.localTransform.setRotation(rotBuf1); + headNode.localTransform.setRotation(rotBuf1); + } + } else { + // Set to zero + hmdNode.localTransform.setTranslation(Vector3f.ZERO); + hmdNode.localTransform.setRotation(Quaternion.IDENTITY); + headNode.localTransform.setRotation(Quaternion.IDENTITY); + } + + if (chestTracker.getRotation(rotBuf1)) { + neckNode.localTransform.setRotation(rotBuf1); + } + if (waistTracker.getRotation(rotBuf1)) { + chestNode.localTransform.setRotation(rotBuf1); + } + if (hipTracker.getRotation(rotBuf1)) { + waistNode.localTransform.setRotation(rotBuf1); + trackerWaistNode.localTransform.setRotation(rotBuf1); + hipNode.localTransform.setRotation(rotBuf1); + } + + // Left Leg + leftLegTracker.getRotation(rotBuf1); + leftAnkleTracker.getRotation(rotBuf2); + + if(extendedKneeModel) + calculateKneeLimits(rotBuf1, rotBuf2, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel()); + + leftHipNode.localTransform.setRotation(rotBuf1); + leftKneeNode.localTransform.setRotation(rotBuf2); + leftAnkleNode.localTransform.setRotation(rotBuf2); + leftFootNode.localTransform.setRotation(rotBuf2); + + if(leftFootTracker != null) { + leftFootTracker.getRotation(rotBuf2); + leftAnkleNode.localTransform.setRotation(rotBuf2); + leftFootNode.localTransform.setRotation(rotBuf2); + } + + // Right Leg + rightLegTracker.getRotation(rotBuf1); + rightAnkleTracker.getRotation(rotBuf2); + + if(extendedKneeModel) + calculateKneeLimits(rotBuf1, rotBuf2, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel()); + + rightHipNode.localTransform.setRotation(rotBuf1); + rightKneeNode.localTransform.setRotation(rotBuf2); + rightAnkleNode.localTransform.setRotation(rotBuf2); + rightFootNode.localTransform.setRotation(rotBuf2); + + if(rightFootTracker != null) { + rightFootTracker.getRotation(rotBuf2); + rightAnkleNode.localTransform.setRotation(rotBuf2); + rightFootNode.localTransform.setRotation(rotBuf2); + } + + if(extendedPelvisModel) { + // Average pelvis between two legs + leftHipNode.localTransform.getRotation(rotBuf1); + rightHipNode.localTransform.getRotation(rotBuf2); + rotBuf2.nlerp(rotBuf1, 0.5f); + chestNode.localTransform.getRotation(rotBuf1); + rotBuf2.nlerp(rotBuf1, 0.3333333f); + hipNode.localTransform.setRotation(rotBuf2); + //trackerWaistNode.localTransform.setRotation(rotBuf2); // <== Provides cursed results from my test in VRChat when sitting or laying down -Erimel + // TODO : Correct the trackerWaistNode without getting cursed results (only correct yaw?) + // TODO : Use vectors to add like 50% of waist tracker yaw to waist node to reduce drift and let user take weird poses + } + } + //#endregion + + //#region Knee Model + // Knee basically has only 1 DoF (pitch), average yaw and roll between knee and hip + protected void calculateKneeLimits(Quaternion hipBuf, Quaternion kneeBuf, float hipConfidence, float kneeConfidence) { + ankleVector.set(0, -1, 0); + hipVector.set(0, -1, 0); + hipBuf.multLocal(hipVector); + kneeBuf.multLocal(ankleVector); + kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee angle + + // Substract knee angle from knee rotation. With perfect leg and perfect + // sensors result should match hip rotation perfectly + kneeBuf.multLocal(kneeRotation.inverse()); + + // Average knee and hip with a slerp + hipBuf.slerp(kneeBuf, 0.5f); // TODO : Use confidence to calculate changeAmt + kneeBuf.set(hipBuf); + + // Return knee angle into knee rotation + kneeBuf.multLocal(kneeRotation); + } + + public static float normalizeRad(float angle) { + return FastMath.normalize(angle, -FastMath.PI, FastMath.PI); + } + + public static float interpolateRadians(float factor, float start, float end) { + float angle = FastMath.abs(end - start); + if(angle > FastMath.PI) { + if(end > start) { + start += FastMath.TWO_PI; + } else { + end += FastMath.TWO_PI; + } + } + float val = start + (end - start) * factor; + return normalizeRad(val); + } + //#endregion + + //#region Update the output trackers + protected void updateComputedTrackers() { + if (computedChestTracker != null) { + computedChestTracker.position.set(chestNode.worldTransform.getTranslation()); + computedChestTracker.rotation.set(neckNode.worldTransform.getRotation()); + computedChestTracker.dataTick(); + } + + if (computedWaistTracker != null) { + computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation()); + computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation()); + computedWaistTracker.dataTick(); + } + + if (computedLeftKneeTracker != null) { + computedLeftKneeTracker.position.set(leftKneeNode.worldTransform.getTranslation()); + computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation()); + computedLeftKneeTracker.dataTick(); + } + + if (computedLeftFootTracker != null) { + computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation()); + computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation()); + computedLeftFootTracker.dataTick(); + } + + if (computedRightKneeTracker != null) { + computedRightKneeTracker.position.set(rightKneeNode.worldTransform.getTranslation()); + computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation()); + computedRightKneeTracker.dataTick(); + } + + if (computedRightFootTracker != null) { + computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation()); + computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation()); + computedRightFootTracker.dataTick(); + } + } + //#endregion + //#endregion + + //#region Skeleton Config + public void setSkeletonConfigs(Map configs) { + configs.forEach(this::setSkeletonConfig); + } + + public void setSkeletonConfig(String joint, float newLength) { + setSkeletonConfig(joint, newLength, false); + } + + public void setSkeletonConfig(String joint, float newLength, boolean updatePose) { + switch(joint) { + case "Head": + headShift = newLength; + headNode.localTransform.setTranslation(0, 0, headShift); + if(updatePose) { + headNode.update(); + updateComputedTrackers(); + } + break; + case "Neck": + neckLength = newLength; + neckNode.localTransform.setTranslation(0, -neckLength, 0); + if(updatePose) { + neckNode.update(); + updateComputedTrackers(); + } + break; + case "Torso": + torsoLength = newLength; + waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); + if(updatePose) { + hipNode.update(); + updateComputedTrackers(); + } + break; + case "Chest": + chestDistance = newLength; + chestNode.localTransform.setTranslation(0, -chestDistance, 0); + waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); + if(updatePose) { + chestNode.update(); + updateComputedTrackers(); + } + break; + case "Waist": + waistDistance = newLength; + waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0); + hipNode.localTransform.setTranslation(0, -waistDistance, 0); + if(updatePose) { + waistNode.update(); + updateComputedTrackers(); + } + break; + case "Hip offset": + hipOffset = newLength; + trackerWaistNode.localTransform.setTranslation(0, hipOffset, 0); + if(updatePose) { + trackerWaistNode.update(); + updateComputedTrackers(); + } + break; + case "Hips width": + hipsWidth = newLength; + leftHipNode.localTransform.setTranslation(-hipsWidth / 2f, 0, 0); + rightHipNode.localTransform.setTranslation(hipsWidth / 2f, 0, 0); + if(updatePose) { + leftHipNode.update(); + rightHipNode.update(); + updateComputedTrackers(); + } + break; + case "Knee height": + kneeHeight = newLength; + leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset); + rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset); + leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); + rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); + if(updatePose) { + leftKneeNode.update(); + rightKneeNode.update(); + updateComputedTrackers(); + } + break; + case "Legs length": + legsLength = newLength; + leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); + rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0); + if(updatePose) { + leftKneeNode.update(); + rightKneeNode.update(); + updateComputedTrackers(); + } + break; + case "Foot length": + footLength = newLength; + leftFootNode.localTransform.setTranslation(0, 0, -footLength); + rightFootNode.localTransform.setTranslation(0, 0, -footLength); + if(updatePose) { + leftFootNode.update(); + rightFootNode.update(); + updateComputedTrackers(); + } + break; + case "Foot offset": + footOffset = newLength; + leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset); + rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset); + if(updatePose) { + leftAnkleNode.update(); + rightAnkleNode.update(); + updateComputedTrackers(); + } + break; + } + } + + public Float getSkeletonConfig(String joint) { + switch(joint) { + case "Head": + return headShift; + case "Neck": + return neckLength; + case "Torso": + return torsoLength; + case "Waist": + return waistDistance; + case "Chest": + return chestDistance; + case "Hip offset": + return hipOffset; + case "Hips width": + return hipsWidth; + case "Knee height": + return kneeHeight; + case "Legs length": + return legsLength; + case "Foot length": + return footLength; + case "Foot offset": + return footOffset; + } + + return null; + } + + public void loadConfigs(YamlFile config) { + // Save waist configs + headShift = config.getFloat("body.headShift", headShift); + neckLength = config.getFloat("body.neckLength", neckLength); + chestDistance = config.getFloat("body.chestDistance", chestDistance); + waistDistance = config.getFloat("body.waistDistance", waistDistance); + torsoLength = config.getFloat("body.torsoLength", torsoLength); + hipOffset = config.getFloat("body.hipOffset", hipOffset); + + // Save leg configs + hipsWidth = config.getFloat("body.hipsWidth", hipsWidth); + kneeHeight = config.getFloat("body.kneeHeight", kneeHeight); + legsLength = config.getFloat("body.legsLength", legsLength); + footLength = config.getFloat("body.footLength", footLength); + footOffset = config.getFloat("body.footOffset", footOffset); + //extendedPelvisModel = config.getBoolean("body.model.extendedPelvis", extendedPelvisModel); + extendedKneeModel = config.getBoolean("body.model.extendedKnee", extendedKneeModel); + } + + public void saveConfigs(YamlFile config) { + // Save waist configs + config.setProperty("body.headShift", headShift); + config.setProperty("body.neckLength", neckLength); + config.setProperty("body.waistDistance", waistDistance); + config.setProperty("body.chestDistance", chestDistance); + config.setProperty("body.torsoLength", torsoLength); + config.setProperty("body.hipOffset", hipOffset); + + // Save leg configs + config.setProperty("body.hipsWidth", hipsWidth); + config.setProperty("body.kneeHeight", kneeHeight); + config.setProperty("body.legsLength", legsLength); + config.setProperty("body.footLength", footLength); + config.setProperty("body.footOffset", footOffset); + //config.setProperty("body.model.extendedPelvis", extendedPelvisModel); + config.setProperty("body.model.extendedKnee", extendedKneeModel); + } + //#endregion +}