Fix tap to assign and other bugs (#795)

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java

@@ -515,7 +515,7 @@ public void setTrackersFromList(List<Tracker> trackers) {
 		resetBones();
 
 		// Update tap detection's trackers
-		tapDetectionManager.updateConfig();
+		tapDetectionManager.updateConfig(trackers);
 	}
 
 	protected void setComputedTracker(Tracker tracker) {
@@ -572,8 +572,8 @@ public void updatePose() {
 		// Don't run leg tweaks if the tracking is paused
 		if (!pauseTracking)
 			tweakLegPos();
-		viveEmulation.update();
 		localizer.update();
+		viveEmulation.update();
 	}
 	// #endregion
 
@@ -1452,7 +1452,9 @@ public void resetTrackersFull(String resetSourceName) {
 
 		// tell the clip corrector to reset its floor level on the next update
 		// of the computed trackers
-		this.legTweaks.resetFloorLevel();
+		if (!localizer.getEnabled()) {
+			this.legTweaks.resetFloorLevel();
+		}
 		this.legTweaks.resetBuffer();
 		this.localizer.reset();
 
@@ -1570,7 +1572,7 @@ public void clearTrackersMounting(String resetSourceName) {
 	}
 
 	public void updateTapDetectionConfig() {
-		tapDetectionManager.updateConfig();
+		tapDetectionManager.updateConfig(null);
 	}
 
 	public void updateLegTweaksConfig() {

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.java

@@ -96,6 +96,7 @@ public class LegTweaks {
 	private boolean leftToeTouched = false;
 	private float rightToeAngle = 0.0f;
 	private boolean rightToeTouched = false;
+	public boolean localizerMode = false;
 
 	// skeleton and config
 	private final HumanSkeleton skeleton;
@@ -125,7 +126,6 @@ public LegTweaks(HumanSkeleton skeleton) {
 
 	public LegTweaks(HumanSkeleton skeleton, LegTweaksConfig config) {
 		this.skeleton = skeleton;
-		// set all the hyperparameters from the config
 		this.config = config;
 		updateConfig();
 	}
@@ -244,8 +244,7 @@ public boolean getFootPlantEnabled() {
 	}
 
 	public void setLocalizerMode(boolean val) {
-		this.initialized = false;
-
+		this.localizerMode = val;
 		if (val)
 			setFloorLevel(0.0f);
 	}
@@ -351,7 +350,7 @@ private boolean preUpdate() {
 		active = isStanding();
 
 		// if the buffer is invalid add all the extra info
-		if (bufferInvalid) {
+		if (bufferInvalid && !localizerMode) {
 			bufferHead.setLeftFootPositionCorrected(leftFootPosition);
 			bufferHead.setRightFootPositionCorrected(rightFootPosition);
 			bufferHead.setLeftKneePositionCorrected(leftKneePosition);
@@ -441,7 +440,7 @@ public void tweakLegs() {
 			correctFootRotations();
 
 		// push the feet up if needed (Floor clip)
-		if (floorclipEnabled)
+		if (floorclipEnabled && !localizerMode)
 			correctClipping();
 
 		// correct for skating if needed (Skating correction)

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/Localizer.kt

@@ -90,6 +90,7 @@ class Localizer(humanSkeleton: HumanSkeleton) {
 
 		if (warmupFrames < WARMUP_FRAMES) {
 			comVelocity = Vector3.NULL
+			targetFoot = Vector3.NULL
 		}
 		warmupFrames++
 
@@ -141,14 +142,14 @@ class Localizer(humanSkeleton: HumanSkeleton) {
 
 	// resets to the starting position
 	fun reset() {
-		skeleton.hmdNode.localTransform.translation = Vector3.NULL
+		if (!enabled) return
 
-		// reset the velocity
+		skeleton.hmdNode.localTransform.translation = Vector3.NULL
 		comVelocity = Vector3.NULL
 
 		// when localizing without a 6 dof device we choose the floor level
 		// 0 happens to be an easy number to use
-		legTweaks.setLocalizerMode(true)
+		legTweaks.setLocalizerMode(enabled)
 		floor = 0.0f
 		uncorrectedFloor = 0.0f - LegTweaks.FLOOR_CALIBRATION_OFFSET
 		warmupFrames = 0
@@ -233,6 +234,12 @@ class Localizer(humanSkeleton: HumanSkeleton) {
 		// get the distance to move the waist to the target waist
 		val dist: Vector3 = hip.minus(targetHip)
 
+		val lowTracker = getLowestTracker()
+
+		if (lowTracker.position.y < uncorrectedFloor) {
+			targetHip = Vector3(targetHip.x, targetHip.y + (uncorrectedFloor - lowTracker.position.y), targetHip.z)
+		}
+
 		// if the world reference is not sitting update the target waist
 		if (worldReference != MovementStates.FOLLOW_SITTING || sittingFrames < SITTING_EARLY) {
 			targetHip = hip
@@ -247,7 +254,6 @@ class Localizer(humanSkeleton: HumanSkeleton) {
 
 		var dist: Vector3 = bufCur.centerOfMass
 		dist = dist.minus(targetCOM)
-
 		return dist
 	}
 
@@ -352,16 +358,14 @@ class Localizer(humanSkeleton: HumanSkeleton) {
 		)
 
 		var minVal = trackerList[0].position.y
-		var tempVal: Float
 		var retVal: Tracker = trackerList[0]
 		for (tracker in trackerList) {
 			if (tracker == null) {
 				continue
 			}
-			// get the max distance to the ground
-			tempVal = tracker.position.y - uncorrectedFloor
-			if (tempVal < minVal) {
-				minVal = tempVal
+
+			if (tracker.position.y < minVal) {
+				minVal = tracker.position.y
 				retVal = tracker
 			}
 		}
@@ -408,7 +412,7 @@ class Localizer(humanSkeleton: HumanSkeleton) {
 			accel += skeleton.chestTracker.getAcceleration()
 			num++
 		}
-		return accel.div(num)
+		return if (num == 0f) accel else accel.div(num)
 	}
 
 	// update the hmd position and rotation
@@ -417,8 +421,8 @@ class Localizer(humanSkeleton: HumanSkeleton) {
 		if (skeleton.headTracker != null) {
 			rot = skeleton.headTracker.getRotation()
 		}
-		val temp = skeleton.hmdNode.localTransform.translation.minus(travel)
 
+		val temp = skeleton.hmdNode.localTransform.translation.minus(travel)
 		skeleton.hmdNode.localTransform.translation = temp
 		skeleton.hmdNode.localTransform.rotation = rot
 	}

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/TapDetection.java

@@ -140,10 +140,6 @@ public void update() {
 			taps = newTaps;
 			detectionTime = time;
 		}
-
-		if (time - detectionTime > timeWindowNS) {
-			tapTimes.clear();
-		}
 	}
 
 	private float getAccelDelta() {

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/TapDetectionManager.java

@@ -68,23 +68,24 @@ public TapDetectionManager(
 		this.resetHandler = resetHandler;
 		this.tapSetupHandler = tapSetupHandler;
 
-		// a list of tap detectors for each tracker
-		tapDetectors = new ArrayList<>();
-		for (Tracker tracker : trackers) {
-			TapDetection tapDetector = new TapDetection(skeleton, tracker);
-			tapDetector.setEnabled(true);
-			tapDetectors.add(tapDetector);
-		}
-
-		updateConfig();
+		updateConfig(trackers);
 	}
 
-	public void updateConfig() {
+	public void updateConfig(List<Tracker> trackers) {
 		// check the skeleton for new trackers
 		yawResetDetector = new TapDetection(skeleton, getTrackerToWatchYawReset());
 		fullResetDetector = new TapDetection(skeleton, getTrackerToWatchFullReset());
 		mountingResetDetector = new TapDetection(skeleton, getTrackerToWatchMountingReset());
 
+		if (trackers != null) {
+			tapDetectors = new ArrayList<>();
+			for (Tracker tracker : trackers) {
+				TapDetection tapDetector = new TapDetection(skeleton, tracker);
+				tapDetector.setEnabled(true);
+				tapDetectors.add(tapDetector);
+			}
+		}
+
 		if (this.config == null) {
 			return;
 		}
@@ -121,6 +122,7 @@ public void update() {
 				|| fullResetDetector == null
 				|| mountingResetDetector == null
 				|| tapDetectors == null
+				|| config == null
 		)
 			return;