diff --git a/src/main/java/io/eiren/gui/CalibrationWindow.java b/src/main/java/io/eiren/gui/CalibrationWindow.java new file mode 100644 index 000000000..3470b0d9d --- /dev/null +++ b/src/main/java/io/eiren/gui/CalibrationWindow.java @@ -0,0 +1,89 @@ +package io.eiren.gui; + + +import java.awt.Container; +import java.awt.event.MouseEvent; + +import javax.swing.BoxLayout; +import javax.swing.JButton; +import javax.swing.JFrame; +import javax.swing.JLabel; +import javax.swing.JTextArea; +import javax.swing.border.EmptyBorder; +import javax.swing.event.MouseInputAdapter; + +import io.eiren.util.ann.AWTThread; +import io.eiren.vr.trackers.CalibratingTracker; +import io.eiren.vr.trackers.Tracker; + +public class CalibrationWindow extends JFrame { + + public final Tracker tracker; + private JTextArea currentCalibration; + private JTextArea newCalibration; + private JButton calibrateButton; + + public CalibrationWindow(Tracker t) { + super(t.getName() + " calibration"); + this.tracker = t; + getContentPane().setLayout(new BoxLayout(getContentPane(), BoxLayout.LINE_AXIS)); + + build(); + } + + public void currentCalibrationRecieved(String str) { + java.awt.EventQueue.invokeLater(() -> { + currentCalibration.setText(str); + pack(); + }); + } + + public void newCalibrationRecieved(String str) { + java.awt.EventQueue.invokeLater(() -> { + calibrateButton.setText("Calibrate"); + newCalibration.setText(str); + pack(); + }); + } + + @AWTThread + private void build() { + Container pane = getContentPane(); + + pane.add(calibrateButton = new JButton("Calibrate") {{ + addMouseListener(new MouseInputAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + calibrateButton.setText("Calibrating..."); + ((CalibratingTracker) tracker).startCalibration(CalibrationWindow.this::newCalibrationRecieved); + } + }); + }}); + + pane.add(new EJBox(BoxLayout.PAGE_AXIS) {{ + setBorder(new EmptyBorder(i(5))); + add(new JLabel("Current calibration")); + add(currentCalibration = new JTextArea(10, 25)); + + ((CalibratingTracker) tracker).requestCalibrationData(CalibrationWindow.this::currentCalibrationRecieved); + }}); + pane.add(new EJBox(BoxLayout.PAGE_AXIS) {{ + setBorder(new EmptyBorder(i(5))); + add(new JLabel("New calibration")); + add(newCalibration = new JTextArea(10, 25)); + }}); + + + // Pack and display + pack(); + setLocationRelativeTo(null); + setVisible(true); + java.awt.EventQueue.invokeLater(new Runnable() { + @Override + public void run() { + toFront(); + repaint(); + } + }); + } +} diff --git a/src/main/java/io/eiren/gui/EJBag.java b/src/main/java/io/eiren/gui/EJBag.java new file mode 100644 index 000000000..a50045d9a --- /dev/null +++ b/src/main/java/io/eiren/gui/EJBag.java @@ -0,0 +1,10 @@ +package io.eiren.gui; + +import java.awt.GridBagLayout; + +public class EJBag extends EJPanel { + + public EJBag() { + super(new GridBagLayout()); + } +} diff --git a/src/main/java/io/eiren/gui/EJBox.java b/src/main/java/io/eiren/gui/EJBox.java new file mode 100644 index 000000000..fd774c690 --- /dev/null +++ b/src/main/java/io/eiren/gui/EJBox.java @@ -0,0 +1,11 @@ +package io.eiren.gui; + +import javax.swing.BoxLayout; + +public class EJBox extends EJPanel { + + public EJBox(int layout) { + super(); + setLayout(new BoxLayout(this, layout)); + } +} diff --git a/src/main/java/io/eiren/gui/EJPanel.java b/src/main/java/io/eiren/gui/EJPanel.java new file mode 100644 index 000000000..f520a7d35 --- /dev/null +++ b/src/main/java/io/eiren/gui/EJPanel.java @@ -0,0 +1,104 @@ +package io.eiren.gui; + +import java.awt.Component; +import java.awt.Dimension; +import java.awt.GridBagConstraints; +import java.awt.Insets; +import java.awt.LayoutManager; + +import javax.swing.Box; +import javax.swing.JPanel; + +public abstract class EJPanel extends JPanel { + + public static boolean NEEDS_DOWNSCALE = false; + public static float DOWNSCALE_FACTOR = 0.75f; + + public EJPanel() { + super(); + } + + public EJPanel(LayoutManager manager) { + super(manager); + } + + public static void s(Component c, int width, int height) { + if(NEEDS_DOWNSCALE) { + width = (int) Math.ceil(width * DOWNSCALE_FACTOR); + height = (int) Math.ceil(height * DOWNSCALE_FACTOR); + } + c.setSize(width, height); + Dimension d = new Dimension(width, height); + c.setPreferredSize(d); + c.setMaximumSize(d); + c.setMinimumSize(d); + } + + public static void minWidth(Component c, int width, int height) { + if(NEEDS_DOWNSCALE) { + height = (int) Math.ceil(height * DOWNSCALE_FACTOR); + width = (int) Math.ceil(width * DOWNSCALE_FACTOR); + } + c.setPreferredSize(new Dimension(Short.MAX_VALUE, height)); + c.setMaximumSize(new Dimension(Short.MAX_VALUE, height)); + c.setMinimumSize(new Dimension(width, height)); + } + + public static void minHeight(Component c, int width, int height) { + if(NEEDS_DOWNSCALE) { + height = (int) Math.ceil(height * DOWNSCALE_FACTOR); + width = (int) Math.ceil(width * DOWNSCALE_FACTOR); + } + c.setPreferredSize(new Dimension(width, Short.MAX_VALUE)); + c.setMaximumSize(new Dimension(width, Short.MAX_VALUE)); + c.setMinimumSize(new Dimension(width, height)); + } + + public static GridBagConstraints c(int x, int y) { + GridBagConstraints c = new GridBagConstraints(); + c.gridx = x; + c.gridy = y; + return c; + } + + public static GridBagConstraints c(int x, int y, int padding) { + GridBagConstraints c = new GridBagConstraints(); + c.gridx = x; + c.gridy = y; + c.insets = new Insets(padding, padding, padding, padding); + return c; + } + + public static GridBagConstraints c(int x, int y, int padding, int anchor) { + GridBagConstraints c = new GridBagConstraints(); + c.gridx = x; + c.gridy = y; + c.insets = new Insets(padding, padding, padding, padding); + c.anchor = anchor; + return c; + } + + public static GridBagConstraints c(int x, int y, Insets insets) { + GridBagConstraints c = new GridBagConstraints(); + c.gridx = x; + c.gridy = y; + c.insets = insets; + return c; + } + + public static Insets i(int s) { + return new Insets(s, s, s, s); + } + + public static Insets i(int h, int v) { + return new Insets(v, h, v, h); + } + + public static Component padding(int width, int height) { + return Box.createRigidArea(new Dimension(width, height)); + } + + public static int fontSize(int baseSize) { + return NEEDS_DOWNSCALE ? (int) Math.ceil(baseSize * DOWNSCALE_FACTOR) : baseSize; + } +} diff --git a/src/main/java/io/eiren/gui/EJlabel.java b/src/main/java/io/eiren/gui/EJlabel.java new file mode 100644 index 000000000..aa64a840a --- /dev/null +++ b/src/main/java/io/eiren/gui/EJlabel.java @@ -0,0 +1,8 @@ +package io.eiren.gui; + +import javax.swing.JLabel; + +public class EJlabel extends JLabel { + + +} diff --git a/src/main/java/io/eiren/gui/SkeletonList.java b/src/main/java/io/eiren/gui/SkeletonList.java new file mode 100644 index 000000000..de4fa9eb8 --- /dev/null +++ b/src/main/java/io/eiren/gui/SkeletonList.java @@ -0,0 +1,105 @@ +package io.eiren.gui; + +import java.awt.GridBagConstraints; +import java.util.List; + +import javax.swing.JLabel; + +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import io.eiren.util.StringUtils; +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.processor.HumanSkeleton; +import io.eiren.vr.processor.TransformNode; + +public class SkeletonList extends EJBag { + + Quaternion q = new Quaternion(); + Vector3f v = new Vector3f(); + float[] angles = new float[3]; + + private final VRServer server; + private final VRServerGUI gui; + private final List nodes = new FastList<>(); + + public SkeletonList(VRServer server, VRServerGUI gui) { + super(); + this.server = server; + this.gui = gui; + + setAlignmentY(TOP_ALIGNMENT); + server.addSkeletonUpdatedCallback(this::skeletonUpdated); + } + + @ThreadSafe + public void skeletonUpdated(HumanSkeleton newSkeleton) { + java.awt.EventQueue.invokeLater(() -> { + removeAll(); + nodes.clear(); + + add(new JLabel("Joint"), c(0, 0, 2)); + add(new JLabel("X"), c(1, 0, 2)); + add(new JLabel("Y"), c(2, 0, 2)); + add(new JLabel("Z"), c(3, 0, 2)); + add(new JLabel("Pitch"), c(4, 0, 2)); + add(new JLabel("Yaw"), c(5, 0, 2)); + add(new JLabel("Roll"), c(6, 0, 2)); + + newSkeleton.getRootNode().depthFirstTraversal((node) -> { + int n = nodes.size(); + nodes.add(new NodeStatus(node, n + 1)); + }); + + + gui.refresh(); + }); + } + + @VRServerThread + public void updateBones() { + java.awt.EventQueue.invokeLater(() -> { + for(int i = 0; i < nodes.size(); ++i) + nodes.get(i).update(); + }); + } + + private class NodeStatus { + + TransformNode n; + JLabel x; + JLabel y; + JLabel z; + JLabel a1; + JLabel a2; + JLabel a3; + + public NodeStatus(TransformNode node, int n) { + this.n = node; + add(new JLabel(node.getName()), c(0, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(x = new JLabel("0"), c(1, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(y = new JLabel("0"), c(2, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(z = new JLabel("0"), c(3, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(a1 = new JLabel("0"), c(4, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(a2 = new JLabel("0"), c(5, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(a3 = new JLabel("0"), c(6, n, 2, GridBagConstraints.FIRST_LINE_START)); + } + + public void update() { + n.worldTransform.getTranslation(v); + n.worldTransform.getRotation(q); + q.toAngles(angles); + + x.setText(StringUtils.prettyNumber(v.x, 2)); + y.setText(StringUtils.prettyNumber(v.y, 2)); + z.setText(StringUtils.prettyNumber(v.z, 2)); + a1.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)); + a2.setText(StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)); + a3.setText(StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); + } + } +} diff --git a/src/main/java/io/eiren/gui/TrackersList.java b/src/main/java/io/eiren/gui/TrackersList.java new file mode 100644 index 000000000..6934bb087 --- /dev/null +++ b/src/main/java/io/eiren/gui/TrackersList.java @@ -0,0 +1,137 @@ +package io.eiren.gui; + +import java.awt.GridBagConstraints; +import java.awt.event.MouseEvent; +import java.util.List; + +import javax.swing.BoxLayout; +import javax.swing.JButton; +import javax.swing.JLabel; +import javax.swing.event.MouseInputAdapter; + +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import io.eiren.util.StringUtils; +import io.eiren.util.ann.AWTThread; +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.CalibratingTracker; +import io.eiren.vr.trackers.IMUTracker; +import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerConfig; + +public class TrackersList extends EJBag { + + Quaternion q = new Quaternion(); + Vector3f v = new Vector3f(); + float[] angles = new float[3]; + + private List trackers = new FastList<>(); + + private final VRServer server; + private final VRServerGUI gui; + + public TrackersList(VRServer server, VRServerGUI gui) { + super(); + this.server = server; + this.gui = gui; + + setAlignmentY(TOP_ALIGNMENT); + add(new JLabel("Tracker"), c(0, 0, 2)); + add(new JLabel("Designation"), c(1, 0, 2)); + add(new JLabel("X"), c(2, 0, 2)); + add(new JLabel("Y"), c(3, 0, 2)); + add(new JLabel("Z"), c(4, 0, 2)); + add(new JLabel("Pitch"), c(5, 0, 2)); + add(new JLabel("Yaw"), c(6, 0, 2)); + add(new JLabel("Roll"), c(7, 0, 2)); + add(new JLabel("Status"), c(8, 0, 2)); + add(new JLabel("TPS"), c(9, 0, 2)); + + server.addNewTrackerConsumer(this::newTrackerAdded); + } + + @VRServerThread + public void updateTrackers() { + java.awt.EventQueue.invokeLater(() -> { + for(int i = 0; i < trackers.size(); ++i) + trackers.get(i).update(); + }); + } + + @ThreadSafe + public void newTrackerAdded(Tracker t) { + java.awt.EventQueue.invokeLater(() -> { + final int n = trackers.size(); + TrackerConfig cfg = server.getTrackerConfig(t); + + trackers.add(new TrackerRow(t, n + 1)); + if(cfg.designation != null) + add(new JLabel(cfg.designation), c(1, n + 1, 2)); + if(t instanceof CalibratingTracker) { + add(new JButton("Calibrate") {{ + addMouseListener(new MouseInputAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + new CalibrationWindow(t); + } + }); + }}, c(10, n + 1, 2)); + } + + gui.refresh(); + }); + } + + private class TrackerRow { + + Tracker t; + JLabel x; + JLabel y; + JLabel z; + JLabel a1; + JLabel a2; + JLabel a3; + JLabel status; + JLabel tps; + + @AWTThread + public TrackerRow(Tracker t, int n) { + this.t = t; + add(new JLabel(t.getName()), c(0, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(x = new JLabel("0"), c(2, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(y = new JLabel("0"), c(3, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(z = new JLabel("0"), c(4, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(a1 = new JLabel("0"), c(5, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(a2 = new JLabel("0"), c(6, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(a3 = new JLabel("0"), c(7, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(status = new JLabel(t.getStatus().toString()), c(8, n, 2, GridBagConstraints.FIRST_LINE_START)); + if(t instanceof IMUTracker) { + add(tps = new JLabel("0"), c(9, n, 2, GridBagConstraints.FIRST_LINE_START)); + } + } + + @AWTThread + public void update() { + t.getRotation(q); + t.getPosition(v); + q.toAngles(angles); + + x.setText(StringUtils.prettyNumber(v.x, 2)); + y.setText(StringUtils.prettyNumber(v.y, 2)); + z.setText(StringUtils.prettyNumber(v.z, 2)); + a1.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)); + a2.setText(StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)); + a3.setText(StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); + status.setText(t.getStatus().toString()); + + if(t instanceof IMUTracker) { + tps.setText(StringUtils.prettyNumber(((IMUTracker) t).getTPS(), 1)); + } + } + } +} diff --git a/src/main/java/io/eiren/gui/VRServerGUI.java b/src/main/java/io/eiren/gui/VRServerGUI.java new file mode 100644 index 000000000..3b4eb0876 --- /dev/null +++ b/src/main/java/io/eiren/gui/VRServerGUI.java @@ -0,0 +1,142 @@ +package io.eiren.gui; + +import javax.swing.*; +import javax.swing.border.EmptyBorder; +import javax.swing.event.MouseInputAdapter; + +import io.eiren.util.ann.AWTThread; +import io.eiren.vr.VRServer; + +import java.awt.Container; +import java.awt.event.MouseEvent; +import java.util.TimerTask; + +import static javax.swing.BoxLayout.PAGE_AXIS; +import static javax.swing.BoxLayout.LINE_AXIS; + +public class VRServerGUI extends JFrame { + + public final VRServer server; + private final TrackersList trackersList; + private final SkeletonList skeletonList; + private java.util.Timer timer = new java.util.Timer(); + private JButton resetButton; + + @AWTThread + public VRServerGUI(VRServer server) { + super("SlimeVR Server"); + increaseFontSize(); + + this.server = server; + setDefaultCloseOperation(EXIT_ON_CLOSE); + getContentPane().setLayout(new BoxLayout(getContentPane(), PAGE_AXIS)); + + this.trackersList = new TrackersList(server, this); + this.skeletonList = new SkeletonList(server, this); + + build(); + } + + public void refresh() { + // Pack and display + pack(); + setVisible(true); + java.awt.EventQueue.invokeLater(new Runnable() { + @Override + public void run() { + toFront(); + repaint(); + } + }); + } + + @AWTThread + private void build() { + Container pane = getContentPane(); + + pane.removeAll(); + + pane.add(new EJBox(LINE_AXIS) {{ + setBorder(new EmptyBorder(i(5))); + add(resetButton = new JButton("RESET") {{ + addMouseListener(new MouseInputAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + reset(); + } + }); + }}); + }}); + + pane.add(new EJBox(LINE_AXIS) {{ + setBorder(new EmptyBorder(i(5))); + add(new EJBox(PAGE_AXIS) {{ + setAlignmentY(TOP_ALIGNMENT); + + add(new JLabel("Trackers")); + add(trackersList); + }}); + + add(new EJBox(PAGE_AXIS) {{ + setAlignmentY(TOP_ALIGNMENT); + add(new JLabel("Sekelton")); + add(skeletonList); + }}); + }}); + + refresh(); + setLocationRelativeTo(null); + + server.addOnTick(trackersList::updateTrackers); + server.addOnTick(skeletonList::updateBones); + } + + private static void increaseFontSize() { + java.util.Enumeration keys = UIManager.getDefaults().keys(); + while(keys.hasMoreElements()) { + Object key = keys.nextElement(); + Object value = UIManager.get(key); + if(value instanceof javax.swing.plaf.FontUIResource) { + javax.swing.plaf.FontUIResource f = (javax.swing.plaf.FontUIResource) value; + javax.swing.plaf.FontUIResource f2 = new javax.swing.plaf.FontUIResource(f.deriveFont(f.getSize() * 2f)); + UIManager.put(key, f2); + } + } + } + + @AWTThread + private void reset() { + resetButton.setText("5"); + timer.schedule(new TimerTask() { + @Override + public void run() { + resetButton.setText("4"); + timer.schedule(new TimerTask() { + @Override + public void run() { + resetButton.setText("3"); + timer.schedule(new TimerTask() { + @Override + public void run() { + resetButton.setText("2"); + timer.schedule(new TimerTask() { + @Override + public void run() { + resetButton.setText("1"); + timer.schedule(new TimerTask() { + @Override + public void run() { + server.resetTrackers(); + resetButton.setText("RESET"); + } + }, 1000); + } + }, 1000); + } + }, 1000); + } + }, 1000); + } + }, 1000); + } +} diff --git a/src/main/java/io/eiren/vr/Main.java b/src/main/java/io/eiren/vr/Main.java new file mode 100644 index 000000000..94ac1075a --- /dev/null +++ b/src/main/java/io/eiren/vr/Main.java @@ -0,0 +1,28 @@ +package io.eiren.vr; + +import java.io.File; + +import io.eiren.gui.VRServerGUI; +import io.eiren.util.logging.LogManager; + + +public class Main { + + public static void main(String[] args) { + System.setProperty("awt.useSystemAAFontSettings", "on"); + System.setProperty("swing.aatext", "true"); + + File dir = new File("").getAbsoluteFile(); + try { + LogManager.initialize(new File(dir, "logs/"), dir); + } catch(Exception e1) { + e1.printStackTrace(); + } + + VRServer vrServer = new VRServer(); + vrServer.start(); + + new VRServerGUI(vrServer); + } + +} diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java index 6f3d0fccc..25bb73e3a 100644 --- a/src/main/java/io/eiren/vr/VRServer.java +++ b/src/main/java/io/eiren/vr/VRServer.java @@ -38,10 +38,12 @@ public class VRServer extends Thread { public final YamlFile config = new YamlFile(); public final HMDTracker hmdTracker; private final List> newTrackersConsumers = new FastList<>(); + private final List onTick = new FastList<>(); public VRServer() { super("VRServer"); hmdTracker = new HMDTracker("HMD"); + hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker); List shareTrackers = humanPoseProcessor.getComputedTrackers(); driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers); @@ -80,6 +82,10 @@ public class VRServer extends Thread { } } + public void addOnTick(Runnable runnable) { + this.onTick.add(runnable); + } + @ThreadSafe public void addNewTrackerConsumer(Consumer consumer) { queueTask(() -> { @@ -146,6 +152,10 @@ public class VRServer extends Thread { task.run(); } while(true); + for(int i = 0; i < onTick.size(); ++i) { + this.onTick.get(i).run(); + } + humanPoseProcessor.update(); final long time = System.currentTimeMillis() - start; diff --git a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java index 71f4e54b7..a4ff3ae4a 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java +++ b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java @@ -26,15 +26,15 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { /** * Distance between centers of both hips */ - protected float hipsWidth = 0.3f; + protected float hipsWidth = 0.22f; /** * Length from waist to knees */ - protected float hipLength = 0.86f; + protected float hipsLength = 0.46f; /** * Distance from waist to ankle */ - protected float ankleLength = 0.42f; + protected float ankleLength = 0.4f; public HumanSekeletonWithLegs(VRServer server, Map trackers, List computedTrackers) { super(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers); @@ -55,6 +55,9 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { computedRightAnkleTracker = rat; lat.setStatus(TrackerStatus.OK); rat.setStatus(TrackerStatus.OK); + hipsWidth = server.config.getFloat("body.hipsWidth", hipsWidth); + hipsLength = server.config.getFloat("body.hipsLength", hipsLength); + ankleLength = server.config.getFloat("body.ankleLength", ankleLength); waistNode.attachChild(leftHipNode); leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0); @@ -63,16 +66,45 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0); leftHipNode.attachChild(leftKneeNode); - leftKneeNode.localTransform.setTranslation(0, -hipLength, 0); + leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0); rightHipNode.attachChild(rightKneeNode); - rightKneeNode.localTransform.setTranslation(0, -hipLength, 0); + rightKneeNode.localTransform.setTranslation(0, -hipsLength, 0); leftKneeNode.attachChild(leftAnkleNode); leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0); rightKneeNode.attachChild(rightAnkleNode); rightAnkleNode.localTransform.setTranslation(0, -ankleLength, 0); + + jointsMap.put(HumanJoint.HIPS_WIDTH, hipsWidth); + jointsMap.put(HumanJoint.HIPS_LENGTH, hipsLength); + jointsMap.put(HumanJoint.LEGS_LENGTH, ankleLength); + } + + @Override + public void sentJointLength(HumanJoint joint, float newLength) { + super.sentJointLength(joint, newLength); + switch(joint) { + case HIPS_WIDTH: + hipsWidth = newLength; + server.config.setProperty("body.hipsWidth", hipsWidth); + leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0); + rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0); + break; + case HIPS_LENGTH: + hipsLength = newLength; + server.config.setProperty("body.hipsLength", hipsLength); + leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0); + rightKneeNode.localTransform.setTranslation(0, -hipsLength, 0); + break; + case LEGS_LENGTH: + ankleLength = newLength; + server.config.setProperty("body.ankleLength", ankleLength); + leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0); + rightAnkleNode.localTransform.setTranslation(0, -ankleLength, 0); + break; + } } @Override diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java index e35d0e300..c4fa78628 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java @@ -1,10 +1,13 @@ package io.eiren.vr.processor; +import java.util.HashMap; import java.util.List; +import java.util.Map; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import io.eiren.util.ann.VRServerThread; import io.eiren.vr.VRServer; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; @@ -12,6 +15,9 @@ import io.eiren.vr.trackers.TrackerStatus; public class HumanSkeleonWithWaist extends HumanSkeleton { + protected final Map jointsMap = new HashMap<>(); + protected final VRServer server; + protected final Quaternion qBuf = new Quaternion(); protected final Vector3f vBuf = new Vector3f(); @@ -27,13 +33,13 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { /** * Distance from eyes to waist */ - protected float waistDistance = 0.72f; + protected float waistDistance = 0.55f; /** * Distance from eyes to waist, defines reported * tracker position, if you want to move resulting * tracker up or down from actual waist */ - protected float trackerWaistDistance = 0.65f; + protected float trackerWaistDistance = 0.55f; /** * Distacne from eyes to the base of the neck */ @@ -41,11 +47,12 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { /** * Distance from eyes to ear */ - protected float headShift = 0.09f; + protected float headShift = 0.00f; public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List computedTrackers) { this.wasitTracker = waistTracker; this.hmdTracker = server.hmdTracker; + this.server = server; ComputedHumanPoseTracker cwt = null; for(int i = 0; i < computedTrackers.size(); ++i) { ComputedHumanPoseTracker t = computedTrackers.get(i); @@ -54,7 +61,10 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { } computedWaistTracker = cwt; cwt.setStatus(TrackerStatus.OK); + headShift = server.config.getFloat("body.headShift", headShift); + neckLength = server.config.getFloat("body.neckLength", neckLength); waistDistance = server.config.getFloat("body.waistDistance", waistDistance); + trackerWaistDistance = server.config.getFloat("body.trackerWaistDistance", trackerWaistDistance); // Build skeleton hmdNode.attachChild(headNode); headNode.localTransform.setTranslation(0, 0, headShift); @@ -63,10 +73,47 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { neckNode.localTransform.setTranslation(0, -neckLength, 0); neckNode.attachChild(waistNode); - waistNode.localTransform.setTranslation(0, -waistDistance + neckLength, 0); + waistNode.localTransform.setTranslation(0, -waistDistance, 0); neckNode.attachChild(trackerWaistNode); - trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance + neckLength, 0); + trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0); + + jointsMap.put(HumanJoint.HEAD, headShift); + jointsMap.put(HumanJoint.NECK, neckLength); + jointsMap.put(HumanJoint.WAIST, waistDistance); + jointsMap.put(HumanJoint.WASIT_VIRTUAL, trackerWaistDistance); + } + + @Override + public Map getJointsMap() { + return jointsMap; + } + + @Override + public void sentJointLength(HumanJoint joint, float newLength) { + jointsMap.put(joint, newLength); + switch(joint) { + case HEAD: + headShift = newLength; + server.config.setProperty("body.headShift", headShift); + headNode.localTransform.setTranslation(0, 0, headShift); + break; + case NECK: + neckLength = newLength; + server.config.setProperty("body.neckLength", neckLength); + neckNode.localTransform.setTranslation(0, -neckLength, 0); + break; + case WAIST: + waistDistance = newLength; + server.config.setProperty("body.waistDistance", waistDistance); + waistNode.localTransform.setTranslation(0, -waistDistance, 0); + break; + case WASIT_VIRTUAL: + trackerWaistDistance = newLength; + server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance); + trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0); + break; + } } @Override @@ -75,6 +122,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { } @Override + @VRServerThread public void updatePose() { updateLocalTransforms(); hmdNode.update(); @@ -91,8 +139,15 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { wasitTracker.getRotation(qBuf); neckNode.localTransform.setRotation(qBuf); - waistNode.localTransform.setRotation(qBuf); trackerWaistNode.localTransform.setRotation(qBuf); + + // Pelvic bone doesn't tilt when humans tilt, unless they really try. + // Can't calculate tilt without additional sensors, so just remove it + // completely. + qBuf.multLocal(vBuf); + vBuf.multLocal(1, 0, 1); // Keep only yaw / Don't normalize, it's done by lookAt() + qBuf.lookAt(vBuf, Vector3f.UNIT_Y); + waistNode.localTransform.setRotation(qBuf); } protected void updateComputedTrackers() { diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleton.java b/src/main/java/io/eiren/vr/processor/HumanSkeleton.java index 1b070dbc6..45bc4083d 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeleton.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeleton.java @@ -1,8 +1,41 @@ package io.eiren.vr.processor; +import java.util.Map; + +import io.eiren.util.ann.ThreadSafe; +import io.eiren.util.ann.VRServerThread; + public abstract class HumanSkeleton { - + + @VRServerThread public abstract void updatePose(); + @ThreadSafe public abstract TransformNode getRootNode(); + + @ThreadSafe + public abstract Map getJointsMap(); + + @ThreadSafe + public abstract void sentJointLength(HumanJoint joint, float newLength); + + public enum HumanJoint { + + HEAD("Head", ""), + NECK("Neck", ""), + WAIST("Waist", ""), + WASIT_VIRTUAL("Virtual waist", ""), + HIPS_WIDTH("Hips width", ""), + HIPS_LENGTH("Hips length", ""), + LEGS_LENGTH("Legs length", ""), + ; + + public final String name; + public final String description; + + private HumanJoint(String name, String description) { + this.name = name; + this.description = description; + } + } } diff --git a/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java index bd4eabf18..577645966 100644 --- a/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java @@ -1,5 +1,6 @@ package io.eiren.vr.trackers; +import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; @@ -7,6 +8,10 @@ public class AdjustedTracker implements Tracker { public final Tracker tracker; public final Quaternion adjustment = new Quaternion(); + private final Quaternion smoothedQuaternion = new Quaternion(); + private float[] angles = new float[3]; + private float[] lastAngles = new float[3]; + public float smooth = 2 * FastMath.DEG_TO_RAD; public AdjustedTracker(Tracker tracker) { this.tracker = tracker; @@ -26,6 +31,14 @@ public class AdjustedTracker implements Tracker { @Override public boolean getRotation(Quaternion store) { tracker.getRotation(store); + 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); + } + adjustment.mult(store, store); return true; } diff --git a/src/main/java/io/eiren/vr/trackers/CalibratingTracker.java b/src/main/java/io/eiren/vr/trackers/CalibratingTracker.java index ae080fde0..d7cc9f058 100644 --- a/src/main/java/io/eiren/vr/trackers/CalibratingTracker.java +++ b/src/main/java/io/eiren/vr/trackers/CalibratingTracker.java @@ -1,6 +1,12 @@ package io.eiren.vr.trackers; +import java.util.function.Consumer; + public interface CalibratingTracker { - public void startCalibration(); + public void startCalibration(Consumer calibrationDataConsumer); + + public void requestCalibrationData(Consumer calibrationDataConsumer); + + public void uploadNewClibrationData(); } diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java index a5b867711..ec8dab181 100644 --- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -1,5 +1,8 @@ package io.eiren.vr.trackers; +import java.nio.ByteBuffer; +import java.util.function.Consumer; + import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; @@ -17,6 +20,7 @@ public class IMUTracker implements Tracker, CalibratingTracker { protected final TrackersUDPServer server; protected BufferedTimer timer = new BufferedTimer(1f); + public CalibrationData newCalibrationData; public IMUTracker(String name, TrackersUDPServer server) { this.name = name; @@ -50,8 +54,8 @@ public class IMUTracker implements Tracker, CalibratingTracker { } @Override - public void startCalibration() { - server.sendCalibrationCommand(this); + public void startCalibration(Consumer calibrationDataConsumer) { + server.sendCalibrationCommand(this, calibrationDataConsumer); } public float getTPS() { @@ -61,4 +65,110 @@ public class IMUTracker implements Tracker, CalibratingTracker { public void dataTick() { timer.update(); } + + @Override + public void requestCalibrationData(Consumer calibrationDataConsumer) { + server.requestCalibrationData(this, calibrationDataConsumer); + } + + @Override + public void uploadNewClibrationData() { + server.uploadNewCalibrationData(this, newCalibrationData); + } + + public static class CalibrationData { + + //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]; + + public CalibrationData(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 CalibrationData(ByteBuffer buffer) { + // 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/TrackerConfig.java b/src/main/java/io/eiren/vr/trackers/TrackerConfig.java index 0283ddb78..6f3a96558 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackerConfig.java +++ b/src/main/java/io/eiren/vr/trackers/TrackerConfig.java @@ -10,6 +10,7 @@ public class TrackerConfig { public String designation; public boolean hide; public Quaternion adjustment; + public float trackerRotation = 0; public TrackerConfig(String trackerName) { this.trackerName = trackerName; @@ -19,6 +20,7 @@ public class TrackerConfig { this.trackerName = node.getString("name"); this.designation = node.getString("designation"); this.hide = node.getBoolean("hide", false); + this.trackerRotation = node.getFloat("rotation", trackerRotation); YamlNode adjNode = node.getNode("adjustment"); if(adjNode != null) { adjustment = new Quaternion(adjNode.getFloat("x", 0), adjNode.getFloat("y", 0), adjNode.getFloat("z", 0), adjNode.getFloat("w", 0)); @@ -47,5 +49,10 @@ public class TrackerConfig { } else { configNode.removeProperty("adj"); } + if(trackerRotation != 0.0f) { + configNode.setProperty("rotation", trackerRotation); + } else { + configNode.removeProperty("rotation"); + } } } diff --git a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java index da9797f61..db694e7c3 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java @@ -20,6 +20,7 @@ import com.jme3.math.Vector3f; import io.eiren.hardware.magentometer.Magneto; import io.eiren.util.Util; import io.eiren.util.collections.FastList; +import io.eiren.vr.trackers.IMUTracker.CalibrationData; /** * Recieves trackers data by UDP using extended owoTrack protocol. @@ -34,11 +35,14 @@ public class TrackersUDPServer extends Thread { private static final byte[] HANDSHAKE_BUFFER = new byte[64]; private static final byte[] KEEPUP_BUFFER = new byte[64]; private static final byte[] CALIBRATION_BUFFER = new byte[64]; + private static final byte[] CALIBRATION_REQUEST_BUFFER = new byte[64]; private final Quaternion buf = new Quaternion(); private final List trackers = new FastList<>(); private final Map trackersMap = new HashMap<>(); + private final Map> calibrationDataRequests = new HashMap<>(); + private final Map> newCalibrationDataRequests = new HashMap<>(); private final Consumer trackersConsumer; private final int port; @@ -51,7 +55,7 @@ public class TrackersUDPServer extends Thread { this.trackersConsumer = trackersConsumer; } - public void sendCalibrationCommand(Tracker tracker) { + public void sendCalibrationCommand(Tracker tracker, Consumer calibrationDataConsumer) { TrackerConnection connection = null; synchronized(trackers) { for(int i = 0; i < trackers.size(); ++i) { @@ -70,6 +74,8 @@ public class TrackersUDPServer extends Thread { connection.isCalibrating = true; connection.rawCalibrationData.clear(); } + if(calibrationDataConsumer != null) + newCalibrationDataRequests.put(tracker, calibrationDataConsumer); try { socket.send(new DatagramPacket(CALIBRATION_BUFFER, CALIBRATION_BUFFER.length, connection.address)); System.out.println("[TrackerServer] Calibrating sensor on " + connection.address); @@ -78,6 +84,48 @@ public class TrackersUDPServer extends Thread { } } + public void requestCalibrationData(Tracker tracker, Consumer consumer) { + TrackerConnection connection = null; + synchronized(trackers) { + for(int i = 0; i < trackers.size(); ++i) { + if(trackers.get(i).tracker == tracker) { + connection = trackers.get(i); + break; + } + } + } + if(connection == null) + return; + calibrationDataRequests.put(tracker, consumer); + try { + socket.send(new DatagramPacket(CALIBRATION_REQUEST_BUFFER, CALIBRATION_REQUEST_BUFFER.length, connection.address)); + System.out.println("[TrackerServer] Requesting config from " + connection.address); + } catch(IOException e) { + e.printStackTrace(); + } + } + + public void uploadNewCalibrationData(Tracker tracker, CalibrationData data) { + TrackerConnection connection = null; + synchronized(trackers) { + for(int i = 0; i < trackers.size(); ++i) { + if(trackers.get(i).tracker == tracker) { + connection = trackers.get(i); + break; + } + } + } + if(connection == null) + return; + // TODO + try { + socket.send(new DatagramPacket(CALIBRATION_REQUEST_BUFFER, CALIBRATION_REQUEST_BUFFER.length, connection.address)); + System.out.println("[TrackerServer] Requesting config from " + connection.address); + } catch(IOException e) { + e.printStackTrace(); + } + } + private void stopCalibration(TrackerConnection sensor) { synchronized(sensor) { if(!sensor.isCalibrating) @@ -117,8 +165,6 @@ public class TrackersUDPServer extends Thread { System.out.println("[TrackerServer] Magentometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size())); Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 2, 100, magBasis, magAInv); - System.out.println("float G_off[3] ="); - System.out.println(String.format(" {%8.2f, %8.2f, %8.2f}", gyroOffset[0], gyroOffset[1], gyroOffset[2])); System.out.println("float A_B[3] ="); System.out.println(String.format(" {%8.2f,%8.2f,%8.2f},", accelBasis[0], accelBasis[1], accelBasis[2])); System.out.println("float A_Ainv[3][3] ="); @@ -131,6 +177,16 @@ public class TrackersUDPServer extends Thread { System.out.println(String.format(" {{%9.5f,%9.5f,%9.5f},", magAInv[0], magAInv[1], magAInv[2])); System.out.println(String.format(" {%9.5f,%9.5f,%9.5f},", magAInv[3], magAInv[4], magAInv[5])); System.out.println(String.format(" {%9.5f,%9.5f,%9.5f}},", magAInv[6], magAInv[7], magAInv[8])); + System.out.println("float G_off[3] ="); + System.out.println(String.format(" {%8.2f, %8.2f, %8.2f}};", gyroOffset[0], gyroOffset[1], gyroOffset[2])); + + IMUTracker.CalibrationData data = new IMUTracker.CalibrationData(accelBasis, accelAInv, magBasis, magAInv, gyroOffset); + sensor.tracker.newCalibrationData = data; + + Consumer consumer = newCalibrationDataRequests.remove(sensor.tracker); + if(consumer != null) { + consumer.accept(data.toTextMatrix()); + } } private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { @@ -158,7 +214,7 @@ public class TrackersUDPServer extends Thread { @Override public void run() { - byte[] rcvBuffer = new byte[64]; + byte[] rcvBuffer = new byte[512]; ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN); try { socket = new DatagramSocket(port); @@ -220,6 +276,16 @@ public class TrackersUDPServer extends Thread { bb.getLong(); sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()}; break; + case 8: // PACKET_CONFIG + if(sensor == null) + break; + bb.getLong(); + IMUTracker.CalibrationData data = new IMUTracker.CalibrationData(bb); + Consumer dataConsumer = calibrationDataRequests.remove(sensor.tracker); + if(dataConsumer != null) { + dataConsumer.accept(data.toTextMatrix()); + } + break; default: System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); break; @@ -268,5 +334,7 @@ public class TrackersUDPServer extends Thread { KEEPUP_BUFFER[3] = 1; CALIBRATION_BUFFER[3] = 4; CALIBRATION_BUFFER[4] = 1; + CALIBRATION_REQUEST_BUFFER[3] = 4; + CALIBRATION_REQUEST_BUFFER[4] = 2; } }