From c02b95346f43917cc6daeffd90d3914796efe419 Mon Sep 17 00:00:00 2001 From: Erimel Date: Thu, 9 Jun 2022 06:21:40 -0400 Subject: [PATCH] Extended knee model + temporary fix for null/optional (#193) --- .../java/dev/slimevr/autobone/AutoBone.java | 16 +- .../java/dev/slimevr/gui/TrackersList.java | 8 +- .../windows/WindowsNamedPipeBridge.java | 2 +- .../dev/slimevr/poserecorder/PoseFrameIO.java | 4 +- .../java/dev/slimevr/protocol/RPCHandler.java | 5 +- .../processor/ComputedHumanPoseTracker.java | 8 +- .../vr/processor/skeleton/HumanSkeleton.java | 173 ++++++++++-------- .../skeleton/SkeletonConfigToggle.java | 8 +- .../slimevr/vr/trackers/ComputedTracker.java | 4 +- .../dev/slimevr/vr/trackers/IMUTracker.java | 6 +- 10 files changed, 134 insertions(+), 100 deletions(-) diff --git a/src/main/java/dev/slimevr/autobone/AutoBone.java b/src/main/java/dev/slimevr/autobone/AutoBone.java index 2ab857f19..ed4b520da 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/autobone/AutoBone.java @@ -654,7 +654,11 @@ public class AutoBone { List trackers = frames.getTrackers(); for (PoseFrameTracker tracker : trackers) { TrackerFrame trackerFrame = tracker.safeGetFrame(cursor); - if (trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) { + if ( + trackerFrame == null + || !trackerFrame.hasData(TrackerFrameData.POSITION) + || trackerFrame.designation.trackerRole.isEmpty() + ) { continue; } @@ -689,7 +693,11 @@ public class AutoBone { } TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2); - if (trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) { + if ( + trackerFrame2 == null + || !trackerFrame2.hasData(TrackerFrameData.POSITION) + || trackerFrame1.designation.trackerRole.isEmpty() + ) { continue; } @@ -699,6 +707,10 @@ public class AutoBone { continue; } + + if (trackerFrame2.designation.trackerRole.isEmpty()) { + continue; + } Vector3f nodePos2 = skeleton2 .getComputedTracker(trackerFrame2.designation.trackerRole.get()).position; if (nodePos2 == null) { diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 54047cf50..705431ff1 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -185,9 +185,11 @@ public class TrackersList extends EJBoxNoStretch { desSelect.addItem(p.name()); } if (cfg.designation != null) { - TrackerPosition p = TrackerPosition.getByDesignation(cfg.designation).get(); - if (p != null) - desSelect.setSelectedItem(p.name()); + TrackerPosition + .getByDesignation(cfg.designation) + .ifPresent( + trackerPosition -> desSelect.setSelectedItem(trackerPosition.name()) + ); } desSelect.addActionListener(new ActionListener() { @Override diff --git a/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java b/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java index 57648dacf..9573c1285 100644 --- a/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java +++ b/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java @@ -114,7 +114,7 @@ public class WindowsNamedPipeBridge extends ProtobufBridge implements ); TrackerRole role = TrackerRole.getById(trackerAdded.getTrackerRole()); if (role != null) { - tracker.setBodyPosition(TrackerPosition.getByTrackerRole(role).get()); + TrackerPosition.getByTrackerRole(role).ifPresent(tracker::setBodyPosition); } return tracker; } diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java index 8fd09ae1d..3a02f78d9 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java @@ -92,7 +92,9 @@ public final class PoseFrameIO { TrackerPosition designation = null; if (TrackerFrameData.DESIGNATION.check(dataFlags)) { - designation = TrackerPosition.getByDesignation(inputStream.readUTF()).get(); + designation = TrackerPosition + .getByDesignation(inputStream.readUTF()) + .orElse(null); } Quaternion rotation = null; diff --git a/src/main/java/dev/slimevr/protocol/RPCHandler.java b/src/main/java/dev/slimevr/protocol/RPCHandler.java index 625c8da6c..73f9b6e58 100644 --- a/src/main/java/dev/slimevr/protocol/RPCHandler.java +++ b/src/main/java/dev/slimevr/protocol/RPCHandler.java @@ -3,10 +3,9 @@ package dev.slimevr.protocol; import com.fazecast.jSerialComm.SerialPort; import com.google.flatbuffers.FlatBufferBuilder; import com.jme3.math.Quaternion; - +import dev.slimevr.autobone.AutoBone.Epoch; import dev.slimevr.autobone.AutoBoneListener; import dev.slimevr.autobone.AutoBoneProcessType; -import dev.slimevr.autobone.AutoBone.Epoch; import dev.slimevr.platform.windows.WindowsNamedPipeBridge; import dev.slimevr.poserecorder.PoseFrames; import dev.slimevr.serial.SerialListener; @@ -197,7 +196,7 @@ public class RPCHandler extends ProtocolHandler if (tracker == null) return; - tracker.setBodyPosition(TrackerPosition.getByBodyPart(req.bodyPosition()).get()); + TrackerPosition.getByBodyPart(req.bodyPosition()).ifPresent(tracker::setBodyPosition); if (tracker instanceof ReferenceAdjustedTracker) { ReferenceAdjustedTracker refTracker = (ReferenceAdjustedTracker) tracker; diff --git a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java index 5b71a14ba..579d2a2a5 100644 --- a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java +++ b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java @@ -1,11 +1,7 @@ package dev.slimevr.vr.processor; -import dev.slimevr.vr.trackers.ComputedTracker; -import dev.slimevr.vr.trackers.ShareableTracker; -import dev.slimevr.vr.trackers.TrackerRole; -import dev.slimevr.vr.trackers.TrackerWithTPS; +import dev.slimevr.vr.trackers.*; import dev.slimevr.vr.trackers.udp.UDPDevice; -import dev.slimevr.vr.trackers.TrackerPosition; import io.eiren.util.BufferedTimer; @@ -25,7 +21,7 @@ public class ComputedHumanPoseTracker extends ComputedTracker this.skeletonPosition = skeletonPosition; this.trackerRole = role; // TODO: Use `TrackerPosition` instead of `TrackerRole` - this.bodyPosition = TrackerPosition.getByTrackerRole(role).get(); + this.bodyPosition = TrackerPosition.getByTrackerRole(role).orElse(null); } @Override diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java index 9881ddec4..dd27f4bcd 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java @@ -129,8 +129,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { private final Quaternion rotBuf2 = new Quaternion(); private final Quaternion rotBuf3 = new Quaternion(); private final Quaternion rotBuf4 = new Quaternion(); - protected final Vector3f vectorBuf1 = new Vector3f(); - protected final Vector3f vectorBuf2 = new Vector3f(); protected boolean hasSpineTracker; protected boolean hasKneeTrackers; protected float minKneePitch = 0f * FastMath.DEG_TO_RAD; @@ -173,17 +171,19 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // #endregion // #region FK Settings - protected boolean extendedPelvisModel = true; protected boolean extendedSpineModel = true; - protected boolean extendedKneeModel = false; + protected boolean extendedPelvisModel = true; + protected boolean extendedKneeModel = true; // Extended Spine Model - protected float waistChestHipFactor = 0.5f; - protected float waistChestPelvisFactor = 0.18f; - protected float hipSpinePelvisFactor = 0.25f; - protected float pelvisHipFactor = FastMath.ONE_THIRD; + protected float waistChestHipAveraging = 0.5f; + protected float waistChestPelvisAveraging = 0.18f; + protected float hipSpinePelvisAveraging = 0.25f; + protected float pelvisHipAveraging = FastMath.ONE_THIRD; // Extended Pelvis Model - protected float pelvisWaistTrackerFactor = 0.75f; + protected float pelvisWaistTrackerAveraging = 0.75f; + // Extended Knee Model + protected float ankleKneeTrackerAveraging = 0.25f; // #endregion // #region Constructors @@ -252,8 +252,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // #endregion // Set default skeleton configuration (callback automatically sets - // initial - // offsets) + // initial offsets) skeletonConfig = new SkeletonConfig(true, this); if (computedTrackers != null) { @@ -859,17 +858,9 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { if (leftLowerLegTracker != null) leftLowerLegTracker.getRotation(rotBuf2); - if (leftUpperLegTracker != null && leftLowerLegTracker != null && extendedKneeModel) - calculateKneeLimits( - rotBuf1, - rotBuf2, - leftUpperLegTracker.getConfidenceLevel(), - leftLowerLegTracker.getConfidenceLevel() - ); - leftHipNode.localTransform.setRotation(rotBuf1); + trackerLeftKneeNode.localTransform.setRotation(rotBuf1); leftKneeNode.localTransform.setRotation(rotBuf2); - trackerLeftKneeNode.localTransform.setRotation(rotBuf2); if (leftFootTracker != null) leftFootTracker.getRotation(rotBuf2); @@ -878,6 +869,32 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { leftFootNode.localTransform.setRotation(rotBuf2); trackerLeftFootNode.localTransform.setRotation(rotBuf2); + // Extended left knee + if (leftUpperLegTracker != null && leftLowerLegTracker != null && extendedKneeModel) { + // Averages the knee's rotation with the local ankle's + // pitch and roll and apply to the tracker node. + leftKneeNode.localTransform.getRotation(rotBuf1); + leftHipNode.localTransform.getRotation(rotBuf2); + + // Get the knee's inverse rotation. + rotBuf3.set(rotBuf2); + rotBuf3.inverseLocal(); + + // Only rotate on local yaw and pitch + // + // R = InverseKnee * Ankle + // C = Quaternion(-R.x, 0, 0, R.w) + // Knee = Knee * R * C + // normalize(Knee) + rotBuf3.multLocal(rotBuf1); + rotBuf1.set(-rotBuf3.getX(), 0, 0, rotBuf3.getW()); + rotBuf1.set(rotBuf2.mult(rotBuf3).mult(rotBuf1)); + rotBuf1.normalizeLocal(); + + rotBuf1.slerpLocal(rotBuf2, ankleKneeTrackerAveraging); + trackerLeftKneeNode.localTransform.setRotation(rotBuf1); + } + // Right Leg rotBuf1.loadIdentity(); rotBuf2.loadIdentity(); @@ -886,17 +903,9 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { if (rightLowerLegTracker != null) rightLowerLegTracker.getRotation(rotBuf2); - if (rightUpperLegTracker != null && rightLowerLegTracker != null && extendedKneeModel) - calculateKneeLimits( - rotBuf1, - rotBuf2, - rightUpperLegTracker.getConfidenceLevel(), - rightLowerLegTracker.getConfidenceLevel() - ); - rightHipNode.localTransform.setRotation(rotBuf1); + trackerRightKneeNode.localTransform.setRotation(rotBuf1); rightKneeNode.localTransform.setRotation(rotBuf2); - trackerRightKneeNode.localTransform.setRotation(rotBuf2); if (rightFootTracker != null) rightFootTracker.getRotation(rotBuf2); @@ -905,6 +914,33 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { rightFootNode.localTransform.setRotation(rotBuf2); trackerRightFootNode.localTransform.setRotation(rotBuf2); + // Extended right knee + if (rightUpperLegTracker != null && rightLowerLegTracker != null && extendedKneeModel) { + // Averages the knee's rotation with the local ankle's + // pitch and roll and apply to the tracker node. + rightKneeNode.localTransform.getRotation(rotBuf1); + rightHipNode.localTransform.getRotation(rotBuf2); + + // Get the knee's inverse rotation. + rotBuf3.set(rotBuf2); + rotBuf3.inverseLocal(); + + // Only rotate on local yaw and pitch + // + // R = InverseKnee * Ankle + // C = Quaternion(-R.x, 0, 0, R.w) + // Knee = Knee * R * C + // normalize(Knee) + rotBuf3.multLocal(rotBuf1); + rotBuf1.set(-rotBuf3.getX(), 0, 0, rotBuf3.getW()); + rotBuf1.set(rotBuf2.mult(rotBuf3).mult(rotBuf1)); + rotBuf1.normalizeLocal(); + + rotBuf1.slerpLocal(rotBuf2, ankleKneeTrackerAveraging); + trackerRightKneeNode.localTransform.setRotation(rotBuf1); + } + + // Extended spine if (extendedSpineModel && hasSpineTracker) { if ( @@ -919,7 +955,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { chestTracker.getRotation(rotBuf1); hipTracker.getRotation(rotBuf2); - rotBuf1.slerpLocal(rotBuf2, waistChestHipFactor); + // Interpolate between the chest and the hip + rotBuf1.slerpLocal(rotBuf2, waistChestHipAveraging); chestNode.localTransform.setRotation(rotBuf1); } else if (hasKneeTrackers) { // Calculates waist from chest + pelvis @@ -937,8 +974,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { rotBuf2.negateLocal(); } + // Average the legs to calculate the pelvis rotBuf1.nlerp(rotBuf2, 0.5f); - rotBuf3.pureSlerpLocal(rotBuf1, waistChestPelvisFactor); + + // Interpolate between the pelvis and the chest + rotBuf3.pureSlerpLocal(rotBuf1, waistChestPelvisAveraging); + chestNode.localTransform.setRotation(rotBuf3); } } @@ -950,8 +991,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { .getFirstAvailableTracker(waistTracker, chestTracker, null) .getRotation(rotBuf3); - // Get the rotation relative to where we expect the upper - // legs to be + // Get the rotation relative to where we expect the + // upper legs to be rotBuf3.mult(FORWARD_QUATERNION, rotBuf4); if (rotBuf4.dot(rotBuf1) < 0.0f) { rotBuf1.negateLocal(); @@ -960,8 +1001,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { rotBuf2.negateLocal(); } + // Average the legs to calculate the pelvis rotBuf1.nlerp(rotBuf2, 0.5f); - rotBuf3.pureSlerpLocal(rotBuf1, hipSpinePelvisFactor); + + // Interpolate between the pelvis and the chest + rotBuf3.pureSlerpLocal(rotBuf1, hipSpinePelvisAveraging); + waistNode.localTransform.setRotation(rotBuf3); } } @@ -975,12 +1020,11 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { rotBuf2.nlerp(rotBuf1, 0.5f); waistNode.localTransform.getRotation(rotBuf1); - rotBuf2.slerpLocal(rotBuf1, pelvisHipFactor); + rotBuf2.slerpLocal(rotBuf1, pelvisHipAveraging); hipNode.localTransform.setRotation(rotBuf2); // Averages the trackerWaistNode's rotation with the calculated - // pelvis' - // on local yaw and roll. git blame AxisAngle :p + // pelvis' on local yaw and roll. git blame AxisAngle :p leftHipNode.localTransform.getRotation(rotBuf1); rightHipNode.localTransform.getRotation(rotBuf2); waistNode.localTransform.getRotation(rotBuf3); @@ -994,17 +1038,22 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { rotBuf2.negateLocal(); } - // Get WaiswaistNode's inverse rotation. + // Get waistNode's inverse rotation. rotBuf4.set(rotBuf3); rotBuf4.inverseLocal(); // Only rotate on local yaw and pitch - rotBuf4.set(rotBuf4.mult(rotBuf1.add(rotBuf2))); + // + // R = InverseWaist * (LeftLeft + RightLeg) + // C = Quaternion(-R.x, 0, 0, R.w) + // Pelvis = Waist * R * C + // normalize(Pelvis) + rotBuf4.multLocal(rotBuf1.add(rotBuf2)); rotBuf2.set(-rotBuf4.getX(), 0, 0, rotBuf4.getW()); rotBuf1.set(rotBuf3.mult(rotBuf4).mult(rotBuf2)); - rotBuf1.normalizeLocal(); - rotBuf1.slerpLocal(rotBuf3, pelvisWaistTrackerFactor); + + rotBuf1.slerpLocal(rotBuf3, pelvisWaistTrackerAveraging); trackerWaistNode.localTransform.setRotation(rotBuf1); } @@ -1099,37 +1148,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { } } - // #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 - ) { - vectorBuf1.set(0, -1, 0); - vectorBuf2.set(0, -1, 0); - hipBuf.multLocal(vectorBuf1); - kneeBuf.multLocal(vectorBuf2); - // Find knee angle - kneeRotation.angleBetweenVectors(vectorBuf1, vectorBuf2); - - // 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 - hipBuf.nlerp(kneeBuf, 0.5f); - // TODO : Use confidence to calculate changeAmt - - kneeBuf.set(hipBuf); - - // Return knee angle into knee rotation - kneeBuf.multLocal(kneeRotation); - } - // #endregion - // #region Update the output trackers protected void updateComputedTrackers() { if (computedChestTracker != null) { @@ -1147,7 +1165,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { if (computedLeftKneeTracker != null) { computedLeftKneeTracker.position .set(trackerLeftKneeNode.worldTransform.getTranslation()); - computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation()); + computedLeftKneeTracker.rotation.set(trackerLeftKneeNode.worldTransform.getRotation()); computedLeftKneeTracker.dataTick(); } @@ -1161,7 +1179,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { if (computedRightKneeTracker != null) { computedRightKneeTracker.position .set(trackerRightKneeNode.worldTransform.getTranslation()); - computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation()); + computedRightKneeTracker.rotation + .set(trackerRightKneeNode.worldTransform.getRotation()); computedRightKneeTracker.dataTick(); } @@ -1236,12 +1255,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // Cache the values of these configs switch (configToggle) { - case EXTENDED_PELVIS_MODEL: - extendedPelvisModel = newValue; - break; case EXTENDED_SPINE_MODEL: extendedSpineModel = newValue; break; + case EXTENDED_PELVIS_MODEL: + extendedPelvisModel = newValue; + break; case EXTENDED_KNEE_MODEL: extendedKneeModel = newValue; break; diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java index 89ec4c511..ffb362c7f 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java @@ -6,12 +6,12 @@ import java.util.Map; public enum SkeletonConfigToggle { - EXTENDED_PELVIS_MODEL("Extended pelvis model", "extendedPelvis", true), - EXTENDED_SPINE_MODEL("Extended spine model", "extendedSpine", true), - EXTENDED_KNEE_MODEL("Extended knee model", "extendedKnee", false),; + EXTENDED_SPINE_MODEL("Extended spine model", "spine", true), + EXTENDED_PELVIS_MODEL("Extended pelvis model", "pelvis", true), + EXTENDED_KNEE_MODEL("Extended knee model", "knee", true),; public static final SkeletonConfigToggle[] values = values(); - private static final String CONFIG_PREFIX = "body.model."; + private static final String CONFIG_PREFIX = "body.extendedModel."; private static final Map byStringVal = new HashMap<>(); static { diff --git a/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java b/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java index 603693abf..9a6ab345c 100644 --- a/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java @@ -46,7 +46,9 @@ public class ComputedTracker implements Tracker, TrackerWithTPS { // not be // allowed if editing is not allowed if (userEditable()) { - bodyPosition = TrackerPosition.getByDesignation(config.designation).get(); + TrackerPosition + .getByDesignation(config.designation) + .ifPresent(trackerPosition -> bodyPosition = trackerPosition); } } diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index c0d8481ca..9aa114b4f 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -4,8 +4,8 @@ import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; import dev.slimevr.VRServer; -import dev.slimevr.vr.trackers.udp.UDPDevice; import dev.slimevr.vr.trackers.udp.TrackersUDPServer; +import dev.slimevr.vr.trackers.udp.UDPDevice; import io.eiren.util.BufferedTimer; @@ -83,7 +83,9 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { } else { rotAdjust.loadIdentity(); } - bodyPosition = TrackerPosition.getByDesignation(config.designation).get(); + TrackerPosition + .getByDesignation(config.designation) + .ifPresent(trackerPosition -> bodyPosition = trackerPosition); setFilter( vrserver.config.getString("filters.type"), vrserver.config.getFloat("filters.amount", 0.3f),