Add full tracker functionality to SimpleSkeleton

This commit is contained in:
ButterscotchVanilla committed 2021-12-04 00:45:37 -05:00
1 parent 318c43077c
commit f49b2556ae
6 files changed
+883 -437

No files matched your search

@@ -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<PoseFrameTracker> 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<Epoch> epochCallback) {
final int frameCount = frames.getMaxFrameCount();
List<PoseFrameTracker> 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<String, Float> 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<PoseFrameTracker> 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<PoseFrameTracker> 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);
}
@@ -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<String, TransformNode> nodes = new HashMap<String, TransformNode>();
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<String, Float> configs, Map<String, Float> 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<String, Float> 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<String, Float> 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);
}
}
@@ -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<? extends ComputedHumanPoseTracker> computedTrackers) {
super(computedTrackers);
}
public PoseFrameSkeleton(VRServer server, List<? extends ComputedHumanPoseTracker> computedTrackers) {
super(server, computedTrackers);
}
public PoseFrameSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers) {
super(trackers, computedTrackers);
}
public PoseFrameSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<String, Float> configs, Map<String, Float> altConfigs) {
super(trackers, computedTrackers, configs, altConfigs);
}
public PoseFrameSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<String, Float> 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;
}
}
@@ -137,7 +137,7 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
return true;
}
store.set(0, 0, 0, 1);
store.set(Quaternion.IDENTITY);
return false;
}
@@ -149,7 +149,7 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
return true;
}
store.set(0, 0, 0);
store.set(Vector3f.ZERO);
return false;
}
@@ -175,7 +175,7 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
@Override
public float getConfidenceLevel() {
return 0;
return 1f;
}
@Override
@@ -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
@@ -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<String, TransformNode> nodes = new HashMap<String, TransformNode>();
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<? extends ComputedHumanPoseTracker> 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<? extends ComputedHumanPoseTracker> computedTrackers) {
this(computedTrackers);
setTrackersFromServer(server);
loadConfigs(server.config);
}
public SimpleSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers) {
this(computedTrackers);
if (trackers != null) {
setTrackersFromList(trackers);
} else {
setTrackersFromList(new FastList<Tracker>(0));
}
}
public SimpleSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<String, Float> configs, Map<String, Float> 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<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<String, Float> configs) {
this(trackers, computedTrackers, configs, null);
}
//#endregion
//#region Set Trackers
public void setTrackersFromList(List<? extends Tracker> 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<? extends Tracker> 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<? extends ComputedHumanPoseTracker> trackers) {
for(int i = 0; i < trackers.size(); ++i) {
setComputedTracker(trackers.get(i));
}
}
public void setComputedTrackersAndFillNull(List<? extends ComputedHumanPoseTracker> 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<String, Float> 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
}