From feb14109d251ed062806d83790277dd045c209f7 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 1 Jul 2021 00:40:34 +0300 Subject: [PATCH] Refactor trackers chaining and reset Pose processor now resets trackers accoring to its own tracker hierarchy Each tracker can now have body part designation independently of pose processor, pose processor scans all trackers to build skeleton hierarchy Trackers requiring adjustments are now created on the tracker supplier side Tracker waist distance is now diff with waist position (default is now 0) Model can now be updated live after tracker body position changed --- build.gradle | 6 +- src/main/java/io/eiren/gui/TrackersList.java | 8 +- .../vr/processor/HumanPoseProcessor.java | 78 +++------- .../vr/processor/HumanSekeletonWithLegs.java | 51 +++++-- .../vr/processor/HumanSkeleonWithWaist.java | 30 +++- .../io/eiren/vr/processor/HumanSkeleton.java | 3 + .../vr/processor/TrackerBodyPosition.java | 1 + .../io/eiren/vr/trackers/ComputedTracker.java | 23 +++ .../java/io/eiren/vr/trackers/HMDTracker.java | 2 + .../trackers/IMUReferenceAdjustedTracker.java | 33 ++++ .../java/io/eiren/vr/trackers/IMUTracker.java | 141 +++--------------- .../java/io/eiren/vr/trackers/MPUTracker.java | 112 ++++++++++++++ ...ker.java => ReferenceAdjustedTracker.java} | 27 +++- .../java/io/eiren/vr/trackers/Tracker.java | 10 ++ .../io/eiren/vr/trackers/TrackerUtils.java | 27 ++++ .../eiren/vr/trackers/TrackersUDPServer.java | 14 +- 16 files changed, 353 insertions(+), 213 deletions(-) create mode 100644 src/main/java/io/eiren/vr/trackers/IMUReferenceAdjustedTracker.java create mode 100644 src/main/java/io/eiren/vr/trackers/MPUTracker.java rename src/main/java/io/eiren/vr/trackers/{AdjustedTracker.java => ReferenceAdjustedTracker.java} (82%) create mode 100644 src/main/java/io/eiren/vr/trackers/TrackerUtils.java diff --git a/build.gradle b/build.gradle index cb47fb551..dc924bdb1 100644 --- a/build.gradle +++ b/build.gradle @@ -22,11 +22,13 @@ dependencies { // This dependency is exported to consumers, that is to say found on their compile classpath. api 'org.apache.commons:commons-math3:3.6.1' api 'org.yaml:snakeyaml:1.25' + api 'net.java.dev.jna:jna:5.6.0' + api 'net.java.dev.jna:jna-platform:5.6.0' + api 'com.illposed.osc:javaosc-core:0.8' // This dependency is used internally, and not exposed to consumers on their own compile classpath. implementation 'com.google.guava:guava:28.2-jre' - implementation 'net.java.dev.jna:jna:5.6.0' - implementation 'net.java.dev.jna:jna-platform:5.6.0' + // Use JUnit test framework testImplementation 'junit:junit:4.12' diff --git a/src/main/java/io/eiren/gui/TrackersList.java b/src/main/java/io/eiren/gui/TrackersList.java index d67455116..41eb100e2 100644 --- a/src/main/java/io/eiren/gui/TrackersList.java +++ b/src/main/java/io/eiren/gui/TrackersList.java @@ -1,12 +1,9 @@ package io.eiren.gui; import java.awt.GridBagConstraints; -import java.awt.event.MouseEvent; import java.util.List; -import javax.swing.JButton; import javax.swing.JLabel; -import javax.swing.event.MouseInputAdapter; import com.jme3.math.FastMath; import com.jme3.math.Quaternion; @@ -18,8 +15,7 @@ import io.eiren.util.ann.ThreadSafe; import io.eiren.util.ann.VRServerThread; import io.eiren.util.collections.FastList; import io.eiren.vr.VRServer; -import io.eiren.vr.trackers.AdjustedTracker; -import io.eiren.vr.trackers.CalibratingTracker; +import io.eiren.vr.trackers.ReferenceAdjustedTracker; import io.eiren.vr.trackers.ComputedTracker; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.IMUTracker; @@ -248,7 +244,7 @@ public class TrackersList extends EJBag { return 1; if(t instanceof IMUTracker) return 2; - if(t instanceof AdjustedTracker) + if(t instanceof ReferenceAdjustedTracker) return 5; return 1000; } diff --git a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java index 59e3b7c81..17960b407 100644 --- a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java +++ b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java @@ -1,35 +1,26 @@ package io.eiren.vr.processor; -import java.util.EnumMap; -import java.util.Iterator; import java.util.List; -import java.util.Map; import java.util.function.Consumer; -import com.jme3.math.Quaternion; - import io.eiren.util.ann.ThreadSafe; import io.eiren.util.ann.VRServerThread; import io.eiren.util.collections.FastList; import io.eiren.vr.VRServer; -import io.eiren.vr.trackers.AdjustedTracker; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; -import io.eiren.vr.trackers.TrackerConfig; import io.eiren.vr.trackers.TrackerStatus; +import io.eiren.vr.trackers.TrackerUtils; public class HumanPoseProcessor { private final VRServer server; - private final HMDTracker hmd; private final List computedTrackers = new FastList<>(); - private final Map trackers = new EnumMap<>(TrackerBodyPosition.class); private final List> onSkeletonUpdated = new FastList<>(); private HumanSkeleton skeleton; public HumanPoseProcessor(VRServer server, HMDTracker hmd) { this.server = server; - this.hmd = hmd; computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.WAIST)); computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT)); computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT)); @@ -62,21 +53,11 @@ public class HumanPoseProcessor { @VRServerThread public void trackerAdded(Tracker tracker) { - TrackerConfig config = server.getTrackerConfig(tracker); - if(config.designation != null) { - TrackerBodyPosition pos = TrackerBodyPosition.getByDesignation(config.designation); - if(pos != null) { - addTracker(tracker, pos); - } - } + updateSekeltonModel(); } @VRServerThread - private void addTracker(Tracker tracker, TrackerBodyPosition position) { - AdjustedTracker tt = new AdjustedTracker(tracker); - - trackers.put(position, tt); - server.registerTracker(tt); + public void trackerUpdated(Tracker tracker) { updateSekeltonModel(); } @@ -84,36 +65,26 @@ public class HumanPoseProcessor { private void updateSekeltonModel() { boolean hasWaist = false; boolean hasBothLegs = false; - boolean hasChest = false; - if(trackers.get(TrackerBodyPosition.WAIST) != null) + List allTrackers = server.getAllTrackers(); + Tracker waist = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST); + Tracker leftAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_ANKLE); + Tracker rightAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_ANKLE); + Tracker leftLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_LEG); + Tracker rightLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_LEG); + if(waist != null) hasWaist = true; - if(trackers.get(TrackerBodyPosition.CHEST) != null) - hasChest = true; - if(trackers.get(TrackerBodyPosition.LEFT_ANKLE) != null && trackers.get(TrackerBodyPosition.LEFT_LEG) != null - && trackers.get(TrackerBodyPosition.RIGHT_ANKLE) != null && trackers.get(TrackerBodyPosition.RIGHT_LEG) != null) + if(leftAnkle != null && rightAnkle != null && leftLeg != null && rightLeg != null) hasBothLegs = true; - if(!hasWaist && !hasChest) { + if(!hasWaist) { skeleton = null; // Can't track anything without waist } else if(hasBothLegs) { disconnectAllTrackers(); - AdjustedTracker waist = trackers.get(TrackerBodyPosition.WAIST); - if(waist == null) - waist = trackers.get(TrackerBodyPosition.CHEST); - AdjustedTracker chest = trackers.get(TrackerBodyPosition.CHEST); - if(chest == null) - chest = trackers.get(TrackerBodyPosition.WAIST); - skeleton = new HumanSekeletonWithLegs(server, waist, chest, trackers, computedTrackers); + skeleton = new HumanSekeletonWithLegs(server, computedTrackers); for(int i = 0; i < onSkeletonUpdated.size(); ++i) onSkeletonUpdated.get(i).accept(skeleton); } else { - AdjustedTracker waist = trackers.get(TrackerBodyPosition.WAIST); - if(waist == null) - waist = trackers.get(TrackerBodyPosition.CHEST); - AdjustedTracker chest = trackers.get(TrackerBodyPosition.CHEST); - if(chest == null) - chest = trackers.get(TrackerBodyPosition.WAIST); disconnectAllTrackers(); - skeleton = new HumanSkeleonWithWaist(server, waist, chest, computedTrackers); + skeleton = new HumanSkeleonWithWaist(server, computedTrackers); for(int i = 0; i < onSkeletonUpdated.size(); ++i) onSkeletonUpdated.get(i).accept(skeleton); } @@ -126,24 +97,15 @@ public class HumanPoseProcessor { } } - @VRServerThread - public void resetTrackers() { - Quaternion hmdRotation = new Quaternion(); - hmd.getRotation(hmdRotation); - - Iterator iterator = trackers.values().iterator(); - while(iterator.hasNext()) { - AdjustedTracker tt = iterator.next(); - tt.adjustFull(hmdRotation); - - TrackerConfig config = server.getTrackerConfig(tt); - tt.saveConfig(config); - } - } - @VRServerThread public void update() { if(skeleton != null) skeleton.updatePose(); } + + @VRServerThread + public void resetTrackers() { + if(skeleton != null) + skeleton.resetTrackersFull(); + } } diff --git a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java index 405c91997..703163ea2 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java +++ b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java @@ -1,14 +1,15 @@ package io.eiren.vr.processor; import java.util.List; -import java.util.Map; import com.jme3.math.FastMath; import com.jme3.math.Quaternion; +import io.eiren.util.ann.VRServerThread; import io.eiren.vr.VRServer; import io.eiren.vr.trackers.Tracker; import io.eiren.vr.trackers.TrackerStatus; +import io.eiren.vr.trackers.TrackerUtils; public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { @@ -53,14 +54,15 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { protected float kneeLerpFactor = 0.5f; - public HumanSekeletonWithLegs(VRServer server, Tracker waistTracker, Tracker chestTracker, Map trackers, List computedTrackers) { - super(server, waistTracker, chestTracker, computedTrackers); - this.leftLegTracker = trackers.get(TrackerBodyPosition.LEFT_LEG); - this.leftAnkleTracker = trackers.get(TrackerBodyPosition.LEFT_ANKLE); - this.leftFootTracker = trackers.get(TrackerBodyPosition.LEFT_FOOT); - this.rightLegTracker = trackers.get(TrackerBodyPosition.RIGHT_LEG); - this.rightAnkleTracker = trackers.get(TrackerBodyPosition.RIGHT_ANKLE); - this.rightFootTracker = trackers.get(TrackerBodyPosition.RIGHT_FOOT); + public HumanSekeletonWithLegs(VRServer server, List computedTrackers) { + super(server, computedTrackers); + List allTracekrs = server.getAllTrackers(); + this.leftLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_LEG); + this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_ANKLE); + this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_FOOT); + this.rightLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_LEG); + this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_ANKLE); + this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_FOOT); ComputedHumanPoseTracker lat = null; ComputedHumanPoseTracker rat = null; for(int i = 0; i < computedTrackers.size(); ++i) { @@ -213,4 +215,35 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation()); computedRightFootTracker.dataTick(); } + + @Override + @VRServerThread + public void resetTrackersFull() { + // Each tracker uses the tracker before it to adjust iteself, + // so trackers that don't need adjustments could be used too + super.resetTrackersFull(); + // Start with waist, it was reset in the parent + Quaternion referenceRotation = new Quaternion(); + this.waistTracker.getRotation(referenceRotation); + + this.leftLegTracker.resetFull(referenceRotation); + this.rightLegTracker.resetFull(referenceRotation); + this.leftLegTracker.getRotation(referenceRotation); + + this.leftAnkleTracker.resetFull(referenceRotation); + this.leftAnkleTracker.getRotation(referenceRotation); + + if(this.leftFootTracker != null) { + this.leftFootTracker.resetFull(referenceRotation); + } + + this.rightLegTracker.getRotation(referenceRotation); + + this.rightAnkleTracker.resetFull(referenceRotation); + this.rightAnkleTracker.getRotation(referenceRotation); + + if(this.rightAnkleTracker != null) { + this.rightAnkleTracker.resetFull(referenceRotation); + } + } } diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java index 509c2a75c..7a101aa16 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java @@ -12,6 +12,7 @@ import io.eiren.vr.VRServer; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; import io.eiren.vr.trackers.TrackerStatus; +import io.eiren.vr.trackers.TrackerUtils; public class HumanSkeleonWithWaist extends HumanSkeleton { @@ -43,7 +44,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { * tracker position, if you want to move resulting * tracker up or down from actual waist */ - protected float trackerWaistDistance = 0.57f; + protected float trackerWaistDistance = 0.0f; /** * Distacne from eyes to the base of the neck */ @@ -53,9 +54,10 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { */ protected float headShift = 0.1f; - public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, Tracker chestTracker, List computedTrackers) { - this.waistTracker = waistTracker; - this.chestTracker = chestTracker; + public HumanSkeleonWithWaist(VRServer server, List computedTrackers) { + List allTracekrs = server.getAllTrackers(); + this.waistTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST); + this.chestTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.CHEST, TrackerBodyPosition.WAIST); this.hmdTracker = server.hmdTracker; this.server = server; ComputedHumanPoseTracker cwt = null; @@ -85,7 +87,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0); chestNode.attachChild(trackerWaistNode); - trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - chestDistance), 0); + trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0); configMap.put("Head", headShift); configMap.put("Neck", neckLength); @@ -123,12 +125,12 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { server.config.setProperty("body.chestDistance", chestDistance); chestNode.localTransform.setTranslation(0, -chestDistance, 0); waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0); - trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - chestDistance), 0); + trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0); break; case "Virtual waist": trackerWaistDistance = newLength; server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance); - trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - chestDistance), 0); + trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0); break; } } @@ -170,4 +172,18 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation()); computedWaistTracker.dataTick(); } + + @Override + @VRServerThread + public void resetTrackersFull() { + // Each tracker uses the tracker before it to adjust iteself, + // so trackers that don't need adjustments could be used too + Quaternion referenceRotation = new Quaternion(); + server.hmdTracker.getRotation(referenceRotation); + + this.chestTracker.resetFull(referenceRotation); + this.chestTracker.getRotation(referenceRotation); + + this.waistTracker.resetFull(referenceRotation); + } } diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleton.java b/src/main/java/io/eiren/vr/processor/HumanSkeleton.java index a1b36cabd..d12fc33d6 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeleton.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeleton.java @@ -18,4 +18,7 @@ public abstract class HumanSkeleton { @ThreadSafe public abstract void setSkeletonConfig(String key, float newLength); + + @VRServerThread + public abstract void resetTrackersFull(); } diff --git a/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java b/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java index d00de3c5f..559337dcf 100644 --- a/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java +++ b/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java @@ -6,6 +6,7 @@ import java.util.Map; public enum TrackerBodyPosition { NONE(""), + HMD("body:HMD"), CHEST("body:chest"), WAIST("body:waist"), LEFT_LEG("body:left_leg"), diff --git a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java index 0f6086d98..f116e7598 100644 --- a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java @@ -3,12 +3,15 @@ package io.eiren.vr.trackers; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import io.eiren.vr.processor.TrackerBodyPosition; + public class ComputedTracker implements Tracker { public final Vector3f position = new Vector3f(); public final Quaternion rotation = new Quaternion(); protected final String name; protected TrackerStatus status = TrackerStatus.DISCONNECTED; + public TrackerBodyPosition bodyPosition = null; public ComputedTracker(String name) { this.name = name; @@ -16,10 +19,12 @@ public class ComputedTracker implements Tracker { @Override public void saveConfig(TrackerConfig config) { + config.setDesignation(bodyPosition == null ? null : bodyPosition.designation); } @Override public void loadConfig(TrackerConfig config) { + bodyPosition = TrackerBodyPosition.getByDesignation(config.designation); } @Override @@ -52,4 +57,22 @@ public class ComputedTracker implements Tracker { public float getConfidenceLevel() { return 1.0f; } + + @Override + public void resetFull(Quaternion reference) { + } + + @Override + public void resetYaw(Quaternion reference) { + } + + @Override + public TrackerBodyPosition getBodyPosition() { + return bodyPosition; + } + + @Override + public void setBodyPosition(TrackerBodyPosition position) { + this.bodyPosition = position; + } } diff --git a/src/main/java/io/eiren/vr/trackers/HMDTracker.java b/src/main/java/io/eiren/vr/trackers/HMDTracker.java index 915916024..4fa309a8e 100644 --- a/src/main/java/io/eiren/vr/trackers/HMDTracker.java +++ b/src/main/java/io/eiren/vr/trackers/HMDTracker.java @@ -1,6 +1,7 @@ package io.eiren.vr.trackers; import io.eiren.util.BufferedTimer; +import io.eiren.vr.processor.TrackerBodyPosition; public class HMDTracker extends ComputedTracker implements TrackerWithTPS { @@ -8,6 +9,7 @@ public class HMDTracker extends ComputedTracker implements TrackerWithTPS { public HMDTracker(String name) { super(name); + setBodyPosition(TrackerBodyPosition.HMD); } @Override diff --git a/src/main/java/io/eiren/vr/trackers/IMUReferenceAdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/IMUReferenceAdjustedTracker.java new file mode 100644 index 000000000..343ac3826 --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/IMUReferenceAdjustedTracker.java @@ -0,0 +1,33 @@ +package io.eiren.vr.trackers; + +public class IMUReferenceAdjustedTracker extends ReferenceAdjustedTracker implements TrackerWithTPS, TrackerWithBattery { + + public IMUReferenceAdjustedTracker(T tracker) { + super(tracker); + } + + @SuppressWarnings("unchecked") + @Override + public float getBatteryLevel() { + return ((T) tracker).getBatteryLevel(); + } + + @SuppressWarnings("unchecked") + @Override + public float getBatteryVoltage() { + return ((T) tracker).getBatteryVoltage(); + } + + @SuppressWarnings("unchecked") + @Override + public float getTPS() { + return ((T) tracker).getTPS(); + } + + @SuppressWarnings("unchecked") + @Override + public void dataTick() { + ((T) tracker).dataTick(); + } + +} diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java index 389e885a3..f9aa6a17b 100644 --- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -1,16 +1,14 @@ package io.eiren.vr.trackers; -import java.nio.ByteBuffer; -import java.util.function.Consumer; - import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; import io.eiren.math.FloatMath; import io.eiren.util.BufferedTimer; +import io.eiren.vr.processor.TrackerBodyPosition; -public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, TrackerWithBattery { +public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { public final Vector3f gyroVector = new Vector3f(); public final Vector3f accelVector = new Vector3f(); @@ -25,11 +23,11 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, protected float batteryVoltage = 0; protected BufferedTimer timer = new BufferedTimer(1f); - public ConfigurationData newCalibrationData; public int ping = -1; public StringBuilder serialBuffer = new StringBuilder(); long lastSerialUpdate = 0; + public TrackerBodyPosition bodyPosition = null; public IMUTracker(String name, TrackersUDPServer server) { this.name = name; @@ -38,13 +36,17 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, @Override public void saveConfig(TrackerConfig config) { + config.setDesignation(bodyPosition == null ? null : bodyPosition.designation); } @Override public void loadConfig(TrackerConfig config) { if(!FloatMath.equalsToZero(config.trackerRotation)) { rotAdjust.fromAngles(0, config.trackerRotation * FastMath.DEG_TO_RAD, 0); + } else { + rotAdjust.loadIdentity(); } + bodyPosition = TrackerBodyPosition.getByDesignation(config.designation); } @Override @@ -74,11 +76,6 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, this.status = status; } - @Override - public void startCalibration(Consumer calibrationDataConsumer) { - //server.sendCalibrationCommand(this, calibrationDataConsumer); - } - @Override public float getTPS() { return timer.getAverageFPS(); @@ -89,16 +86,6 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, timer.update(); } - @Override - public void requestCalibrationData(Consumer calibrationDataConsumer) { - //server.requestCalibrationData(this, calibrationDataConsumer); - } - - @Override - public void uploadNewClibrationData() { - //server.uploadNewCalibrationData(this, newCalibrationData); - } - @Override public float getConfidenceLevel() { return confidence; @@ -121,104 +108,22 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, public void setBatteryVoltage(float voltage) { this.batteryVoltage = voltage; } - - public static class ConfigurationData { - //acel offsets and correction matrix - float[] A_B = new float[3]; - float[][] A_Ainv = new float[3][3]; - // mag offsets and correction matrix - float[] M_B = new float[3]; - float[][] M_Ainv = new float[3][3]; - //raw offsets, determined for gyro at rest - float[] G_off = new float[3]; - int deviceId = -1; - int deviceMode = -1; - - public ConfigurationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) { - A_B[0] = (float) accelBasis[0]; - A_B[1] = (float) accelBasis[1]; - A_B[2] = (float) accelBasis[2]; - - A_Ainv[0][0] = (float) accelAInv[0]; - A_Ainv[0][1] = (float) accelAInv[1]; - A_Ainv[0][2] = (float) accelAInv[2]; - A_Ainv[1][0] = (float) accelAInv[3]; - A_Ainv[1][1] = (float) accelAInv[4]; - A_Ainv[1][2] = (float) accelAInv[5]; - A_Ainv[2][0] = (float) accelAInv[6]; - A_Ainv[2][1] = (float) accelAInv[7]; - A_Ainv[2][2] = (float) accelAInv[8]; - - M_B[0] = (float) magBasis[0]; - M_B[1] = (float) magBasis[1]; - M_B[2] = (float) magBasis[2]; - - M_Ainv[0][0] = (float) magAInv[0]; - M_Ainv[0][1] = (float) magAInv[1]; - M_Ainv[0][2] = (float) magAInv[2]; - M_Ainv[1][0] = (float) magAInv[3]; - M_Ainv[1][1] = (float) magAInv[4]; - M_Ainv[1][2] = (float) magAInv[5]; - M_Ainv[2][0] = (float) magAInv[6]; - M_Ainv[2][1] = (float) magAInv[7]; - M_Ainv[2][2] = (float) magAInv[8]; - - G_off[0] = (float) gyroOffset[0]; - G_off[1] = (float) gyroOffset[1]; - G_off[2] = (float) gyroOffset[2]; - } - - public ConfigurationData(ByteBuffer buffer) { - deviceMode = buffer.getInt(); - deviceId = buffer.getInt(); - // Data is read in reverse, because it was reversed when sending - G_off[2] = buffer.getFloat(); - G_off[1] = buffer.getFloat(); - G_off[0] = buffer.getFloat(); - - M_Ainv[2][2] = buffer.getFloat(); - M_Ainv[2][1] = buffer.getFloat(); - M_Ainv[2][0] = buffer.getFloat(); - M_Ainv[1][2] = buffer.getFloat(); - M_Ainv[1][1] = buffer.getFloat(); - M_Ainv[1][0] = buffer.getFloat(); - M_Ainv[0][2] = buffer.getFloat(); - M_Ainv[0][1] = buffer.getFloat(); - M_Ainv[0][0] = buffer.getFloat(); - - M_B[2] = buffer.getFloat(); - M_B[1] = buffer.getFloat(); - M_B[0] = buffer.getFloat(); - - A_Ainv[2][2] = buffer.getFloat(); - A_Ainv[2][1] = buffer.getFloat(); - A_Ainv[2][0] = buffer.getFloat(); - A_Ainv[1][2] = buffer.getFloat(); - A_Ainv[1][1] = buffer.getFloat(); - A_Ainv[1][0] = buffer.getFloat(); - A_Ainv[0][2] = buffer.getFloat(); - A_Ainv[0][1] = buffer.getFloat(); - A_Ainv[0][0] = buffer.getFloat(); - - A_B[2] = buffer.getFloat(); - A_B[1] = buffer.getFloat(); - A_B[0] = buffer.getFloat(); - } - - public String toTextMatrix() { - StringBuilder sb = new StringBuilder(); - sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", A_B[0], A_B[1], A_B[2])); - sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", A_Ainv[0][0], A_Ainv[0][1], A_Ainv[0][2])); - sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", A_Ainv[1][0], A_Ainv[1][1], A_Ainv[1][2])); - sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", A_Ainv[2][0], A_Ainv[2][1], A_Ainv[2][2])); - sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", M_B[0], M_B[1], M_B[2])); - sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", M_Ainv[0][0], M_Ainv[0][1], M_Ainv[0][2])); - sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", M_Ainv[1][0], M_Ainv[1][1], M_Ainv[1][2])); - sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", M_Ainv[2][0], M_Ainv[2][1], M_Ainv[2][2])); - sb.append(String.format("{%8.2f, %8.2f, %8.2f}};\n", G_off[0], G_off[1], G_off[2])); - - return sb.toString(); - } + @Override + public void resetFull(Quaternion reference) { + } + + @Override + public void resetYaw(Quaternion reference) { + } + + @Override + public TrackerBodyPosition getBodyPosition() { + return bodyPosition; + } + + @Override + public void setBodyPosition(TrackerBodyPosition position) { + this.bodyPosition = position; } } diff --git a/src/main/java/io/eiren/vr/trackers/MPUTracker.java b/src/main/java/io/eiren/vr/trackers/MPUTracker.java new file mode 100644 index 000000000..4c74c8835 --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/MPUTracker.java @@ -0,0 +1,112 @@ +package io.eiren.vr.trackers; + +import java.nio.ByteBuffer; + +public class MPUTracker extends IMUTracker { + + public ConfigurationData newCalibrationData; + + public MPUTracker(String name, TrackersUDPServer server) { + super(name, server); + } + + public static class ConfigurationData { + + //acel offsets and correction matrix + float[] A_B = new float[3]; + float[][] A_Ainv = new float[3][3]; + // mag offsets and correction matrix + float[] M_B = new float[3]; + float[][] M_Ainv = new float[3][3]; + //raw offsets, determined for gyro at rest + float[] G_off = new float[3]; + int deviceId = -1; + int deviceMode = -1; + + public ConfigurationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) { + A_B[0] = (float) accelBasis[0]; + A_B[1] = (float) accelBasis[1]; + A_B[2] = (float) accelBasis[2]; + + A_Ainv[0][0] = (float) accelAInv[0]; + A_Ainv[0][1] = (float) accelAInv[1]; + A_Ainv[0][2] = (float) accelAInv[2]; + A_Ainv[1][0] = (float) accelAInv[3]; + A_Ainv[1][1] = (float) accelAInv[4]; + A_Ainv[1][2] = (float) accelAInv[5]; + A_Ainv[2][0] = (float) accelAInv[6]; + A_Ainv[2][1] = (float) accelAInv[7]; + A_Ainv[2][2] = (float) accelAInv[8]; + + M_B[0] = (float) magBasis[0]; + M_B[1] = (float) magBasis[1]; + M_B[2] = (float) magBasis[2]; + + M_Ainv[0][0] = (float) magAInv[0]; + M_Ainv[0][1] = (float) magAInv[1]; + M_Ainv[0][2] = (float) magAInv[2]; + M_Ainv[1][0] = (float) magAInv[3]; + M_Ainv[1][1] = (float) magAInv[4]; + M_Ainv[1][2] = (float) magAInv[5]; + M_Ainv[2][0] = (float) magAInv[6]; + M_Ainv[2][1] = (float) magAInv[7]; + M_Ainv[2][2] = (float) magAInv[8]; + + G_off[0] = (float) gyroOffset[0]; + G_off[1] = (float) gyroOffset[1]; + G_off[2] = (float) gyroOffset[2]; + } + + public ConfigurationData(ByteBuffer buffer) { + deviceMode = buffer.getInt(); + deviceId = buffer.getInt(); + // Data is read in reverse, because it was reversed when sending + G_off[2] = buffer.getFloat(); + G_off[1] = buffer.getFloat(); + G_off[0] = buffer.getFloat(); + + M_Ainv[2][2] = buffer.getFloat(); + M_Ainv[2][1] = buffer.getFloat(); + M_Ainv[2][0] = buffer.getFloat(); + M_Ainv[1][2] = buffer.getFloat(); + M_Ainv[1][1] = buffer.getFloat(); + M_Ainv[1][0] = buffer.getFloat(); + M_Ainv[0][2] = buffer.getFloat(); + M_Ainv[0][1] = buffer.getFloat(); + M_Ainv[0][0] = buffer.getFloat(); + + M_B[2] = buffer.getFloat(); + M_B[1] = buffer.getFloat(); + M_B[0] = buffer.getFloat(); + + A_Ainv[2][2] = buffer.getFloat(); + A_Ainv[2][1] = buffer.getFloat(); + A_Ainv[2][0] = buffer.getFloat(); + A_Ainv[1][2] = buffer.getFloat(); + A_Ainv[1][1] = buffer.getFloat(); + A_Ainv[1][0] = buffer.getFloat(); + A_Ainv[0][2] = buffer.getFloat(); + A_Ainv[0][1] = buffer.getFloat(); + A_Ainv[0][0] = buffer.getFloat(); + + A_B[2] = buffer.getFloat(); + A_B[1] = buffer.getFloat(); + A_B[0] = buffer.getFloat(); + } + + public String toTextMatrix() { + StringBuilder sb = new StringBuilder(); + sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", A_B[0], A_B[1], A_B[2])); + sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", A_Ainv[0][0], A_Ainv[0][1], A_Ainv[0][2])); + sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", A_Ainv[1][0], A_Ainv[1][1], A_Ainv[1][2])); + sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", A_Ainv[2][0], A_Ainv[2][1], A_Ainv[2][2])); + sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", M_B[0], M_B[1], M_B[2])); + sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", M_Ainv[0][0], M_Ainv[0][1], M_Ainv[0][2])); + sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", M_Ainv[1][0], M_Ainv[1][1], M_Ainv[1][2])); + sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", M_Ainv[2][0], M_Ainv[2][1], M_Ainv[2][2])); + sb.append(String.format("{%8.2f, %8.2f, %8.2f}};\n", G_off[0], G_off[1], G_off[2])); + + return sb.toString(); + } + } +} diff --git a/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java similarity index 82% rename from src/main/java/io/eiren/vr/trackers/AdjustedTracker.java rename to src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java index bd014e46d..41e4d67dd 100644 --- a/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java @@ -4,7 +4,9 @@ import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; -public class AdjustedTracker implements Tracker { +import io.eiren.vr.processor.TrackerBodyPosition; + +public class ReferenceAdjustedTracker implements Tracker { public final Tracker tracker; private final Quaternion smoothedQuaternion = new Quaternion(); @@ -13,12 +15,9 @@ public class AdjustedTracker implements Tracker { 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; - public AdjustedTracker(Tracker tracker) { + public ReferenceAdjustedTracker(Tracker tracker) { this.tracker = tracker; } @@ -30,8 +29,9 @@ public class AdjustedTracker implements Tracker { public void saveConfig(TrackerConfig config) { } - public void adjustFull(Quaternion reference) { - adjustYaw(reference); + @Override + public void resetFull(Quaternion reference) { + resetYaw(reference); Quaternion sensorRotation = new Quaternion(); tracker.getRotation(sensorRotation); @@ -40,7 +40,8 @@ public class AdjustedTracker implements Tracker { adjustmentAttachment.set(sensorRotation).inverseLocal(); } - public void adjustYaw(Quaternion reference) { + @Override + public void resetYaw(Quaternion reference) { Quaternion targetTrackerRotation = new Quaternion(reference); // Use only yaw HMD rotation @@ -100,4 +101,14 @@ public class AdjustedTracker implements Tracker { public float getConfidenceLevel() { return tracker.getConfidenceLevel() * confidenceMultiplier; } + + @Override + public TrackerBodyPosition getBodyPosition() { + return tracker.getBodyPosition(); + } + + @Override + public void setBodyPosition(TrackerBodyPosition position) { + tracker.setBodyPosition(position); + } } \ No newline at end of file diff --git a/src/main/java/io/eiren/vr/trackers/Tracker.java b/src/main/java/io/eiren/vr/trackers/Tracker.java index 8c25e87fe..5592a6d94 100644 --- a/src/main/java/io/eiren/vr/trackers/Tracker.java +++ b/src/main/java/io/eiren/vr/trackers/Tracker.java @@ -3,6 +3,8 @@ package io.eiren.vr.trackers; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import io.eiren.vr.processor.TrackerBodyPosition; + public interface Tracker { public boolean getPosition(Vector3f store); @@ -18,4 +20,12 @@ public interface Tracker { public void saveConfig(TrackerConfig config); public float getConfidenceLevel(); + + public void resetFull(Quaternion reference); + + public void resetYaw(Quaternion reference); + + public TrackerBodyPosition getBodyPosition(); + + public void setBodyPosition(TrackerBodyPosition position); } diff --git a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java new file mode 100644 index 000000000..25adec295 --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java @@ -0,0 +1,27 @@ +package io.eiren.vr.trackers; + +import java.util.List; + +import io.eiren.vr.processor.TrackerBodyPosition; + +public class TrackerUtils { + + private TrackerUtils() { + } + + public static Tracker findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position) { + for(int i = 0; i < allTrackers.size(); ++i) { + Tracker t = allTrackers.get(i); + if(t.getBodyPosition() == position) + return t; + } + return null; + } + + public static Tracker findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { + Tracker t = findTrackerForBodyPosition(allTrackers, position); + if(t != null) + return t; + return findTrackerForBodyPosition(allTrackers, altPosition); + } +} diff --git a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java index 6cafce59f..e4cfdfdba 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java @@ -41,13 +41,13 @@ public class TrackersUDPServer extends Thread { private final List trackers = new FastList<>(); private final Map trackersMap = new HashMap<>(); private final Map> calibrationDataRequests = new HashMap<>(); - private final Consumer trackersConsumer; + private final Consumer trackersConsumer; private final int port; protected DatagramSocket socket = null; protected long lastKeepup = System.currentTimeMillis(); - public TrackersUDPServer(int port, String name, Consumer trackersConsumer) { + public TrackersUDPServer(int port, String name, Consumer trackersConsumer) { super(name); this.port = port; this.trackersConsumer = trackersConsumer; @@ -90,7 +90,8 @@ public class TrackersUDPServer extends Thread { if(sb.length() == 0) sb.append("owoTrack"); IMUTracker imu = new IMUTracker("udp:/" + handshakePacket.getAddress().toString(), this); - trackersConsumer.accept(imu); + IMUReferenceAdjustedTracker adjustedTracker = new IMUReferenceAdjustedTracker<>(imu); + trackersConsumer.accept(adjustedTracker); sensor = new TrackerConnection(imu, addr); int i = 0; synchronized(trackers) { @@ -108,7 +109,8 @@ public class TrackersUDPServer extends Thread { System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.tracker.getName()); IMUTracker imu = new IMUTracker(connection.tracker.getName() + "/1", this); connection.secondTracker = imu; - trackersConsumer.accept(imu); + IMUReferenceAdjustedTracker adjustedTracker = new IMUReferenceAdjustedTracker<>(imu); + trackersConsumer.accept(adjustedTracker); System.out.println("[TrackerServer] Sensor added with address " + imu.getName()); } @@ -135,6 +137,8 @@ public class TrackersUDPServer extends Thread { sensor.lastPacket = System.currentTimeMillis(); int packetId; switch(packetId = bb.getInt()) { + case 0: + break; case 3: setUpNewSensor(recieve, bb); break; @@ -196,7 +200,7 @@ public class TrackersUDPServer extends Thread { if(sensor == null) break; bb.getLong(); - IMUTracker.ConfigurationData data = new IMUTracker.ConfigurationData(bb); + MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb); Consumer dataConsumer = calibrationDataRequests.remove(sensor.tracker); if(dataConsumer != null) { dataConsumer.accept(data.toTextMatrix());