+
byStringVal = new HashMap<>();
diff --git a/server/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java b/server/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java
index 67be63d8c..dd184d467 100644
--- a/server/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java
+++ b/server/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java
@@ -1239,6 +1239,8 @@ public class HumanSkeleton {
case SKATING_CORRECTION -> legTweaks.setSkatingReductionEnabled(newValue);
case FLOOR_CLIP -> legTweaks.setFloorclipEnabled(newValue);
case VIVE_EMULATION -> viveEmulation.setEnabled(newValue);
+ case TOE_SNAP -> legTweaks.setToeSnap(newValue);
+ case FOOT_PLANT -> legTweaks.setFootPlant(newValue);
}
}
diff --git a/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweakBuffer.java b/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweakBuffer.java
index d08d9ab0e..c0a83e88f 100644
--- a/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweakBuffer.java
+++ b/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweakBuffer.java
@@ -6,16 +6,16 @@ import com.jme3.math.FastMath;
/**
- * class that holds data related to the state and other variuse attributes of
+ * class that holds data related to the state and other various 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
+ * rules: The conditions for an unlock are as follows: 1. the foot is too far
+ * from its correct position 2. a velocity higher than a threshold is achieved
* 3. a large acceleration is applied to the foot 4. angular velocity of the
* foot goes higher than a threshold. The conditions for a lock are the opposite
* of the above but require a lower value for all of the above conditions. The
- * afformentioned thresholds are computed by applying scalers to a base
+ * aforementioned thresholds are computed by applying scalars to a base
* threshold value. This allows one set of initial values to be applicable to a
* large range of actions and body types.
*/
@@ -50,6 +50,8 @@ public class LegTweakBuffer {
private Vector3f leftKneePositionCorrected = new Vector3f();
private Vector3f rightKneePositionCorrected = new Vector3f();
private Vector3f waistPositionCorrected = new Vector3f();
+ private Quaternion leftFootRotationCorrected = new Quaternion();
+ private Quaternion rightFootRotationCorrected = new Quaternion();
// velocities
private Vector3f leftFootVelocity = new Vector3f();
@@ -80,15 +82,15 @@ public class LegTweakBuffer {
// hyperparameters
public static final float SKATING_DISTANCE_CUTOFF = 0.5f;
- static float SKATING_VELOCITY_THRESHOLD = 2.6f;
- static float SKATING_ACCELERATION_THRESHOLD = 0.8f;
+ static float SKATING_VELOCITY_THRESHOLD = 2.4f;
+ static float SKATING_ACCELERATION_THRESHOLD = 0.7f;
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 SIX_TRACKER_TOLERANCE = -0.10f;
private static final Vector3f FORCE_VECTOR_TO_PRESSURE = new Vector3f(0.25f, 1.0f, 0.25f);
- private static final float FORCE_ERROR_TOLLERANCE = 4.0f;
+ private static final float FORCE_ERROR_TOLERANCE_SQR = FastMath.sqr(4.0f);
private static final float[] FORCE_VECTOR_FALLBACK = new float[] { 0.1f, 0.1f };
static float PARAM_SCALAR_MAX = 3.2f;
@@ -109,9 +111,9 @@ public class LegTweakBuffer {
private static final float MIN_SCALAR_ACTIVE = 1.75f;
private static final float MAX_SCALAR_ACTIVE = 0.1f;
- // maximum scalers for the pressure on each foot
- private static final float PRESSURE_SCALER_MIN = 0.1f;
- private static final float PRESSURE_SCALER_MAX = 1.9f;
+ // maximum scalars for the pressure on each foot
+ private static final float PRESSURE_SCALAR_MIN = 0.1f;
+ private static final float PRESSURE_SCALAR_MAX = 1.9f;
private float leftFootSensitivityVel = 1.0f;
private float rightFootSensitivityVel = 1.0f;
@@ -204,6 +206,28 @@ public class LegTweakBuffer {
this.rightFootRotation.set(rightFootRotation);
}
+ public Quaternion getLeftFootRotationCorrected(Quaternion quat) {
+ if (quat == null)
+ quat = new Quaternion();
+
+ return quat.set(leftFootRotationCorrected);
+ }
+
+ public void setLeftFootRotationCorrected(Quaternion quat) {
+ this.leftFootRotationCorrected.set(quat);
+ }
+
+ public Quaternion getRightFootRotationCorrected(Quaternion quat) {
+ if (quat == null)
+ quat = new Quaternion();
+
+ return quat.set(rightFootRotationCorrected);
+ }
+
+ public void setRightFootRotationCorrected(Quaternion quat) {
+ this.rightFootRotationCorrected.set(quat);
+ }
+
public Vector3f getLeftFootPositionCorrected(Vector3f vec) {
if (vec == null)
vec = new Vector3f();
@@ -374,14 +398,14 @@ public class LegTweakBuffer {
this.detectionMode = mode;
}
- // calculate momvent attributes
+ // calculate movement attributes
public void calculateFootAttributes(boolean active) {
updateFrameNumber(0);
// compute attributes of the legs
computeVelocity();
computeAccelerationMagnitude();
- computeComAtributes();
+ computeComAttributes();
// check if the acceleration triggers forced unlock
if (detectionMode == FOOT_ACCEL) {
@@ -424,9 +448,10 @@ public class LegTweakBuffer {
// check if a locked foot should stay locked or be released
private int checkStateLeft() {
float timeStep = getTimeDelta();
+
if (parent.leftLegState == UNLOCKED) {
if (
- parent.getLeftFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE
+ parent.getLeftFootHorizontalDifference() > SKATING_CUTOFF_ENGAGE
|| leftFootVelocityMagnitude * timeStep
> SKATING_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
|| leftFootAngleDiff * timeStep
@@ -441,7 +466,7 @@ public class LegTweakBuffer {
}
if (
- parent.getLeftFootHorizantalDifference() > SKATING_DISTANCE_CUTOFF
+ parent.getLeftFootHorizontalDifference() > SKATING_DISTANCE_CUTOFF
|| leftFootVelocityMagnitude * timeStep
> SKATING_VELOCITY_THRESHOLD * leftFootSensitivityVel
|| leftFootAngleDiff * timeStep
@@ -461,11 +486,11 @@ public class LegTweakBuffer {
if (parent.rightLegState == UNLOCKED) {
if (
- parent.getRightFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE
+ parent.getRightFootHorizontalDifference() > SKATING_CUTOFF_ENGAGE
|| rightFootVelocityMagnitude * timeStep
- > SKATING_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
+ > SKATING_VELOCITY_CUTOFF_ENGAGE * rightFootSensitivityVel
|| rightFootAngleDiff * timeStep
- > SKATING_ROTATIONAL_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
+ > SKATING_ROTATIONAL_VELOCITY_CUTOFF_ENGAGE * rightFootSensitivityVel
|| rightFootPosition.y > rightFloorLevel + FLOOR_DISTANCE_CUTOFF
|| accelerationAboveThresholdRight
) {
@@ -476,7 +501,7 @@ public class LegTweakBuffer {
}
if (
- parent.getRightFootHorizantalDifference() > SKATING_DISTANCE_CUTOFF
+ parent.getRightFootHorizontalDifference() > SKATING_DISTANCE_CUTOFF
|| rightFootVelocityMagnitude * timeStep
> SKATING_VELOCITY_THRESHOLD * rightFootSensitivityVel
|| rightFootAngleDiff * timeStep
@@ -491,14 +516,14 @@ public class LegTweakBuffer {
}
// get the difference in feet position between the kinematic and corrected
- // positions of the feet disregarding vertical displacment
- private float getLeftFootHorizantalDifference() {
+ // positions of the feet disregarding vertical displacement
+ private float getLeftFootHorizontalDifference() {
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() {
+ private float getRightFootHorizontalDifference() {
return rightFootPositionCorrected.subtract(rightFootPosition).setY(0).length();
}
@@ -550,7 +575,7 @@ public class LegTweakBuffer {
}
// compute the velocity and acceleration of the center of mass
- private void computeComAtributes() {
+ private void computeComAttributes() {
centerOfMassVelocity = centerOfMass.subtract(parent.centerOfMass);
centerOfMassAcceleration = centerOfMassVelocity.subtract(parent.centerOfMassVelocity);
}
@@ -567,9 +592,9 @@ public class LegTweakBuffer {
// determine lock/unlock
private void computeAccelerationAboveThresholdAnkleTrackers() {
accelerationAboveThresholdLeft = leftFootAccelerationMagnitude
- > (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLLERANCE) * leftFootSensitivityAccel;
+ > (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLERANCE) * leftFootSensitivityAccel;
accelerationAboveThresholdRight = rightFootAccelerationMagnitude
- > (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLLERANCE) * rightFootSensitivityAccel;
+ > (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLERANCE) * rightFootSensitivityAccel;
}
// using the parent lock/unlock states, velocity, and acceleration,
@@ -581,12 +606,12 @@ public class LegTweakBuffer {
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
+ // get the second set of scalars that is based off 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 likely
// planted on the ground unless you are moving fast)
- float leftFootScalarVel = getLeftFootLockLiklyHood();
- float rightFootScalarVel = getRightFootLockLiklyHood();
+ float leftFootScalarVel = getLeftFootLockLikelihood();
+ float rightFootScalarVel = getRightFootLockLikelihood();
// get the third set of scalars that is based on where the COM is
float[] pressureScalars = getPressurePrediction();
@@ -594,10 +619,10 @@ public class LegTweakBuffer {
// combine the scalars to get the final scalars
leftFootSensitivityVel = (leftFootScalarAccel
+ leftFootScalarVel / 2.0f)
- * FastMath.clamp(pressureScalars[0] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX);
+ * FastMath.clamp(pressureScalars[0] * 2.0f, PRESSURE_SCALAR_MIN, PRESSURE_SCALAR_MAX);
rightFootSensitivityVel = (rightFootScalarAccel
+ rightFootScalarVel / 2.0f)
- * FastMath.clamp(pressureScalars[1] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX);
+ * FastMath.clamp(pressureScalars[1] * 2.0f, PRESSURE_SCALAR_MIN, PRESSURE_SCALAR_MAX);
leftFootSensitivityAccel = leftFootScalarVel;
rightFootSensitivityAccel = rightFootScalarVel;
@@ -638,7 +663,7 @@ public class LegTweakBuffer {
// states to calculate a scalar to apply to the non acceleration based
// hyperparameters when calculating
// lock states
- private float getLeftFootLockLiklyHood() {
+ private float getLeftFootLockLikelihood() {
if (leftLegState == LOCKED && rightLegState == LOCKED) {
Vector3f velocityDiff = leftFootVelocity.subtract(rightFootVelocity);
velocityDiff.setY(0.0f);
@@ -654,24 +679,24 @@ public class LegTweakBuffer {
}
// calculate the 'unlockedness factor' and use that to
- // determine the scalar (go as low as 0.5 as as high as
+ // determine the scalar (go as low as 0.5 and as high as
// param_scalar_max)
- float velocityDifAbs = Math.abs(leftFootVelocityMagnitude)
- - Math.abs(rightFootVelocityMagnitude);
+ float velocityDiffAbs = FastMath
+ .abs(leftFootVelocityMagnitude - rightFootVelocityMagnitude);
- if (velocityDifAbs > MIN_SCALAR_ACTIVE) {
+ if (velocityDiffAbs > MIN_SCALAR_ACTIVE) {
return PARAM_SCALAR_MIN;
- } else if (velocityDifAbs < MAX_SCALAR_ACTIVE) {
+ } else if (velocityDiffAbs < MAX_SCALAR_ACTIVE) {
return PARAM_SCALAR_MAX;
}
return PARAM_SCALAR_MAX
- * (velocityDifAbs - MIN_SCALAR_ACTIVE)
+ * (velocityDiffAbs - MIN_SCALAR_ACTIVE)
/ (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE)
- PARAM_SCALAR_MID;
}
- private float getRightFootLockLiklyHood() {
+ private float getRightFootLockLikelihood() {
if (rightLegState == LOCKED && leftLegState == LOCKED) {
Vector3f velocityDiff = rightFootVelocity.subtract(leftFootVelocity);
velocityDiff.setY(0.0f);
@@ -687,19 +712,19 @@ public class LegTweakBuffer {
}
// calculate the 'unlockedness factor' and use that to
- // determine the scalar (go as low as 0.5 as as high as
+ // determine the scalar (go as low as 0.5 and as high as
// param_scalar_max)
- float velocityDifAbs = Math.abs(rightFootVelocityMagnitude)
- - Math.abs(leftFootVelocityMagnitude);
+ float velocityDiffAbs = FastMath
+ .abs(rightFootVelocityMagnitude - leftFootVelocityMagnitude);
- if (velocityDifAbs > MIN_SCALAR_ACTIVE) {
+ if (velocityDiffAbs > MIN_SCALAR_ACTIVE) {
return PARAM_SCALAR_MIN;
- } else if (velocityDifAbs < MAX_SCALAR_ACTIVE) {
+ } else if (velocityDiffAbs < MAX_SCALAR_ACTIVE) {
return PARAM_SCALAR_MAX;
}
return PARAM_SCALAR_MAX
- * (velocityDifAbs - MIN_SCALAR_ACTIVE)
+ * (velocityDiffAbs - MIN_SCALAR_ACTIVE)
/ (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE)
- PARAM_SCALAR_MID;
}
@@ -768,7 +793,7 @@ public class LegTweakBuffer {
private void findForceVectors(Vector3f leftFootForce, Vector3f rightFootForce) {
int iterations = 100;
float stepSize = 0.01f;
- // setup the temporary variables
+ // set up the temporary variables
Vector3f tempLeftFootForce1 = leftFootForce.clone();
Vector3f tempLeftFootForce2 = leftFootForce.clone();
Vector3f tempRightFootForce1 = rightFootForce.clone();
@@ -817,11 +842,11 @@ public class LegTweakBuffer {
}
// detect any outside forces on the body such
- // as a wall or a chair. returns true if there is a outside force
+ // as a wall or a chair. returns true if there is an outside force
private boolean detectOutsideForces(Vector3f f1, Vector3f f2) {
Vector3f force = GRAVITY.add(f1).add(f2);
Vector3f error = centerOfMassAcceleration.subtract(force);
- return error.lengthSquared() > FastMath.sqr(FORCE_ERROR_TOLLERANCE);
+ return error.lengthSquared() > FORCE_ERROR_TOLERANCE_SQR;
}
// simple error function for the force vector gradient descent
diff --git a/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.java b/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.java
index af0ba3e89..591776fe9 100644
--- a/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.java
+++ b/server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.java
@@ -5,21 +5,91 @@ import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.config.LegTweaksConfig;
import dev.slimevr.tracking.processor.TransformNode;
+import dev.slimevr.tracking.processor.config.SkeletonConfigToggles;
public class LegTweaks {
+ /**
+ * here is an explanation 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_DISENGAGEMENT_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 displacement
+ * MAX_DYNAMIC_DISPLACEMENT 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 game world MIN_ACCEPTABLE_ERROR and
+ * MAX_ACCEPTABLE_ERROR Defines the distance 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)
+ static float DYNAMIC_DISPLACEMENT_CUTOFF = 1.0f;
+ private static final float FLOOR_CALIBRATION_OFFSET = 0.015f;
+
+ // hyperparameters (skating correction)
+ private static final float MIN_ACCEPTABLE_ERROR = 0.01f;
+ private static final float MAX_ACCEPTABLE_ERROR = 0.05f;
+ private static final float CORRECTION_WEIGHT_MIN = 0.55f;
+ private static final float CORRECTION_WEIGHT_MAX = 0.70f;
+ private static final float CONTINUOUS_CORRECTION_DIST = 0.5f;
+ private static final int CONTINUOUS_CORRECTION_WARMUP = 175;
+
+ // hyperparameters (knee / waist correction)
+ private static final float KNEE_CORRECTION_WEIGHT = 0.00f;
+ private static final float KNEE_LATERAL_WEIGHT = 0.8f;
+ private static final float WAIST_PUSH_WEIGHT = 0.2f;
+
+ // hyperparameters (COM calculation)
+ // mass percentages of the body
+ private static final float HEAD_MASS = 0.082f;
+ private static final float CHEST_MASS = 0.25f;
+ private static final float WAIST_MASS = 0.209f;
+ private static final float THIGH_MASS = 0.128f;
+ private static final float CALF_MASS = 0.0535f;
+ private static final float UPPER_ARM_MASS = 0.031f;
+ private static final float FOREARM_MASS = 0.017f;
+
+ // hyperparameters (rotation correction)
+ private static final float ROTATION_CORRECTION_VERTICAL = 0.1f;
+ private static final float MAXIMUM_CORRECTION_ANGLE = 0.4f;
+ private static final float MAXIMUM_CORRECTION_ANGLE_DELTA = 0.7f;
+ private static final float MAXIMUM_TOE_DOWN_ANGLE = 0.8f;
+ private static final float TOE_SNAP_COOLDOWN = 3.0f;
+
+ // hyperparameters (misc)
+ static final float NEARLY_ZERO = 0.001f;
+ private static final float STANDING_CUTOFF_VERTICAL = 0.65f;
+ private static final float MAX_DISENGAGEMENT_OFFSET = 0.30f;
+ private static final float DEFAULT_ARM_DISTANCE = 0.15f;
+ private static final float MAX_CORRECTION_STRENGTH_DELTA = 1.0f;
+
// state variables
private float floorLevel;
private float waistToFloorDist;
private float currentDisengagementOffset = 0.0f;
+ private float footLength = 0.0f;
private static float currentCorrectionStrength = 0.3f; // default value
private boolean initialized = true;
private boolean enabled = true; // master switch
private boolean floorclipEnabled = false;
private boolean skatingCorrectionEnabled = false;
+ private boolean toeSnap = false;
+ private boolean footPlant = false;
private boolean active = false;
private boolean rightLegActive = false;
private boolean leftLegActive = false;
+ private int leftFramesLocked = 0;
+ private int rightFramesLocked = 0;
+ private int leftFramesUnlocked = 0;
+ private int rightFramesUnlocked = 0;
+ private float leftToeAngle = 0.0f;
+ private boolean leftToeTouched = false;
+ private float rightToeAngle = 0.0f;
+ private boolean rightToeTouched = false;
// skeleton and config
private HumanSkeleton skeleton;
@@ -44,65 +114,7 @@ public class LegTweaks {
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)
- static float DYNAMIC_DISPLACEMENT_CUTOFF = 1.0f;
- static float MAX_DYNAMIC_DISPLACEMENT = 0.06f;
- private static final float FLOOR_CALIBRATION_OFFSET = 0.015f;
-
- // hyperparameters (skating correction)
- private static final float MIN_ACCEPTABLE_ERROR = 0.01f;
- private static final float MAX_ACCEPTABLE_ERROR = 0.225f;
- private static final float CORRECTION_WEIGHT_MIN = 0.40f;
- private static final float CORRECTION_WEIGHT_MAX = 0.70f;
- private static final float CONTINUOUS_CORRECTION_DIST = 0.5f;
- private static final int CONTINUOUS_CORRECTION_WARMUP = 175;
-
- // hyperparameters (knee / waist correction)
- private static final float KNEE_CORRECTION_WEIGHT = 0.00f;
- private static final float KNEE_LATERAL_WEIGHT = 0.8f;
- private static final float WAIST_PUSH_WEIGHT = 0.2f;
-
- // hyperparameters (COM calculation)
- // mass percentages of the body
- private static final float HEAD_MASS = 0.082f;
- private static final float CHEST_MASS = 0.25f;
- private static final float WAIST_MASS = 0.209f;
- private static final float THIGH_MASS = 0.128f;
- private static final float CALF_MASS = 0.0535f;
- private static final float UPPER_ARM_MASS = 0.031f;
- private static final float FOREARM_MASS = 0.017f;
-
- // hyperparameters (misc)
- static final float NEARLY_ZERO = 0.001f;
- private static final float STANDING_CUTOFF_VERTICAL = 0.65f;
- private static final float MAX_DISENGAGMENT_OFFSET = 0.30f;
- private static final float DEFAULT_ARM_DISTANCE = 0.15f;
- private static final float MAX_CORRECTION_STRENGTH_DELTA = 1.0f;
-
- // counters
- private int leftFramesLocked = 0;
- private int rightFramesLocked = 0;
- private int leftFramesUnlocked = 0;
- private int rightFramesUnlocked = 0;
-
- // buffer for holding previus frames of data
+ // buffer for holding previous frames of data
private LegTweakBuffer bufferHead = new LegTweakBuffer();
private boolean bufferInvalid = true;
@@ -201,6 +213,14 @@ public class LegTweaks {
this.bufferInvalid = true;
}
+ public void setToeSnap(boolean val) {
+ this.toeSnap = val;
+ }
+
+ public void setFootPlant(boolean val) {
+ this.footPlant = val;
+ }
+
public boolean getEnabled() {
return this.enabled;
}
@@ -213,6 +233,14 @@ public class LegTweaks {
return this.skatingCorrectionEnabled;
}
+ public boolean getToeSnap() {
+ return this.toeSnap;
+ }
+
+ public boolean getFootPlant() {
+ return this.footPlant;
+ }
+
public void resetBuffer() {
bufferInvalid = true;
}
@@ -224,9 +252,15 @@ public class LegTweaks {
public void updateConfig() {
LegTweaks.updateHyperParameters(config.getCorrectionStrength());
+
+ floorclipEnabled = skeleton.humanPoseManager.getToggle(SkeletonConfigToggles.FLOOR_CLIP);
+ skatingCorrectionEnabled = skeleton.humanPoseManager
+ .getToggle(SkeletonConfigToggles.SKATING_CORRECTION);
+ toeSnap = skeleton.humanPoseManager.getToggle(SkeletonConfigToggles.TOE_SNAP);
+ footPlant = skeleton.humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT);
}
- // update the hyper parameters with the config
+ // update the hyperparameters with the config
public static void updateHyperParameters(float newStrength) {
LegTweakBuffer.SKATING_VELOCITY_THRESHOLD = getScaledHyperParameter(
newStrength,
@@ -248,7 +282,7 @@ public class LegTweaks {
private void setVectors() {
// set the positions of the feet and knees to the skeletons current
// positions
- if (skeleton.computedLeftKneeTracker != null || skeleton.computedRightKneeTracker != null) {
+ if (skeleton.computedLeftKneeTracker != null && skeleton.computedRightKneeTracker != null) {
kneesActive = true;
leftKneePosition = skeleton.computedLeftKneeTracker.position;
rightKneePosition = skeleton.computedRightKneeTracker.position;
@@ -300,13 +334,16 @@ public class LegTweaks {
floorLevel = (leftFootPosition.y + rightFootPosition.y) / 2f + FLOOR_CALIBRATION_OFFSET;
waistToFloorDist = waistPosition.y - floorLevel;
- // invalidate the buffer since the non initialized output may be
+ // invalidate the buffer since the non-initialized output may be
// very wrong
bufferInvalid = true;
initialized = true;
}
- // if not enabled do nothing and return false
+ // update the foot length
+ footLength = skeleton.leftFootNode.localTransform.getTranslation().length();
+
+ // if not enabled, do nothing and return false
if (!enabled)
return false;
@@ -329,7 +366,7 @@ public class LegTweaks {
bufferHead.setLeftLegState(LegTweakBuffer.UNLOCKED);
bufferHead.setRightLegState(LegTweakBuffer.UNLOCKED);
- // if the system is active propulate the buffer with corrected floor
+ // if the system is active, populate the buffer with corrected floor
// clip feet positions
if (active && isStanding()) {
correctClipping();
@@ -353,19 +390,19 @@ public class LegTweaks {
currentFrame
.setLeftFloorLevel(
- (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset()))
+ (floorLevel + (footLength * getLeftFootOffset()))
- currentDisengagementOffset
);
currentFrame
.setRightFloorLevel(
- (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset()))
+ (floorLevel + (footLength * 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)
+ // quantity in the buffer
+ // (if feet are not available, fall back to 6 tracker mode)
if (skeleton.leftFootTracker != null && skeleton.rightFootTracker != null) {
currentFrame.setLeftFootAcceleration(leftFootAcceleration);
currentFrame.setRightFootAcceleration(rightFootAcceleration);
@@ -395,6 +432,9 @@ public class LegTweaks {
if (!preUpdate())
return;
+ // correct foot rotation's
+ correctFootRotations();
+
// push the feet up if needed
if (floorclipEnabled)
correctClipping();
@@ -476,13 +516,13 @@ public class LegTweaks {
// move the feet to their new positions
if (
leftFootPosition.y
- < (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * leftOffset))
+ < (floorLevel + (footLength * leftOffset))
- currentDisengagementOffset
) {
float displacement = Math
.abs(
floorLevel
- + (MAX_DYNAMIC_DISPLACEMENT * leftOffset)
+ + (footLength * leftOffset)
- leftFootPosition.y
- currentDisengagementOffset
);
@@ -494,13 +534,13 @@ public class LegTweaks {
if (
rightFootPosition.y
- < (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * rightOffset))
+ < (floorLevel + (footLength * rightOffset))
- currentDisengagementOffset
) {
float displacement = Math
.abs(
floorLevel
- + (MAX_DYNAMIC_DISPLACEMENT * rightOffset)
+ + (footLength * rightOffset)
- rightFootPosition.y
- currentDisengagementOffset
);
@@ -713,6 +753,176 @@ public class LegTweaks {
}
}
+ // correct the rotations of the feet
+ // this is done by planting the foot better and by snapping the toes to the
+ // ground
+ private void correctFootRotations() {
+ // null check's
+ if (bufferHead == null || bufferHead.getParent() == null)
+ return;
+
+ // if there is a foot tracker for a foot don't correct it
+ if (skeleton.leftFootTracker != null || skeleton.rightFootTracker != null)
+ return;
+
+ // get the foot positions
+ Quaternion leftFootRotation = bufferHead.getLeftFootRotation(null);
+ Quaternion rightFootRotation = bufferHead.getRightFootRotation(null);
+
+ // between maximum correction angle and maximum correction angle delta
+ // the values are interpolated
+ float kneeAngleL = getXZAmount(leftFootPosition, leftKneePosition);
+ float kneeAngleR = getXZAmount(rightFootPosition, rightKneePosition);
+
+ float masterWeightL = getMasterWeight(kneeAngleL);
+ float masterWeightR = getMasterWeight(kneeAngleR);
+
+ // corrects rotations when planted firmly on the ground
+ if (footPlant) {
+ // prepare the weight vars for this correction step
+ float weightL = 0.0f;
+ float weightR = 0.0f;
+
+ // the further from the ground the foot is, the less weight it
+ // should have
+ weightL = getFootPlantWeight(leftFootPosition);
+ weightR = getFootPlantWeight(rightFootPosition);
+
+ // perform the correction
+ leftFootRotation
+ .set(
+ leftFootRotation
+ .slerp(
+ leftFootRotation,
+ isolateYaw(leftFootRotation),
+ weightL * masterWeightL
+ )
+ );
+
+ rightFootRotation
+ .set(
+ rightFootRotation
+ .slerp(
+ rightFootRotation,
+ isolateYaw(rightFootRotation),
+ weightR * masterWeightR
+ )
+ );
+ }
+
+ // corrects rotations when the foot is in the air by rotating the foot
+ // down so that the toes are touching
+ if (toeSnap) {
+ // this correction step has its own weight vars
+ float weightL = 0.0f;
+ float weightR = 0.0f;
+
+ // first compute the angle of the foot
+ float angleL = getToeSnapAngle(leftFootPosition);
+ float angleR = getToeSnapAngle(rightFootPosition);
+
+ // then compute the weight of the correction
+ weightL = getToeSnapWeight(leftFootPosition, angleL);
+ weightR = getToeSnapWeight(rightFootPosition, angleR);
+
+ // depending on the state variables, the correction weights should
+ // be clamped
+ if (!leftToeTouched) {
+ weightL = Math.min(weightL, leftToeAngle);
+ }
+ if (!rightToeTouched) {
+ weightR = Math.min(weightR, rightToeAngle);
+ }
+
+ // then slerp the rotation to the new rotation based on the weight
+ leftFootRotation
+ .slerp(
+ leftFootRotation,
+ replacePitch(leftFootRotation, -angleL),
+ weightL * masterWeightL
+ );
+ rightFootRotation
+ .slerp(
+ rightFootRotation,
+ replacePitch(rightFootRotation, -angleR),
+ weightR * masterWeightR
+ );
+
+ // update state variables regarding toe snap
+ if (leftFootPosition.y - floorLevel > footLength * MAXIMUM_TOE_DOWN_ANGLE) {
+ leftToeTouched = false;
+ leftToeAngle = weightL;
+ } else if (leftFootPosition.y - floorLevel < 0.0f) {
+ leftToeTouched = true;
+ leftToeAngle = 1.0f;
+ }
+ if (rightFootPosition.y - floorLevel > footLength * MAXIMUM_TOE_DOWN_ANGLE) {
+ rightToeTouched = false;
+ rightToeAngle = weightR;
+ } else if (rightFootPosition.y - floorLevel < 0.0f) {
+ rightToeTouched = true;
+ rightToeAngle = 1.0f;
+ }
+ }
+
+ // update the foot rotations in the buffer
+ bufferHead.setLeftFootRotationCorrected(leftFootRotation);
+ bufferHead.setRightFootRotationCorrected(rightFootRotation);
+
+ // finally update the skeletons rotations with the new rotations
+ skeleton.computedLeftFootTracker.rotation.set(leftFootRotation);
+ skeleton.computedRightFootTracker.rotation.set(rightFootRotation);
+ }
+
+ // returns the length of the xz components of the normalized difference
+ // between two vectors
+ public float getXZAmount(Vector3f vec1, Vector3f vec2) {
+ return vec1
+ .subtract(vec2)
+ .normalizeLocal()
+ .setY(0.0f)
+ .length();
+ }
+
+ // returns a float between 0 and 1 that represents the master weight for
+ // foot rotation correciton
+ private float getMasterWeight(float kneeAngle) {
+ float masterWeight = (kneeAngle > MAXIMUM_CORRECTION_ANGLE
+ && kneeAngle < MAXIMUM_CORRECTION_ANGLE_DELTA)
+ ? 1.0f
+ - ((kneeAngle - MAXIMUM_CORRECTION_ANGLE)
+ / (MAXIMUM_CORRECTION_ANGLE_DELTA - MAXIMUM_CORRECTION_ANGLE))
+ : 0.0f;
+ return (kneeAngle < MAXIMUM_CORRECTION_ANGLE) ? 1.0f : masterWeight;
+ }
+
+ // return the weight of the correction for toe snap
+ private float getToeSnapWeight(Vector3f footPos, float angle) {
+ // then compute the weight of the correction
+ float weight = ((footPos.y - floorLevel) > footLength * TOE_SNAP_COOLDOWN)
+ ? 0.0f
+ : 1.0f
+ - ((footPos.y - floorLevel - footLength)
+ / (footLength * (TOE_SNAP_COOLDOWN - 1.0f)));
+ return FastMath.clamp(weight, 0.0f, 1.0f);
+ }
+
+ // returns the angle of the foot for toe snap
+ private float getToeSnapAngle(Vector3f footPos) {
+ float angle = FastMath.clamp(footPos.y - floorLevel, 0.0f, footLength);
+ return (angle > footLength * MAXIMUM_TOE_DOWN_ANGLE)
+ ? FastMath.asin((footLength * MAXIMUM_TOE_DOWN_ANGLE) / footLength)
+ : FastMath.asin((angle / footLength));
+ }
+
+ // returns the weight for floor plant
+ private float getFootPlantWeight(Vector3f footPos) {
+ float weight = (footPos.y - floorLevel > ROTATION_CORRECTION_VERTICAL)
+ ? 0.0f
+ : 1.0f - ((footPos.y - floorLevel) / ROTATION_CORRECTION_VERTICAL);
+ return FastMath.clamp(weight, 0.0f, 1.0f);
+ }
+
// 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
@@ -722,7 +932,7 @@ public class LegTweaks {
if (waistPosition.y < cutoff) {
currentDisengagementOffset = (1 - waistPosition.y / cutoff)
- * MAX_DISENGAGMENT_OFFSET;
+ * MAX_DISENGAGEMENT_OFFSET;
return false;
}
@@ -741,7 +951,7 @@ public class LegTweaks {
Vector3f tempLeft;
Vector3f tempRight;
- // before moveing the knees back closer to the waist nodes offset them
+ // before moving 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;
@@ -869,7 +1079,7 @@ public class LegTweaks {
centerOfMass = centerOfMass.add(leftForearm.mult(FOREARM_MASS));
centerOfMass = centerOfMass.add(rightForearm.mult(FOREARM_MASS));
} else {
- // if the arms are not avaliable put them slightly in front
+ // if the arms are not available put them slightly in front
// of the chest.
Vector3f chestUnitVector = computeUnitVector(
skeleton.chestNode.worldTransform.getRotation()
@@ -932,6 +1142,31 @@ public class LegTweaks {
}
}
+ // remove the x and z components of the given quaternion
+ private Quaternion isolateYaw(Quaternion quaternion) {
+ return new Quaternion(
+ 0,
+ quaternion.getY(),
+ 0,
+ quaternion.getW()
+ );
+ }
+
+ // return a quaternion that has been rotated by the new pitch amount
+ private Quaternion replacePitch(Quaternion quaternion, float newPitch) {
+ // first get the axis of the current pitch
+ Vector3f currentPitchAxis = quaternion.getRotationColumn(0).normalize();
+
+ // then get the current pitch
+ float currentPitch = (float) Math.asin(currentPitchAxis.y);
+
+ // then add the new pitch
+ Quaternion newQuat = new Quaternion();
+ newQuat.fromAngleAxis(newPitch + currentPitch, currentPitchAxis);
+
+ return newQuat.mult(quaternion);
+ }
+
// 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;
diff --git a/solarxr-protocol b/solarxr-protocol
index 378ca6e23..05647cfad 160000
--- a/solarxr-protocol
+++ b/solarxr-protocol
@@ -1 +1 @@
-Subproject commit 378ca6e2360d7a8b50cfd2f21c9165d3d895e0f8
+Subproject commit 05647cfad0cdde80e99360d8cbe8b69ce86d2eeb