Removed leg yaw averaging, it was causing leg gimbal lock

SlimeVR · Jul 8, 2021 · c2b4d30 · c2b4d30
1 parent 755ab59
commit c2b4d30
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 18 deletions.
diff --git a/src/main/java/io/eiren/gui/TrackersList.java b/src/main/java/io/eiren/gui/TrackersList.java
@@ -101,6 +101,7 @@ private class TrackerRow extends EJBag {
 		JLabel tps;
 		JLabel bat;
 		JLabel ping;
+		JLabel raw;
 
 		@AWTThread
 		public TrackerRow(Tracker t) {
@@ -175,12 +176,17 @@ public void actionPerformed(ActionEvent e) {
 			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, 4, 0, GridBagConstraints.FIRST_LINE_START));
 			add(new JLabel("Battery:"), c(2, 4, 0, GridBagConstraints.FIRST_LINE_START));
 			add(bat = new JLabel("0"), c(3, 4, 0, GridBagConstraints.FIRST_LINE_START));
+			if(t instanceof ReferenceAdjustedTracker) {
+				add(new JLabel("Raw:"), c(0, 5, 0, GridBagConstraints.FIRST_LINE_START));
+				add(raw = new JLabel("0 0 0 0"), s(c(1, 5, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+			}
 
 			setBorder(BorderFactory.createLineBorder(new Color(0x663399), 4, true));
 			TrackersList.this.add(this);
 			return this;
 		}
 
+		@SuppressWarnings("unchecked")
 		@AWTThread
 		public void update() {
 			if(position == null)
@@ -204,6 +210,14 @@ public void update() {
 				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) t).getBatteryVoltage(), 1));
 			if(t instanceof IMUTracker)
 				ping.setText(String.valueOf(((IMUTracker) t).ping));
+			if(t instanceof ReferenceAdjustedTracker) {
+				ReferenceAdjustedTracker<Tracker> t2 = (ReferenceAdjustedTracker<Tracker>) t;
+				t2.getTracker().getRotation(q);
+				raw.setText(StringUtils.prettyNumber(q.getX(), 4)
+						+ " " + StringUtils.prettyNumber(q.getY(), 4)
+						+ " " + StringUtils.prettyNumber(q.getZ(), 4)
+						+ " " + StringUtils.prettyNumber(q.getW(), 4));
+			}
 		}
 	}
 

diff --git a/src/main/java/io/eiren/gui/VRServerGUI.java b/src/main/java/io/eiren/gui/VRServerGUI.java
@@ -63,7 +63,6 @@ public void refresh() {
 		java.awt.EventQueue.invokeLater(new Runnable() {
 			@Override
 			public void run() {
-				toFront();
 				repaint();
 			}
 		});

diff --git a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java
@@ -188,7 +188,7 @@ public void updateLocalTransforms() {
 		leftLegTracker.getRotation(hipBuf);
 		leftAnkleTracker.getRotation(kneeBuf);
 
-		calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
+		//calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
 
 		leftHipNode.localTransform.setRotation(hipBuf);
 		leftKneeNode.localTransform.setRotation(kneeBuf);
@@ -205,7 +205,7 @@ public void updateLocalTransforms() {
 		rightLegTracker.getRotation(hipBuf);
 		rightAnkleTracker.getRotation(kneeBuf);
 
-		calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
+		//calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
 
 		rightHipNode.localTransform.setRotation(hipBuf);
 		rightKneeNode.localTransform.setRotation(kneeBuf);

diff --git a/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java
@@ -1,6 +1,5 @@
 package io.eiren.vr.trackers;
 
-import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
@@ -9,12 +8,8 @@
 public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 
 	public final E tracker;
-	private final Quaternion smoothedQuaternion = new Quaternion();
 	public final Quaternion adjustmentYaw = new Quaternion();
 	public final Quaternion adjustmentAttachment = new Quaternion();
-	protected float[] lastAngles = new float[3];
-	public float smooth = 0 * FastMath.DEG_TO_RAD;
-	private final float[] angles = new float[3];
 	protected float confidenceMultiplier = 1.0f;
 
 	public ReferenceAdjustedTracker(E tracker) {
@@ -88,7 +83,6 @@ public void resetYaw(Quaternion reference) {
 		adjustmentYaw.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
 
 		confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
-		lastAngles[0] = 1000;
 	}
 
 	protected void adjustInternal(Quaternion store) {
@@ -99,15 +93,6 @@ protected void adjustInternal(Quaternion store) {
 	@Override
 	public boolean getRotation(Quaternion store) {
 		tracker.getRotation(store);
-		if(smooth > 0) {
-			store.toAngles(angles);
-			if(Math.abs(angles[0] - lastAngles[0]) > smooth || Math.abs(angles[1] - lastAngles[1]) > smooth || Math.abs(angles[2] - lastAngles[2]) > smooth) {
-				smoothedQuaternion.set(store);
-				store.toAngles(lastAngles);
-			} else {
-				store.set(smoothedQuaternion);
-			}
-		}
 		adjustInternal(store);
 		return true;
 	}