mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Add full tracker functionality to SimpleSkeleton
This commit is contained in:
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
|
||||
}
|
||||
Reference in new issue
Block a user