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());