mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-04-06 02:01:58 +02:00
Leg tweaks (#223)
This commit is contained in:
Collin Kees
GitHub
Submodule solarxr-protocol updated: 40e3c5ef84...8ec46c9f89
@@ -243,6 +243,24 @@ public class VRServer extends Thread {
|
||||
});
|
||||
}
|
||||
|
||||
public void setLegTweaksEnabled(boolean value) {
|
||||
queueTask(() -> {
|
||||
humanPoseProcessor.setLegTweaksEnabled(value);
|
||||
});
|
||||
}
|
||||
|
||||
public void setSkatingReductionEnabled(boolean value) {
|
||||
queueTask(() -> {
|
||||
humanPoseProcessor.setSkatingCorrectionEnabled(value);
|
||||
});
|
||||
}
|
||||
|
||||
public void setFloorClipEnabled(boolean value) {
|
||||
queueTask(() -> {
|
||||
humanPoseProcessor.setFloorClipEnabled(value);
|
||||
});
|
||||
}
|
||||
|
||||
public int getTrackersCount() {
|
||||
return trackers.size();
|
||||
}
|
||||
|
||||
@@ -478,6 +478,9 @@ public class AutoBone {
|
||||
intermediateOffsets
|
||||
);
|
||||
|
||||
skeleton1.setLegTweaksEnabled(false);
|
||||
skeleton2.setLegTweaksEnabled(false);
|
||||
|
||||
// If target height isn't specified, auto-detect
|
||||
if (targetHeight < 0f) {
|
||||
targetHeight = getTargetHeight(frames);
|
||||
|
||||
@@ -147,6 +147,7 @@ public class TrackersList extends EJBoxNoStretch {
|
||||
JLabel rotQuat;
|
||||
JLabel rotAdj;
|
||||
JLabel temperature;
|
||||
JLabel accel;
|
||||
|
||||
@AWTThread
|
||||
public TrackerPanel(Tracker t) {
|
||||
@@ -321,6 +322,10 @@ public class TrackersList extends EJBoxNoStretch {
|
||||
add(new JLabel("RotAdj:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
|
||||
add(rotAdj = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
|
||||
row++;
|
||||
|
||||
add(new JLabel("Accel:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
|
||||
add(accel = new JLabel("0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
|
||||
row++;
|
||||
}
|
||||
|
||||
if (debug && t instanceof ReferenceAdjustedTracker) {
|
||||
@@ -527,6 +532,16 @@ public class TrackersList extends EJBoxNoStretch {
|
||||
temperature.setText(StringUtils.prettyNumber(imu.temperature, 1) + "∘C");
|
||||
}
|
||||
}
|
||||
if (accel != null) {
|
||||
accel
|
||||
.setText(
|
||||
StringUtils.prettyNumber(imu.accelVector.x, 1)
|
||||
+ " "
|
||||
+ StringUtils.prettyNumber(imu.accelVector.y, 1)
|
||||
+ " "
|
||||
+ StringUtils.prettyNumber(imu.accelVector.z, 1)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -8,6 +8,7 @@ import dev.slimevr.gui.swing.EJBagNoStretch;
|
||||
import dev.slimevr.gui.swing.EJBox;
|
||||
import dev.slimevr.gui.swing.EJBoxNoStretch;
|
||||
import dev.slimevr.platform.windows.WindowsNamedPipeBridge;
|
||||
import dev.slimevr.vr.processor.skeleton.SkeletonConfigToggles;
|
||||
import dev.slimevr.vr.trackers.TrackerRole;
|
||||
import io.eiren.util.MacOSX;
|
||||
import io.eiren.util.OperatingSystem;
|
||||
@@ -42,6 +43,8 @@ public class VRServerGUI extends JFrame {
|
||||
private final SkeletonList skeletonList;
|
||||
private final EJBox pane;
|
||||
private JButton resetButton;
|
||||
private JButton floorClipButton;
|
||||
private JButton skatingCorrectionButton;
|
||||
|
||||
private WindowConfig config;
|
||||
|
||||
@@ -213,6 +216,50 @@ public class VRServerGUI extends JFrame {
|
||||
});
|
||||
}
|
||||
});
|
||||
add(Box.createHorizontalStrut(10));
|
||||
add(floorClipButton = new JButton("Toggle Floor Clip") {
|
||||
{
|
||||
addMouseListener(new MouseInputAdapter() {
|
||||
@Override
|
||||
public void mouseClicked(MouseEvent e) {
|
||||
boolean[] state = server.humanPoseProcessor.getLegTweaksState();
|
||||
setFloorClipEnabled(!state[0]);
|
||||
}
|
||||
});
|
||||
}
|
||||
});
|
||||
|
||||
setFloorClipEnabled(
|
||||
server
|
||||
.getConfigManager()
|
||||
.getVrConfig()
|
||||
.getSkeleton()
|
||||
.getToggles()
|
||||
.get(SkeletonConfigToggles.FLOOR_CLIP.configKey)
|
||||
);
|
||||
|
||||
add(Box.createHorizontalStrut(10));
|
||||
add(skatingCorrectionButton = new JButton("Toggle Skating Correction") {
|
||||
{
|
||||
addMouseListener(new MouseInputAdapter() {
|
||||
@Override
|
||||
public void mouseClicked(MouseEvent e) {
|
||||
boolean[] state = server.humanPoseProcessor.getLegTweaksState();
|
||||
setSkatingReductionEnabled(!state[1]);
|
||||
}
|
||||
});
|
||||
}
|
||||
});
|
||||
|
||||
setSkatingReductionEnabled(
|
||||
server
|
||||
.getConfigManager()
|
||||
.getVrConfig()
|
||||
.getSkeleton()
|
||||
.getToggles()
|
||||
.get(SkeletonConfigToggles.SKATING_CORRECTION.configKey)
|
||||
);
|
||||
|
||||
add(Box.createHorizontalGlue());
|
||||
add(new JButton("Record BVH") {
|
||||
{
|
||||
@@ -501,4 +548,29 @@ public class VRServerGUI extends JFrame {
|
||||
private void reset() {
|
||||
ButtonTimer.runTimer(resetButton, 3, "RESET", server::resetTrackers);
|
||||
}
|
||||
|
||||
@AWTThread
|
||||
private void setSkatingReductionEnabled(Boolean value) {
|
||||
if (value == null)
|
||||
value = false;
|
||||
|
||||
skatingCorrectionButton.setBackground(value ? Color.GREEN : Color.RED);
|
||||
|
||||
skatingCorrectionButton
|
||||
.setText("Skating Correction: " + (value ? "ON" : "OFF"));
|
||||
|
||||
server.setSkatingReductionEnabled(value);
|
||||
}
|
||||
|
||||
@AWTThread
|
||||
private void setFloorClipEnabled(Boolean value) {
|
||||
if (value == null)
|
||||
value = false;
|
||||
|
||||
floorClipButton.setBackground(value ? Color.GREEN : Color.RED);
|
||||
|
||||
// update the button
|
||||
floorClipButton.setText("Floor clip: " + (value ? "ON" : "OFF"));
|
||||
server.setFloorClipEnabled(value);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -162,6 +162,12 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getAcceleration(Vector3f store) {
|
||||
store.set(0, 0, 0);
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return name;
|
||||
|
||||
@@ -105,6 +105,12 @@ public final class TrackerFrame implements Tracker {
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getAcceleration(Vector3f store) {
|
||||
store.set(0, 0, 0);
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "TrackerFrame:/" + (designation != null ? designation.designation : "null");
|
||||
|
||||
@@ -312,7 +312,9 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
|
||||
config.getToggle(SkeletonConfigToggles.EXTENDED_SPINE_MODEL),
|
||||
config.getToggle(SkeletonConfigToggles.EXTENDED_PELVIS_MODEL),
|
||||
config.getToggle(SkeletonConfigToggles.EXTENDED_KNEE_MODEL),
|
||||
config.getToggle(SkeletonConfigToggles.FORCE_ARMS_FROM_HMD)
|
||||
config.getToggle(SkeletonConfigToggles.FORCE_ARMS_FROM_HMD),
|
||||
config.getToggle(SkeletonConfigToggles.SKATING_CORRECTION),
|
||||
config.getToggle(SkeletonConfigToggles.FLOOR_CLIP)
|
||||
);
|
||||
int ratiosOffset = ModelRatios
|
||||
.createModelRatios(
|
||||
@@ -377,34 +379,59 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
|
||||
var ratios = modelSettings.ratios();
|
||||
|
||||
if (toggles != null) {
|
||||
if (toggles.hasExtendedSpine()) {
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_SPINE_MODEL,
|
||||
toggles.extendedSpine()
|
||||
);
|
||||
}
|
||||
if (toggles.hasExtendedPelvis()) {
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_PELVIS_MODEL,
|
||||
toggles.extendedPelvis()
|
||||
);
|
||||
}
|
||||
if (toggles.hasExtendedKnee()) {
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_KNEE_MODEL,
|
||||
toggles.extendedKnee()
|
||||
);
|
||||
}
|
||||
if (toggles.forceArmsFromHmd()) {
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.FORCE_ARMS_FROM_HMD,
|
||||
toggles.forceArmsFromHmd()
|
||||
);
|
||||
}
|
||||
// Note: toggles.has____ returns the same as toggles._____ this
|
||||
// seems like a bug
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_SPINE_MODEL,
|
||||
toggles.extendedSpine()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_PELVIS_MODEL,
|
||||
toggles.extendedPelvis()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_KNEE_MODEL,
|
||||
toggles.extendedKnee()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.FORCE_ARMS_FROM_HMD,
|
||||
toggles.forceArmsFromHmd()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_SPINE_MODEL,
|
||||
toggles.extendedSpine()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_PELVIS_MODEL,
|
||||
toggles.extendedPelvis()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.EXTENDED_KNEE_MODEL,
|
||||
toggles.extendedKnee()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.FORCE_ARMS_FROM_HMD,
|
||||
toggles.forceArmsFromHmd()
|
||||
);
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.FLOOR_CLIP,
|
||||
toggles.floorClip()
|
||||
);
|
||||
|
||||
cfg
|
||||
.setToggle(
|
||||
SkeletonConfigToggles.SKATING_CORRECTION,
|
||||
toggles.skatingCorrection()
|
||||
);
|
||||
}
|
||||
|
||||
if (ratios != null) {
|
||||
|
||||
@@ -6,6 +6,7 @@ import dev.slimevr.vr.processor.skeleton.Skeleton;
|
||||
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
|
||||
import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
|
||||
import dev.slimevr.vr.processor.skeleton.SkeletonConfigOffsets;
|
||||
import dev.slimevr.vr.processor.skeleton.SkeletonConfigToggles;
|
||||
import dev.slimevr.vr.trackers.*;
|
||||
import io.eiren.util.ann.ThreadSafe;
|
||||
import io.eiren.util.collections.FastList;
|
||||
@@ -194,4 +195,45 @@ public class HumanPoseProcessor {
|
||||
if (skeleton != null)
|
||||
skeleton.resetTrackersYaw();
|
||||
}
|
||||
|
||||
@ThreadSafe
|
||||
public boolean[] getLegTweaksState() {
|
||||
return skeleton.getLegTweaksState();
|
||||
}
|
||||
|
||||
@VRServerThread
|
||||
public void setLegTweaksEnabled(boolean value) {
|
||||
if (skeleton != null)
|
||||
skeleton.setLegTweaksEnabled(value);
|
||||
}
|
||||
|
||||
@VRServerThread
|
||||
public void setFloorClipEnabled(boolean value) {
|
||||
if (skeleton != null) {
|
||||
skeleton.setFloorclipEnabled(value);
|
||||
server
|
||||
.getConfigManager()
|
||||
.getVrConfig()
|
||||
.getSkeleton()
|
||||
.getToggles()
|
||||
.put(SkeletonConfigToggles.FLOOR_CLIP.configKey, value);
|
||||
|
||||
server.getConfigManager().saveConfig();
|
||||
}
|
||||
}
|
||||
|
||||
@VRServerThread
|
||||
public void setSkatingCorrectionEnabled(boolean value) {
|
||||
if (skeleton != null) {
|
||||
skeleton.setSkatingCorrectionEnabled(value);
|
||||
server
|
||||
.getConfigManager()
|
||||
.getVrConfig()
|
||||
.getSkeleton()
|
||||
.getToggles()
|
||||
.put(SkeletonConfigToggles.SKATING_CORRECTION.configKey, value);
|
||||
|
||||
server.getConfigManager().saveConfig();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -131,6 +131,10 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
|
||||
protected boolean sendAllBones = false;
|
||||
// #endregion
|
||||
|
||||
// #region Clip Correction
|
||||
protected LegTweaks legTweaks = new LegTweaks(this);
|
||||
// #endregion
|
||||
|
||||
// #region Constructors
|
||||
protected HumanSkeleton(List<? extends ComputedHumanPoseTracker> computedTrackers) {
|
||||
assembleSkeleton(false);
|
||||
@@ -787,6 +791,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
|
||||
updateLocalTransforms();
|
||||
updateRootTrackers();
|
||||
updateComputedTrackers();
|
||||
tweakLegPos();
|
||||
}
|
||||
// #endregion
|
||||
|
||||
@@ -800,6 +805,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
|
||||
}
|
||||
}
|
||||
|
||||
// correct any clipping that is happening to the feet trackers
|
||||
private void tweakLegPos() {
|
||||
// correct the foot positions
|
||||
legTweaks.tweakLegs();
|
||||
}
|
||||
|
||||
// #region Update the node transforms from the trackers
|
||||
protected void updateLocalTransforms() {
|
||||
// #region Pass all trackers through trackerPreUpdate for Autobone
|
||||
@@ -1335,6 +1346,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
|
||||
case FORCE_ARMS_FROM_HMD:
|
||||
forceArmsFromHMD = newValue;
|
||||
break;
|
||||
case SKATING_CORRECTION:
|
||||
legTweaks.setSkatingReductionEnabled(newValue);
|
||||
break;
|
||||
case FLOOR_CLIP:
|
||||
legTweaks.setFloorclipEnabled(newValue);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1802,6 +1819,11 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
|
||||
tracker.resetFull(referenceRotation);
|
||||
}
|
||||
}
|
||||
|
||||
// tell the clip corrector to reset its floor level on the next update
|
||||
// of the computed trackers
|
||||
this.legTweaks.resetFloorLevel();
|
||||
this.legTweaks.resetBuffer();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -1820,5 +1842,33 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
|
||||
tracker.resetYaw(referenceRotation);
|
||||
}
|
||||
}
|
||||
this.legTweaks.resetBuffer();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean[] getLegTweaksState() {
|
||||
boolean[] state = new boolean[2];
|
||||
state[0] = this.legTweaks.getFloorclipEnabled();
|
||||
state[1] = this.legTweaks.getSkatingReductionEnabled();
|
||||
return state;
|
||||
}
|
||||
|
||||
// master enable/disable of all leg tweaks (for autobone)
|
||||
@Override
|
||||
@VRServerThread
|
||||
public void setLegTweaksEnabled(boolean value) {
|
||||
this.legTweaks.setEnabled(value);
|
||||
}
|
||||
|
||||
@Override
|
||||
@VRServerThread
|
||||
public void setFloorclipEnabled(boolean value) {
|
||||
this.skeletonConfig.setToggle(SkeletonConfigToggles.FLOOR_CLIP, value);
|
||||
}
|
||||
|
||||
@Override
|
||||
@VRServerThread
|
||||
public void setSkatingCorrectionEnabled(boolean value) {
|
||||
this.skeletonConfig.setToggle(SkeletonConfigToggles.SKATING_CORRECTION, value);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,657 @@
|
||||
package dev.slimevr.vr.processor.skeleton;
|
||||
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
|
||||
|
||||
/**
|
||||
* class that holds data related to the state and other variuse attributes of
|
||||
* the legs such as the position of the foot, knee, and waist, after and before
|
||||
* correction, the velocity of the foot and the computed state of the feet at
|
||||
* that frame. mainly calculates the state of the legs per frame using these
|
||||
* rules: The conditions for an unlock are as follows: 1. the foot is to far
|
||||
* from its correct position 2. a velocity higher than a threashold is achived
|
||||
* 3. a large acceleration is applied to the foot 4. angular velocity of the
|
||||
* foot goes higher than a threashold The conditions for a lock are the opposite
|
||||
* of the above but require a lower value for all of the above conditions
|
||||
*/
|
||||
|
||||
public class LegTweakBuffer {
|
||||
public static final int STATE_UNKNOWN = 0; // fall back state
|
||||
public static final int LOCKED = 1;
|
||||
public static final int UNLOCKED = 2;
|
||||
public static final int FOOT_ACCEL = 3;
|
||||
public static final int ANKLE_ACCEL = 4;
|
||||
|
||||
public static final float NS_CONVERT = 1000000000.0f;
|
||||
private static final Vector3f placeHolderVec = new Vector3f();
|
||||
private static final Quaternion placeHolderQuat = new Quaternion();
|
||||
|
||||
// states for the legs
|
||||
private int leftLegState = STATE_UNKNOWN;
|
||||
private int rightLegState = STATE_UNKNOWN;
|
||||
|
||||
// positions and rotations
|
||||
private Vector3f leftFootPosition = placeHolderVec;
|
||||
private Vector3f rightFootPosition = placeHolderVec;
|
||||
private Vector3f leftKneePosition = placeHolderVec;
|
||||
private Vector3f rightKneePosition = placeHolderVec;
|
||||
private Vector3f waistPosition = placeHolderVec;
|
||||
private Quaternion leftFootRotation = placeHolderQuat;
|
||||
private Quaternion rightFootRotation = placeHolderQuat;
|
||||
|
||||
private Vector3f leftFootPositionCorrected = placeHolderVec;
|
||||
private Vector3f rightFootPositionCorrected = placeHolderVec;
|
||||
private Vector3f leftKneePositionCorrected = placeHolderVec;
|
||||
private Vector3f rightKneePositionCorrected = placeHolderVec;
|
||||
private Vector3f waistPositionCorrected = placeHolderVec;
|
||||
|
||||
// velocities
|
||||
private Vector3f leftFootVelocity = placeHolderVec;
|
||||
private float leftFootVelocityMagnitude = 0;
|
||||
private Vector3f rightFootVelocity = placeHolderVec;
|
||||
private float rightFootVelocityMagnitude = 0;
|
||||
private float leftFootAngleDiff = 0;
|
||||
private float rightFootAngleDiff = 0;
|
||||
|
||||
// acceleration
|
||||
private Vector3f leftFootAcceleration = placeHolderVec;
|
||||
private float leftFootAccelerationMagnitude = 0;
|
||||
private Vector3f rightFootAcceleration = placeHolderVec;
|
||||
private float rightFootAccelerationMagnitude = 0;
|
||||
|
||||
// other data
|
||||
private long timeOfFrame = System.nanoTime();
|
||||
private LegTweakBuffer parent = null; // frame before this one
|
||||
private int frameNumber = 0; // higher number is older frame
|
||||
private int detectionMode = ANKLE_ACCEL; // detection mode
|
||||
private boolean accelerationAboveThresholdLeft = true;
|
||||
private boolean accelerationAboveThresholdRight = true;
|
||||
private float leftFloorLevel;
|
||||
private float rightFloorLevel;
|
||||
|
||||
// hyperparameters
|
||||
public static final float SKATING_DISTANCE_CUTOFF = 0.225f;
|
||||
private static final float SKATING_VELOCITY_THRESHOLD = 4.25f;
|
||||
private static final float SKATING_ACCELERATION_THRESHOLD = 1.15f;
|
||||
private static final float SKATING_ROTVELOCITY_THRESHOLD = 4.5f;
|
||||
private static final float SKATING_LOCK_ENGAGE_PERCENT = 0.85f;
|
||||
private static final float SKATING_ACCELERATION_Y_USE_PERCENT = 0.25f;
|
||||
private static final float FLOOR_DISTANCE_CUTOFF = 0.125f;
|
||||
private static final float SIX_TRACKER_TOLLERANCE = 0.10f;
|
||||
|
||||
private static final float PARAM_SCALAR_MAX = 2.5f;
|
||||
private static final float PARAM_SCALAR_MIN = 0.5f;
|
||||
private static final float PARAM_SCALAR_MID = 1.0f;
|
||||
|
||||
// the point at which the scalar is at the max or min depending on accel
|
||||
private static final float MAX_SCALAR_ACCEL = 0.3f;
|
||||
private static final float MIN_SCALAR_ACCEL = 0.9f;
|
||||
|
||||
// the point at which the scalar is at it max or min in a double locked foot
|
||||
// situation
|
||||
private static final float MAX_SCALAR_DORMANT = 0.6f;
|
||||
private static final float MIN_SCALAR_DORMANT = 2.0f;
|
||||
|
||||
// the point at which the scalar is at it max or min in a single locked foot
|
||||
// situation
|
||||
private static final float MIN_SCALAR_ACTIVE = 3.0f;
|
||||
private static final float MAX_SCALAR_ACTIVE = 0.2f;
|
||||
|
||||
private float leftFootSensitivityVel = 1.0f;
|
||||
private float rightFootSensitivityVel = 1.0f;
|
||||
private float leftFootSensitivityAccel = 1.0f;
|
||||
private float rightFootSensitivityAccel = 1.0f;
|
||||
|
||||
private static final float SKATING_CUTOFF_ENGAGE = SKATING_DISTANCE_CUTOFF
|
||||
* SKATING_LOCK_ENGAGE_PERCENT;
|
||||
private static final float SKATING_VELOCITY_CUTOFF_ENGAGE = SKATING_VELOCITY_THRESHOLD
|
||||
* SKATING_LOCK_ENGAGE_PERCENT;
|
||||
private static final float SKATING_ACCELERATION_CUTOFF_ENGAGE = SKATING_ACCELERATION_THRESHOLD
|
||||
* SKATING_LOCK_ENGAGE_PERCENT;
|
||||
private static final float SKATING_ROTATIONAL_VELOCITY_CUTOFF_ENGAGE = SKATING_ROTVELOCITY_THRESHOLD
|
||||
* SKATING_LOCK_ENGAGE_PERCENT;
|
||||
|
||||
// getters and setters
|
||||
public Vector3f getLeftFootPosition(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(leftFootPosition);
|
||||
}
|
||||
|
||||
public void setLeftFootPosition(Vector3f leftFootPosition) {
|
||||
this.leftFootPosition = leftFootPosition.clone();
|
||||
}
|
||||
|
||||
public Vector3f getRightFootPosition(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(rightFootPosition);
|
||||
}
|
||||
|
||||
public void setRightFootPosition(Vector3f rightFootPosition) {
|
||||
this.rightFootPosition = rightFootPosition.clone();
|
||||
}
|
||||
|
||||
public Vector3f getLeftKneePosition(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(leftKneePosition);
|
||||
}
|
||||
|
||||
public void setLeftKneePosition(Vector3f leftKneePosition) {
|
||||
this.leftKneePosition = leftKneePosition.clone();
|
||||
}
|
||||
|
||||
public Vector3f getRightKneePosition(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
return vec.set(rightKneePosition);
|
||||
}
|
||||
|
||||
public void setRightKneePosition(Vector3f rightKneePosition) {
|
||||
this.rightKneePosition = rightKneePosition.clone();
|
||||
}
|
||||
|
||||
public Vector3f getWaistPosition(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(waistPosition);
|
||||
}
|
||||
|
||||
public void setWaistPosition(Vector3f waistPosition) {
|
||||
this.waistPosition = waistPosition.clone();
|
||||
}
|
||||
|
||||
public Quaternion getLeftFootRotation(Quaternion quat) {
|
||||
if (quat == null)
|
||||
quat = new Quaternion();
|
||||
|
||||
return quat.set(leftFootRotation);
|
||||
}
|
||||
|
||||
public void setLeftFootRotation(Quaternion leftFootRotation) {
|
||||
this.leftFootRotation = leftFootRotation.clone();
|
||||
}
|
||||
|
||||
public Quaternion getRightFootRotation(Quaternion quat) {
|
||||
if (quat == null)
|
||||
quat = new Quaternion();
|
||||
|
||||
return quat.set(rightFootRotation);
|
||||
}
|
||||
|
||||
public void setRightFootRotation(Quaternion rightFootRotation) {
|
||||
this.rightFootRotation = rightFootRotation.clone();
|
||||
}
|
||||
|
||||
public Vector3f getLeftFootPositionCorrected(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(leftFootPositionCorrected);
|
||||
}
|
||||
|
||||
public void setLeftFootPositionCorrected(Vector3f leftFootPositionCorrected) {
|
||||
this.leftFootPositionCorrected = leftFootPositionCorrected.clone();
|
||||
}
|
||||
|
||||
public Vector3f getRightFootPositionCorrected(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(rightFootPositionCorrected);
|
||||
}
|
||||
|
||||
public void setRightFootPositionCorrected(Vector3f rightFootPositionCorrected) {
|
||||
this.rightFootPositionCorrected = rightFootPositionCorrected.clone();
|
||||
}
|
||||
|
||||
public Vector3f getLeftKneePositionCorrected(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(leftKneePositionCorrected);
|
||||
}
|
||||
|
||||
public void setLeftKneePositionCorrected(Vector3f leftKneePositionCorrected) {
|
||||
this.leftKneePositionCorrected = leftKneePositionCorrected.clone();
|
||||
}
|
||||
|
||||
public Vector3f getRightKneePositionCorrected(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(rightKneePositionCorrected);
|
||||
}
|
||||
|
||||
public void setRightKneePositionCorrected(Vector3f rightKneePositionCorrected) {
|
||||
this.rightKneePositionCorrected = rightKneePositionCorrected.clone();
|
||||
}
|
||||
|
||||
public Vector3f getWaistPositionCorrected(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(waistPositionCorrected);
|
||||
}
|
||||
|
||||
public void setWaistPositionCorrected(Vector3f waistPositionCorrected) {
|
||||
this.waistPositionCorrected = waistPositionCorrected.clone();
|
||||
}
|
||||
|
||||
public Vector3f getLeftFootVelocity(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(leftFootVelocity);
|
||||
}
|
||||
|
||||
public Vector3f getRightFootVelocity(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(rightFootVelocity);
|
||||
}
|
||||
|
||||
public void setLeftFloorLevel(float leftFloorLevel) {
|
||||
this.leftFloorLevel = leftFloorLevel;
|
||||
}
|
||||
|
||||
public void setRightFloorLevel(float rightFloorLevel) {
|
||||
this.rightFloorLevel = rightFloorLevel;
|
||||
}
|
||||
|
||||
public int getLeftLegState() {
|
||||
return leftLegState;
|
||||
}
|
||||
|
||||
public void setLeftLegState(int leftLegState) {
|
||||
this.leftLegState = leftLegState;
|
||||
}
|
||||
|
||||
public int getRightLegState() {
|
||||
return rightLegState;
|
||||
}
|
||||
|
||||
public void setRightLegState(int rightLegState) {
|
||||
this.rightLegState = rightLegState;
|
||||
}
|
||||
|
||||
public void setParent(LegTweakBuffer parent) {
|
||||
this.parent = parent;
|
||||
}
|
||||
|
||||
public LegTweakBuffer getParent() {
|
||||
return parent;
|
||||
}
|
||||
|
||||
public void setLeftFootAcceleration(Vector3f leftFootAcceleration) {
|
||||
this.leftFootAcceleration = leftFootAcceleration.clone();
|
||||
}
|
||||
|
||||
public void setRightFootAcceleration(Vector3f rightFootAcceleration) {
|
||||
this.rightFootAcceleration = rightFootAcceleration.clone();
|
||||
}
|
||||
|
||||
public Vector3f getLeftFootAcceleration(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(leftFootAcceleration);
|
||||
}
|
||||
|
||||
public Vector3f getRightFootAcceleration(Vector3f vec) {
|
||||
if (vec == null)
|
||||
vec = new Vector3f();
|
||||
|
||||
return vec.set(rightFootAcceleration);
|
||||
}
|
||||
|
||||
public float getLeftFootAccelerationMagnitude() {
|
||||
return this.leftFootAcceleration.length();
|
||||
}
|
||||
|
||||
public float getRightFootAccelerationMagnitude() {
|
||||
return this.rightFootAcceleration.length();
|
||||
}
|
||||
|
||||
public float getLeftFootAccelerationY() {
|
||||
return this.leftFootAcceleration.y;
|
||||
}
|
||||
|
||||
public float getRightFootAccelerationY() {
|
||||
return this.rightFootAcceleration.y;
|
||||
}
|
||||
|
||||
public void setDetectionMode(int mode) {
|
||||
this.detectionMode = mode;
|
||||
}
|
||||
|
||||
// calculate momvent attributes
|
||||
public void calculateFootAttributes(boolean active) {
|
||||
updateFrameNumber(0);
|
||||
|
||||
// compute attributes of the legs
|
||||
computeVelocity();
|
||||
computeAccelerationMagnitude();
|
||||
|
||||
// check if the acceleration triggers forced unlock
|
||||
if (detectionMode == FOOT_ACCEL) {
|
||||
computeAccelerationAboveThresholdFootTrackers();
|
||||
} else {
|
||||
computeAccelerationAboveThresholdAnkleTrackers();
|
||||
}
|
||||
|
||||
// calculate the scalar for other parameters
|
||||
computeScalar();
|
||||
|
||||
// if correction is inactive state is unknown (default to unlocked)
|
||||
if (!active) {
|
||||
leftLegState = UNLOCKED;
|
||||
rightLegState = UNLOCKED;
|
||||
} else {
|
||||
computeState();
|
||||
}
|
||||
}
|
||||
|
||||
// update the frame number of all the frames
|
||||
public void updateFrameNumber(int frameNumber) {
|
||||
this.frameNumber = frameNumber;
|
||||
|
||||
if (this.frameNumber >= 10) {
|
||||
this.parent = null; // once a frame is 10 frames old, it is no
|
||||
// longer
|
||||
// needed
|
||||
}
|
||||
|
||||
if (parent != null) {
|
||||
parent.updateFrameNumber(frameNumber + 1);
|
||||
}
|
||||
}
|
||||
|
||||
// compute the state of the legs
|
||||
private void computeState() {
|
||||
// based on the last state of the legs compute their state for this
|
||||
// individual frame
|
||||
leftLegState = checkStateLeft();
|
||||
rightLegState = checkStateRight();
|
||||
}
|
||||
|
||||
// check if a locked foot should stay locked or be released
|
||||
private int checkStateLeft() {
|
||||
float timeStep = 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT);
|
||||
|
||||
if (parent.leftLegState == UNLOCKED) {
|
||||
if (
|
||||
parent.getLeftFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE
|
||||
|| leftFootVelocityMagnitude * timeStep
|
||||
> SKATING_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
|
||||
|| leftFootAngleDiff * timeStep
|
||||
> SKATING_ROTATIONAL_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
|
||||
|| leftFootPosition.y > leftFloorLevel + FLOOR_DISTANCE_CUTOFF
|
||||
|| accelerationAboveThresholdLeft
|
||||
) {
|
||||
return UNLOCKED;
|
||||
}
|
||||
|
||||
return LOCKED;
|
||||
}
|
||||
|
||||
if (
|
||||
parent.getLeftFootHorizantalDifference() > SKATING_DISTANCE_CUTOFF
|
||||
|| leftFootVelocityMagnitude * timeStep
|
||||
> SKATING_VELOCITY_THRESHOLD * leftFootSensitivityVel
|
||||
|| leftFootAngleDiff * timeStep
|
||||
> SKATING_ROTVELOCITY_THRESHOLD * leftFootSensitivityVel
|
||||
|| leftFootPosition.y > leftFloorLevel + FLOOR_DISTANCE_CUTOFF
|
||||
|| accelerationAboveThresholdLeft
|
||||
) {
|
||||
return UNLOCKED;
|
||||
}
|
||||
|
||||
return LOCKED;
|
||||
}
|
||||
|
||||
// check if a locked foot should stay locked or be released
|
||||
private int checkStateRight() {
|
||||
float timeStep = 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT);
|
||||
|
||||
if (parent.rightLegState == UNLOCKED) {
|
||||
if (
|
||||
parent.getRightFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE
|
||||
|| rightFootVelocityMagnitude * timeStep
|
||||
> SKATING_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
|
||||
|| rightFootAngleDiff * timeStep
|
||||
> SKATING_ROTATIONAL_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
|
||||
|| rightFootPosition.y > rightFloorLevel + FLOOR_DISTANCE_CUTOFF
|
||||
|| accelerationAboveThresholdRight
|
||||
) {
|
||||
return UNLOCKED;
|
||||
}
|
||||
|
||||
return LOCKED;
|
||||
}
|
||||
|
||||
if (
|
||||
parent.getRightFootHorizantalDifference() > SKATING_DISTANCE_CUTOFF
|
||||
|| rightFootVelocityMagnitude * timeStep
|
||||
> SKATING_VELOCITY_THRESHOLD * rightFootSensitivityVel
|
||||
|| rightFootAngleDiff * timeStep
|
||||
> SKATING_ROTVELOCITY_THRESHOLD * rightFootSensitivityVel
|
||||
|| rightFootPosition.y > rightFloorLevel + FLOOR_DISTANCE_CUTOFF
|
||||
|| accelerationAboveThresholdRight
|
||||
) {
|
||||
return UNLOCKED;
|
||||
}
|
||||
|
||||
return LOCKED;
|
||||
}
|
||||
|
||||
// get the difference in feet position between the kinematic and corrected
|
||||
// positions of the feet disregarding vertical displacment
|
||||
private float getLeftFootHorizantalDifference() {
|
||||
return leftFootPositionCorrected.subtract(leftFootPosition).setY(0).length();
|
||||
}
|
||||
|
||||
// get the difference in feet position between the kinematic and corrected
|
||||
// positions of the feet
|
||||
private float getRightFootHorizantalDifference() {
|
||||
return rightFootPositionCorrected.subtract(rightFootPosition).setY(0).length();
|
||||
}
|
||||
|
||||
// get the angular velocity of the left foot (kinda we just want a scalar)
|
||||
private float getLeftFootAngularVelocity() {
|
||||
return leftFootRotation
|
||||
.getRotationColumn(2)
|
||||
.distance(parent.leftFootRotation.getRotationColumn(2));
|
||||
}
|
||||
|
||||
// get the angular velocity of the right foot (kinda we just want a scalar)
|
||||
private float getRightFootAngularVelocity() {
|
||||
return rightFootRotation
|
||||
.getRotationColumn(2)
|
||||
.distance(parent.rightFootRotation.getRotationColumn(2));
|
||||
}
|
||||
|
||||
// compute the velocity of the feet from the position in the last frames
|
||||
private void computeVelocity() {
|
||||
if (parent == null)
|
||||
return;
|
||||
|
||||
leftFootVelocity = leftFootPosition.subtract(parent.leftFootPosition);
|
||||
leftFootVelocityMagnitude = leftFootVelocity.length();
|
||||
rightFootVelocity = rightFootPosition.subtract(parent.rightFootPosition);
|
||||
rightFootVelocityMagnitude = rightFootVelocity.length();
|
||||
leftFootAngleDiff = getLeftFootAngularVelocity();
|
||||
rightFootAngleDiff = getRightFootAngularVelocity();
|
||||
}
|
||||
|
||||
// get the nth parent of this frame
|
||||
private LegTweakBuffer getNParent(int n) {
|
||||
if (n == 0 || parent == null)
|
||||
return this;
|
||||
|
||||
return parent.getNParent(n - 1);
|
||||
}
|
||||
|
||||
// compute the acceleration magnitude of the feet from the acceleration
|
||||
// given by the imus (exclude y)
|
||||
private void computeAccelerationMagnitude() {
|
||||
leftFootAccelerationMagnitude = leftFootAcceleration
|
||||
.setY(leftFootAcceleration.y * SKATING_ACCELERATION_Y_USE_PERCENT)
|
||||
.length();
|
||||
|
||||
rightFootAccelerationMagnitude = rightFootAcceleration
|
||||
.setY(rightFootAcceleration.y * SKATING_ACCELERATION_Y_USE_PERCENT)
|
||||
.length();
|
||||
}
|
||||
|
||||
// for 8 trackers the data from the imus is enough to determine lock/unlock
|
||||
private void computeAccelerationAboveThresholdFootTrackers() {
|
||||
accelerationAboveThresholdLeft = leftFootAccelerationMagnitude
|
||||
> SKATING_ACCELERATION_CUTOFF_ENGAGE * leftFootSensitivityAccel;
|
||||
accelerationAboveThresholdRight = rightFootAccelerationMagnitude
|
||||
> SKATING_ACCELERATION_CUTOFF_ENGAGE * rightFootSensitivityAccel;
|
||||
}
|
||||
|
||||
// for any setup without foot trackers the data from the imus is enough to
|
||||
// determine lock/unlock
|
||||
private void computeAccelerationAboveThresholdAnkleTrackers() {
|
||||
accelerationAboveThresholdLeft = leftFootAccelerationMagnitude
|
||||
> (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLLERANCE) * leftFootSensitivityAccel;
|
||||
accelerationAboveThresholdRight = rightFootAccelerationMagnitude
|
||||
> (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLLERANCE) * rightFootSensitivityAccel;
|
||||
}
|
||||
|
||||
// using the parent lock/unlock states, velocity, and acceleration,
|
||||
// determine the scalars to apply to the hyperparameters when computing the
|
||||
// lock state
|
||||
private void computeScalar() {
|
||||
// get the first set of scalars that are based on acceleration from the
|
||||
// imus
|
||||
float leftFootScalarAccel = getLeftFootScalarAccel();
|
||||
float rightFootScalarAccel = getRightFootScalarAccel();
|
||||
|
||||
// get the second set of scalars that is based of of how close each foot
|
||||
// is to a lock and dynamically adjusting the scalars
|
||||
// (based off the assumption that if you are standing one foot is likly
|
||||
// planted on the ground unless you are moving fast)
|
||||
float leftFootScalarVel = getLeftFootLockLiklyHood();
|
||||
float rightFootScalarVel = getRightFootLockLiklyHood();
|
||||
|
||||
// combine these two sets of scalars to get the final scalars
|
||||
// ( -1 just makes is so if both scalars are 1.0 the final scalar is 1.0
|
||||
// not 2)
|
||||
leftFootSensitivityVel = leftFootScalarAccel + leftFootScalarVel - 1.0f;
|
||||
rightFootSensitivityVel = rightFootScalarAccel + rightFootScalarVel - 1.0f;
|
||||
|
||||
leftFootSensitivityAccel = leftFootScalarVel;
|
||||
rightFootSensitivityAccel = rightFootScalarVel;
|
||||
}
|
||||
|
||||
// calculate a scalar using acceleration to apply to the non acceleration
|
||||
// based hyperparameters when calculating
|
||||
// lock states
|
||||
private float getLeftFootScalarAccel() {
|
||||
if (leftLegState == LOCKED) {
|
||||
if (leftFootAccelerationMagnitude < MAX_SCALAR_ACCEL) {
|
||||
return PARAM_SCALAR_MAX;
|
||||
} else if (leftFootAccelerationMagnitude > MIN_SCALAR_ACCEL) {
|
||||
return PARAM_SCALAR_MAX
|
||||
* (leftFootAccelerationMagnitude - MIN_SCALAR_ACCEL)
|
||||
/ (MAX_SCALAR_ACCEL - MIN_SCALAR_ACCEL);
|
||||
}
|
||||
}
|
||||
|
||||
return PARAM_SCALAR_MID;
|
||||
}
|
||||
|
||||
private float getRightFootScalarAccel() {
|
||||
if (rightLegState == LOCKED) {
|
||||
if (rightFootAccelerationMagnitude < MAX_SCALAR_ACCEL) {
|
||||
return PARAM_SCALAR_MAX;
|
||||
} else if (rightFootAccelerationMagnitude > MIN_SCALAR_ACCEL) {
|
||||
return PARAM_SCALAR_MAX
|
||||
* (rightFootAccelerationMagnitude - MIN_SCALAR_ACCEL)
|
||||
/ (MAX_SCALAR_ACCEL - MIN_SCALAR_ACCEL);
|
||||
}
|
||||
}
|
||||
|
||||
return PARAM_SCALAR_MID;
|
||||
}
|
||||
|
||||
// calculate a scalar using the velocity of the foot trackers and the lock
|
||||
// states to calculate a scalar to apply to the non acceleration based
|
||||
// hyperparameters when calculating
|
||||
// lock states
|
||||
private float getLeftFootLockLiklyHood() {
|
||||
if (leftLegState == LOCKED && rightLegState == LOCKED) {
|
||||
Vector3f velocityDiff = leftFootVelocity.subtract(rightFootVelocity);
|
||||
velocityDiff.setY(0.0f);
|
||||
float velocityDiffMagnitude = velocityDiff.length();
|
||||
|
||||
if (velocityDiffMagnitude < MAX_SCALAR_DORMANT) {
|
||||
return PARAM_SCALAR_MAX;
|
||||
} else if (velocityDiffMagnitude > MIN_SCALAR_DORMANT) {
|
||||
return PARAM_SCALAR_MAX
|
||||
* (velocityDiffMagnitude - MIN_SCALAR_DORMANT)
|
||||
/ (MAX_SCALAR_DORMANT - MIN_SCALAR_DORMANT);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the 'unlockedness factor' and use that to
|
||||
// determine the scalar (go as low as 0.5 as as high as
|
||||
// param_scalar_max)
|
||||
float velocityDifAbs = Math.abs(leftFootVelocityMagnitude)
|
||||
- Math.abs(rightFootVelocityMagnitude);
|
||||
|
||||
if (velocityDifAbs > MIN_SCALAR_ACTIVE) {
|
||||
return PARAM_SCALAR_MIN;
|
||||
} else if (velocityDifAbs < MAX_SCALAR_ACTIVE) {
|
||||
return PARAM_SCALAR_MAX;
|
||||
}
|
||||
|
||||
return PARAM_SCALAR_MAX
|
||||
* (velocityDifAbs - MIN_SCALAR_ACTIVE)
|
||||
/ (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE)
|
||||
- PARAM_SCALAR_MID;
|
||||
}
|
||||
|
||||
private float getRightFootLockLiklyHood() {
|
||||
if (rightLegState == LOCKED && leftLegState == LOCKED) {
|
||||
Vector3f velocityDiff = rightFootVelocity.subtract(leftFootVelocity);
|
||||
velocityDiff.setY(0.0f);
|
||||
float velocityDiffMagnitude = velocityDiff.length();
|
||||
|
||||
if (velocityDiffMagnitude < MAX_SCALAR_DORMANT) {
|
||||
return PARAM_SCALAR_MAX;
|
||||
} else if (velocityDiffMagnitude > MIN_SCALAR_DORMANT) {
|
||||
return PARAM_SCALAR_MAX
|
||||
* (velocityDiffMagnitude - MIN_SCALAR_DORMANT)
|
||||
/ (MAX_SCALAR_DORMANT - MIN_SCALAR_DORMANT);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the 'unlockedness factor' and use that to
|
||||
// determine the scalar (go as low as 0.5 as as high as
|
||||
// param_scalar_max)
|
||||
float velocityDifAbs = Math.abs(rightFootVelocityMagnitude)
|
||||
- Math.abs(leftFootVelocityMagnitude);
|
||||
|
||||
if (velocityDifAbs > MIN_SCALAR_ACTIVE) {
|
||||
return PARAM_SCALAR_MIN;
|
||||
} else if (velocityDifAbs < MAX_SCALAR_ACTIVE) {
|
||||
return PARAM_SCALAR_MAX;
|
||||
}
|
||||
|
||||
return PARAM_SCALAR_MAX
|
||||
* (velocityDifAbs - MIN_SCALAR_ACTIVE)
|
||||
/ (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE)
|
||||
- PARAM_SCALAR_MID;
|
||||
}
|
||||
}
|
||||
887
src/main/java/dev/slimevr/vr/processor/skeleton/LegTweaks.java
Normal file
887
src/main/java/dev/slimevr/vr/processor/skeleton/LegTweaks.java
Normal file
@@ -0,0 +1,887 @@
|
||||
package dev.slimevr.vr.processor.skeleton;
|
||||
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
|
||||
|
||||
public class LegTweaks {
|
||||
// state variables
|
||||
private float floorLevel;
|
||||
private float waistToFloorDist;
|
||||
private float currentDisengagementOffset = 0.0f;
|
||||
private boolean initialized = true;
|
||||
private boolean enabled = true; // master switch
|
||||
private boolean floorclipEnabled = false;
|
||||
private boolean skatingCorrectionEnabled = false;
|
||||
private boolean active = false;
|
||||
private boolean rightLegActive = false;
|
||||
private boolean leftLegActive = false;
|
||||
|
||||
// skeleton
|
||||
private HumanSkeleton skeleton;
|
||||
|
||||
// leg data
|
||||
private Vector3f leftFootPosition = new Vector3f();
|
||||
private Vector3f rightFootPosition = new Vector3f();
|
||||
private Vector3f leftKneePosition = new Vector3f();
|
||||
private Vector3f rightKneePosition = new Vector3f();
|
||||
private Vector3f waistPosition = new Vector3f();
|
||||
private Quaternion leftFootRotation = new Quaternion();
|
||||
private Quaternion rightFootRotation = new Quaternion();
|
||||
private Vector3f leftWaistUpperLegOffset = new Vector3f();
|
||||
private Vector3f rightWaistUpperLegOffset = new Vector3f();
|
||||
|
||||
private Vector3f leftFootAcceleration = new Vector3f();
|
||||
private Vector3f rightFootAcceleration = new Vector3f();
|
||||
private Vector3f leftLowerLegAcceleration = new Vector3f();
|
||||
private Vector3f rightLowerLegAcceleration = new Vector3f();
|
||||
|
||||
// knee placeholder
|
||||
private Vector3f leftKneePlaceholder = new Vector3f();
|
||||
private Vector3f rightKneePlaceholder = new Vector3f();
|
||||
private boolean kneesActive = false;
|
||||
|
||||
/**
|
||||
* here is a explination of each parameter that may need explaining
|
||||
* STANDING_CUTOFF_VERTICAL is the percentage the waist has to be below its
|
||||
* position at calibration to register as the user not standing
|
||||
* MAX_DISENGAGMENT_OFFSET is how much the floor will be shifted to allow an
|
||||
* offset to happen smoothly DYNAMIC_DISPLACEMENT_CUTOFF is the percent of
|
||||
* downwards rotation that can contribute to dynamic displacment
|
||||
* MAX_DYNAMIC_DISPLACMENT is the max amount the floor will be moved up to
|
||||
* account for the foot rotating downward and needing to be put higher to
|
||||
* avoid clipping in the gameworld MIN_ACCEPTABLE_ERROR and
|
||||
* MAX_ACCEPTABLE_ERROR Defines the disitance where CORRECTION_WEIGHT_MIN
|
||||
* and CORRECTION_WEIGHT_MAX are calculating a percent of velocity to
|
||||
* correct rather than using the min or max FLOOR_CALIBRATION_OFFSET is the
|
||||
* amount the floor plane is shifted up. This can help the feet from
|
||||
* floating slightly above the ground
|
||||
*/
|
||||
|
||||
// hyperparameters (clip correction)
|
||||
private static final float DYNAMIC_DISPLACEMENT_CUTOFF = 0.8f;
|
||||
private static final float MAX_DYNAMIC_DISPLACEMENT = 0.08f;
|
||||
private static final float FLOOR_CALIBRATION_OFFSET = 0.02f;
|
||||
|
||||
// hyperparameters (skating correction)
|
||||
private static final float MIN_ACCEPTABLE_ERROR = 0.05f;
|
||||
private static final float MAX_ACCEPTABLE_ERROR = LegTweakBuffer.SKATING_DISTANCE_CUTOFF;
|
||||
private static final float CORRECTION_WEIGHT_MIN = 0.25f;
|
||||
private static final float CORRECTION_WEIGHT_MAX = 0.70f;
|
||||
|
||||
// hyperparameters (floating feet correction)
|
||||
private static final float FOOT_Y_CORRECTION_WEIGHT = 0.45f;
|
||||
private static final float FOOT_Y_MAX_ACCELERATION = 0.40f;
|
||||
private static final float FOOT_Y_DIFF_CUTOFF = 0.15f;
|
||||
|
||||
// hyperparameters (knee / waist correction)
|
||||
private static final float KNEE_CORRECTION_WEIGHT = 0.00f;
|
||||
private static final float KNEE_LATERAL_WEIGHT = 0.9f;
|
||||
private static final float WAIST_PUSH_WEIGHT = 0.2f;
|
||||
|
||||
// hyperparameters (misc)
|
||||
private static final float NEARLY_ZERO = 0.005f;
|
||||
private static final float STANDING_CUTOFF_VERTICAL = 0.65f;
|
||||
private static final float MAX_DISENGAGMENT_OFFSET = 0.30f;
|
||||
|
||||
// buffer for holding previus frames of data
|
||||
private LegTweakBuffer bufferHead = new LegTweakBuffer();
|
||||
private boolean bufferInvalid = true;
|
||||
|
||||
public LegTweaks(HumanSkeleton skeleton) {
|
||||
this.skeleton = skeleton;
|
||||
}
|
||||
|
||||
// update the offsets for the waist and upper leg
|
||||
// this is used for correcting the knee tracker position
|
||||
public void updateOffsets(Vector3f upperLeftLeg, Vector3f upperRightLeg, Vector3f waist) {
|
||||
// update the relevant leg data
|
||||
this.leftWaistUpperLegOffset = upperLeftLeg.subtract(waist);
|
||||
this.rightWaistUpperLegOffset = upperRightLeg.subtract(waist);
|
||||
}
|
||||
|
||||
public Vector3f getLeftFootPosition() {
|
||||
return leftFootPosition;
|
||||
}
|
||||
|
||||
public void setLeftFootPosition(Vector3f leftFootPosition) {
|
||||
this.leftFootPosition = leftFootPosition.clone();
|
||||
}
|
||||
|
||||
public void setLeftFootRotation(Quaternion leftFootRotation) {
|
||||
this.leftFootRotation = leftFootRotation.clone();
|
||||
}
|
||||
|
||||
public Vector3f getRightFootPosition() {
|
||||
return rightFootPosition;
|
||||
}
|
||||
|
||||
public void setRightFootPosition(Vector3f rightFootPosition) {
|
||||
this.rightFootPosition = rightFootPosition.clone();
|
||||
}
|
||||
|
||||
public void setRightFootRotation(Quaternion rightFootRotation) {
|
||||
this.rightFootRotation = rightFootRotation.clone();
|
||||
}
|
||||
|
||||
public Vector3f getLeftKneePosition() {
|
||||
return leftKneePosition;
|
||||
}
|
||||
|
||||
public void setLeftKneePosition(Vector3f leftKneePosition) {
|
||||
this.leftKneePosition = leftKneePosition.clone();
|
||||
}
|
||||
|
||||
public Vector3f getRightKneePosition() {
|
||||
return rightKneePosition;
|
||||
}
|
||||
|
||||
public void setRightKneePosition(Vector3f rightKneePosition) {
|
||||
this.rightKneePosition = rightKneePosition.clone();
|
||||
}
|
||||
|
||||
public Vector3f getWaistPosition() {
|
||||
return waistPosition;
|
||||
}
|
||||
|
||||
public void setWaistPosition(Vector3f waistPosition) {
|
||||
this.waistPosition = waistPosition.clone();
|
||||
}
|
||||
|
||||
public double getFloorLevel() {
|
||||
return floorLevel;
|
||||
}
|
||||
|
||||
public void setFloorLevel(float floorLevel) {
|
||||
this.floorLevel = floorLevel;
|
||||
}
|
||||
|
||||
public void resetFloorLevel() {
|
||||
this.initialized = false;
|
||||
}
|
||||
|
||||
public boolean getActive() {
|
||||
return this.active;
|
||||
}
|
||||
|
||||
public void setEnabled(boolean enabled) {
|
||||
this.enabled = enabled;
|
||||
}
|
||||
|
||||
public void setFloorclipEnabled(boolean floorclipEnabled) {
|
||||
this.floorclipEnabled = floorclipEnabled;
|
||||
|
||||
// reset the buffer
|
||||
this.bufferHead = new LegTweakBuffer();
|
||||
this.bufferInvalid = true;
|
||||
}
|
||||
|
||||
public void setSkatingReductionEnabled(boolean skatingCorrectionEnabled) {
|
||||
this.skatingCorrectionEnabled = skatingCorrectionEnabled;
|
||||
|
||||
// reset the buffer
|
||||
this.bufferHead = new LegTweakBuffer();
|
||||
this.bufferInvalid = true;
|
||||
}
|
||||
|
||||
public boolean getEnabled() {
|
||||
return this.enabled;
|
||||
}
|
||||
|
||||
public boolean getFloorclipEnabled() {
|
||||
return this.floorclipEnabled;
|
||||
}
|
||||
|
||||
public boolean getSkatingReductionEnabled() {
|
||||
return this.skatingCorrectionEnabled;
|
||||
}
|
||||
|
||||
public void resetBuffer() {
|
||||
bufferInvalid = true;
|
||||
}
|
||||
|
||||
// set the vectors in this object to the vectors in the skeleton
|
||||
private void setVectors() {
|
||||
// set the positions of the feet and knees to the skeletons current
|
||||
// positions
|
||||
if (skeleton.computedLeftKneeTracker != null || skeleton.computedRightKneeTracker != null) {
|
||||
kneesActive = true;
|
||||
leftKneePosition = skeleton.computedLeftKneeTracker.position;
|
||||
rightKneePosition = skeleton.computedRightKneeTracker.position;
|
||||
} else {
|
||||
kneesActive = false;
|
||||
leftKneePosition = leftKneePlaceholder;
|
||||
rightKneePosition = rightKneePlaceholder;
|
||||
}
|
||||
|
||||
if (
|
||||
skeleton.computedLeftFootTracker != null
|
||||
&& skeleton.computedRightFootTracker != null
|
||||
&& skeleton.computedWaistTracker != null
|
||||
) {
|
||||
leftFootPosition = skeleton.computedLeftFootTracker.position;
|
||||
rightFootPosition = skeleton.computedRightFootTracker.position;
|
||||
waistPosition = skeleton.computedWaistTracker.position;
|
||||
leftFootRotation = skeleton.computedLeftFootTracker.rotation;
|
||||
rightFootRotation = skeleton.computedRightFootTracker.rotation;
|
||||
}
|
||||
|
||||
// get the vector for acceleration of the feet and knees
|
||||
// (if feet are not available, fallback to 6 tracker mode)
|
||||
if (skeleton.leftFootTracker != null && skeleton.rightFootTracker != null) {
|
||||
skeleton.leftFootTracker.getAcceleration(leftFootAcceleration);
|
||||
skeleton.rightFootTracker.getAcceleration(rightFootAcceleration);
|
||||
} else {
|
||||
leftFootAcceleration.set(0, 0, 0);
|
||||
rightFootAcceleration.set(0, 0, 0);
|
||||
}
|
||||
|
||||
if (skeleton.leftLowerLegTracker != null && skeleton.rightLowerLegTracker != null) {
|
||||
skeleton.leftLowerLegTracker.getAcceleration(leftLowerLegAcceleration);
|
||||
skeleton.rightLowerLegTracker.getAcceleration(rightLowerLegAcceleration);
|
||||
} else {
|
||||
leftLowerLegAcceleration.set(0, 0, 0);
|
||||
rightLowerLegAcceleration.set(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// updates the object with the latest data from the skeleton
|
||||
private boolean preUpdate() {
|
||||
// populate the vectors with the latest data
|
||||
setVectors();
|
||||
|
||||
// if not initialized, we need to calculate some values from this frame
|
||||
// to be used later (must happen immediately after reset)
|
||||
if (!initialized) {
|
||||
floorLevel = (leftFootPosition.y + rightFootPosition.y) / 2f + FLOOR_CALIBRATION_OFFSET;
|
||||
waistToFloorDist = waistPosition.y - floorLevel;
|
||||
|
||||
// invalidate the buffer since the non initialized output may be
|
||||
// very wrong
|
||||
bufferInvalid = true;
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
// if not enabled do nothing and return false
|
||||
if (!enabled)
|
||||
return false;
|
||||
|
||||
// if the user is standing start checking for a good time to enable leg
|
||||
// tweaks
|
||||
active = isStanding();
|
||||
|
||||
// if the buffer is invalid set it up
|
||||
if (bufferInvalid) {
|
||||
bufferHead.setLeftFootPositionCorrected(leftFootPosition);
|
||||
bufferHead.setRightFootPositionCorrected(rightFootPosition);
|
||||
bufferHead.setLeftKneePositionCorrected(leftKneePosition);
|
||||
bufferHead.setRightKneePositionCorrected(rightKneePosition);
|
||||
bufferHead.setWaistPositionCorrected(waistPosition);
|
||||
bufferHead.setLeftFootPosition(leftFootPosition);
|
||||
bufferHead.setRightFootPosition(rightFootPosition);
|
||||
bufferHead.setLeftKneePosition(leftKneePosition);
|
||||
bufferHead.setRightKneePosition(rightKneePosition);
|
||||
bufferHead.setWaistPosition(waistPosition);
|
||||
bufferHead.setLeftLegState(LegTweakBuffer.UNLOCKED);
|
||||
bufferHead.setRightLegState(LegTweakBuffer.UNLOCKED);
|
||||
bufferInvalid = false;
|
||||
}
|
||||
|
||||
// update offsets for knee correction if the knees are not null
|
||||
if (kneesActive) {
|
||||
Vector3f temp1 = new Vector3f();
|
||||
Vector3f temp2 = new Vector3f();
|
||||
Vector3f temp3 = new Vector3f();
|
||||
|
||||
// get offsets from the waist to the upper legs
|
||||
skeleton.leftHipNode.localTransform.getTranslation(temp1);
|
||||
skeleton.rightHipNode.localTransform.getTranslation(temp2);
|
||||
skeleton.trackerWaistNode.localTransform.getTranslation(temp3);
|
||||
updateOffsets(temp1, temp2, temp3);
|
||||
}
|
||||
|
||||
// update the buffer
|
||||
LegTweakBuffer currentFrame = new LegTweakBuffer();
|
||||
currentFrame.setLeftFootPosition(leftFootPosition);
|
||||
currentFrame.setLeftFootRotation(leftFootRotation);
|
||||
currentFrame.setLeftKneePosition(leftKneePosition);
|
||||
currentFrame.setRightFootPosition(rightFootPosition);
|
||||
currentFrame.setRightFootRotation(rightFootRotation);
|
||||
currentFrame.setRightKneePosition(rightKneePosition);
|
||||
currentFrame.setWaistPosition(waistPosition);
|
||||
|
||||
this.bufferHead
|
||||
.setLeftFloorLevel(
|
||||
(floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset()))
|
||||
- currentDisengagementOffset
|
||||
);
|
||||
|
||||
this.bufferHead
|
||||
.setRightFloorLevel(
|
||||
(floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset()))
|
||||
- currentDisengagementOffset
|
||||
);
|
||||
|
||||
// put the acceleration vector that is applicable to the tracker
|
||||
// quantity in the the buffer
|
||||
// (if feet are not available, fallback to 6 tracker mode)
|
||||
if (skeleton.leftFootTracker != null && skeleton.rightFootTracker != null) {
|
||||
currentFrame.setLeftFootAcceleration(leftFootAcceleration);
|
||||
currentFrame.setRightFootAcceleration(rightFootAcceleration);
|
||||
currentFrame.setDetectionMode(LegTweakBuffer.FOOT_ACCEL);
|
||||
} else if (skeleton.leftLowerLegTracker != null && skeleton.rightLowerLegTracker != null) {
|
||||
currentFrame.setLeftFootAcceleration(leftLowerLegAcceleration);
|
||||
currentFrame.setRightFootAcceleration(rightLowerLegAcceleration);
|
||||
currentFrame.setDetectionMode(LegTweakBuffer.ANKLE_ACCEL);
|
||||
}
|
||||
|
||||
currentFrame.setParent(bufferHead);
|
||||
this.bufferHead = currentFrame;
|
||||
this.bufferHead.calculateFootAttributes(active);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// tweak the position of the legs based on data from the last frames
|
||||
public void tweakLegs() {
|
||||
// update the class with the latest data from the skeleton
|
||||
// if false is returned something indicated that the legs should not be
|
||||
// tweaked
|
||||
if (!preUpdate())
|
||||
return;
|
||||
|
||||
// push the feet up if needed
|
||||
if (floorclipEnabled)
|
||||
correctClipping();
|
||||
|
||||
// calculate acceleration and velocity of the feet using the buffer
|
||||
// (only needed if skating correction is enabled)
|
||||
if (skatingCorrectionEnabled) {
|
||||
correctSkating();
|
||||
correctFloat();
|
||||
}
|
||||
|
||||
// determine if either leg is in a position to activate or deactivate
|
||||
// (use the buffer to get the positions before corrections)
|
||||
float leftFootDif = bufferHead
|
||||
.getLeftFootPosition(null)
|
||||
.subtract(leftFootPosition)
|
||||
.setX(0)
|
||||
.setZ(0)
|
||||
.length();
|
||||
|
||||
float rightFootDif = bufferHead
|
||||
.getRightFootPosition(null)
|
||||
.subtract(rightFootPosition)
|
||||
.setX(0)
|
||||
.setZ(0)
|
||||
.length();
|
||||
|
||||
if (!active && leftFootDif < NEARLY_ZERO) {
|
||||
leftLegActive = false;
|
||||
} else if (active && leftFootDif < NEARLY_ZERO) {
|
||||
leftLegActive = true;
|
||||
}
|
||||
|
||||
if (!active && rightFootDif < NEARLY_ZERO) {
|
||||
rightLegActive = false;
|
||||
} else if (active && rightFootDif < NEARLY_ZERO) {
|
||||
rightLegActive = true;
|
||||
}
|
||||
|
||||
// restore the y positions of inactive legs
|
||||
if (!leftLegActive) {
|
||||
leftFootPosition.y = bufferHead.getLeftFootPosition(null).y;
|
||||
leftKneePosition.y = bufferHead.getLeftKneePosition(null).y;
|
||||
}
|
||||
|
||||
if (!rightLegActive) {
|
||||
rightFootPosition.y = bufferHead.getRightFootPosition(null).y;
|
||||
rightKneePosition.y = bufferHead.getRightKneePosition(null).y;
|
||||
}
|
||||
|
||||
// calculate the correction for the knees
|
||||
if (kneesActive && initialized)
|
||||
solveLowerBody();
|
||||
|
||||
// populate the corrected data into the current frame
|
||||
this.bufferHead.setLeftFootPositionCorrected(leftFootPosition);
|
||||
this.bufferHead.setRightFootPositionCorrected(rightFootPosition);
|
||||
this.bufferHead.setLeftKneePositionCorrected(leftKneePosition);
|
||||
this.bufferHead.setRightKneePositionCorrected(rightKneePosition);
|
||||
this.bufferHead.setWaistPositionCorrected(waistPosition);
|
||||
}
|
||||
|
||||
// returns true if the foot is clipped and false if it is not
|
||||
public boolean isClipped(float leftOffset, float rightOffset) {
|
||||
return (leftFootPosition.y < floorLevel + leftOffset
|
||||
|| rightFootPosition.y < floorLevel + rightOffset);
|
||||
}
|
||||
|
||||
// corrects the foot position to be above the floor level that is calculated
|
||||
// on calibration
|
||||
private void correctClipping() {
|
||||
// calculate how angled down the feet are as a scalar value between 0
|
||||
// and 1 (0 = flat, 1 = max angle)
|
||||
float leftOffset = getLeftFootOffset();
|
||||
float rightOffset = getRightFootOffset();
|
||||
float avgOffset = 0;
|
||||
|
||||
// if there is no clipping, or clipping is not enabled, return false
|
||||
if (!isClipped(leftOffset, rightOffset) || !enabled)
|
||||
return;
|
||||
|
||||
// move the feet to their new positions
|
||||
if (
|
||||
leftFootPosition.y
|
||||
< (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * leftOffset))
|
||||
- currentDisengagementOffset
|
||||
) {
|
||||
float displacement = Math
|
||||
.abs(
|
||||
floorLevel
|
||||
+ (MAX_DYNAMIC_DISPLACEMENT * leftOffset)
|
||||
- leftFootPosition.y
|
||||
- currentDisengagementOffset
|
||||
);
|
||||
|
||||
leftFootPosition.y += displacement;
|
||||
leftKneePosition.y += displacement;
|
||||
avgOffset += displacement;
|
||||
}
|
||||
|
||||
if (
|
||||
rightFootPosition.y
|
||||
< (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * rightOffset))
|
||||
- currentDisengagementOffset
|
||||
) {
|
||||
float displacement = Math
|
||||
.abs(
|
||||
floorLevel
|
||||
+ (MAX_DYNAMIC_DISPLACEMENT * rightOffset)
|
||||
- rightFootPosition.y
|
||||
- currentDisengagementOffset
|
||||
);
|
||||
|
||||
rightFootPosition.y += displacement;
|
||||
rightKneePosition.y += displacement;
|
||||
avgOffset += displacement;
|
||||
}
|
||||
|
||||
waistPosition.y += (avgOffset / 2) * WAIST_PUSH_WEIGHT;
|
||||
}
|
||||
|
||||
// based on the data from the last frame compute a new position that reduces
|
||||
// ice skating
|
||||
private void correctSkating() {
|
||||
// for either foot that is locked get its position (x and z only we let
|
||||
// y move freely) and set it to be there
|
||||
if (bufferHead.getLeftLegState() == LegTweakBuffer.LOCKED) {
|
||||
leftFootPosition.x = bufferHead
|
||||
.getParent()
|
||||
.getLeftFootPositionCorrected(null).x;
|
||||
|
||||
leftFootPosition.z = bufferHead
|
||||
.getParent()
|
||||
.getLeftFootPositionCorrected(null).z;
|
||||
}
|
||||
|
||||
if (bufferHead.getRightLegState() == LegTweakBuffer.LOCKED) {
|
||||
rightFootPosition.x = bufferHead
|
||||
.getParent()
|
||||
.getRightFootPositionCorrected(null).x;
|
||||
|
||||
rightFootPosition.z = bufferHead
|
||||
.getParent()
|
||||
.getRightFootPositionCorrected(null).z;
|
||||
}
|
||||
|
||||
// for either foot that is unlocked get its last position and calculate
|
||||
// its position for this frame. the amount of displacement is based on
|
||||
// the distance between the last position, the current position, and
|
||||
// the hyperparameters
|
||||
correctUnlockedLeftFootTracker();
|
||||
correctUnlockedRightFootTracker();
|
||||
}
|
||||
|
||||
private void correctUnlockedLeftFootTracker() {
|
||||
if (bufferHead.getLeftLegState() == LegTweakBuffer.UNLOCKED) {
|
||||
Vector3f leftFootDif = leftFootPosition
|
||||
.subtract(bufferHead.getParent().getLeftFootPositionCorrected(null))
|
||||
.setY(0);
|
||||
|
||||
if (leftFootDif.length() > NEARLY_ZERO) {
|
||||
float leftY = leftFootPosition.y;
|
||||
|
||||
Vector3f temp = bufferHead.getParent().getLeftFootPositionCorrected(null);
|
||||
Vector3f velocity = bufferHead.getLeftFootVelocity(null);
|
||||
|
||||
// first add the difference from the last frame to this frame
|
||||
temp = temp
|
||||
.subtract(
|
||||
bufferHead
|
||||
.getParent()
|
||||
.getLeftFootPosition(null)
|
||||
.subtract(bufferHead.getLeftFootPosition(null))
|
||||
);
|
||||
|
||||
leftFootPosition.y = leftY;
|
||||
leftFootPosition.x = temp.x;
|
||||
leftFootPosition.z = temp.z;
|
||||
|
||||
// if velocity and dif are pointing in the same direction,
|
||||
// add a small amount of velocity to the dif
|
||||
// else subtract a small amount of velocity from the dif
|
||||
// calculate the correction weight
|
||||
float weight = calculateCorrectionWeight(
|
||||
leftFootPosition,
|
||||
bufferHead.getParent().getLeftFootPositionCorrected(null)
|
||||
);
|
||||
|
||||
if (velocity.x * leftFootDif.x > 0) {
|
||||
leftFootPosition.x += velocity.x * weight;
|
||||
} else {
|
||||
leftFootPosition.x -= velocity.x * weight;
|
||||
}
|
||||
|
||||
if (velocity.z * leftFootDif.z > 0) {
|
||||
leftFootPosition.z += velocity.z * weight;
|
||||
} else {
|
||||
leftFootPosition.z -= velocity.z * weight;
|
||||
}
|
||||
|
||||
// if the foot overshot the target, move it back to the target
|
||||
if (
|
||||
checkOverShoot(
|
||||
this.bufferHead.getLeftFootPosition(null).x,
|
||||
this.bufferHead.getParent().getLeftFootPositionCorrected(null).x,
|
||||
leftFootPosition.x
|
||||
)
|
||||
) {
|
||||
leftFootPosition.x = bufferHead.getLeftFootPosition(null).x;
|
||||
}
|
||||
|
||||
if (
|
||||
checkOverShoot(
|
||||
this.bufferHead.getLeftFootPosition(null).z,
|
||||
this.bufferHead.getParent().getLeftFootPositionCorrected(null).z,
|
||||
leftFootPosition.z
|
||||
)
|
||||
) {
|
||||
leftFootPosition.z = bufferHead.getLeftFootPosition(null).z;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void correctUnlockedRightFootTracker() {
|
||||
if (bufferHead.getRightLegState() == LegTweakBuffer.UNLOCKED) {
|
||||
Vector3f rightFootDif = rightFootPosition
|
||||
.subtract(bufferHead.getParent().getRightFootPositionCorrected(null))
|
||||
.setY(0);
|
||||
|
||||
if (rightFootDif.length() > NEARLY_ZERO) {
|
||||
float rightY = rightFootPosition.y;
|
||||
Vector3f temp = bufferHead
|
||||
.getParent()
|
||||
.getRightFootPositionCorrected(null);
|
||||
Vector3f velocity = bufferHead.getRightFootVelocity(null);
|
||||
|
||||
// first add the difference from the last frame to this frame
|
||||
temp = temp
|
||||
.subtract(
|
||||
bufferHead
|
||||
.getParent()
|
||||
.getRightFootPosition(null)
|
||||
.subtract(bufferHead.getRightFootPosition(null))
|
||||
);
|
||||
|
||||
rightFootPosition.y = rightY;
|
||||
rightFootPosition.x = temp.x;
|
||||
rightFootPosition.z = temp.z;
|
||||
|
||||
// if velocity and dif are pointing in the same direction,
|
||||
// add a small amount of velocity to the dif
|
||||
// else subtract a small amount of velocity from the dif
|
||||
// calculate the correction weight
|
||||
float weight = calculateCorrectionWeight(
|
||||
rightFootPosition,
|
||||
bufferHead.getParent().getRightFootPositionCorrected(null)
|
||||
);
|
||||
|
||||
if (velocity.x * rightFootDif.x > 0) {
|
||||
rightFootPosition.x += velocity.x * weight;
|
||||
} else {
|
||||
rightFootPosition.x -= velocity.x * weight;
|
||||
}
|
||||
|
||||
if (velocity.z * rightFootDif.z > 0) {
|
||||
rightFootPosition.z += velocity.z * weight;
|
||||
} else {
|
||||
rightFootPosition.z -= velocity.z * weight;
|
||||
}
|
||||
|
||||
// if the foot overshot the target, move it back to the target
|
||||
if (
|
||||
checkOverShoot(
|
||||
this.bufferHead.getRightFootPosition(null).x,
|
||||
this.bufferHead.getParent().getRightFootPositionCorrected(null).x,
|
||||
rightFootPosition.x
|
||||
)
|
||||
) {
|
||||
rightFootPosition.x = bufferHead.getRightFootPosition(null).x;
|
||||
}
|
||||
|
||||
if (
|
||||
checkOverShoot(
|
||||
this.bufferHead.getRightFootPosition(null).z,
|
||||
this.bufferHead.getParent().getRightFootPositionCorrected(null).z,
|
||||
rightFootPosition.z
|
||||
)
|
||||
) {
|
||||
rightFootPosition.z = bufferHead.getRightFootPosition(null).z;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check if the foot is begining to float over the floor when it should not
|
||||
// be if it is move it back down to the floor
|
||||
// note: very small corrections are all that is allowed to prevent momemnt
|
||||
// that looks unrealistic
|
||||
private void correctFloat() {
|
||||
boolean correctLeft = true;
|
||||
boolean correctRight = true;
|
||||
|
||||
if (bufferHead.getLeftLegState() == LegTweakBuffer.LOCKED) {
|
||||
float lastPositionY = bufferHead
|
||||
.getParent()
|
||||
.getLeftFootPositionCorrected(null).y;
|
||||
if (
|
||||
Math.abs(leftFootPosition.y - lastPositionY) < FOOT_Y_DIFF_CUTOFF
|
||||
&& leftFootPosition.y > lastPositionY
|
||||
&& bufferHead.getLeftFootAccelerationY() < FOOT_Y_MAX_ACCELERATION
|
||||
) {
|
||||
leftFootPosition.y = lastPositionY;
|
||||
correctLeft = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (bufferHead.getRightLegState() == LegTweakBuffer.LOCKED) {
|
||||
float lastPositionY = bufferHead
|
||||
.getParent()
|
||||
.getRightFootPositionCorrected(null).y;
|
||||
|
||||
if (
|
||||
Math.abs(rightFootPosition.y - lastPositionY) < FOOT_Y_DIFF_CUTOFF
|
||||
&& rightFootPosition.y > lastPositionY
|
||||
&& bufferHead.getRightFootAccelerationY() < FOOT_Y_MAX_ACCELERATION
|
||||
) {
|
||||
rightFootPosition.y = lastPositionY;
|
||||
correctRight = false;
|
||||
}
|
||||
}
|
||||
|
||||
// using the velocity correct the position
|
||||
if (correctLeft)
|
||||
correctLeftFootTrackerY();
|
||||
|
||||
if (correctRight)
|
||||
correctRightFootTrackerY();
|
||||
}
|
||||
|
||||
private void correctLeftFootTrackerY() {
|
||||
Vector3f temp;
|
||||
Vector3f leftFootDif = leftFootPosition
|
||||
.subtract(bufferHead.getParent().getLeftFootPositionCorrected(null));
|
||||
|
||||
if (Math.abs(leftFootDif.y) > NEARLY_ZERO) {
|
||||
temp = bufferHead
|
||||
.getParent()
|
||||
.getLeftFootPositionCorrected(null)
|
||||
.subtract(
|
||||
bufferHead
|
||||
.getParent()
|
||||
.getLeftFootPosition(null)
|
||||
.subtract(bufferHead.getLeftFootPosition(null))
|
||||
);
|
||||
|
||||
float leftFloor = (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset()))
|
||||
- currentDisengagementOffset;
|
||||
|
||||
leftFootPosition.y = temp.y < leftFloor ? leftFloor : temp.y;
|
||||
|
||||
Vector3f velocity = bufferHead.getLeftFootVelocity(null);
|
||||
|
||||
if (velocity.y * leftFootDif.y > 0) {
|
||||
leftFootPosition.y += velocity.y * FOOT_Y_CORRECTION_WEIGHT;
|
||||
} else {
|
||||
leftFootPosition.y -= velocity.y * FOOT_Y_CORRECTION_WEIGHT;
|
||||
}
|
||||
|
||||
// check for overshoot and correct if necessary
|
||||
if (
|
||||
checkOverShoot(
|
||||
this.bufferHead.getLeftFootPosition(null).y,
|
||||
this.bufferHead.getParent().getLeftFootPositionCorrected(null).y,
|
||||
leftFootPosition.y
|
||||
)
|
||||
) {
|
||||
leftFootPosition.y = bufferHead.getLeftFootPosition(null).y;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void correctRightFootTrackerY() {
|
||||
Vector3f temp;
|
||||
Vector3f rightFootDif = rightFootPosition
|
||||
.subtract(bufferHead.getParent().getRightFootPositionCorrected(null));
|
||||
|
||||
if (Math.abs(rightFootDif.y) > NEARLY_ZERO) {
|
||||
temp = bufferHead
|
||||
.getParent()
|
||||
.getRightFootPositionCorrected(null)
|
||||
.subtract(
|
||||
bufferHead
|
||||
.getParent()
|
||||
.getRightFootPosition(null)
|
||||
.subtract(bufferHead.getRightFootPosition(null))
|
||||
);
|
||||
|
||||
float rightFloor = (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset()))
|
||||
- currentDisengagementOffset;
|
||||
|
||||
rightFootPosition.y = temp.y < rightFloor ? rightFloor : temp.y;
|
||||
|
||||
Vector3f velocity = bufferHead.getRightFootVelocity(null);
|
||||
|
||||
if (velocity.y * rightFootDif.y > 0) {
|
||||
rightFootPosition.y += velocity.y * FOOT_Y_CORRECTION_WEIGHT;
|
||||
} else {
|
||||
rightFootPosition.y -= velocity.y * FOOT_Y_CORRECTION_WEIGHT;
|
||||
}
|
||||
|
||||
// check for overshoot and correct if necessary
|
||||
if (
|
||||
checkOverShoot(
|
||||
this.bufferHead.getRightFootPosition(null).y,
|
||||
this.bufferHead.getParent().getRightFootPositionCorrected(null).y,
|
||||
rightFootPosition.y
|
||||
)
|
||||
) {
|
||||
rightFootPosition.y = bufferHead.getRightFootPosition(null).y;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// returns true if it is likely the user is standing
|
||||
public boolean isStanding() {
|
||||
// if the waist is below the vertical cutoff, user is not standing
|
||||
float cutoff = floorLevel
|
||||
+ waistToFloorDist
|
||||
- (waistToFloorDist * STANDING_CUTOFF_VERTICAL);
|
||||
|
||||
if (waistPosition.y < cutoff) {
|
||||
currentDisengagementOffset = (1 - waistPosition.y / cutoff)
|
||||
* MAX_DISENGAGMENT_OFFSET;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
currentDisengagementOffset = 0f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// move the knees in to a position that is closer to the truth
|
||||
private void solveLowerBody() {
|
||||
// calculate the left and right waist nodes in standing space
|
||||
Vector3f leftWaist = waistPosition;// .add(leftWaistUpperLegOffset);
|
||||
Vector3f rightWaist = waistPosition;// .add(rightWaistUpperLegOffset);
|
||||
|
||||
Vector3f tempLeft;
|
||||
Vector3f tempRight;
|
||||
|
||||
// before moveing the knees back closer to the waist nodes offset them
|
||||
// the same amount the foot trackers where offset
|
||||
float leftXDif = leftFootPosition.x - bufferHead.getLeftFootPosition(null).x;
|
||||
float rightXDif = rightFootPosition.x - bufferHead.getRightFootPosition(null).x;
|
||||
float leftZDif = leftFootPosition.z - bufferHead.getLeftFootPosition(null).z;
|
||||
float rightZDif = rightFootPosition.z - bufferHead.getRightFootPosition(null).z;
|
||||
|
||||
leftKneePosition.x += leftXDif * KNEE_LATERAL_WEIGHT;
|
||||
leftKneePosition.z += leftZDif * KNEE_LATERAL_WEIGHT;
|
||||
rightKneePosition.x += rightXDif * KNEE_LATERAL_WEIGHT;
|
||||
rightKneePosition.z += rightZDif * KNEE_LATERAL_WEIGHT;
|
||||
|
||||
// calculate the bone distances
|
||||
float leftKneeWaist = bufferHead.getLeftKneePosition(null).distance(leftWaist);
|
||||
float rightKneeWaist = bufferHead.getRightKneePosition(null).distance(rightWaist);
|
||||
|
||||
float leftKneeWaistNew = leftKneePosition.distance(leftWaist);
|
||||
float rightKneeWaistNew = rightKneePosition.distance(rightWaist);
|
||||
float leftKneeOffset = leftKneeWaistNew - leftKneeWaist;
|
||||
float rightKneeOffset = rightKneeWaistNew - rightKneeWaist;
|
||||
|
||||
// get the vector from the waist to the knee
|
||||
Vector3f leftKneeVector = leftKneePosition
|
||||
.subtract(leftWaist)
|
||||
.normalize()
|
||||
.mult(leftKneeOffset * KNEE_CORRECTION_WEIGHT);
|
||||
|
||||
Vector3f rightKneeVector = rightKneePosition
|
||||
.subtract(rightWaist)
|
||||
.normalize()
|
||||
.mult(rightKneeOffset * KNEE_CORRECTION_WEIGHT);
|
||||
|
||||
// correct the knees
|
||||
tempLeft = leftKneePosition.subtract(leftKneeVector);
|
||||
tempRight = rightKneePosition.subtract(rightKneeVector);
|
||||
leftKneePosition.set(tempLeft);
|
||||
rightKneePosition.set(tempRight);
|
||||
}
|
||||
|
||||
private float getLeftFootOffset() {
|
||||
float offset = computeUnitVector(this.leftFootRotation).y;
|
||||
return clamp(0, DYNAMIC_DISPLACEMENT_CUTOFF, offset);
|
||||
}
|
||||
|
||||
private float getRightFootOffset() {
|
||||
float offset = computeUnitVector(this.rightFootRotation).y;
|
||||
return clamp(0, DYNAMIC_DISPLACEMENT_CUTOFF, offset);
|
||||
}
|
||||
|
||||
// calculate the weight of foot correction
|
||||
private float calculateCorrectionWeight(
|
||||
Vector3f foot,
|
||||
Vector3f footCorrected
|
||||
) {
|
||||
Vector3f footDif = foot.subtract(footCorrected).setY(0);
|
||||
|
||||
if (footDif.length() < MIN_ACCEPTABLE_ERROR) {
|
||||
return CORRECTION_WEIGHT_MIN;
|
||||
} else if (footDif.length() > MAX_ACCEPTABLE_ERROR) {
|
||||
return CORRECTION_WEIGHT_MAX;
|
||||
}
|
||||
|
||||
return CORRECTION_WEIGHT_MIN
|
||||
+ (footDif.length() - MIN_ACCEPTABLE_ERROR)
|
||||
/ (MAX_ACCEPTABLE_ERROR - MIN_ACCEPTABLE_ERROR)
|
||||
* (CORRECTION_WEIGHT_MAX - CORRECTION_WEIGHT_MIN);
|
||||
}
|
||||
|
||||
// check if the difference between two floats flipped after correction
|
||||
private boolean checkOverShoot(float trueVal, float valBefore, float valAfter) {
|
||||
return (trueVal - valBefore) * (trueVal - valAfter) < 0;
|
||||
}
|
||||
|
||||
// get the unit vector of the given rotation
|
||||
private Vector3f computeUnitVector(Quaternion quaternion) {
|
||||
return quaternion.getRotationColumn(2).normalize();
|
||||
}
|
||||
|
||||
// clamp a float between two values
|
||||
private float clamp(float min, float max, float val) {
|
||||
return Math.min(max, Math.max(min, val));
|
||||
}
|
||||
}
|
||||
@@ -39,4 +39,16 @@ public abstract class Skeleton {
|
||||
|
||||
@VRServerThread
|
||||
public abstract void resetTrackersYaw();
|
||||
|
||||
@VRServerThread
|
||||
public abstract boolean[] getLegTweaksState();
|
||||
|
||||
@VRServerThread
|
||||
public abstract void setLegTweaksEnabled(boolean value);
|
||||
|
||||
@VRServerThread
|
||||
public abstract void setFloorclipEnabled(boolean value);
|
||||
|
||||
@VRServerThread
|
||||
public abstract void setSkatingCorrectionEnabled(boolean value);
|
||||
}
|
||||
|
||||
@@ -9,7 +9,9 @@ public enum SkeletonConfigToggles {
|
||||
EXTENDED_SPINE_MODEL(1, "Extended spine model", "extendedSpine", true),
|
||||
EXTENDED_PELVIS_MODEL(2, "Extended pelvis model", "extendedPelvis", true),
|
||||
EXTENDED_KNEE_MODEL(3, "Extended knee model", "extendedKnee", true),
|
||||
FORCE_ARMS_FROM_HMD(4, "Force arms from HMD", "forceArmsFromHMD", true),;
|
||||
FORCE_ARMS_FROM_HMD(4, "Force arms from HMD", "forceArmsFromHMD", true),
|
||||
FLOOR_CLIP(5, "Floor clip", "floorClip", true),
|
||||
SKATING_CORRECTION(6, "Skating correction", "skatingCorrection", true),;
|
||||
|
||||
public static final SkeletonConfigToggles[] values = values();
|
||||
private static final Map<String, SkeletonConfigToggles> byStringVal = new HashMap<>();
|
||||
|
||||
@@ -74,6 +74,12 @@ public class ComputedTracker implements Tracker, TrackerWithTPS {
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getAcceleration(Vector3f store) {
|
||||
store.set(0, 0, 0);
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public TrackerStatus getStatus() {
|
||||
return status;
|
||||
|
||||
@@ -18,7 +18,7 @@ public class IMUTracker
|
||||
public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG;
|
||||
|
||||
// public final Vector3f gyroVector = new Vector3f();
|
||||
// public final Vector3f accelVector = new Vector3f();
|
||||
public final Vector3f accelVector = new Vector3f();
|
||||
public final Vector3f magVector = new Vector3f();
|
||||
public final Quaternion rotQuaternion = new Quaternion();
|
||||
public final Quaternion rotMagQuaternion = new Quaternion();
|
||||
@@ -170,6 +170,12 @@ public class IMUTracker
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getAcceleration(Vector3f store) {
|
||||
store.set(accelVector);
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRotation(Quaternion store) {
|
||||
if (movementFilterTickCount > 0 && movementFilterAmount != 1 && previousRots.size() > 0) {
|
||||
|
||||
@@ -108,6 +108,11 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getAcceleration(Vector3f store) {
|
||||
return tracker.getAcceleration(store);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getPosition(Vector3f store) {
|
||||
return tracker.getPosition(store);
|
||||
|
||||
@@ -20,6 +20,8 @@ public interface Tracker {
|
||||
|
||||
boolean getRotation(Quaternion store);
|
||||
|
||||
boolean getAcceleration(Vector3f store);
|
||||
|
||||
String getName();
|
||||
|
||||
TrackerStatus getStatus();
|
||||
|
||||
@@ -12,6 +12,7 @@ import dev.slimevr.vr.trackers.TrackerStatus;
|
||||
import io.eiren.util.Util;
|
||||
import io.eiren.util.collections.FastList;
|
||||
import io.eiren.util.logging.LogManager;
|
||||
|
||||
import org.apache.commons.lang3.ArrayUtils;
|
||||
|
||||
import java.io.IOException;
|
||||
@@ -425,8 +426,22 @@ public class TrackersUDPServer extends Thread {
|
||||
break;
|
||||
tracker.magnetometerAccuracy = magAccuracy.accuracyInfo;
|
||||
break;
|
||||
|
||||
case UDPProtocolParser.PACKET_ACCEL:
|
||||
if (connection == null)
|
||||
break;
|
||||
|
||||
UDPPacket4Acceleration accelPacket = (UDPPacket4Acceleration) packet;
|
||||
tracker = connection.getTracker(accelPacket.getSensorId());
|
||||
|
||||
if (tracker == null)
|
||||
break;
|
||||
|
||||
Vector3f acceleration = tracker.rotQuaternion.mult(accelPacket.acceleration);
|
||||
tracker.accelVector.set(acceleration);
|
||||
break;
|
||||
|
||||
case 2: // PACKET_GYRO
|
||||
case 4: // PACKET_ACCEL
|
||||
case 5: // PACKET_MAG
|
||||
case 9: // PACKET_RAW_MAGENTOMETER
|
||||
break; // None of these packets are used by SlimeVR trackers and
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
package dev.slimevr.vr.trackers.udp;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.nio.BufferUnderflowException;
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
import com.jme3.math.Vector3f;
|
||||
|
||||
|
||||
public class UDPPacket4Acceleration extends UDPPacket implements SensorSpecificPacket {
|
||||
|
||||
public int sensorId;
|
||||
public Vector3f acceleration = new Vector3f();
|
||||
|
||||
public UDPPacket4Acceleration() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getPacketId() {
|
||||
return 4;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void readData(ByteBuffer buf) throws IOException {
|
||||
acceleration.set(buf.getFloat(), buf.getFloat(), buf.getFloat());
|
||||
|
||||
try {
|
||||
sensorId = buf.get() & 0xFF;
|
||||
} catch (BufferUnderflowException e) {
|
||||
// for owo track app
|
||||
sensorId = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void writeData(ByteBuffer buf) throws IOException {
|
||||
// Never sent back in current protocol
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getSensorId() {
|
||||
return sensorId;
|
||||
}
|
||||
}
|
||||
@@ -11,7 +11,7 @@ public class UDPProtocolParser {
|
||||
public static final int PACKET_ROTATION = 1; // Deprecated
|
||||
// public static final int PACKET_GYRO = 2; // Deprecated
|
||||
public static final int PACKET_HANDSHAKE = 3;
|
||||
// public static final int PACKET_ACCEL = 4; // Not parsed by server
|
||||
public static final int PACKET_ACCEL = 4;
|
||||
// public static final int PACKET_MAG = 5; // Deprecated
|
||||
// public static final int PACKET_RAW_CALIBRATION_DATA = 6; // Not parsed by
|
||||
// server
|
||||
@@ -105,6 +105,8 @@ public class UDPProtocolParser {
|
||||
return new UDPPacket3Handshake();
|
||||
case PACKET_PING_PONG:
|
||||
return new UDPPacket10PingPong();
|
||||
case PACKET_ACCEL:
|
||||
return new UDPPacket4Acceleration();
|
||||
case PACKET_SERIAL:
|
||||
return new UDPPacket11Serial();
|
||||
case PACKET_BATTERY_LEVEL:
|
||||
|
||||
Reference in New Issue
Block a user