From 9dc3fb20dc550ff3951f675739c8324cdd0eca73 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 27 May 2021 21:31:02 +0300 Subject: [PATCH] Fix sensor attachment alignment correction to use quaternions and separate it from sensor yaw difference (should work better with feet tracking) --- .../vr/processor/HumanPoseProcessor.java | 4 +- .../vr/trackers/AdjustedFullTracker.java | 42 ----------------- .../io/eiren/vr/trackers/AdjustedTracker.java | 45 ++++++++++++++++--- .../eiren/vr/trackers/AdjustedYawTracker.java | 38 ---------------- 4 files changed, 42 insertions(+), 87 deletions(-) delete mode 100644 src/main/java/io/eiren/vr/trackers/AdjustedFullTracker.java delete mode 100644 src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java diff --git a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java index 38001e2d8..3d393c537 100644 --- a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java +++ b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java @@ -74,7 +74,7 @@ public class HumanPoseProcessor { @VRServerThread private void addTracker(Tracker tracker, TrackerBodyPosition position) { - AdjustedTracker tt = new AdjustedFullTracker(tracker); + AdjustedTracker tt = new AdjustedTracker(tracker); trackers.put(position, tt); server.registerTracker(tt); @@ -135,7 +135,7 @@ public class HumanPoseProcessor { Iterator iterator = trackers.values().iterator(); while(iterator.hasNext()) { AdjustedTracker tt = iterator.next(); - tt.adjust(hmdRotation); + tt.adjustFull(hmdRotation); TrackerConfig config = server.getTrackerConfig(tt); tt.saveConfig(config); diff --git a/src/main/java/io/eiren/vr/trackers/AdjustedFullTracker.java b/src/main/java/io/eiren/vr/trackers/AdjustedFullTracker.java deleted file mode 100644 index 7b43bab28..000000000 --- a/src/main/java/io/eiren/vr/trackers/AdjustedFullTracker.java +++ /dev/null @@ -1,42 +0,0 @@ -package io.eiren.vr.trackers; - -import com.jme3.math.Quaternion; - -public class AdjustedFullTracker extends AdjustedYawTracker { - - private final float[] angles = new float[3]; - private float yawCorrection = 0; - private float pitchCorrection = 0; - private float rollCorrection = 0; - - public AdjustedFullTracker(Tracker tracker) { - super(tracker); - } - - @Override - public void adjust(Quaternion reference) { - float[] angles = this.angles; - reference.toAngles(angles); - // Use only yaw HMD rotation - angles[0] = 0; - angles[2] = 0; - - Quaternion sensorRotation = new Quaternion(); - tracker.getRotation(sensorRotation); - float[] angles2 = new float[3]; - sensorRotation.toAngles(angles2); - yawCorrection = angles[1] - angles2[1]; - pitchCorrection = angles[0] - angles2[0]; - rollCorrection = angles[2] - angles2[2]; - } - - @Override - protected void adjustInternal(Quaternion store) { - float[] angles = this.angles; - store.toAngles(angles); - angles[0] += pitchCorrection; - angles[1] += yawCorrection; - angles[2] += rollCorrection; - store.fromAngles(angles); - } -} diff --git a/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java index f5c120d0e..bd014e46d 100644 --- a/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java @@ -4,13 +4,17 @@ import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; -public abstract class AdjustedTracker implements Tracker { +public class AdjustedTracker implements Tracker { public final Tracker tracker; private final Quaternion smoothedQuaternion = new Quaternion(); - private float[] angles = new float[3]; + 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]; + private float pitchCorrection = 0; + private float rollCorrection = 0; protected float confidenceMultiplier = 1.0f; @@ -25,8 +29,41 @@ public abstract class AdjustedTracker implements Tracker { @Override public void saveConfig(TrackerConfig config) { } + + public void adjustFull(Quaternion reference) { + adjustYaw(reference); + + Quaternion sensorRotation = new Quaternion(); + tracker.getRotation(sensorRotation); + adjustmentYaw.mult(sensorRotation, sensorRotation); + + adjustmentAttachment.set(sensorRotation).inverseLocal(); + } - public abstract void adjust(Quaternion reference); + public void adjustYaw(Quaternion reference) { + Quaternion targetTrackerRotation = new Quaternion(reference); + + // Use only yaw HMD rotation + float[] angles = new float[3]; + targetTrackerRotation.toAngles(angles); + targetTrackerRotation.fromAngles(0, angles[1], 0); + + Quaternion sensorRotation = new Quaternion(); + tracker.getRotation(sensorRotation); + + sensorRotation.toAngles(angles); + sensorRotation.fromAngles(0, angles[1], 0); + + adjustmentYaw.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation); + + confidenceMultiplier = 1.0f / tracker.getConfidenceLevel(); + lastAngles[0] = 1000; + } + + protected void adjustInternal(Quaternion store) { + store.multLocal(adjustmentAttachment); + adjustmentYaw.mult(store, store); + } @Override public boolean getRotation(Quaternion store) { @@ -43,8 +80,6 @@ public abstract class AdjustedTracker implements Tracker { adjustInternal(store); return true; } - - protected abstract void adjustInternal(Quaternion store); @Override public boolean getPosition(Vector3f store) { diff --git a/src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java b/src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java deleted file mode 100644 index 0997f759e..000000000 --- a/src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java +++ /dev/null @@ -1,38 +0,0 @@ -package io.eiren.vr.trackers; - -import com.jme3.math.Quaternion; - -public class AdjustedYawTracker extends AdjustedTracker { - - public final Quaternion adjustment = new Quaternion(); - - public AdjustedYawTracker(Tracker tracker) { - super(tracker); - } - - @Override - public void adjust(Quaternion reference) { - Quaternion targetTrackerRotation = new Quaternion(reference); - - // Use only yaw HMD rotation - float[] angles = new float[3]; - targetTrackerRotation.toAngles(angles); - targetTrackerRotation.fromAngles(0, angles[1], 0); - - Quaternion sensorRotation = new Quaternion(); - tracker.getRotation(sensorRotation); - - sensorRotation.toAngles(angles); - sensorRotation.fromAngles(0, angles[1], 0); - - adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation); - - confidenceMultiplier = 1.0f / tracker.getConfidenceLevel(); - lastAngles[0] = 1000; - } - - @Override - protected void adjustInternal(Quaternion store) { - adjustment.mult(store, store); - } -}