Move rest of the classes to dev.slimev package, comment out wrong unit-test, minor cleanups

This commit is contained in:
Eiren Rain committed 2021-12-24 21:18:58 +02:00
1 parent b81458d034
commit 7b15d242f7
57 files changed
+159 -158

No files matched your search

+72
View File
@@ -0,0 +1,72 @@
package dev.slimevr;
import java.io.File;
import java.io.IOException;
import java.net.ServerSocket;
import javax.swing.JOptionPane;
import org.apache.commons.lang3.JavaVersion;
import org.apache.commons.lang3.SystemUtils;
import dev.slimevr.gui.Keybinding;
import dev.slimevr.gui.VRServerGUI;
import io.eiren.util.logging.LogManager;
public class Main {
public static String VERSION = "0.1.2";
public static VRServer vrServer;
@SuppressWarnings("unused")
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();
}
if (!SystemUtils.isJavaVersionAtLeast(JavaVersion.JAVA_11)) {
LogManager.log.severe("SlimeVR start-up error! A minimum of Java 11 is required.");
JOptionPane.showMessageDialog(null, "SlimeVR start-up error! A minimum of Java 11 is required.", "SlimeVR: Java Runtime Mismatch", JOptionPane.ERROR_MESSAGE);
return;
}
try {
new ServerSocket(6969).close();
new ServerSocket(35903).close();
new ServerSocket(21110).close();
} catch (IOException e) {
LogManager.log.severe("SlimeVR start-up error! Required ports are busy. Make sure there is no other instance of SlimeVR Server running.");
JOptionPane.showMessageDialog(null, "SlimeVR start-up error! Required ports are busy. Make sure there is no other instance of SlimeVR Server running.", "SlimeVR: Ports are busy", JOptionPane.ERROR_MESSAGE);
return;
}
try {
vrServer = new VRServer();
vrServer.start();
new Keybinding(vrServer);
new VRServerGUI(vrServer);
} catch(Throwable e) {
e.printStackTrace();
try {
Thread.sleep(2000L);
} catch(InterruptedException e2) {
e.printStackTrace();
}
System.exit(1); // Exit in case error happened on init and window not appeared, but some thread started
} finally {
try {
Thread.sleep(2000L);
} catch(InterruptedException e) {
e.printStackTrace();
}
}
}
}
+286
View File
@@ -0,0 +1,286 @@
package dev.slimevr;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.net.InetAddress;
import java.net.UnknownHostException;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Queue;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.function.Consumer;
import dev.slimevr.bridge.Bridge;
import dev.slimevr.bridge.NamedPipeBridge;
import dev.slimevr.bridge.SteamVRPipeInputBridge;
import dev.slimevr.bridge.VMCBridge;
import dev.slimevr.bridge.WebSocketVRBridge;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.processor.HumanPoseProcessor;
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
import dev.slimevr.vr.trackers.HMDTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerConfig;
import dev.slimevr.vr.trackers.TrackersUDPServer;
import io.eiren.util.OperatingSystem;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.ann.ThreadSecure;
import io.eiren.util.collections.FastList;
import io.eiren.yaml.YamlException;
import io.eiren.yaml.YamlFile;
import io.eiren.yaml.YamlNode;
public class VRServer extends Thread {
private final List<Tracker> trackers = new FastList<>();
public final HumanPoseProcessor humanPoseProcessor;
private final TrackersUDPServer trackersServer;
private final List<Bridge> bridges = new FastList<>();
private final Queue<Runnable> tasks = new LinkedBlockingQueue<>();
private final Map<String, TrackerConfig> configuration = new HashMap<>();
public final YamlFile config = new YamlFile();
public final HMDTracker hmdTracker;
private final List<Consumer<Tracker>> newTrackersConsumers = new FastList<>();
private final List<Runnable> onTick = new FastList<>();
private final List<? extends ShareableTracker> shareTrackers;
public VRServer() {
super("VRServer");
loadConfig();
hmdTracker = new HMDTracker("HMD");
hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging
// TODO Multiple processors
humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker);
shareTrackers = humanPoseProcessor.getComputedTrackers();
// Start server for SlimeVR trackers
trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker);
// OpenVR bridge currently only supports Windows
if(OperatingSystem.getCurrentPlatform() == OperatingSystem.WINDOWS) {
/*
// Create named pipe bridge for SteamVR driver
NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers, this);
tasks.add(() -> driverBridge.startBridge());
bridges.add(driverBridge);
//*/
// Create named pipe bridge for SteamVR input
SteamVRPipeInputBridge steamVRInput = new SteamVRPipeInputBridge(this);
tasks.add(() -> steamVRInput.startBridge());
bridges.add(steamVRInput);
//*/
NamedPipeBridge driverBridge = new NamedPipeBridge(hmdTracker, "steamvr", "SteamVR Driver Bridge", "\\\\.\\pipe\\SlimeVRDriver", shareTrackers);
tasks.add(() -> driverBridge.startBridge());
bridges.add(driverBridge);
}
// Create WebSocket server
WebSocketVRBridge wsBridge = new WebSocketVRBridge(hmdTracker, shareTrackers, this);
tasks.add(() -> wsBridge.startBridge());
bridges.add(wsBridge);
// Create VMCBridge
try {
VMCBridge vmcBridge = new VMCBridge(39539, 39540, InetAddress.getLocalHost());
tasks.add(() -> vmcBridge.startBridge());
bridges.add(vmcBridge);
} catch(UnknownHostException e) {
e.printStackTrace();
}
registerTracker(hmdTracker);
for(int i = 0; i < shareTrackers.size(); ++i)
registerTracker(shareTrackers.get(i));
}
public boolean hasBridge(Class<? extends Bridge> bridgeClass) {
for(int i = 0; i < bridges.size(); ++i) {
if(bridgeClass.isAssignableFrom(bridges.get(i).getClass()))
return true;
}
return false;
}
@ThreadSafe
public <E extends Bridge> E getVRBridge(Class<E> bridgeClass) {
for(int i = 0; i < bridges.size(); ++i) {
Bridge b = bridges.get(i);
if(bridgeClass.isAssignableFrom(b.getClass()))
return bridgeClass.cast(b);
}
return null;
}
@ThreadSafe
public TrackerConfig getTrackerConfig(Tracker tracker) {
synchronized(configuration) {
TrackerConfig config = configuration.get(tracker.getName());
if(config == null) {
config = new TrackerConfig(tracker);
configuration.put(tracker.getName(), config);
}
return config;
}
}
private void loadConfig() {
try {
config.load(new FileInputStream(new File("vrconfig.yml")));
} catch(FileNotFoundException e) {
// Config file didn't exist, is not an error
} catch(YamlException e) {
e.printStackTrace();
}
List<YamlNode> trackersConfig = config.getNodeList("trackers", null);
for(int i = 0; i < trackersConfig.size(); ++i) {
TrackerConfig cfg = new TrackerConfig(trackersConfig.get(i));
synchronized(configuration) {
configuration.put(cfg.trackerName, cfg);
}
}
}
public void addOnTick(Runnable runnable) {
this.onTick.add(runnable);
}
@ThreadSafe
public void addNewTrackerConsumer(Consumer<Tracker> consumer) {
queueTask(() -> {
newTrackersConsumers.add(consumer);
for(int i = 0; i < trackers.size(); ++i)
consumer.accept(trackers.get(i));
});
}
@ThreadSafe
public void trackerUpdated(Tracker tracker) {
queueTask(() -> {
humanPoseProcessor.trackerUpdated(tracker);
TrackerConfig tc = getTrackerConfig(tracker);
tracker.saveConfig(tc);
saveConfig();
});
}
@ThreadSafe
public void addSkeletonUpdatedCallback(Consumer<HumanSkeleton> consumer) {
queueTask(() -> {
humanPoseProcessor.addSkeletonUpdatedCallback(consumer);
});
}
@ThreadSafe
public synchronized void saveConfig() {
List<YamlNode> nodes = config.getNodeList("trackers", null);
List<Map<String, Object>> trackersConfig = new FastList<>(nodes.size());
for(int i = 0; i < nodes.size(); ++i) {
trackersConfig.add(nodes.get(i).root);
}
config.setProperty("trackers", trackersConfig);
synchronized(configuration) {
Iterator<TrackerConfig> iterator = configuration.values().iterator();
while(iterator.hasNext()) {
TrackerConfig tc = iterator.next();
Map<String, Object> cfg = null;
for(int i = 0; i < trackersConfig.size(); ++i) {
Map<String, Object> c = trackersConfig.get(i);
if(tc.trackerName.equals(c.get("name"))) {
cfg = c;
break;
}
}
if(cfg == null) {
cfg = new HashMap<>();
trackersConfig.add(cfg);
}
tc.saveConfig(new YamlNode(cfg));
}
}
File cfgFile = new File("vrconfig.yml");
try {
config.save(new FileOutputStream(cfgFile));
} catch(IOException e) {
e.printStackTrace();
}
}
@Override
@VRServerThread
public void run() {
trackersServer.start();
while(true) {
//final long start = System.currentTimeMillis();
do {
Runnable task = tasks.poll();
if(task == null)
break;
task.run();
} while(true);
for(int i = 0; i < onTick.size(); ++i) {
this.onTick.get(i).run();
}
for(int i = 0; i < bridges.size(); ++i)
bridges.get(i).dataRead();
for(int i = 0; i < trackers.size(); ++i)
trackers.get(i).tick();
humanPoseProcessor.update();
for(int i = 0; i < bridges.size(); ++i)
bridges.get(i).dataWrite();
//final long time = System.currentTimeMillis() - start;
try {
Thread.sleep(1); // 1000Hz
} catch(InterruptedException e) {
}
}
}
@ThreadSafe
public void queueTask(Runnable r) {
tasks.add(r);
}
@VRServerThread
private void trackerAdded(Tracker tracker) {
humanPoseProcessor.trackerAdded(tracker);
}
@ThreadSecure
public void registerTracker(Tracker tracker) {
TrackerConfig config = getTrackerConfig(tracker);
tracker.loadConfig(config);
queueTask(() -> {
trackers.add(tracker);
trackerAdded(tracker);
for(int i = 0; i < newTrackersConsumers.size(); ++i)
newTrackersConsumers.get(i).accept(tracker);
});
}
public void resetTrackers() {
queueTask(() -> {
humanPoseProcessor.resetTrackers();
});
}
public void resetTrackersYaw() {
queueTask(() -> {
humanPoseProcessor.resetTrackersYaw();
});
}
public int getTrackersCount() {
return trackers.size();
}
public List<Tracker> getAllTrackers() {
return new FastList<>(trackers);
}
}
@@ -9,6 +9,7 @@ import java.util.function.Consumer;
import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;
import dev.slimevr.VRServer;
import dev.slimevr.poserecorder.PoseFrameSkeleton;
import dev.slimevr.poserecorder.PoseFrameTracker;
import dev.slimevr.poserecorder.PoseFrames;
@@ -18,13 +19,12 @@ import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
import dev.slimevr.vr.processor.skeleton.SimpleSkeleton;
import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.TrackerUtils;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.logging.LogManager;
import io.eiren.util.collections.FastList;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.TrackerPosition;
import io.eiren.vr.trackers.TrackerRole;
import io.eiren.vr.trackers.TrackerUtils;
public class AutoBone {
+2 -2
View File
@@ -1,7 +1,7 @@
package dev.slimevr.bridge;
import io.eiren.util.ann.VRServerThread;
import io.eiren.vr.trackers.ShareableTracker;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.trackers.ShareableTracker;
/**
* Bridge handles sending and recieving tracker data
@@ -9,17 +9,17 @@ import com.sun.jna.platform.win32.WinBase;
import com.sun.jna.platform.win32.WinError;
import com.sun.jna.ptr.IntByReference;
import dev.slimevr.Main;
import dev.slimevr.bridge.Pipe.PipeState;
import dev.slimevr.bridge.ProtobufMessages.ProtobufMessage;
import dev.slimevr.bridge.ProtobufMessages.TrackerAdded;
import io.eiren.util.ann.VRServerThread;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.trackers.HMDTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.VRTracker;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.Main;
import io.eiren.vr.trackers.HMDTracker;
import io.eiren.vr.trackers.ShareableTracker;
import io.eiren.vr.trackers.TrackerPosition;
import io.eiren.vr.trackers.TrackerRole;
import io.eiren.vr.trackers.VRTracker;
public class NamedPipeBridge extends ProtobufBridge<VRTracker> implements Runnable {
@@ -12,15 +12,15 @@ import com.sun.jna.platform.win32.WinBase;
import com.sun.jna.platform.win32.WinNT.HANDLE;
import com.sun.jna.ptr.IntByReference;
import dev.slimevr.VRServer;
import dev.slimevr.bridge.Pipe.PipeState;
import dev.slimevr.vr.trackers.ComputedTracker;
import dev.slimevr.vr.trackers.HMDTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerStatus;
import io.eiren.util.collections.FastList;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.ComputedTracker;
import io.eiren.vr.trackers.HMDTracker;
import io.eiren.vr.trackers.ShareableTracker;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerStatus;
public class NamedPipeVRBridge extends Thread implements Bridge {
@@ -1,7 +1,7 @@
package dev.slimevr.bridge;
import io.eiren.util.ann.VRServerThread;
import io.eiren.vr.trackers.ShareableTracker;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.trackers.ShareableTracker;
public class OpenVRNativeBridge implements Bridge {
@@ -11,21 +11,21 @@ import java.util.concurrent.LinkedBlockingQueue;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.Main;
import dev.slimevr.bridge.ProtobufMessages.Position;
import dev.slimevr.bridge.ProtobufMessages.ProtobufMessage;
import dev.slimevr.bridge.ProtobufMessages.TrackerAdded;
import dev.slimevr.bridge.ProtobufMessages.TrackerStatus;
import dev.slimevr.bridge.ProtobufMessages.UserAction;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.trackers.ComputedTracker;
import dev.slimevr.vr.trackers.HMDTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.VRTracker;
import io.eiren.util.ann.Synchronize;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.ann.VRServerThread;
import io.eiren.util.collections.FastList;
import io.eiren.vr.Main;
import io.eiren.vr.trackers.ComputedTracker;
import io.eiren.vr.trackers.HMDTracker;
import io.eiren.vr.trackers.ShareableTracker;
import io.eiren.vr.trackers.TrackerRole;
import io.eiren.vr.trackers.VRTracker;
public abstract class ProtobufBridge<T extends VRTracker> implements Bridge {
@@ -189,7 +189,7 @@ public abstract class ProtobufBridge<T extends VRTracker> implements Bridge {
protected void trackerStatusRecieved(TrackerStatus trackerStatus) {
T tracker = getInternalRemoteTrackerById(trackerStatus.getTrackerId());
if(tracker != null) {
tracker.setStatus(io.eiren.vr.trackers.TrackerStatus.getById(trackerStatus.getStatusValue()));
tracker.setStatus(dev.slimevr.vr.trackers.TrackerStatus.getById(trackerStatus.getStatusValue()));
}
}
@@ -214,11 +214,11 @@ public abstract class ProtobufBridge<T extends VRTracker> implements Bridge {
synchronized(remoteTrackersByTrackerId) {
Iterator<Entry<Integer, T>> iterator = remoteTrackersByTrackerId.entrySet().iterator();
while(iterator.hasNext()) {
iterator.next().getValue().setStatus(io.eiren.vr.trackers.TrackerStatus.DISCONNECTED);
iterator.next().getValue().setStatus(dev.slimevr.vr.trackers.TrackerStatus.DISCONNECTED);
}
}
if(hmdTracker != null) {
hmd.setStatus(io.eiren.vr.trackers.TrackerStatus.DISCONNECTED);
hmd.setStatus(dev.slimevr.vr.trackers.TrackerStatus.DISCONNECTED);
}
}
@@ -16,14 +16,14 @@ import com.sun.jna.platform.win32.WinBase;
import com.sun.jna.platform.win32.WinError;
import com.sun.jna.ptr.IntByReference;
import dev.slimevr.VRServer;
import dev.slimevr.bridge.Pipe.PipeState;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerStatus;
import dev.slimevr.vr.trackers.VRTracker;
import io.eiren.util.collections.FastList;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.VRTracker;
import io.eiren.vr.trackers.ShareableTracker;
import io.eiren.vr.trackers.TrackerPosition;
import io.eiren.vr.trackers.TrackerStatus;
public class SteamVRPipeInputBridge extends Thread implements Bridge {
@@ -2,8 +2,8 @@ package dev.slimevr.bridge;
import java.net.InetAddress;
import io.eiren.vr.trackers.ShareableTracker;
import io.eiren.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.Tracker;
public class VMCBridge extends Thread implements Bridge {
@@ -17,15 +17,15 @@ import org.json.JSONObject;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.Main;
import dev.slimevr.VRServer;
import dev.slimevr.vr.trackers.ComputedTracker;
import dev.slimevr.vr.trackers.HMDTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerStatus;
import io.eiren.util.collections.FastList;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.Main;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.ComputedTracker;
import io.eiren.vr.trackers.HMDTracker;
import io.eiren.vr.trackers.ShareableTracker;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerStatus;
public class WebSocketVRBridge extends WebSocketServer implements Bridge {
@@ -10,19 +10,18 @@ import javax.swing.border.EmptyBorder;
import java.awt.event.MouseEvent;
import java.io.File;
import java.util.List;
import java.util.Map.Entry;
import java.util.concurrent.Future;
import io.eiren.util.StringUtils;
import io.eiren.util.ann.AWTThread;
import io.eiren.util.collections.FastList;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.VRServer;
import javax.swing.event.MouseInputAdapter;
import org.apache.commons.lang3.tuple.Pair;
import dev.slimevr.VRServer;
import dev.slimevr.autobone.AutoBone;
import dev.slimevr.gui.swing.EJBox;
import dev.slimevr.poserecorder.PoseFrames;
@@ -13,9 +13,9 @@ import javax.swing.border.EmptyBorder;
import javax.swing.event.MouseInputAdapter;
import dev.slimevr.gui.swing.EJBox;
import dev.slimevr.vr.trackers.CalibratingTracker;
import dev.slimevr.vr.trackers.Tracker;
import io.eiren.util.ann.AWTThread;
import io.eiren.vr.trackers.CalibratingTracker;
import io.eiren.vr.trackers.Tracker;
public class CalibrationWindow extends JFrame {
@@ -0,0 +1,66 @@
package dev.slimevr.gui;
import com.melloware.jintellitype.JIntellitype;
import com.melloware.jintellitype.JIntellitypeException;
import dev.slimevr.VRServer;
import com.melloware.jintellitype.HotkeyListener;
import io.eiren.util.ann.AWTThread;
import io.eiren.util.logging.LogManager;
public class Keybinding implements HotkeyListener {
public final VRServer server;
private static final int RESET = 1;
private static final int QUICK_RESET = 2;
@AWTThread
public Keybinding(VRServer server) {
this.server = server;
try {
if(JIntellitype.getInstance() instanceof JIntellitype) {
JIntellitype.getInstance().addHotKeyListener(this);
String resetBinding = this.server.config.getString("keybindings.reset");
if(resetBinding == null) {
resetBinding = "CTRL+ALT+SHIFT+Y";
this.server.config.setProperty("keybindings.reset", resetBinding);
}
JIntellitype.getInstance().registerHotKey(RESET, resetBinding);
LogManager.log.info("[Keybinding] Bound reset to " + resetBinding);
String quickResetBinding = this.server.config.getString("keybindings.quickReset");
if(quickResetBinding == null) {
quickResetBinding = "CTRL+ALT+SHIFT+U";
this.server.config.setProperty("keybindings.quickReset", quickResetBinding);
}
JIntellitype.getInstance().registerHotKey(QUICK_RESET, quickResetBinding);
LogManager.log.info("[Keybinding] Bound quick reset to " + quickResetBinding);
}
}
catch (JIntellitypeException je)
{
LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer.");
}
catch (ExceptionInInitializerError e)
{
LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer.");
}
}
@AWTThread
@Override
public void onHotKey(int identifier) {
switch(identifier) {
case RESET:
LogManager.log.info("[Keybinding] Reset pressed");
server.resetTrackers();
break;
case QUICK_RESET:
LogManager.log.info("[Keybinding] Quick reset pressed");
server.resetTrackersYaw();
break;
}
}
}
@@ -8,13 +8,13 @@ import javax.swing.JButton;
import javax.swing.JLabel;
import javax.swing.event.MouseInputAdapter;
import dev.slimevr.VRServer;
import dev.slimevr.gui.swing.ButtonTimer;
import dev.slimevr.gui.swing.EJBagNoStretch;
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
import io.eiren.util.StringUtils;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.vr.VRServer;
public class SkeletonConfigGUI extends EJBagNoStretch {
@@ -9,14 +9,14 @@ import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.VRServer;
import dev.slimevr.gui.swing.EJBagNoStretch;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.processor.TransformNode;
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
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.TransformNode;
public class SkeletonList extends EJBagNoStretch {
+11 -11
View File
@@ -17,23 +17,23 @@ import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.VRServer;
import dev.slimevr.gui.swing.EJBagNoStretch;
import dev.slimevr.gui.swing.EJBoxNoStretch;
import dev.slimevr.vr.trackers.ComputedTracker;
import dev.slimevr.vr.trackers.HMDTracker;
import dev.slimevr.vr.trackers.IMUTracker;
import dev.slimevr.vr.trackers.ReferenceAdjustedTracker;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerConfig;
import dev.slimevr.vr.trackers.TrackerMountingRotation;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerWithBattery;
import dev.slimevr.vr.trackers.TrackerWithTPS;
import io.eiren.util.StringUtils;
import io.eiren.util.ann.AWTThread;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.collections.FastList;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.ReferenceAdjustedTracker;
import io.eiren.vr.trackers.ComputedTracker;
import io.eiren.vr.trackers.HMDTracker;
import io.eiren.vr.trackers.IMUTracker;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerConfig;
import io.eiren.vr.trackers.TrackerMountingRotation;
import io.eiren.vr.trackers.TrackerPosition;
import io.eiren.vr.trackers.TrackerWithBattery;
import io.eiren.vr.trackers.TrackerWithTPS;
public class TrackersList extends EJBoxNoStretch {
@@ -5,18 +5,18 @@ import javax.swing.*;
import javax.swing.border.EmptyBorder;
import javax.swing.event.MouseInputAdapter;
import dev.slimevr.Main;
import dev.slimevr.VRServer;
import dev.slimevr.bridge.NamedPipeBridge;
import dev.slimevr.gui.swing.ButtonTimer;
import dev.slimevr.gui.swing.EJBagNoStretch;
import dev.slimevr.gui.swing.EJBox;
import dev.slimevr.gui.swing.EJBoxNoStretch;
import dev.slimevr.vr.trackers.TrackerRole;
import io.eiren.util.MacOSX;
import io.eiren.util.OperatingSystem;
import io.eiren.util.StringUtils;
import io.eiren.util.ann.AWTThread;
import io.eiren.vr.Main;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.TrackerRole;
import java.awt.Component;
import java.awt.Container;
@@ -0,0 +1,13 @@
package dev.slimevr.hardware.magentometer;
import com.sun.jna.Library;
import com.sun.jna.Native;
public interface Magneto extends Library {
Magneto INSTANCE = Native.load("MagnetoLib", Magneto.class);
void calculate(double[] data, int nlines, double nxsrej, double hm, double[] B, double[] A_1);
double calculateHnorm(double data[], int nlines);
}
@@ -11,9 +11,9 @@ import java.io.FileOutputStream;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.vr.trackers.TrackerPosition;
import io.eiren.util.collections.FastList;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.trackers.TrackerPosition;
public final class PoseFrameIO {
@@ -3,11 +3,11 @@ package dev.slimevr.poserecorder;
import java.util.List;
import java.util.Map;
import dev.slimevr.VRServer;
import dev.slimevr.vr.processor.ComputedHumanPoseTracker;
import dev.slimevr.vr.processor.skeleton.SimpleSkeleton;
import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
import io.eiren.vr.VRServer;
import io.eiren.vr.processor.ComputedHumanPoseTracker;
import io.eiren.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.Tracker;
public class PoseFrameSkeleton extends SimpleSkeleton {
@@ -5,11 +5,11 @@ import java.util.Iterator;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerConfig;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerStatus;
import io.eiren.util.collections.FastList;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerConfig;
import io.eiren.vr.trackers.TrackerPosition;
import io.eiren.vr.trackers.TrackerStatus;
public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
@@ -4,8 +4,8 @@ import java.util.Iterator;
import java.util.List;
import java.util.NoSuchElementException;
import dev.slimevr.vr.trackers.Tracker;
import io.eiren.util.collections.FastList;
import io.eiren.vr.trackers.Tracker;
public final class PoseFrames implements Iterable<TrackerFrame[]> {
@@ -7,11 +7,11 @@ import java.util.concurrent.Future;
import org.apache.commons.lang3.tuple.Pair;
import io.eiren.util.ann.VRServerThread;
import dev.slimevr.VRServer;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.trackers.Tracker;
import io.eiren.util.collections.FastList;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.Tracker;
public class PoseRecorder {
@@ -3,10 +3,10 @@ package dev.slimevr.poserecorder;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerConfig;
import io.eiren.vr.trackers.TrackerPosition;
import io.eiren.vr.trackers.TrackerStatus;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerConfig;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerStatus;
public final class TrackerFrame implements Tracker {
@@ -15,8 +15,8 @@ import com.jme3.math.Vector3f;
import org.apache.commons.lang3.StringUtils;
import dev.slimevr.vr.processor.TransformNode;
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
import io.eiren.vr.processor.TransformNode;
public class BVHFileStream extends PoseDataStream {
@@ -2,10 +2,10 @@ package dev.slimevr.posestreamer;
import java.io.IOException;
import dev.slimevr.VRServer;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
import io.eiren.util.ann.VRServerThread;
import io.eiren.util.logging.LogManager;
import io.eiren.vr.VRServer;
public class PoseStreamer {
@@ -4,7 +4,7 @@ import java.io.File;
import java.io.FileNotFoundException;
import java.io.OutputStream;
import io.eiren.vr.processor.TransformNode;
import dev.slimevr.vr.processor.TransformNode;
public class StdBVHFileStream extends BVHFileStream {
@@ -5,8 +5,8 @@ import java.util.List;
import com.jme3.math.Quaternion;
import com.jme3.math.Transform;
import dev.slimevr.vr.processor.TransformNode;
import io.eiren.util.collections.FastList;
import io.eiren.vr.processor.TransformNode;
public class TransformNodeWrapper {
@@ -0,0 +1,9 @@
package dev.slimevr.util.ann;
import java.lang.annotation.Retention;
import java.lang.annotation.RetentionPolicy;
@Retention(value = RetentionPolicy.SOURCE)
public @interface VRServerThread {
}
@@ -0,0 +1,35 @@
package dev.slimevr.vr.processor;
import dev.slimevr.vr.trackers.ComputedTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.TrackerWithTPS;
import io.eiren.util.BufferedTimer;
public class ComputedHumanPoseTracker extends ComputedTracker implements TrackerWithTPS, ShareableTracker {
public final ComputedHumanPoseTrackerPosition skeletonPosition;
protected final TrackerRole trackerRole;
protected BufferedTimer timer = new BufferedTimer(1f);
public ComputedHumanPoseTracker(int trackerId, ComputedHumanPoseTrackerPosition skeletonPosition, TrackerRole role) {
super(trackerId, "human://" + skeletonPosition.name(), true, true);
this.skeletonPosition = skeletonPosition;
this.trackerRole = role;
}
@Override
public float getTPS() {
return timer.getAverageFPS();
}
@Override
public void dataTick() {
timer.update();
}
@Override
public TrackerRole getTrackerRole() {
return trackerRole;
}
}
@@ -0,0 +1,11 @@
package dev.slimevr.vr.processor;
public enum ComputedHumanPoseTrackerPosition {
WAIST,
CHEST,
LEFT_FOOT,
RIGHT_FOOT,
LEFT_KNEE,
RIGHT_KNEE;
}
@@ -0,0 +1,126 @@
package dev.slimevr.vr.processor;
import java.util.List;
import java.util.function.Consumer;
import dev.slimevr.VRServer;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
import dev.slimevr.vr.processor.skeleton.SimpleSkeleton;
import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
import dev.slimevr.vr.trackers.HMDTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.TrackerStatus;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.collections.FastList;
public class HumanPoseProcessor {
private final VRServer server;
private final List<ComputedHumanPoseTracker> computedTrackers = new FastList<>();
private final List<Consumer<HumanSkeleton>> onSkeletonUpdated = new FastList<>();
private HumanSkeleton skeleton;
public HumanPoseProcessor(VRServer server, HMDTracker hmd) {
this.server = server;
computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.WAIST, TrackerRole.WAIST));
computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerRole.LEFT_FOOT));
computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerRole.RIGHT_FOOT));
computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.CHEST, TrackerRole.CHEST));
computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_KNEE, TrackerRole.LEFT_KNEE));
computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_KNEE, TrackerRole.RIGHT_KNEE));
}
public HumanSkeleton getSkeleton() {
return skeleton;
}
@VRServerThread
public void addSkeletonUpdatedCallback(Consumer<HumanSkeleton> consumer) {
onSkeletonUpdated.add(consumer);
if(skeleton != null)
consumer.accept(skeleton);
}
@ThreadSafe
public void setSkeletonConfig(SkeletonConfigValue key, float newLength) {
if(skeleton != null)
skeleton.getSkeletonConfig().setConfig(key, newLength);
}
@ThreadSafe
public void resetSkeletonConfig(SkeletonConfigValue key) {
if(skeleton != null)
skeleton.resetSkeletonConfig(key);
}
@ThreadSafe
public void resetAllSkeletonConfigs() {
if(skeleton != null)
skeleton.resetAllSkeletonConfigs();
}
@ThreadSafe
public SkeletonConfig getSkeletonConfig() {
return skeleton.getSkeletonConfig();
}
@ThreadSafe
public float getSkeletonConfig(SkeletonConfigValue key) {
if(skeleton != null) {
return skeleton.getSkeletonConfig().getConfig(key);
}
return 0.0f;
}
@ThreadSafe
public List<? extends ShareableTracker> getComputedTrackers() {
return computedTrackers;
}
@VRServerThread
public void trackerAdded(Tracker tracker) {
updateSekeltonModel();
}
@VRServerThread
public void trackerUpdated(Tracker tracker) {
updateSekeltonModel();
}
@VRServerThread
private void updateSekeltonModel() {
disconnectAllTrackers();
skeleton = new SimpleSkeleton(server, computedTrackers);
for(int i = 0; i < onSkeletonUpdated.size(); ++i)
onSkeletonUpdated.get(i).accept(skeleton);
}
@VRServerThread
private void disconnectAllTrackers() {
for(int i = 0; i < computedTrackers.size(); ++i) {
computedTrackers.get(i).setStatus(TrackerStatus.DISCONNECTED);
}
}
@VRServerThread
public void update() {
if(skeleton != null)
skeleton.updatePose();
}
@VRServerThread
public void resetTrackers() {
if(skeleton != null)
skeleton.resetTrackersFull();
}
@VRServerThread
public void resetTrackersYaw() {
if(skeleton != null)
skeleton.resetTrackersYaw();
}
}
@@ -0,0 +1,79 @@
package dev.slimevr.vr.processor;
import java.util.List;
import java.util.function.Consumer;
import com.jme3.math.Transform;
import io.eiren.util.collections.FastList;
public class TransformNode {
public final Transform localTransform = new Transform();
public final Transform worldTransform = new Transform();
public final List<TransformNode> children = new FastList<>();
public boolean localRotation = false;
protected TransformNode parent;
protected String name;
public TransformNode(String name, boolean localRotation) {
this.name = name;
this.localRotation = localRotation;
}
public TransformNode(String name) {
this(name, true);
}
public void attachChild(TransformNode node) {
if (node.parent != null) {
throw new IllegalArgumentException("The child node must not already have a parent");
}
this.children.add(node);
node.parent = this;
}
public TransformNode getParent() {
return parent;
}
public void update() {
updateWorldTransforms(); // Call update on each frame because we have relatively few nodes
for(int i = 0; i < children.size(); ++i)
children.get(i).update();
}
protected synchronized void updateWorldTransforms() {
if(parent == null) {
worldTransform.set(localTransform);
} else {
worldTransform.set(localTransform);
if(localRotation)
worldTransform.combineWithParent(parent.worldTransform);
else
combineWithParentGlobalRotation(parent.worldTransform);
}
}
public void depthFirstTraversal(Consumer<TransformNode> visitor) {
for(int i = 0; i < children.size(); ++i) {
children.get(i).depthFirstTraversal(visitor);
}
visitor.accept(this);
}
public String getName() {
return name;
}
public void combineWithParentGlobalRotation(Transform parent) {
worldTransform.getScale().multLocal(parent.getScale());
worldTransform.getTranslation().multLocal(parent.getScale());
parent
.getRotation()
.multLocal(worldTransform.getTranslation())
.addLocal(parent.getTranslation());
}
}
@@ -1,10 +1,8 @@
package dev.slimevr.vr.processor.skeleton;
import java.util.Map;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.processor.TransformNode;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.ann.VRServerThread;
import io.eiren.vr.processor.TransformNode;
public abstract class HumanSkeleton {
@@ -7,17 +7,17 @@ import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import io.eiren.util.ann.VRServerThread;
import dev.slimevr.VRServer;
import dev.slimevr.util.ann.VRServerThread;
import dev.slimevr.vr.processor.ComputedHumanPoseTracker;
import dev.slimevr.vr.processor.ComputedHumanPoseTrackerPosition;
import dev.slimevr.vr.processor.TransformNode;
import dev.slimevr.vr.trackers.Tracker;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.TrackerStatus;
import dev.slimevr.vr.trackers.TrackerUtils;
import io.eiren.util.collections.FastList;
import io.eiren.vr.VRServer;
import io.eiren.vr.processor.ComputedHumanPoseTracker;
import io.eiren.vr.processor.ComputedHumanPoseTrackerPosition;
import io.eiren.vr.processor.TransformNode;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerPosition;
import io.eiren.vr.trackers.TrackerRole;
import io.eiren.vr.trackers.TrackerStatus;
import io.eiren.vr.trackers.TrackerUtils;
public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallback {
@@ -0,0 +1,21 @@
package dev.slimevr.vr.trackers;
public class BnoTap {
public final boolean doubleTap;
public BnoTap(int tapBits) {
doubleTap = (tapBits & 0x40) > 0;
}
@Override
public String toString() {
return "Tap{" + (doubleTap ? "double" : "") + "}";
}
public enum TapAxis {
X,
Y,
Z;
}
}
@@ -0,0 +1,12 @@
package dev.slimevr.vr.trackers;
import java.util.function.Consumer;
public interface CalibratingTracker {
public void startCalibration(Consumer<String> calibrationDataConsumer);
public void requestCalibrationData(Consumer<String> calibrationDataConsumer);
public void uploadNewClibrationData();
}
@@ -0,0 +1,134 @@
package dev.slimevr.vr.trackers;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
public class ComputedTracker implements Tracker, TrackerWithTPS {
public final Vector3f position = new Vector3f();
public final Quaternion rotation = new Quaternion();
protected final String name;
protected final String serial;
protected TrackerStatus status = TrackerStatus.DISCONNECTED;
public TrackerPosition bodyPosition = null;
protected final boolean hasRotation;
protected final boolean hasPosition;
protected final int trackerId;
public ComputedTracker(int trackerId, String serial, String name, boolean hasRotation, boolean hasPosition) {
this.name = name;
this.serial = serial;
this.hasRotation = hasRotation;
this.hasPosition = hasPosition;
this.trackerId = trackerId;
}
public ComputedTracker(int trackerId, String name, boolean hasRotation, boolean hasPosition) {
this(trackerId, name, name, hasRotation, hasPosition);
}
@Override
public void saveConfig(TrackerConfig config) {
config.setDesignation(bodyPosition == null ? null : bodyPosition.designation);
}
@Override
public void loadConfig(TrackerConfig config) {
// Loading a config is an act of user editing, therefore it shouldn't not be allowed if editing is not allowed
if (userEditable()) {
bodyPosition = TrackerPosition.getByDesignation(config.designation);
}
}
@Override
public String getName() {
return this.serial;
}
@Override
public String getDescriptiveName() {
return this.name;
}
@Override
public boolean getPosition(Vector3f store) {
store.set(position);
return true;
}
@Override
public boolean getRotation(Quaternion store) {
store.set(rotation);
return true;
}
@Override
public TrackerStatus getStatus() {
return status;
}
public void setStatus(TrackerStatus status) {
this.status = status;
}
@Override
public float getConfidenceLevel() {
return 1.0f;
}
@Override
public void resetFull(Quaternion reference) {
}
@Override
public void resetYaw(Quaternion reference) {
}
@Override
public TrackerPosition getBodyPosition() {
return bodyPosition;
}
@Override
public void setBodyPosition(TrackerPosition position) {
this.bodyPosition = position;
}
@Override
public boolean userEditable() {
return false;
}
@Override
public void dataTick() {
}
@Override
public void tick() {
}
@Override
public boolean hasRotation() {
return hasRotation;
}
@Override
public boolean hasPosition() {
return hasPosition;
}
@Override
public boolean isComputed() {
return true;
}
@Override
public float getTPS() {
return -1;
}
@Override
public int getTrackerId() {
return this.trackerId;
}
}
@@ -0,0 +1,9 @@
package dev.slimevr.vr.trackers;
public enum DeviceType {
HMD,
CONTROLLER,
TRACKER,
TRACKING_REFERENCE,
;
}
@@ -0,0 +1,28 @@
package dev.slimevr.vr.trackers;
import io.eiren.util.BufferedTimer;
public class HMDTracker extends ComputedTracker implements TrackerWithTPS {
protected BufferedTimer timer = new BufferedTimer(1f);
public HMDTracker(String name) {
super(0, name, true, true);
setBodyPosition(TrackerPosition.HMD);
}
@Override
public float getTPS() {
return timer.getAverageFPS();
}
@Override
public void dataTick() {
timer.update();
}
@Override
public boolean isComputed() {
return false;
}
}
@@ -0,0 +1,259 @@
package dev.slimevr.vr.trackers;
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;
public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG;
public final Vector3f gyroVector = new Vector3f();
public final Vector3f accelVector = new Vector3f();
public final Vector3f magVector = new Vector3f();
public final Quaternion rotQuaternion = new Quaternion();
public final Quaternion rotMagQuaternion = new Quaternion();
protected final Quaternion rotAdjust = new Quaternion();
protected final Quaternion correction = new Quaternion();
protected TrackerMountingRotation mounting = null;
protected TrackerStatus status = TrackerStatus.OK;
protected final int trackerId;
protected final String name;
protected final String descriptiveName;
protected final TrackersUDPServer server;
protected float confidence = 0;
protected float batteryVoltage = 0;
public int calibrationStatus = 0;
public int magCalibrationStatus = 0;
public float magnetometerAccuracy = 0;
protected boolean magentometerCalibrated = false;
public boolean hasNewCorrectionData = false;
protected BufferedTimer timer = new BufferedTimer(1f);
public int ping = -1;
public StringBuilder serialBuffer = new StringBuilder();
long lastSerialUpdate = 0;
public TrackerPosition bodyPosition = null;
public IMUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) {
this.name = name;
this.server = server;
this.trackerId = trackerId;
this.descriptiveName = descriptiveName;
}
@Override
public void saveConfig(TrackerConfig config) {
config.setDesignation(bodyPosition == null ? null : bodyPosition.designation);
config.mountingRotation = mounting != null ? mounting.name() : null;
}
@Override
public void loadConfig(TrackerConfig config) {
// Loading a config is an act of user editing, therefore it shouldn't not be allowed if editing is not allowed
if (userEditable()) {
if(config.mountingRotation != null) {
mounting = TrackerMountingRotation.valueOf(config.mountingRotation);
if(mounting != null) {
rotAdjust.set(mounting.quaternion);
} else {
rotAdjust.loadIdentity();
}
} else {
rotAdjust.loadIdentity();
}
bodyPosition = TrackerPosition.getByDesignation(config.designation);
}
}
public TrackerMountingRotation getMountingRotation() {
return mounting;
}
public void setMountingRotation(TrackerMountingRotation mr) {
mounting = mr;
if(mounting != null) {
rotAdjust.set(mounting.quaternion);
} else {
rotAdjust.loadIdentity();
}
}
@Override
public void tick() {
if(magentometerCalibrated && hasNewCorrectionData) {
hasNewCorrectionData = false;
if(magnetometerAccuracy <= MAX_MAG_CORRECTION_ACCURACY) {
// Adjust gyro rotation to match magnetometer rotation only if magnetometer
// accuracy is within the parameters
calculateLiveMagnetometerCorrection();
}
}
}
@Override
public String getName() {
return this.name;
}
@Override
public boolean getPosition(Vector3f store) {
store.set(0, 0, 0);
return false;
}
@Override
public boolean getRotation(Quaternion store) {
store.set(rotQuaternion);
//correction.mult(store, store); // Correction is not used now to preven accidental errors while debugging other things
store.multLocal(rotAdjust);
return true;
}
public void getCorrection(Quaternion store) {
store.set(correction);
}
@Override
public TrackerStatus getStatus() {
return status;
}
public void setStatus(TrackerStatus status) {
this.status = status;
}
@Override
public float getTPS() {
return timer.getAverageFPS();
}
@Override
public void dataTick() {
timer.update();
}
@Override
public float getConfidenceLevel() {
return confidence;
}
public void setConfidence(float newConf) {
this.confidence = newConf;
}
@Override
public float getBatteryLevel() {
return FloatMath.mapValue(getBatteryVoltage(), 3.6f, 4.2f, 0f, 1f);
}
@Override
public float getBatteryVoltage() {
return batteryVoltage;
}
public void setBatteryVoltage(float voltage) {
this.batteryVoltage = voltage;
}
@Override
public void resetFull(Quaternion reference) {
resetYaw(reference);
}
/**
* Does not perform actual gyro reset to reference, that's the task of
* reference adjusted tracker. Only aligns gyro with magnetometer if
* it's reliable
*/
@Override
public void resetYaw(Quaternion reference) {
if(magCalibrationStatus >= CalibrationAccuracy.HIGH.status) {
magentometerCalibrated = true;
// During calibration set correction to match magnetometer readings exactly
// TODO : Correct only yaw
correction.set(rotQuaternion).inverseLocal().multLocal(rotMagQuaternion);
}
}
/**
* Calculate correction between normal and magnetometer
* readings up to accuracy threshold
*/
protected void calculateLiveMagnetometerCorrection() {
// TODO Magic, correct only yaw
// TODO Print "jump" length when correcing if it's more than 1 degree
}
@Override
public TrackerPosition getBodyPosition() {
return bodyPosition;
}
@Override
public void setBodyPosition(TrackerPosition position) {
this.bodyPosition = position;
}
@Override
public boolean userEditable() {
return true;
}
@Override
public boolean hasRotation() {
return true;
}
@Override
public boolean hasPosition() {
return false;
}
@Override
public boolean isComputed() {
return false;
}
@Override
public int getTrackerId() {
return this.trackerId;
}
@Override
public String getDescriptiveName() {
return this.descriptiveName;
}
public enum CalibrationAccuracy {
UNRELIABLE(0),
LOW(1),
MEDIUM(2),
HIGH(3),
;
private static final CalibrationAccuracy[] byStatus = new CalibrationAccuracy[4];
public final int status;
private CalibrationAccuracy(int status) {
this.status = status;
}
public static CalibrationAccuracy getByStatus(int status) {
if(status < 0 || status > 3)
return null;
return byStatus[status];
}
static {
for(CalibrationAccuracy ca : values())
byStatus[ca.status] = ca;
}
}
}
@@ -0,0 +1,112 @@
package dev.slimevr.vr.trackers;
import java.nio.ByteBuffer;
public class MPUTracker extends IMUTracker {
public ConfigurationData newCalibrationData;
public MPUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) {
super(trackerId, name, descriptiveName, 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();
}
}
}
@@ -0,0 +1,172 @@
package dev.slimevr.vr.trackers;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
public final E tracker;
public final Quaternion yawFix = new Quaternion();
public final Quaternion gyroFix = new Quaternion();
public final Quaternion attachmentFix = new Quaternion();
protected float confidenceMultiplier = 1.0f;
public ReferenceAdjustedTracker(E tracker) {
this.tracker = tracker;
}
public E getTracker() {
return this.tracker;
}
@Override
public boolean userEditable() {
return this.tracker.userEditable();
}
@Override
public void loadConfig(TrackerConfig config) {
this.tracker.loadConfig(config);
}
@Override
public void saveConfig(TrackerConfig config) {
this.tracker.saveConfig(config);
}
/**
* Reset the tracker so that it's current rotation
* is counted as (0, <HMD Yaw>, 0). This allows tracker
* to be strapped to body at any pitch and roll.
* <p>Performs {@link #resetYaw(Quaternion)} for yaw
* drift correction.
*/
@Override
public void resetFull(Quaternion reference) {
tracker.resetFull(reference);
fixGyroscope();
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
gyroFix.mult(sensorRotation, sensorRotation);
attachmentFix.set(sensorRotation).inverseLocal();
fixYaw(reference);
}
/**
* Reset the tracker so that it's current yaw rotation
* is counted as <HMD Yaw>. This allows the tracker
* to have yaw independant of the HMD. Tracker should
* still report yaw as if it was mounted facing HMD,
* mounting position should be corrected in the source.
*/
@Override
public void resetYaw(Quaternion reference) {
tracker.resetYaw(reference);
fixYaw(reference);
}
private void fixYaw(Quaternion reference) {
// Use only yaw HMD rotation
Quaternion targetTrackerRotation = new Quaternion(reference);
float[] angles = new float[3];
targetTrackerRotation.toAngles(angles);
targetTrackerRotation.fromAngles(0, angles[1], 0);
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
gyroFix.mult(sensorRotation, sensorRotation);
sensorRotation.multLocal(attachmentFix);
sensorRotation.toAngles(angles);
sensorRotation.fromAngles(0, angles[1], 0);
yawFix.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
}
private void fixGyroscope() {
float[] angles = new float[3];
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
sensorRotation.toAngles(angles);
sensorRotation.fromAngles(0, angles[1], 0);
gyroFix.set(sensorRotation).inverseLocal();
}
protected void adjustInternal(Quaternion store) {
gyroFix.mult(store, store);
store.multLocal(attachmentFix);
yawFix.mult(store, store);
}
@Override
public boolean getRotation(Quaternion store) {
tracker.getRotation(store);
adjustInternal(store);
return true;
}
@Override
public boolean getPosition(Vector3f store) {
return tracker.getPosition(store);
}
@Override
public String getName() {
return tracker.getName() + "/adj";
}
@Override
public TrackerStatus getStatus() {
return tracker.getStatus();
}
@Override
public float getConfidenceLevel() {
return tracker.getConfidenceLevel() * confidenceMultiplier;
}
@Override
public TrackerPosition getBodyPosition() {
return tracker.getBodyPosition();
}
@Override
public void setBodyPosition(TrackerPosition position) {
tracker.setBodyPosition(position);
}
@Override
public void tick() {
tracker.tick();
}
@Override
public boolean hasRotation() {
return tracker.hasRotation();
}
@Override
public boolean hasPosition() {
return tracker.hasPosition();
}
@Override
public boolean isComputed() {
return tracker.isComputed();
}
@Override
public int getTrackerId() {
return tracker.getTrackerId();
}
@Override
public String getDescriptiveName() {
return tracker.getDescriptiveName();
}
}
@@ -0,0 +1,6 @@
package dev.slimevr.vr.trackers;
public interface ShareableTracker extends Tracker {
public TrackerRole getTrackerRole();
}
@@ -0,0 +1,53 @@
package dev.slimevr.vr.trackers;
import java.util.concurrent.atomic.AtomicInteger;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
public interface Tracker {
public static final AtomicInteger nextLocalTrackerId = new AtomicInteger();
public boolean getPosition(Vector3f store);
public boolean getRotation(Quaternion store);
public String getName();
public TrackerStatus getStatus();
public void loadConfig(TrackerConfig config);
public void saveConfig(TrackerConfig config);
public float getConfidenceLevel();
public void resetFull(Quaternion reference);
public void resetYaw(Quaternion reference);
public void tick();
public TrackerPosition getBodyPosition();
public void setBodyPosition(TrackerPosition position);
public boolean userEditable();
public boolean hasRotation();
public boolean hasPosition();
public boolean isComputed();
public int getTrackerId();
public default String getDescriptiveName() {
return getName();
}
public static int getNextLocalTrackerId() {
return nextLocalTrackerId.incrementAndGet();
}
}
@@ -0,0 +1,67 @@
package dev.slimevr.vr.trackers;
import com.jme3.math.Quaternion;
import io.eiren.yaml.YamlNode;
public class TrackerConfig {
public final String trackerName;
public String designation;
public String description;
public boolean hide;
public Quaternion adjustment;
public String mountingRotation;
public TrackerConfig(Tracker tracker) {
this.trackerName = tracker.getName();
this.description = tracker.getDescriptiveName();
this.designation = tracker.getBodyPosition() != null ? tracker.getBodyPosition().designation : null;
}
public TrackerConfig(YamlNode node) {
this.trackerName = node.getString("name");
this.description = node.getString("description");
this.designation = node.getString("designation");
this.hide = node.getBoolean("hide", false);
this.mountingRotation = node.getString("rotation");
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));
}
}
public void setDesignation(String newDesignation) {
this.designation = newDesignation;
}
public void saveConfig(YamlNode configNode) {
configNode.setProperty("name", trackerName);
if(designation != null)
configNode.setProperty("designation", designation);
else
configNode.removeProperty("designation");
if(hide)
configNode.setProperty("hide", hide);
else
configNode.removeProperty("hide");
if(adjustment != null) {
configNode.setProperty("adj.x", adjustment.getX());
configNode.setProperty("adj.y", adjustment.getY());
configNode.setProperty("adj.z", adjustment.getZ());
configNode.setProperty("adj.w", adjustment.getW());
} else {
configNode.removeProperty("adj");
}
if(mountingRotation != null) {
configNode.setProperty("rotation", mountingRotation);
} else {
configNode.removeProperty("rotation");
}
if(description != null) {
configNode.setProperty("description", description);
} else {
configNode.removeProperty("description");
}
}
}
@@ -0,0 +1,22 @@
package dev.slimevr.vr.trackers;
import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
public enum TrackerMountingRotation {
FORWARD(180),
LEFT(90),
BACK(0),
RIGHT(-90);
public final float angle;
public final Quaternion quaternion = new Quaternion();
public static final TrackerMountingRotation[] values = values();
private TrackerMountingRotation(float angle) {
this.angle = angle;
quaternion.fromAngles(0, angle * FastMath.DEG_TO_RAD, 0);
}
}
@@ -0,0 +1,55 @@
package dev.slimevr.vr.trackers;
import java.util.EnumMap;
import java.util.HashMap;
import java.util.Map;
public enum TrackerPosition {
NONE("", TrackerRole.NONE),
HMD("HMD", TrackerRole.HMD),
CHEST("body:chest", TrackerRole.CHEST),
WAIST("body:waist", TrackerRole.WAIST),
HIP("body:hip", null),
LEFT_LEG("body:left_leg", TrackerRole.LEFT_KNEE),
RIGHT_LEG("body:right_leg", TrackerRole.RIGHT_KNEE),
LEFT_ANKLE("body:left_ankle", null),
RIGHT_ANKLE("body:right_ankle", null),
LEFT_FOOT("body:left_foot", TrackerRole.LEFT_FOOT),
RIGHT_FOOT("body:right_foot", TrackerRole.RIGHT_FOOT),
LEFT_CONTROLLER("body:left_controller", TrackerRole.LEFT_CONTROLLER),
RIGHT_CONTROLLER("body:right_conroller", TrackerRole.RIGHT_CONTROLLER),
;
public final String designation;
public final TrackerRole trackerRole;
public static final TrackerPosition[] values = values();
private static final Map<String, TrackerPosition> byDesignation = new HashMap<>();
private static final EnumMap<TrackerRole, TrackerPosition> byRole = new EnumMap<>(TrackerRole.class);
private TrackerPosition(String designation, TrackerRole trackerRole) {
this.designation = designation;
this.trackerRole = trackerRole;
}
public static TrackerPosition getByDesignation(String designation) {
return designation == null ? null : byDesignation.get(designation.toLowerCase());
}
public static TrackerPosition getByRole(TrackerRole role) {
return byRole.get(role);
}
static {
for(TrackerPosition tbp : values()) {
byDesignation.put(tbp.designation.toLowerCase(), tbp);
if(tbp.trackerRole != null) {
TrackerPosition old = byRole.get(tbp.trackerRole);
if(old != null)
throw new AssertionError("Only one tracker position can match tracker role. " + tbp.trackerRole + " is occupied by " + old + " when adding " + tbp);
byRole.put(tbp.trackerRole, tbp);
}
}
}
}
@@ -0,0 +1,55 @@
package dev.slimevr.vr.trackers;
public enum TrackerRole {
NONE(0, "", "", null),
WAIST(1, "vive_tracker_waist", "TrackerRole_Waist", DeviceType.TRACKER),
LEFT_FOOT(2, "vive_tracker_left_foot", "TrackerRole_LeftFoot", DeviceType.TRACKER),
RIGHT_FOOT(3, "vive_tracker_right_foot", "TrackerRole_RightFoot", DeviceType.TRACKER),
CHEST(4, "vive_tracker_chest", "TrackerRole_Chest", DeviceType.TRACKER),
LEFT_KNEE(5, "vive_tracker_left_knee", "TrackerRole_LeftKnee", DeviceType.TRACKER),
RIGHT_KNEE(6, "vive_tracker_right_knee", "TrackerRole_RightKnee", DeviceType.TRACKER),
LEFT_ELBOW(7, "vive_tracker_left_elbow", "TrackerRole_LeftElbow", DeviceType.TRACKER),
RIGHT_ELBOW(8, "vive_tracker_right_elbow", "TrackerRole_RightElbow", DeviceType.TRACKER),
LEFT_SHOULDER(9, "vive_tracker_left_shoulder", "TrackerRole_LeftShoulder", DeviceType.TRACKER),
RIGHT_SHOULDER(10, "vive_tracker_right_shoulder", "TrackerRole_RightShoulder", DeviceType.TRACKER),
LEFT_HAND(11, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.TRACKER),
RIGHT_HAND(12, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.TRACKER),
LEFT_CONTROLLER(13, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER),
RIGHT_CONTROLLER(14, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER),
HEAD(15, "", "", DeviceType.TRACKER),
NECK(16, "", "", DeviceType.TRACKER),
CAMERA(17, "vive_tracker_camera", "TrackerRole_Camera", DeviceType.TRACKER),
KEYBOARD(18, "vive_tracker_keyboard", "TrackerRole_Keyboard", DeviceType.TRACKER),
HMD(19, "", "", DeviceType.HMD),
BEACON(20, "", "", DeviceType.TRACKING_REFERENCE),
GENERIC_CONTROLLER(21, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER),
;
public final int id;
public final String roleHint;
public final String viveRole;
public final DeviceType deviceType;
public static final TrackerRole[] values = values();
private static final TrackerRole[] byId = new TrackerRole[22];
private TrackerRole(int id, String roleHint, String viveRole, DeviceType deviceType) {
this.id = id;
this.roleHint = roleHint;
this.viveRole = viveRole;
this.deviceType = deviceType;
}
public static TrackerRole getById(int id) {
return id < 0 || id >= byId.length ? null : byId[id];
}
static {
for(TrackerRole tr : values) {
if(byId[tr.id] != null)
throw new AssertionError("Tracker role id " + tr.id + " occupied by " + byId[tr.id] + " when adding " + tr);
byId[tr.id] = tr;
}
}
}
@@ -0,0 +1,32 @@
package dev.slimevr.vr.trackers;
public enum TrackerStatus {
DISCONNECTED(0, false),
OK(1, true),
BUSY(2, true),
ERROR(3, false),
OCCLUDED(4, false),
;
private static final TrackerStatus byId[] = new TrackerStatus[5];
public final int id;
public final boolean sendData;
private TrackerStatus(int id, boolean sendData) {
this.sendData = sendData;
this.id = id;
}
public static TrackerStatus getById(int id) {
if(id < 0 || id >= byId.length)
return null;
return byId[id];
}
static {
for(TrackerStatus st : values())
byId[st.id] = st;
}
}
@@ -0,0 +1,71 @@
package dev.slimevr.vr.trackers;
import java.util.List;
public class TrackerUtils {
private TrackerUtils() {
}
public static <T extends Tracker> T findTrackerForBodyPosition(T[] allTrackers, TrackerPosition position) {
if(position == null)
return null;
for(int i = 0; i < allTrackers.length; ++i) {
T t = allTrackers[i];
if(t != null && t.getBodyPosition() == position)
return t;
}
return null;
}
public static <T extends Tracker> T findTrackerForBodyPosition(List<T> allTrackers, TrackerPosition position) {
if(position == null)
return null;
for(int i = 0; i < allTrackers.size(); ++i) {
T t = allTrackers.get(i);
if(t != null && t.getBodyPosition() == position)
return t;
}
return null;
}
public static <T extends Tracker> T findTrackerForBodyPosition(List<T> allTrackers, TrackerPosition position, TrackerPosition altPosition) {
T t = findTrackerForBodyPosition(allTrackers, position);
if(t != null)
return t;
return findTrackerForBodyPosition(allTrackers, altPosition);
}
public static <T extends Tracker> T findTrackerForBodyPosition(T[] allTrackers, TrackerPosition position, TrackerPosition altPosition, TrackerPosition secondAltPosition) {
T t = findTrackerForBodyPosition(allTrackers, position);
if(t != null)
return t;
t = findTrackerForBodyPosition(allTrackers, altPosition);
if(t != null)
return t;
return findTrackerForBodyPosition(allTrackers, secondAltPosition);
}
public static Tracker findTrackerForBodyPositionOrEmpty(List<? extends Tracker> allTrackers, TrackerPosition position, TrackerPosition altPosition, TrackerPosition secondAltPosition) {
Tracker t = findTrackerForBodyPosition(allTrackers, position);
if(t != null)
return t;
t = findTrackerForBodyPosition(allTrackers, altPosition);
if(t != null)
return t;
t = findTrackerForBodyPosition(allTrackers, secondAltPosition);
if(t != null)
return t;
return new ComputedTracker(Tracker.getNextLocalTrackerId(), "Empty tracker", false, false);
}
public static Tracker findTrackerForBodyPositionOrEmpty(Tracker[] allTrackers, TrackerPosition position, TrackerPosition altPosition) {
Tracker t = findTrackerForBodyPosition(allTrackers, position);
if(t != null)
return t;
t = findTrackerForBodyPosition(allTrackers, altPosition);
if(t != null)
return t;
return new ComputedTracker(Tracker.getNextLocalTrackerId(), "Empty tracker", false, false);
}
}
@@ -0,0 +1,8 @@
package dev.slimevr.vr.trackers;
public interface TrackerWithBattery {
public float getBatteryLevel();
public float getBatteryVoltage();
}
@@ -0,0 +1,8 @@
package dev.slimevr.vr.trackers;
public interface TrackerWithTPS {
public float getTPS();
public void dataTick();
}
@@ -0,0 +1,464 @@
package dev.slimevr.vr.trackers;
import java.io.IOException;
import java.io.UnsupportedEncodingException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.Inet6Address;
import java.net.InetAddress;
import java.net.InetSocketAddress;
import java.net.NetworkInterface;
import java.net.SocketAddress;
import java.net.SocketTimeoutException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.Enumeration;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Random;
import java.util.function.Consumer;
import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import io.eiren.util.Util;
import io.eiren.util.collections.FastList;
/**
* Recieves trackers data by UDP using extended owoTrack protocol.
*/
public class TrackersUDPServer extends Thread {
/**
* Change between IMU axises and OpenGL/SteamVR axises
*/
private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X);
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 Random random = new Random();
private final List<TrackerConnection> trackers = new FastList<>();
private final Map<InetAddress, TrackerConnection> trackersMap = new HashMap<>();
private final Map<Tracker, Consumer<String>> calibrationDataRequests = new HashMap<>();
private final Consumer<Tracker> trackersConsumer;
private final int port;
protected DatagramSocket socket = null;
protected long lastKeepup = System.currentTimeMillis();
public TrackersUDPServer(int port, String name, Consumer<Tracker> trackersConsumer) {
super(name);
this.port = port;
this.trackersConsumer = trackersConsumer;
}
private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException {
System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort());
InetAddress addr = handshakePacket.getAddress();
TrackerConnection sensor;
synchronized(trackers) {
sensor = trackersMap.get(addr);
}
if(sensor == null) {
boolean isOwo = false;
data.getLong(); // Skip packet number
int boardType = -1;
int imuType = -1;
int firmwareBuild = -1;
StringBuilder firmware = new StringBuilder();
byte[] mac = new byte[6];
String macString = null;
if(data.remaining() > 0) {
if(data.remaining() > 3)
boardType = data.getInt();
if(data.remaining() > 3)
imuType = data.getInt();
if(data.remaining() > 3)
data.getInt(); // MCU TYPE
if(data.remaining() > 11) {
data.getInt(); // IMU info
data.getInt();
data.getInt();
}
if(data.remaining() > 3)
firmwareBuild = data.getInt();
int length = 0;
if(data.remaining() > 0)
length = data.get() & 0xFF; // firmware version length is 1 longer than that because it's nul-terminated
while(length > 0 && data.remaining() != 0) {
char c = (char) data.get();
if(c == 0)
break;
firmware.append(c);
length--;
}
if(data.remaining() > mac.length) {
data.get(mac);
macString = String.format("%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
if(macString.equals("00:00:00:00:00:00"))
macString = null;
}
}
if(firmware.length() == 0) {
firmware.append("owoTrack");
isOwo = true;
}
String trackerName = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString();
String descriptiveName = "udp:/" + handshakePacket.getAddress().toString();
IMUTracker imu = new IMUTracker(Tracker.getNextLocalTrackerId(), trackerName, descriptiveName, this);
ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
trackersConsumer.accept(adjustedTracker);
sensor = new TrackerConnection(imu, handshakePacket.getSocketAddress());
sensor.isOwoTrack = isOwo;
int i = 0;
synchronized(trackers) {
i = trackers.size();
trackers.add(sensor);
trackersMap.put(addr, sensor);
}
System.out.println("[TrackerServer] Sensor " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + trackerName);
}
sensor.sensors.get(0).setStatus(TrackerStatus.OK);
socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
}
private void setUpAuxilarySensor(TrackerConnection connection, int trackerId) throws IOException {
System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.sensors.get(0).getName());
IMUTracker imu = connection.sensors.get(trackerId);
if(imu == null) {
imu = new IMUTracker(Tracker.getNextLocalTrackerId(), connection.sensors.get(0).getName() + "/" + trackerId, connection.sensors.get(0).getDescriptiveName() + "/" + trackerId, this);
connection.sensors.put(trackerId, imu);
ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
trackersConsumer.accept(adjustedTracker);
System.out.println("[TrackerServer] Sensor added with address " + imu.getName());
}
imu.setStatus(TrackerStatus.OK);
}
@Override
public void run() {
byte[] rcvBuffer = new byte[512];
ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN);
StringBuilder serialBuffer2 = new StringBuilder();
try {
socket = new DatagramSocket(port);
// Why not just 255.255.255.255? Because Windows.
// https://social.technet.microsoft.com/Forums/windows/en-US/72e7387a-9f2c-4bf4-a004-c89ddde1c8aa/how-to-fix-the-global-broadcast-address-255255255255-behavior-on-windows
ArrayList<SocketAddress> addresses = new ArrayList<SocketAddress>();
Enumeration<NetworkInterface> ifaces = NetworkInterface.getNetworkInterfaces();
while (ifaces.hasMoreElements()) {
NetworkInterface iface = ifaces.nextElement();
// Ignore loopback, PPP, virtual and disabled devices
if (iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) {
continue;
}
Enumeration<InetAddress> iaddrs = iface.getInetAddresses();
while (iaddrs.hasMoreElements()) {
InetAddress iaddr = iaddrs.nextElement();
// Ignore IPv6 addresses
if (iaddr instanceof Inet6Address) {
continue;
}
String[] iaddrParts = iaddr.getHostAddress().split("\\.");
addresses.add(new InetSocketAddress(String.format("%s.%s.%s.255", iaddrParts[0], iaddrParts[1], iaddrParts[2]), port));
}
}
byte[] dummyPacket = new byte[] {0x0};
long prevPacketTime = System.currentTimeMillis();
socket.setSoTimeout(250);
while(true) {
try {
boolean hasActiveTrackers = false;
for (TrackerConnection tracker: trackers) {
if (tracker.sensors.get(0).getStatus() == TrackerStatus.OK) {
hasActiveTrackers = true;
break;
}
}
if (!hasActiveTrackers) {
long discoveryPacketTime = System.currentTimeMillis();
if ((discoveryPacketTime - prevPacketTime) >= 2000) {
for (SocketAddress addr: addresses) {
socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr));
}
prevPacketTime = discoveryPacketTime;
}
}
DatagramPacket recieve = new DatagramPacket(rcvBuffer, rcvBuffer.length);
socket.receive(recieve);
bb.rewind();
TrackerConnection connection;
IMUTracker tracker = null;
synchronized(trackers) {
connection = trackersMap.get(recieve.getAddress());
}
if(connection != null)
connection.lastPacket = System.currentTimeMillis();
int packetId;
switch(packetId = bb.getInt()) {
case 0:
break;
case 3:
setUpNewSensor(recieve, bb);
break;
case 1: // PACKET_ROTATION
case 16: // PACKET_ROTATION_2
if(connection == null)
break;
bb.getLong();
buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
offset.mult(buf, buf);
if(packetId == 1) {
tracker = connection.sensors.get(0);
} else {
tracker = connection.sensors.get(1);
}
if(tracker == null)
break;
tracker.rotQuaternion.set(buf);
tracker.dataTick();
break;
case 17: // PACKET_ROTATION_DATA
if(connection == null)
break;
if(connection.isOwoTrack)
break;
bb.getLong();
int sensorId = bb.get() & 0xFF;
tracker = connection.sensors.get(sensorId);
if(tracker == null)
break;
int dataType = bb.get() & 0xFF;
buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
offset.mult(buf, buf);
int calibrationInfo = bb.get() & 0xFF;
switch(dataType) {
case 1: // DATA_TYPE_NORMAL
tracker.rotQuaternion.set(buf);
tracker.calibrationStatus = calibrationInfo;
tracker.dataTick();
break;
case 2: // DATA_TYPE_CORRECTION
tracker.rotMagQuaternion.set(buf);
tracker.magCalibrationStatus = calibrationInfo;
tracker.hasNewCorrectionData = true;
break;
}
break;
case 18: // PACKET_MAGENTOMETER_ACCURACY
if(connection == null)
break;
if(connection.isOwoTrack)
break;
bb.getLong();
sensorId = bb.get() & 0xFF;
tracker = connection.sensors.get(sensorId);
if(tracker == null)
break;
float accuracyInfo = bb.getFloat();
tracker.magnetometerAccuracy = accuracyInfo;
break;
case 2: // PACKET_GYRO
case 4: // PACKET_ACCEL
case 5: // PACKET_MAG
case 9: // PACKET_RAW_MAGENTOMETER
break; // None of these packets are used by SlimeVR trackers and are deprecated, use more generic PACKET_ROTATION_DATA
case 8: // PACKET_CONFIG
if(connection == null)
break;
if(connection.isOwoTrack)
break;
bb.getLong();
MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb);
Consumer<String> dataConsumer = calibrationDataRequests.remove(connection.sensors.get(0));
if(dataConsumer != null) {
dataConsumer.accept(data.toTextMatrix());
}
break;
case 10: // PACKET_PING_PONG:
if(connection == null)
break;
if(connection.isOwoTrack)
break;
int pingId = bb.getInt();
if(connection.lastPingPacketId == pingId) {
for(int i = 0; i < connection.sensors.size(); ++i) {
tracker = connection.sensors.get(i);
tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2;
tracker.dataTick();
}
}
break;
case 11: // PACKET_SERIAL
if(connection == null)
break;
if(connection.isOwoTrack)
break;
tracker = connection.sensors.get(0);
bb.getLong();
int length = bb.getInt();
for(int i = 0; i < length; ++i) {
char ch = (char) bb.get();
if(ch == '\n') {
serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer);
System.out.println(serialBuffer2.toString());
serialBuffer2.setLength(0);
tracker.serialBuffer.setLength(0);
} else {
tracker.serialBuffer.append(ch);
}
}
break;
case 12: // PACKET_BATTERY_VOLTAGE
if(connection == null)
break;
tracker = connection.sensors.get(0);
bb.getLong();
tracker.setBatteryVoltage(bb.getFloat());
break;
case 13: // PACKET_TAP
if(connection == null)
break;
if(connection.isOwoTrack)
break;
bb.getLong();
sensorId = bb.get() & 0xFF;
tracker = connection.sensors.get(sensorId);
if(tracker == null)
break;
int tap = bb.get() & 0xFF;
BnoTap tapObj = new BnoTap(tap);
System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj + " (b" + Integer.toBinaryString(tap) + ")");
break;
case 14: // PACKET_RESET_REASON
bb.getLong();
byte reason = bb.get();
System.out.println("[TrackerServer] Reset recieved from " + recieve.getSocketAddress() + ": " + reason);
if(connection == null)
break;
sensorId = bb.get() & 0xFF;
tracker = connection.sensors.get(sensorId);
if(tracker == null)
break;
tracker.setStatus(TrackerStatus.ERROR);
break;
case 15: // PACKET_SENSOR_INFO
if(connection == null)
break;
bb.getLong();
sensorId = bb.get() & 0xFF;
int sensorStatus = bb.get() & 0xFF;
if(sensorId > 0 && sensorStatus == 1) {
setUpAuxilarySensor(connection, sensorId);
}
bb.rewind();
bb.putInt(15);
bb.put((byte) sensorId);
bb.put((byte) sensorStatus);
socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
System.out.println("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus);
break;
default:
System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
break;
}
} catch(SocketTimeoutException e) {
} catch(Exception e) {
e.printStackTrace();
}
if(lastKeepup + 500 < System.currentTimeMillis()) {
lastKeepup = System.currentTimeMillis();
synchronized(trackers) {
for(int i = 0; i < trackers.size(); ++i) {
TrackerConnection conn = trackers.get(i);
socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, conn.address));
if(conn.lastPacket + 1000 < System.currentTimeMillis()) {
Iterator<IMUTracker> iterator = conn.sensors.values().iterator();
while(iterator.hasNext()) {
IMUTracker tracker = iterator.next();
if(tracker.getStatus() == TrackerStatus.OK)
tracker.setStatus(TrackerStatus.DISCONNECTED);
}
} else {
Iterator<IMUTracker> iterator = conn.sensors.values().iterator();
while(iterator.hasNext()) {
IMUTracker tracker = iterator.next();
if(tracker.getStatus() == TrackerStatus.DISCONNECTED)
tracker.setStatus(TrackerStatus.OK);
}
}
IMUTracker tracker = conn.sensors.get(0);
if(tracker == null)
continue;
if(tracker.serialBuffer.length() > 0) {
if(tracker.lastSerialUpdate + 500L < System.currentTimeMillis()) {
serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer);
System.out.println(serialBuffer2.toString());
serialBuffer2.setLength(0);
tracker.serialBuffer.setLength(0);
}
}
if(conn.lastPingPacketTime + 500 < System.currentTimeMillis()) {
conn.lastPingPacketId = random.nextInt();
conn.lastPingPacketTime = System.currentTimeMillis();
bb.rewind();
bb.putInt(10);
bb.putInt(conn.lastPingPacketId);
socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address));
}
}
}
}
}
} catch(Exception e) {
e.printStackTrace();
} finally {
Util.close(socket);
}
}
private class TrackerConnection {
Map<Integer, IMUTracker> sensors = new HashMap<>();
SocketAddress address;
public long lastPacket = System.currentTimeMillis();
public int lastPingPacketId = -1;
public long lastPingPacketTime = 0;
public boolean isOwoTrack = false;
public TrackerConnection(IMUTracker tracker, SocketAddress address) {
this.sensors.put(0, tracker);
this.address = address;
}
}
static {
try {
HANDSHAKE_BUFFER[0] = 3;
byte[] str = "Hey OVR =D 5".getBytes("ASCII");
System.arraycopy(str, 0, HANDSHAKE_BUFFER, 1, str.length);
} catch(UnsupportedEncodingException e) {
throw new AssertionError(e);
}
KEEPUP_BUFFER[3] = 1;
CALIBRATION_BUFFER[3] = 4;
CALIBRATION_BUFFER[4] = 1;
CALIBRATION_REQUEST_BUFFER[3] = 4;
CALIBRATION_REQUEST_BUFFER[4] = 2;
}
}
@@ -0,0 +1,36 @@
package dev.slimevr.vr.trackers;
import io.eiren.util.BufferedTimer;
public class VRTracker extends ComputedTracker {
protected BufferedTimer timer = new BufferedTimer(1f);
public VRTracker(int id, String serial, String name, boolean hasRotation, boolean hasPosition) {
super(id, serial, name, hasRotation, hasPosition);
}
public VRTracker(int id, String name, boolean hasRotation, boolean hasPosition) {
super(id, name, name, hasRotation, hasPosition);
}
@Override
public float getTPS() {
return timer.getAverageFPS();
}
@Override
public void dataTick() {
timer.update();
}
@Override
public boolean userEditable() {
return true;
}
@Override
public boolean isComputed() {
return false;
}
}