diff --git a/server/src/main/java/com/jme3/math/FastMath.java b/server/src/main/java/com/jme3/math/FastMath.java index ed0917d14..89be15977 100644 --- a/server/src/main/java/com/jme3/math/FastMath.java +++ b/server/src/main/java/com/jme3/math/FastMath.java @@ -70,9 +70,59 @@ final public class FastMath { public static final float DEG_TO_RAD = PI / 180.0f; /** A value to multiply a radian value by, to convert it to degrees. */ public static final float RAD_TO_DEG = 180.0f / PI; - /** A precreated random object for random numbers. */ + /** A premade random object for random numbers. */ public static final Random rand = new Random(System.currentTimeMillis()); + /** + * Returns true if the number is within the specified {@code tolerance} + * value. + * + * @param value The number to check. + * @param tolerance The tolerance to zero (must be positive). + * @return True if the number is within the specified {@code tolerance} + * value. + */ + public static boolean isApproxZero(float value, float tolerance) { + return value < tolerance && value > tolerance; + } + + /** + * Returns true if the number is within the {@link #ZERO_TOLERANCE} value. + * + * @param value The number to check. + * @return True if the number is within the {@link #ZERO_TOLERANCE} value. + */ + public static boolean isApproxZero(float value) { + return isApproxZero(value, ZERO_TOLERANCE); + } + + /** + * Returns true if the two numbers are equal within the specified + * {@code tolerance} value. + * + * @param valueOne The first number to check. + * @param valueTwo The second number to check. + * @param tolerance The tolerance to zero (must be positive). + * @return True if the numbers are approximately equal within the specified + * {@code tolerance} value. + */ + public static boolean isApproxEqual(float valueOne, float valueTwo, float tolerance) { + return isApproxZero(valueTwo - valueOne, tolerance); + } + + /** + * Returns true if the two numbers are equal within the + * {@link #ZERO_TOLERANCE} value. + * + * @param valueOne The first number to check. + * @param valueTwo The second number to check. + * @return True if the numbers are approximately equal within the + * {@link #ZERO_TOLERANCE} value. + */ + public static boolean isApproxEqual(float valueOne, float valueTwo) { + return isApproxEqual(valueOne, valueTwo, ZERO_TOLERANCE); + } + /** * Returns true if the number is a power of 2 (2,4,8,16...) * @@ -99,7 +149,7 @@ final public class FastMath { * * @param scale scale value to use. if 1, use endValue, if 0, use * startValue. - * @param startValue Begining value. 0% of f + * @param startValue Beginning value. 0% of f * @param endValue ending value. 100% of f * @return The interpolated value between startValue and endValue. */ @@ -122,7 +172,7 @@ final public class FastMath { * * @param scale scale value to use. if 1, use endValue, if 0, use * startValue. - * @param startValue Begining value. 0% of f + * @param startValue Beginning value. 0% of f * @param endValue ending value. 100% of f * @param store a vector3f to store the result * @return The interpolated value between startValue and endValue. @@ -148,7 +198,7 @@ final public class FastMath { * * @param scale scale value to use. if 1, use endValue, if 0, use * startValue. - * @param startValue Begining value. 0% of f + * @param startValue Beginning value. 0% of f * @param endValue ending value. 100% of f * @return The interpolated value between startValue and endValue. */ @@ -392,7 +442,7 @@ final public class FastMath { } /** - * Compute the lenght on a catmull rom spline between control point 1 and 2 + * Compute the length on a catmull rom spline between control point 1 and 2 * * @param p0 control point 0 * @param p1 control point 1 @@ -437,7 +487,7 @@ final public class FastMath { } /** - * Compute the lenght on a bezier spline between control point 1 and 2 + * Compute the length on a bezier spline between control point 1 and 2 * * @param p0 control point 0 * @param p1 control point 1 @@ -657,7 +707,7 @@ final public class FastMath { /** * Returns the value squared. fValue ^ 2 * - * @param fValue The vaule to square. + * @param fValue The value to square. * @return The square of the given value. */ public static float sqr(float fValue) { diff --git a/server/src/main/java/com/jme3/math/Quaternion.java b/server/src/main/java/com/jme3/math/Quaternion.java index 460095856..d83718be5 100644 --- a/server/src/main/java/com/jme3/math/Quaternion.java +++ b/server/src/main/java/com/jme3/math/Quaternion.java @@ -279,7 +279,7 @@ public final class Quaternion implements Cloneable, java.io.Serializable { /** * toAngles returns this quaternion converted to Euler rotation - * angles (yaw,roll,pitch).
+ * angles (pitch, yaw, roll).
* Note that the result is not always 100% accurate due to the implications * of euler angles. * @@ -288,7 +288,7 @@ public final class Quaternion implements Cloneable, java.io.Serializable { * * @param angles the float[] in which the angles should be stored, or null * if you want a new float[] to be created - * @return the float[] in which the angles are stored. + * @return the float[] in which the angles are stored (pitch, yaw, roll). */ public float[] toAngles(float[] angles) { if (angles == null) { diff --git a/server/src/main/java/dev/slimevr/VRServer.java b/server/src/main/java/dev/slimevr/VRServer.java index 2cb8f80c5..b3a1b9f75 100644 --- a/server/src/main/java/dev/slimevr/VRServer.java +++ b/server/src/main/java/dev/slimevr/VRServer.java @@ -5,6 +5,7 @@ import dev.slimevr.autobone.AutoBoneHandler; import dev.slimevr.bridge.Bridge; import dev.slimevr.bridge.VMCBridge; import dev.slimevr.config.ConfigManager; +import dev.slimevr.osc.VRCOSCHandler; import dev.slimevr.platform.windows.WindowsNamedPipeBridge; import dev.slimevr.poserecorder.BVHRecorder; import dev.slimevr.protocol.ProtocolAPI; @@ -28,6 +29,8 @@ import java.net.InetAddress; import java.net.UnknownHostException; import java.util.List; import java.util.Queue; +import java.util.Timer; +import java.util.TimerTask; import java.util.concurrent.LinkedBlockingQueue; import java.util.function.Consumer; @@ -43,12 +46,14 @@ public class VRServer extends Thread { private final List> newTrackersConsumers = new FastList<>(); private final List onTick = new FastList<>(); private final List shareTrackers; + private final VRCOSCHandler VRCOSCHandler; private final DeviceManager deviceManager; private final BVHRecorder bvhRecorder; private final SerialHandler serialHandler; private final AutoBoneHandler autoBoneHandler; private final ProtocolAPI protocolAPI; private final ConfigManager configManager; + private final Timer timer = new Timer(); private final NanoTimer fpsTimer = new NanoTimer(); /** @@ -74,17 +79,18 @@ public class VRServer extends Thread { hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier // debugging // TODO Multiple processors - humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker); + humanPoseProcessor = new HumanPoseProcessor(this); shareTrackers = humanPoseProcessor.getComputedTrackers(); // Start server for SlimeVR trackers trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker); // OpenVR bridge currently only supports Windows + WindowsNamedPipeBridge driverBridge = null; if (OperatingSystem.getCurrentPlatform() == OperatingSystem.WINDOWS) { // Create named pipe bridge for SteamVR driver - WindowsNamedPipeBridge driverBridge = new WindowsNamedPipeBridge( + driverBridge = new WindowsNamedPipeBridge( this, hmdTracker, "steamvr", @@ -123,6 +129,15 @@ public class VRServer extends Thread { e.printStackTrace(); } + // Initialize OSC + VRCOSCHandler = new VRCOSCHandler( + hmdTracker, + humanPoseProcessor, + driverBridge, + getConfigManager().getVrConfig().getVrcOSC(), + shareTrackers + ); + bvhRecorder = new BVHRecorder(this); registerTracker(hmdTracker); @@ -184,8 +199,8 @@ public class VRServer extends Thread { public void run() { trackersServer.start(); while (true) { - fpsTimer.update(); // final long start = System.currentTimeMillis(); + fpsTimer.update(); do { Runnable task = tasks.poll(); if (task == null) @@ -205,6 +220,7 @@ public class VRServer extends Thread { for (Bridge bridge : bridges) { bridge.dataWrite(); } + VRCOSCHandler.update(); // final long time = System.currentTimeMillis() - start; try { Thread.sleep(1); // 1000Hz @@ -242,6 +258,43 @@ public class VRServer extends Thread { queueTask(humanPoseProcessor::resetTrackersYaw); } + public void resetTrackersMounting() { + queueTask(humanPoseProcessor::resetTrackersMounting); + } + + public void scheduleResetTrackers(long delay) { + TimerTask resetTask = new resetTask(); + timer.schedule(resetTask, delay); + } + + public void scheduleResetTrackersYaw(long delay) { + TimerTask yawResetTask = new yawResetTask(); + timer.schedule(yawResetTask, delay); + } + + public void scheduleResetTrackersMounting(long delay) { + TimerTask resetMountingTask = new resetMountingTask(); + timer.schedule(resetMountingTask, delay); + } + + class resetTask extends TimerTask { + public void run() { + queueTask(humanPoseProcessor::resetTrackers); + } + } + + class yawResetTask extends TimerTask { + public void run() { + queueTask(humanPoseProcessor::resetTrackersYaw); + } + } + + class resetMountingTask extends TimerTask { + public void run() { + queueTask(humanPoseProcessor::resetTrackersMounting); + } + } + public void setLegTweaksEnabled(boolean value) { queueTask(() -> humanPoseProcessor.setLegTweaksEnabled(value)); } @@ -306,6 +359,10 @@ public class VRServer extends Thread { return trackersServer; } + public VRCOSCHandler getVRCOSCHandler() { + return VRCOSCHandler; + } + public DeviceManager getDeviceManager() { return deviceManager; } diff --git a/server/src/main/java/dev/slimevr/config/KeybindingsConfig.java b/server/src/main/java/dev/slimevr/config/KeybindingsConfig.java index eb3f40482..095b90f5c 100644 --- a/server/src/main/java/dev/slimevr/config/KeybindingsConfig.java +++ b/server/src/main/java/dev/slimevr/config/KeybindingsConfig.java @@ -6,6 +6,15 @@ public class KeybindingsConfig { private String quickResetBinding = "CTRL+ALT+SHIFT+U"; + private String resetMountingBinding = "CTRL+ALT+SHIFT+I"; + + private long resetDelay = 0L; + + private long quickResetDelay = 0L; + + private long resetMountingDelay = 0L; + + public KeybindingsConfig() { } @@ -16,4 +25,32 @@ public class KeybindingsConfig { public String getQuickResetBinding() { return quickResetBinding; } + + public String getResetMountingBinding() { + return resetMountingBinding; + } + + public long getResetDelay() { + return resetDelay; + } + + public void setResetDelay(long delay) { + resetDelay = delay; + } + + public long getQuickResetDelay() { + return quickResetDelay; + } + + public void setQuickResetDelay(long delay) { + quickResetDelay = delay; + } + + public long getResetMountingDelay() { + return resetMountingDelay; + } + + public void setResetMountingDelay(long delay) { + resetMountingDelay = delay; + } } diff --git a/server/src/main/java/dev/slimevr/config/OSCConfig.java b/server/src/main/java/dev/slimevr/config/OSCConfig.java new file mode 100644 index 000000000..2e5247fc2 --- /dev/null +++ b/server/src/main/java/dev/slimevr/config/OSCConfig.java @@ -0,0 +1,91 @@ +package dev.slimevr.config; + + +import com.fasterxml.jackson.databind.annotation.JsonDeserialize; +import com.fasterxml.jackson.databind.annotation.JsonSerialize; +import com.fasterxml.jackson.databind.ser.std.StdKeySerializers; +import dev.slimevr.config.serializers.BooleanMapDeserializer; +import dev.slimevr.vr.trackers.TrackerRole; + +import java.net.InetAddress; +import java.net.UnknownHostException; +import java.util.HashMap; +import java.util.Map; + + +public class OSCConfig { + + // Is OSC enabled for the app + private boolean enabled = false; + + // Port to receive OSC messages from + private int portIn = 9001; + + // Port to send out OSC messages at + private int portOut = 9000; + + // Address to send out OSC messages at + private String address = "127.0.0.1"; + + // Which trackers' data to send + @JsonDeserialize(using = BooleanMapDeserializer.class) + @JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer.class) + public Map trackers = new HashMap<>(); + private final TrackerRole[] defaultRoles = new TrackerRole[] { TrackerRole.WAIST, + TrackerRole.LEFT_FOOT, TrackerRole.RIGHT_FOOT }; + + public OSCConfig() { + // Initialize default tracker role settings + for (TrackerRole role : defaultRoles) { + setOSCTrackerRole( + role, + getOSCTrackerRole(role, true) + ); + } + + } + + public boolean getEnabled() { + return enabled; + } + + public void setEnabled(boolean value) { + enabled = value; + } + + public int getPortIn() { + return portIn; + } + + public void setPortIn(int portIn) { + this.portIn = portIn; + } + + public int getPortOut() { + return portOut; + } + + public void setPortOut(int portOut) { + this.portOut = portOut; + } + + public InetAddress getAddress() { + try { + return InetAddress.getByName(address); + } catch (UnknownHostException e) { + throw new RuntimeException(e); + } + } + + public void setAddress(String address) { + this.address = address; + } + + public boolean getOSCTrackerRole(TrackerRole role, boolean def) { + return trackers.getOrDefault(role.name().toLowerCase(), def); + } + + public void setOSCTrackerRole(TrackerRole role, boolean val) { + this.trackers.put(role.name().toLowerCase(), val); + } +} diff --git a/server/src/main/java/dev/slimevr/config/VRConfig.java b/server/src/main/java/dev/slimevr/config/VRConfig.java index 7f28fb1d3..92f57918c 100644 --- a/server/src/main/java/dev/slimevr/config/VRConfig.java +++ b/server/src/main/java/dev/slimevr/config/VRConfig.java @@ -17,25 +17,27 @@ import java.util.Map; ) public class VRConfig { - private WindowConfig window = new WindowConfig(); + private final WindowConfig window = new WindowConfig(); - private FiltersConfig filters = new FiltersConfig(); + private final FiltersConfig filters = new FiltersConfig(); - private AutoBoneConfig autobone = new AutoBoneConfig(); + private final OSCConfig vrcOSC = new OSCConfig(); - private KeybindingsConfig keybindings = new KeybindingsConfig(); + private final AutoBoneConfig autobone = new AutoBoneConfig(); - private SkeletonConfig skeleton = new SkeletonConfig(); + private final KeybindingsConfig keybindings = new KeybindingsConfig(); + + private final SkeletonConfig skeleton = new SkeletonConfig(); @JsonDeserialize(using = TrackerConfigMapDeserializer.class) @JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer.class) - private Map trackers = new HashMap<>(); + private final Map trackers = new HashMap<>(); @JsonDeserialize(using = BridgeConfigMapDeserializer.class) @JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer.class) - private Map bridges = new HashMap<>(); + private final Map bridges = new HashMap<>(); - private OverlayConfig overlay = new OverlayConfig(); + private final OverlayConfig overlay = new OverlayConfig(); public WindowConfig getWindow() { return window; @@ -45,6 +47,10 @@ public class VRConfig { return filters; } + public OSCConfig getVrcOSC() { + return vrcOSC; + } + public AutoBoneConfig getAutoBone() { return autobone; } diff --git a/server/src/main/java/dev/slimevr/gui/Keybinding.java b/server/src/main/java/dev/slimevr/gui/Keybinding.java index 534d7ce51..85d0364b3 100644 --- a/server/src/main/java/dev/slimevr/gui/Keybinding.java +++ b/server/src/main/java/dev/slimevr/gui/Keybinding.java @@ -12,9 +12,8 @@ import io.eiren.util.logging.LogManager; public class Keybinding implements HotkeyListener { private static final int RESET = 1; private static final int QUICK_RESET = 2; + private static final int RESET_MOUNTING = 3; public final VRServer server; - - public final KeybindingsConfig config; @AWTThread @@ -42,10 +41,14 @@ public class Keybinding implements HotkeyListener { String quickResetBinding = this.config.getQuickResetBinding(); JIntellitype.getInstance().registerHotKey(QUICK_RESET, quickResetBinding); LogManager.info("[Keybinding] Bound quick reset to " + quickResetBinding); + + String resetMountingBinding = this.config.getResetMountingBinding(); + JIntellitype.getInstance().registerHotKey(RESET_MOUNTING, resetMountingBinding); + LogManager.info("[Keybinding] Bound reset mounting to " + resetMountingBinding); } } catch (Throwable e) { LogManager - .info( + .warning( "[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer." ); } @@ -57,11 +60,15 @@ public class Keybinding implements HotkeyListener { switch (identifier) { case RESET -> { LogManager.info("[Keybinding] Reset pressed"); - server.resetTrackers(); + server.scheduleResetTrackers(this.config.getResetDelay()); } case QUICK_RESET -> { LogManager.info("[Keybinding] Quick reset pressed"); - server.resetTrackersYaw(); + server.scheduleResetTrackersYaw(this.config.getQuickResetDelay()); + } + case RESET_MOUNTING -> { + LogManager.info("[Keybinding] Reset mounting pressed"); + server.scheduleResetTrackersMounting(this.config.getResetMountingDelay()); } } } diff --git a/server/src/main/java/dev/slimevr/gui/TrackersList.java b/server/src/main/java/dev/slimevr/gui/TrackersList.java index 1e0dfd70a..6f84e7470 100644 --- a/server/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/server/src/main/java/dev/slimevr/gui/TrackersList.java @@ -142,6 +142,7 @@ public class TrackersList extends EJBoxNoStretch { JLabel adj; JLabel adjYaw; JLabel adjGyro; + JLabel adjMounting; JLabel correction; JLabel signalStrength; JLabel rotQuat; @@ -345,10 +346,15 @@ public class TrackersList extends EJBoxNoStretch { adjGyro = new JLabel("0 0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START) ); - add(new JLabel("Temp:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel("Mount Fix:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add( + adjMounting = new JLabel("0 0 0 0"), + c(3, row, 2, GridBagConstraints.FIRST_LINE_START) + ); + add(new JLabel("Temp:"), c(4, row, 2, GridBagConstraints.FIRST_LINE_START)); add( temperature = new JLabel("?"), - c(3, row, 2, GridBagConstraints.FIRST_LINE_START) + c(5, row, 2, GridBagConstraints.FIRST_LINE_START) ); } @@ -446,6 +452,17 @@ public class TrackersList extends EJBoxNoStretch { + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0) ); } + if (adjMounting != null) { + rat.mountRotFix.toAngles(angles); + adjMounting + .setText( + StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0) + + " " + + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0) + + " " + + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0) + ); + } } if (tracker instanceof TrackerWithWireless) { diff --git a/server/src/main/java/dev/slimevr/gui/VRServerGUI.java b/server/src/main/java/dev/slimevr/gui/VRServerGUI.java index ebb852eb0..ca51565ce 100644 --- a/server/src/main/java/dev/slimevr/gui/VRServerGUI.java +++ b/server/src/main/java/dev/slimevr/gui/VRServerGUI.java @@ -43,6 +43,7 @@ public class VRServerGUI extends JFrame { private final SkeletonList skeletonList; private final EJBox pane; private JButton resetButton; + private JButton resetMountingButton; private JButton floorClipButton; private JButton skatingCorrectionButton; @@ -199,6 +200,17 @@ public class VRServerGUI extends JFrame { } }); add(Box.createHorizontalStrut(10)); + add(resetMountingButton = new JButton("Reset Mounting") { + { + addMouseListener(new MouseInputAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + resetMounting(); + } + }); + } + }); + add(Box.createHorizontalStrut(10)); add(new JButton("Fast Reset") { { addMouseListener(new MouseInputAdapter() { @@ -544,6 +556,12 @@ public class VRServerGUI extends JFrame { ButtonTimer.runTimer(resetButton, 3, "RESET", server::resetTrackers); } + @AWTThread + private void resetMounting() { + ButtonTimer + .runTimer(resetMountingButton, 3, "Reset Mounting", server::resetTrackersMounting); + } + @AWTThread private void setSkatingReductionEnabled(Boolean value) { if (value == null) diff --git a/server/src/main/java/dev/slimevr/osc/VRCOSCHandler.java b/server/src/main/java/dev/slimevr/osc/VRCOSCHandler.java new file mode 100644 index 000000000..8c0e5fc3b --- /dev/null +++ b/server/src/main/java/dev/slimevr/osc/VRCOSCHandler.java @@ -0,0 +1,261 @@ +package dev.slimevr.osc; + +import com.illposed.osc.*; +import com.illposed.osc.messageselector.OSCPatternAddressMessageSelector; +import com.illposed.osc.transport.OSCPortIn; +import com.illposed.osc.transport.OSCPortOut; +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; +import dev.slimevr.config.OSCConfig; +import dev.slimevr.platform.windows.WindowsNamedPipeBridge; +import dev.slimevr.vr.processor.HumanPoseProcessor; +import dev.slimevr.vr.trackers.HMDTracker; +import dev.slimevr.vr.trackers.ShareableTracker; +import dev.slimevr.vr.trackers.TrackerRole; +import dev.slimevr.vr.trackers.TrackerStatus; +import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; + +import java.io.IOException; +import java.net.InetAddress; +import java.util.List; + + +/** + * VRChat OSCTracker documentation: https://docs.vrchat.com/docs/osc-trackers + */ +public class VRCOSCHandler { + private OSCPortIn oscReceiver; + private OSCPortOut oscSender; + private OSCMessage oscMessage; + private final OSCConfig config; + private final HMDTracker hmd; + private final WindowsNamedPipeBridge steamvrBridge; + private final HumanPoseProcessor humanPoseProcessor; + private final List shareableTrackers; + private final FastList oscArgs = new FastList<>(3); + private final Vector3f vec = new Vector3f(); + private final Quaternion quatBuf = new Quaternion(); + private final float[] floatBuf = new float[3]; + private final boolean[] trackersEnabled; + private long timeAtLastOSCMessageReceived; + private static final long HMD_TIMEOUT = 15000; + + public VRCOSCHandler( + HMDTracker hmd, + HumanPoseProcessor humanPoseProcessor, + WindowsNamedPipeBridge steamvrBridge, + OSCConfig oscConfig, + List shareableTrackers + ) { + this.hmd = hmd; + this.humanPoseProcessor = humanPoseProcessor; + this.steamvrBridge = steamvrBridge; + this.config = oscConfig; + this.shareableTrackers = shareableTrackers; + + trackersEnabled = new boolean[shareableTrackers.size()]; + + refreshSettings(); + } + + public void refreshSettings() { + // Sets which trackers are enabled and force HEAD to false + for (int i = 0; i < shareableTrackers.size(); i++) { + if (shareableTrackers.get(i).getTrackerRole() != TrackerRole.HEAD) { + trackersEnabled[i] = config + .getOSCTrackerRole(shareableTrackers.get(i).getTrackerRole(), false); + } + } + + // Stops listening and closes OSC port + if (oscReceiver != null && oscReceiver.isListening()) { + oscReceiver.stopListening(); + } + if (oscSender != null && oscSender.isConnected()) { + try { + oscSender.close(); + } catch (IOException e) { + LogManager.severe("[VRCOSCHandler] Error closing the OSC sender: " + e); + } + } + + if (config.getEnabled()) { + // Instantiates the OSC receiver + try { + int port = config.getPortIn(); + oscReceiver = new OSCPortIn( + port + ); + LogManager.info("[VRCOSCHandler] Listening to port " + port); + } catch (IOException e) { + LogManager + .severe("[VRCOSCHandler] Error listening to the port " + config.getPortIn()); + } + + // Starts listening for the Upright parameter from VRC + OSCMessageListener listener = this::handleReceivedMessage; + MessageSelector selector = new OSCPatternAddressMessageSelector( + "/avatar/parameters/Upright" + ); + oscReceiver.getDispatcher().addListener(selector, listener); + oscReceiver.startListening(); + + // Instantiate the OSC sender + try { + InetAddress address = config.getAddress(); + int port = config.getPortOut(); + oscSender = new OSCPortOut( + address, + port + ); + oscSender.connect(); + LogManager + .info( + "[VRCOSCHandler] Sending to port " + + port + + " at address " + + address.toString() + ); + } catch (IOException e) { + LogManager + .severe( + "[VRCOSCHandler] Error connecting to port " + + config.getPortOut() + + " at the address " + + config.getAddress() + ); + } + } + } + + void handleReceivedMessage(OSCMessageEvent event) { + timeAtLastOSCMessageReceived = System.currentTimeMillis(); + + if (steamvrBridge != null && !steamvrBridge.isConnected()) { + // Sets HMD status to OK + if (hmd.getStatus() != TrackerStatus.OK) { + hmd.setStatus(TrackerStatus.OK); + } + + // Sets the HMD y position to + // the vrc Upright parameter (0-1) * the user's height + hmd.position + .set( + 0f, + (float) event + .getMessage() + .getArguments() + .get(0) * humanPoseProcessor.getUserHeightFromConfig(), + 0f + ); + hmd.rotation.set(Quaternion.IDENTITY); + + hmd.dataTick(); + } + } + + public void update() { + // Manage HMD state with timeout + if (oscReceiver != null) { + if ( + ((steamvrBridge != null + && steamvrBridge.isConnected()) + || + System.currentTimeMillis() - timeAtLastOSCMessageReceived > HMD_TIMEOUT + || + !oscReceiver.isListening()) + && hmd.getStatus() == TrackerStatus.OK + ) { + hmd.setStatus(TrackerStatus.DISCONNECTED); + } + } + + // Send OSC data + if (oscSender != null && oscSender.isConnected()) { + for (int i = 0; i < shareableTrackers.size(); i++) { + if (trackersEnabled[i]) { + // Send regular trackers' positions + shareableTrackers.get(i).getPosition(vec); + oscArgs.clear(); + oscArgs.add(-vec.x); + oscArgs.add(vec.y); + oscArgs.add(vec.z); + oscMessage = new OSCMessage( + "/tracking/trackers/" + (i + 1) + "/position", + oscArgs + ); + try { + oscSender.send(oscMessage); + } catch (IOException | OSCSerializeException e) { + LogManager + .warning( + "[VRCOSCHandler] Error sending tracker positions to VRChat: " + e + ); + } + + // Send regular trackers' rotations + shareableTrackers.get(i).getRotation(quatBuf); + quatBuf.toAngles(floatBuf); + oscArgs.clear(); + oscArgs.add(floatBuf[0] * FastMath.RAD_TO_DEG); + oscArgs.add(-floatBuf[1] * FastMath.RAD_TO_DEG); + oscArgs.add(-floatBuf[2] * FastMath.RAD_TO_DEG); + oscMessage = new OSCMessage( + "/tracking/trackers/" + (i + 1) + "/rotation", + oscArgs + ); + try { + oscSender.send(oscMessage); + } catch (IOException | OSCSerializeException e) { + LogManager + .warning( + "[VRCOSCHandler] Error sending tracker rotations to VRChat: " + e + ); + } + } + + if (shareableTrackers.get(i).getTrackerRole() == TrackerRole.HEAD) { + // Send HMD position + shareableTrackers.get(i).getPosition(vec); + oscArgs.clear(); + oscArgs.add(-vec.x); + oscArgs.add(vec.y); + oscArgs.add(vec.z); + oscMessage = new OSCMessage( + "/tracking/trackers/head/position", + oscArgs + ); + try { + oscSender.send(oscMessage); + } catch (IOException | OSCSerializeException e) { + LogManager + .warning("[VRCOSCHandler] Error sending head position to VRChat: " + e); + } + } + } + } + } + + /** + * Sends the expected HMD rotation upon reset to align the trackers in VRC + */ + public void yawAlign() { + if (oscSender != null && oscSender.isConnected()) { + oscArgs.clear(); + oscArgs.add(0f); + oscArgs.add(180f); + oscArgs.add(0f); + oscMessage = new OSCMessage( + "/tracking/trackers/head/rotation", + oscArgs + ); + try { + oscSender.send(oscMessage); + } catch (IOException | OSCSerializeException e) { + LogManager.warning("[VRCOSCHandler] Error sending HMD rotation to VRChat: " + e); + } + } + } +} diff --git a/server/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java b/server/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java index dc0ead0ae..fd915fb74 100644 --- a/server/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java +++ b/server/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java @@ -304,4 +304,9 @@ public class WindowsNamedPipeBridge extends ProtobufBridge implements ); return false; } + + public boolean isConnected() { + return pipe != null + && pipe.state == PipeState.OPEN; + } } diff --git a/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java b/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java index 75cce92e2..4a9bdf74a 100644 --- a/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java +++ b/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java @@ -207,6 +207,11 @@ public class PoseFrameTracker implements Tracker, Iterable { throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration"); } + @Override + public void resetMounting(boolean reverseYaw) { + throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration"); + } + @Override public void tick() { throw new UnsupportedOperationException("PoseFrameTracker does not implement this method"); diff --git a/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java b/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java index 19e0a21d4..501e93e7f 100644 --- a/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java +++ b/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java @@ -146,6 +146,11 @@ public final class TrackerFrame implements Tracker { throw new UnsupportedOperationException("TrackerFrame does not implement calibration"); } + @Override + public void resetMounting(boolean reverseYaw) { + throw new UnsupportedOperationException("TrackerFrame does not implement calibration"); + } + @Override public void tick() { throw new UnsupportedOperationException("TrackerFrame does not implement this method"); diff --git a/server/src/main/java/dev/slimevr/protocol/RPCHandler.java b/server/src/main/java/dev/slimevr/protocol/RPCHandler.java index c74c3bb24..14d3d8116 100644 --- a/server/src/main/java/dev/slimevr/protocol/RPCHandler.java +++ b/server/src/main/java/dev/slimevr/protocol/RPCHandler.java @@ -7,8 +7,10 @@ import dev.slimevr.autobone.AutoBone.Epoch; import dev.slimevr.autobone.AutoBoneListener; import dev.slimevr.autobone.AutoBoneProcessType; import dev.slimevr.config.FiltersConfig; +import dev.slimevr.config.OSCConfig; import dev.slimevr.config.OverlayConfig; import dev.slimevr.filtering.TrackerFilters; +import dev.slimevr.osc.VRCOSCHandler; import dev.slimevr.platform.windows.WindowsNamedPipeBridge; import dev.slimevr.poserecorder.PoseFrames; import dev.slimevr.serial.SerialListener; @@ -21,12 +23,14 @@ import dev.slimevr.vr.trackers.TrackerPosition; import dev.slimevr.vr.trackers.TrackerRole; import io.eiren.util.logging.LogManager; import solarxr_protocol.MessageBundle; +import solarxr_protocol.datatypes.Ipv4Address; import solarxr_protocol.datatypes.TransactionId; import solarxr_protocol.rpc.*; import solarxr_protocol.rpc.settings.ModelRatios; import solarxr_protocol.rpc.settings.ModelSettings; import solarxr_protocol.rpc.settings.ModelToggles; +import java.nio.ByteBuffer; import java.util.EnumMap; import java.util.Map.Entry; import java.util.function.BiConsumer; @@ -243,6 +247,8 @@ public class RPCHandler extends ProtocolHandler this.api.server.resetTrackersYaw(); if (req.resetType() == ResetType.Full) this.api.server.resetTrackers(); + if (req.resetType() == ResetType.Mounting) + this.api.server.resetTrackersMounting(); LogManager.severe("[WebSocketAPI] Reset performed"); } @@ -313,6 +319,15 @@ public class RPCHandler extends ProtocolHandler filtersConfig.getAmount() ); + OSCConfig vrcOSCConfig = this.api.server + .getConfigManager() + .getVrConfig() + .getVrcOSC(); + int vrcOSCSettings = createOSCSettings( + fbb, + vrcOSCConfig + ); + int modelSettings; { var config = this.api.server.humanPoseProcessor.getSkeletonConfig(); @@ -340,12 +355,54 @@ public class RPCHandler extends ProtocolHandler } int settings = SettingsResponse - .createSettingsResponse(fbb, steamvrTrackerSettings, filterSettings, 0, modelSettings); + .createSettingsResponse( + fbb, + steamvrTrackerSettings, + filterSettings, + vrcOSCSettings, + modelSettings + ); int outbound = createRPCMessage(fbb, RpcMessage.SettingsResponse, settings); fbb.finish(outbound); conn.send(fbb.dataBuffer()); } + private static int createOSCSettings( + FlatBufferBuilder fbb, + OSCConfig config + ) { + VRCOSCSettings.startVRCOSCSettings(fbb); + VRCOSCSettings.addEnabled(fbb, config.getEnabled()); + VRCOSCSettings.addPortIn(fbb, config.getPortIn()); + VRCOSCSettings.addPortOut(fbb, config.getPortOut()); + VRCOSCSettings + .addAddress( + fbb, + Ipv4Address + .createIpv4Address( + fbb, + ByteBuffer.wrap(config.getAddress().getAddress()).getInt() + ) + ); + VRCOSCSettings + .addTrackers( + fbb, + OSCTrackersSetting + .createOSCTrackersSetting( + fbb, + config.getOSCTrackerRole(TrackerRole.HEAD, false), + config.getOSCTrackerRole(TrackerRole.CHEST, false), + config.getOSCTrackerRole(TrackerRole.WAIST, false), + config.getOSCTrackerRole(TrackerRole.LEFT_KNEE, false), + config.getOSCTrackerRole(TrackerRole.LEFT_FOOT, false), + config.getOSCTrackerRole(TrackerRole.LEFT_ELBOW, false), + config.getOSCTrackerRole(TrackerRole.LEFT_HAND, false) + ) + ); + + return VRCOSCSettings.endVRCOSCSettings(fbb); + } + public void onChangeSettingsRequest(GenericConnection conn, RpcMessageHeader messageHeader) { ChangeSettingsRequest req = (ChangeSettingsRequest) messageHeader @@ -382,6 +439,31 @@ public class RPCHandler extends ProtocolHandler } } + if (req.vrcOsc() != null) { + OSCConfig vrcOSCConfig = this.api.server + .getConfigManager() + .getVrConfig() + .getVrcOSC(); + VRCOSCHandler VRCOSCHandler = this.api.server.getVRCOSCHandler(); + var trackers = req.vrcOsc().trackers(); + + vrcOSCConfig.setEnabled(req.vrcOsc().enabled()); + vrcOSCConfig.setPortIn(req.vrcOsc().portIn()); + vrcOSCConfig.setPortOut(req.vrcOsc().portOut()); + vrcOSCConfig.setAddress(req.vrcOsc().address().toString()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.head()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.CHEST, trackers.chest()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.WAIST, trackers.waist()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.LEFT_KNEE, trackers.knees()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.RIGHT_KNEE, trackers.knees()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.feet()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.elbows()); + vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.hands()); + + + VRCOSCHandler.refreshSettings(); + } + var modelSettings = req.modelSettings(); if (modelSettings != null) { var cfg = this.api.server.humanPoseProcessor.getSkeletonConfig(); diff --git a/server/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTrackerPosition.java b/server/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTrackerPosition.java index 64ed51c44..fcab02373 100644 --- a/server/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTrackerPosition.java +++ b/server/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTrackerPosition.java @@ -2,6 +2,7 @@ package dev.slimevr.vr.processor; public enum ComputedHumanPoseTrackerPosition { + HEAD, WAIST, CHEST, LEFT_FOOT, diff --git a/server/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java b/server/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java index 0f0603b8a..17ee769e1 100644 --- a/server/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java +++ b/server/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java @@ -2,12 +2,11 @@ package dev.slimevr.vr.processor; import dev.slimevr.VRServer; import dev.slimevr.util.ann.VRServerThread; -import dev.slimevr.vr.processor.skeleton.Skeleton; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; -import dev.slimevr.vr.processor.skeleton.SkeletonConfig; -import dev.slimevr.vr.processor.skeleton.SkeletonConfigOffsets; -import dev.slimevr.vr.processor.skeleton.SkeletonConfigToggles; -import dev.slimevr.vr.trackers.*; +import dev.slimevr.vr.processor.skeleton.*; +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; @@ -22,8 +21,24 @@ public class HumanPoseProcessor { private final List> onSkeletonUpdated = new FastList<>(); private Skeleton skeleton; - public HumanPoseProcessor(VRServer server, HMDTracker hmd) { + public HumanPoseProcessor(VRServer server) { this.server = server; + computedTrackers + .add( + new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.HEAD, + TrackerRole.HEAD + ) + ); + computedTrackers + .add( + new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.CHEST, + TrackerRole.CHEST + ) + ); computedTrackers .add( new ComputedHumanPoseTracker( @@ -48,14 +63,6 @@ public class HumanPoseProcessor { TrackerRole.RIGHT_FOOT ) ); - computedTrackers - .add( - new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.CHEST, - TrackerRole.CHEST - ) - ); computedTrackers .add( new ComputedHumanPoseTracker( @@ -155,16 +162,16 @@ public class HumanPoseProcessor { @VRServerThread public void trackerAdded(Tracker tracker) { - updateSekeltonModel(); + updateSkeletonModel(); } @VRServerThread public void trackerUpdated(Tracker tracker) { - updateSekeltonModel(); + updateSkeletonModel(); } @VRServerThread - private void updateSekeltonModel() { + private void updateSkeletonModel() { disconnectAllTrackers(); skeleton = new HumanSkeleton(server, computedTrackers); for (Consumer sc : onSkeletonUpdated) @@ -186,8 +193,16 @@ public class HumanPoseProcessor { @VRServerThread public void resetTrackers() { - if (skeleton != null) + if (skeleton != null) { skeleton.resetTrackersFull(); + server.getVRCOSCHandler().yawAlign(); + } + } + + @VRServerThread + public void resetTrackersMounting() { + if (skeleton != null) + skeleton.resetTrackersMounting(); } @VRServerThread @@ -236,4 +251,12 @@ public class HumanPoseProcessor { server.getConfigManager().saveConfig(); } } + + @VRServerThread + public float getUserHeightFromConfig() { + if (skeleton != null) { + return getSkeletonConfig().getUserHeightFromOffsets(); + } + return 0f; + } } diff --git a/server/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java b/server/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java index 8eac4125a..ae538f5c0 100644 --- a/server/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java +++ b/server/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java @@ -23,6 +23,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // @formatter:off protected final TransformNode hmdNode = new TransformNode("HMD", false); protected final TransformNode headNode = new TransformNode("Head", false); + protected final TransformNode trackerHeadNode = new TransformNode("Head-Tracker", false); protected final TransformNode neckNode = new TransformNode("Neck", false); protected final TransformNode chestNode = new TransformNode("Chest", false); protected final TransformNode trackerChestNode = new TransformNode("Chest-Tracker", false); @@ -102,6 +103,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { protected Tracker rightShoulderTracker; // #endregion // #region Tracker Output + protected ComputedHumanPoseTracker computedHeadTracker; protected ComputedHumanPoseTracker computedChestTracker; protected ComputedHumanPoseTracker computedWaistTracker; protected ComputedHumanPoseTracker computedLeftKneeTracker; @@ -226,6 +228,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // #endregion // #region Attach tracker nodes for tracker offsets + neckNode.attachChild(trackerHeadNode); chestNode.attachChild(trackerChestNode); hipNode.attachChild(trackerWaistNode); @@ -618,6 +621,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { public void setComputedTracker(ComputedHumanPoseTracker tracker) { switch (tracker.getTrackerRole()) { + case HEAD -> computedHeadTracker = tracker; case CHEST -> computedChestTracker = tracker; case WAIST -> computedWaistTracker = tracker; case LEFT_KNEE -> computedLeftKneeTracker = tracker; @@ -641,6 +645,22 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // #endregion public void fillNullComputedTrackers() { + if (computedHeadTracker == null) { + computedHeadTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.HEAD, + TrackerRole.HEAD + ); + computedHeadTracker.setStatus(TrackerStatus.OK); + } + if (computedChestTracker == null) { + computedChestTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.CHEST, + TrackerRole.CHEST + ); + computedChestTracker.setStatus(TrackerStatus.OK); + } if (computedWaistTracker == null) { computedWaistTracker = new ComputedHumanPoseTracker( Tracker.getNextLocalTrackerId(), @@ -665,14 +685,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { ); computedRightFootTracker.setStatus(TrackerStatus.OK); } - if (computedChestTracker == null) { - computedChestTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.CHEST, - TrackerRole.CHEST - ); - computedChestTracker.setStatus(TrackerStatus.OK); - } if (computedLeftKneeTracker == null) { computedLeftKneeTracker = new ComputedHumanPoseTracker( Tracker.getNextLocalTrackerId(), @@ -726,6 +738,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // #region Get Trackers public ComputedHumanPoseTracker getComputedTracker(TrackerRole trackerRole) { switch (trackerRole) { + case HEAD: + return computedHeadTracker; case CHEST: return computedChestTracker; case WAIST: @@ -824,6 +838,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { hmdTracker.getRotation(rotBuf1); hmdNode.localTransform.setRotation(rotBuf1); + trackerHeadNode.localTransform.setRotation(rotBuf1); if (neckTracker != null) neckTracker.getRotation(rotBuf1); @@ -832,6 +847,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // Set to zero hmdNode.localTransform.setTranslation(Vector3f.ZERO); hmdNode.localTransform.setRotation(Quaternion.IDENTITY); + trackerHeadNode.localTransform.setRotation(Quaternion.IDENTITY); headNode.localTransform.setRotation(Quaternion.IDENTITY); } @@ -1219,6 +1235,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { // #region Update the output trackers protected void updateComputedTrackers() { + if (computedHeadTracker != null) { + computedHeadTracker.position.set(trackerHeadNode.worldTransform.getTranslation()); + computedHeadTracker.rotation.set(trackerHeadNode.worldTransform.getRotation()); + computedHeadTracker.dataTick(); + } + if (computedChestTracker != null) { computedChestTracker.position.set(trackerChestNode.worldTransform.getTranslation()); computedChestTracker.rotation.set(trackerChestNode.worldTransform.getRotation()); @@ -1544,6 +1566,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { return new TransformNode[] { hmdNode, headNode, + trackerHeadNode, neckNode, chestNode, trackerChestNode, @@ -1624,7 +1647,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { hmdTracker.getPosition(vec); height = vec.y; if (height > 0.5f) { // Reset only if floor level seems right, - // TODO: read floor level from SteamVR skeletonConfig .setOffset( SkeletonConfigOffsets.TORSO, @@ -1656,7 +1678,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { hmdTracker.getPosition(vec); height = vec.y; if (height > 0.5f) { // Reset only if floor level seems right, - // TODO: read floor level from SteamVR skeletonConfig .setOffset( SkeletonConfigOffsets.LEGS_LENGTH, @@ -1760,6 +1781,38 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { this.legTweaks.resetBuffer(); } + private boolean shouldResetMounting(TrackerPosition position) { + return position != null + // TODO: Feet can't currently be reset using this method, maybe + // they'll need a separate step just for them? + && position != TrackerPosition.LEFT_FOOT + && position != TrackerPosition.RIGHT_FOOT; + } + + private boolean shouldReverseYaw(TrackerPosition position) { + return position != null + && (position == TrackerPosition.LEFT_UPPER_LEG + || position == TrackerPosition.RIGHT_UPPER_LEG + || position == TrackerPosition.LEFT_LOWER_ARM + || position == TrackerPosition.RIGHT_LOWER_ARM + || position == TrackerPosition.LEFT_HAND + || position == TrackerPosition.RIGHT_HAND); + } + + @Override + @VRServerThread + public void resetTrackersMounting() { + // Pass all trackers through trackerPreUpdate + Tracker[] trackersToReset = getTrackersToReset(); + + for (Tracker tracker : trackersToReset) { + if (tracker != null && shouldResetMounting(tracker.getBodyPosition())) { + tracker.resetMounting(shouldReverseYaw(tracker.getBodyPosition())); + } + } + this.legTweaks.resetBuffer(); + } + @Override @VRServerThread public void resetTrackersYaw() { diff --git a/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweakBuffer.java b/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweakBuffer.java index cf7091f3a..c9ef9aa57 100644 --- a/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweakBuffer.java +++ b/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweakBuffer.java @@ -2,6 +2,7 @@ package dev.slimevr.vr.processor.skeleton; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import com.jme3.math.FastMath; /** @@ -12,7 +13,7 @@ import com.jme3.math.Vector3f; * rules: The conditions for an unlock are as follows: 1. the foot is to far * from its correct position 2. a velocity higher than a threashold is achived * 3. a large acceleration is applied to the foot 4. angular velocity of the - * foot goes higher than a threashold The conditions for a lock are the opposite + * foot goes higher than a threshold. The conditions for a lock are the opposite * of the above but require a lower value for all of the above conditions */ @@ -24,40 +25,41 @@ public class LegTweakBuffer { public static final int ANKLE_ACCEL = 4; public static final float NS_CONVERT = 1000000000.0f; - private static final Vector3f placeHolderVec = new Vector3f(); - private static final Quaternion placeHolderQuat = new Quaternion(); + private static final Vector3f GRAVITY = new Vector3f(0, -9.81f, 0); + private static final float GRAVITY_MAGNITUDE = GRAVITY.length(); + private static final int BUFFER_LEN = 10; // states for the legs private int leftLegState = STATE_UNKNOWN; private int rightLegState = STATE_UNKNOWN; // positions and rotations - private Vector3f leftFootPosition = placeHolderVec; - private Vector3f rightFootPosition = placeHolderVec; - private Vector3f leftKneePosition = placeHolderVec; - private Vector3f rightKneePosition = placeHolderVec; - private Vector3f waistPosition = placeHolderVec; - private Quaternion leftFootRotation = placeHolderQuat; - private Quaternion rightFootRotation = placeHolderQuat; + private Vector3f leftFootPosition = new Vector3f(); + private Vector3f rightFootPosition = new Vector3f(); + private Vector3f leftKneePosition = new Vector3f(); + private Vector3f rightKneePosition = new Vector3f(); + private Vector3f waistPosition = new Vector3f(); + private Quaternion leftFootRotation = new Quaternion(); + private Quaternion rightFootRotation = new Quaternion(); - private Vector3f leftFootPositionCorrected = placeHolderVec; - private Vector3f rightFootPositionCorrected = placeHolderVec; - private Vector3f leftKneePositionCorrected = placeHolderVec; - private Vector3f rightKneePositionCorrected = placeHolderVec; - private Vector3f waistPositionCorrected = placeHolderVec; + private Vector3f leftFootPositionCorrected = new Vector3f(); + private Vector3f rightFootPositionCorrected = new Vector3f(); + private Vector3f leftKneePositionCorrected = new Vector3f(); + private Vector3f rightKneePositionCorrected = new Vector3f(); + private Vector3f waistPositionCorrected = new Vector3f(); // velocities - private Vector3f leftFootVelocity = placeHolderVec; + private Vector3f leftFootVelocity = new Vector3f(); private float leftFootVelocityMagnitude = 0; - private Vector3f rightFootVelocity = placeHolderVec; + private Vector3f rightFootVelocity = new Vector3f(); private float rightFootVelocityMagnitude = 0; private float leftFootAngleDiff = 0; private float rightFootAngleDiff = 0; // acceleration - private Vector3f leftFootAcceleration = placeHolderVec; + private Vector3f leftFootAcceleration = new Vector3f(); private float leftFootAccelerationMagnitude = 0; - private Vector3f rightFootAcceleration = placeHolderVec; + private Vector3f rightFootAcceleration = new Vector3f(); private float rightFootAccelerationMagnitude = 0; // other data @@ -67,36 +69,46 @@ public class LegTweakBuffer { private int detectionMode = ANKLE_ACCEL; // detection mode private boolean accelerationAboveThresholdLeft = true; private boolean accelerationAboveThresholdRight = true; + private Vector3f centerOfMass = new Vector3f(); + private Vector3f centerOfMassVelocity = new Vector3f(); + private Vector3f centerOfMassAcceleration = new Vector3f(); private float leftFloorLevel; private float rightFloorLevel; // hyperparameters - public static final float SKATING_DISTANCE_CUTOFF = 0.225f; - private static final float SKATING_VELOCITY_THRESHOLD = 4.25f; - private static final float SKATING_ACCELERATION_THRESHOLD = 1.15f; + public static final float SKATING_DISTANCE_CUTOFF = 0.5f; + private static final float SKATING_VELOCITY_THRESHOLD = 3.25f; + private static final float SKATING_ACCELERATION_THRESHOLD = 1.00f; private static final float SKATING_ROTVELOCITY_THRESHOLD = 4.5f; private static final float SKATING_LOCK_ENGAGE_PERCENT = 0.85f; private static final float SKATING_ACCELERATION_Y_USE_PERCENT = 0.25f; private static final float FLOOR_DISTANCE_CUTOFF = 0.125f; - private static final float SIX_TRACKER_TOLLERANCE = 0.10f; + private static final float SIX_TRACKER_TOLLERANCE = -0.10f; + private static final Vector3f FORCE_VECTOR_TO_PRESSURE = new Vector3f(0.25f, 1.0f, 0.25f); + private static final float FORCE_ERROR_TOLLERANCE = 4.0f; + private static final float[] FORCE_VECTOR_FALLBACK = new float[] { 0.1f, 0.1f }; - private static final float PARAM_SCALAR_MAX = 2.5f; - private static final float PARAM_SCALAR_MIN = 0.5f; + private static final float PARAM_SCALAR_MAX = 3.2f; + private static final float PARAM_SCALAR_MIN = 0.25f; private static final float PARAM_SCALAR_MID = 1.0f; // the point at which the scalar is at the max or min depending on accel - private static final float MAX_SCALAR_ACCEL = 0.3f; + private static final float MAX_SCALAR_ACCEL = 0.2f; private static final float MIN_SCALAR_ACCEL = 0.9f; // the point at which the scalar is at it max or min in a double locked foot // situation - private static final float MAX_SCALAR_DORMANT = 0.6f; - private static final float MIN_SCALAR_DORMANT = 2.0f; + private static final float MAX_SCALAR_DORMANT = 0.2f; + private static final float MIN_SCALAR_DORMANT = 1.50f; // the point at which the scalar is at it max or min in a single locked foot // situation - private static final float MIN_SCALAR_ACTIVE = 3.0f; - private static final float MAX_SCALAR_ACTIVE = 0.2f; + private static final float MIN_SCALAR_ACTIVE = 1.75f; + private static final float MAX_SCALAR_ACTIVE = 0.1f; + + // maximum scalers for the pressure on each foot + private static final float PRESSURE_SCALER_MIN = 0.1f; + private static final float PRESSURE_SCALER_MAX = 1.9f; private float leftFootSensitivityVel = 1.0f; private float rightFootSensitivityVel = 1.0f; @@ -121,7 +133,7 @@ public class LegTweakBuffer { } public void setLeftFootPosition(Vector3f leftFootPosition) { - this.leftFootPosition = leftFootPosition.clone(); + this.leftFootPosition.set(leftFootPosition); } public Vector3f getRightFootPosition(Vector3f vec) { @@ -132,7 +144,7 @@ public class LegTweakBuffer { } public void setRightFootPosition(Vector3f rightFootPosition) { - this.rightFootPosition = rightFootPosition.clone(); + this.rightFootPosition.set(rightFootPosition); } public Vector3f getLeftKneePosition(Vector3f vec) { @@ -143,7 +155,7 @@ public class LegTweakBuffer { } public void setLeftKneePosition(Vector3f leftKneePosition) { - this.leftKneePosition = leftKneePosition.clone(); + this.leftKneePosition.set(leftKneePosition); } public Vector3f getRightKneePosition(Vector3f vec) { @@ -153,7 +165,7 @@ public class LegTweakBuffer { } public void setRightKneePosition(Vector3f rightKneePosition) { - this.rightKneePosition = rightKneePosition.clone(); + this.rightKneePosition.set(rightKneePosition); } public Vector3f getWaistPosition(Vector3f vec) { @@ -164,7 +176,7 @@ public class LegTweakBuffer { } public void setWaistPosition(Vector3f waistPosition) { - this.waistPosition = waistPosition.clone(); + this.waistPosition.set(waistPosition); } public Quaternion getLeftFootRotation(Quaternion quat) { @@ -175,7 +187,7 @@ public class LegTweakBuffer { } public void setLeftFootRotation(Quaternion leftFootRotation) { - this.leftFootRotation = leftFootRotation.clone(); + this.leftFootRotation.set(leftFootRotation); } public Quaternion getRightFootRotation(Quaternion quat) { @@ -186,7 +198,7 @@ public class LegTweakBuffer { } public void setRightFootRotation(Quaternion rightFootRotation) { - this.rightFootRotation = rightFootRotation.clone(); + this.rightFootRotation.set(rightFootRotation); } public Vector3f getLeftFootPositionCorrected(Vector3f vec) { @@ -197,7 +209,7 @@ public class LegTweakBuffer { } public void setLeftFootPositionCorrected(Vector3f leftFootPositionCorrected) { - this.leftFootPositionCorrected = leftFootPositionCorrected.clone(); + this.leftFootPositionCorrected.set(leftFootPositionCorrected); } public Vector3f getRightFootPositionCorrected(Vector3f vec) { @@ -208,7 +220,7 @@ public class LegTweakBuffer { } public void setRightFootPositionCorrected(Vector3f rightFootPositionCorrected) { - this.rightFootPositionCorrected = rightFootPositionCorrected.clone(); + this.rightFootPositionCorrected.set(rightFootPositionCorrected); } public Vector3f getLeftKneePositionCorrected(Vector3f vec) { @@ -219,7 +231,7 @@ public class LegTweakBuffer { } public void setLeftKneePositionCorrected(Vector3f leftKneePositionCorrected) { - this.leftKneePositionCorrected = leftKneePositionCorrected.clone(); + this.leftKneePositionCorrected.set(leftKneePositionCorrected); } public Vector3f getRightKneePositionCorrected(Vector3f vec) { @@ -230,7 +242,7 @@ public class LegTweakBuffer { } public void setRightKneePositionCorrected(Vector3f rightKneePositionCorrected) { - this.rightKneePositionCorrected = rightKneePositionCorrected.clone(); + this.rightKneePositionCorrected.set(rightKneePositionCorrected); } public Vector3f getWaistPositionCorrected(Vector3f vec) { @@ -241,7 +253,7 @@ public class LegTweakBuffer { } public void setWaistPositionCorrected(Vector3f waistPositionCorrected) { - this.waistPositionCorrected = waistPositionCorrected.clone(); + this.waistPositionCorrected.set(waistPositionCorrected); } public Vector3f getLeftFootVelocity(Vector3f vec) { @@ -258,6 +270,17 @@ public class LegTweakBuffer { return vec.set(rightFootVelocity); } + public Vector3f getCenterOfMass(Vector3f vec) { + if (vec == null) + vec = new Vector3f(); + + return vec.set(centerOfMass); + } + + public void setCenterOfMass(Vector3f centerOfMass) { + this.centerOfMass.set(centerOfMass); + } + public void setLeftFloorLevel(float leftFloorLevel) { this.leftFloorLevel = leftFloorLevel; } @@ -291,11 +314,11 @@ public class LegTweakBuffer { } public void setLeftFootAcceleration(Vector3f leftFootAcceleration) { - this.leftFootAcceleration = leftFootAcceleration.clone(); + this.leftFootAcceleration.set(leftFootAcceleration); } public void setRightFootAcceleration(Vector3f rightFootAcceleration) { - this.rightFootAcceleration = rightFootAcceleration.clone(); + this.rightFootAcceleration.set(rightFootAcceleration); } public Vector3f getLeftFootAcceleration(Vector3f vec) { @@ -328,6 +351,22 @@ public class LegTweakBuffer { return this.rightFootAcceleration.y; } + public float getLeftFloorLevel() { + return leftFloorLevel; + } + + public float getRightFloorLevel() { + return rightFloorLevel; + } + + // returns 1 / delta time + public float getTimeDelta() { + if (parent == null) + return 0.0f; + + return 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT); + } + public void setDetectionMode(int mode) { this.detectionMode = mode; } @@ -339,6 +378,7 @@ public class LegTweakBuffer { // compute attributes of the legs computeVelocity(); computeAccelerationMagnitude(); + computeComAtributes(); // check if the acceleration triggers forced unlock if (detectionMode == FOOT_ACCEL) { @@ -359,19 +399,15 @@ public class LegTweakBuffer { } } - // update the frame number of all the frames + // update the frame number and discard frames older than BUFFER_LEN public void updateFrameNumber(int frameNumber) { this.frameNumber = frameNumber; - if (this.frameNumber >= 10) { - this.parent = null; // once a frame is 10 frames old, it is no - // longer - // needed - } + if (this.frameNumber >= BUFFER_LEN) + this.parent = null; - if (parent != null) { + if (parent != null) parent.updateFrameNumber(frameNumber + 1); - } } // compute the state of the legs @@ -384,8 +420,7 @@ public class LegTweakBuffer { // check if a locked foot should stay locked or be released private int checkStateLeft() { - float timeStep = 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT); - + float timeStep = getTimeDelta(); if (parent.leftLegState == UNLOCKED) { if ( parent.getLeftFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE @@ -419,7 +454,7 @@ public class LegTweakBuffer { // check if a locked foot should stay locked or be released private int checkStateRight() { - float timeStep = 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT); + float timeStep = getTimeDelta(); if (parent.rightLegState == UNLOCKED) { if ( @@ -511,6 +546,12 @@ public class LegTweakBuffer { .length(); } + // compute the velocity and acceleration of the center of mass + private void computeComAtributes() { + centerOfMassVelocity = centerOfMass.subtract(parent.centerOfMass); + centerOfMassAcceleration = centerOfMassVelocity.subtract(parent.centerOfMassVelocity); + } + // for 8 trackers the data from the imus is enough to determine lock/unlock private void computeAccelerationAboveThresholdFootTrackers() { accelerationAboveThresholdLeft = leftFootAccelerationMagnitude @@ -544,11 +585,16 @@ public class LegTweakBuffer { float leftFootScalarVel = getLeftFootLockLiklyHood(); float rightFootScalarVel = getRightFootLockLiklyHood(); - // combine these two sets of scalars to get the final scalars - // ( -1 just makes is so if both scalars are 1.0 the final scalar is 1.0 - // not 2) - leftFootSensitivityVel = leftFootScalarAccel + leftFootScalarVel - 1.0f; - rightFootSensitivityVel = rightFootScalarAccel + rightFootScalarVel - 1.0f; + // get the third set of scalars that is based on where the COM is + float[] pressureScalars = getPressurePrediction(); + + // combine the scalars to get the final scalars + leftFootSensitivityVel = (leftFootScalarAccel + + leftFootScalarVel / 2.0f) + * FastMath.clamp(pressureScalars[0] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX); + rightFootSensitivityVel = (rightFootScalarAccel + + rightFootScalarVel / 2.0f) + * FastMath.clamp(pressureScalars[1] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX); leftFootSensitivityAccel = leftFootScalarVel; rightFootSensitivityAccel = rightFootScalarVel; @@ -654,4 +700,130 @@ public class LegTweakBuffer { / (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE) - PARAM_SCALAR_MID; } + + // get the pressure prediction for the feet based of the center of mass + // (assume mass is 1) + // for understanding in the future this is assuming that the mass is one and + // the force of gravity + // is 9.8 m/s^2 this allows for the force sum to map directly to the + // acceleration of the center of mass + // since F = ma and if m is 1 then F = a + private float[] getPressurePrediction() { + float leftFootPressure = 0; + float rightFootPressure = 0; + + // get the vectors from the com to each foot + Vector3f leftFootVector = leftFootPosition.subtract(centerOfMass).normalizeLocal(); + Vector3f rightFootVector = rightFootPosition.subtract(centerOfMass).normalizeLocal(); + + // get the magnitude of the force on each foot + float leftFootMagnitude = GRAVITY_MAGNITUDE * leftFootVector.y / leftFootVector.length(); + float rightFootMagnitude = GRAVITY_MAGNITUDE * rightFootVector.y / rightFootVector.length(); + + // get the force vector each foot could apply to the com + Vector3f leftFootForce = leftFootVector.mult(leftFootMagnitude / 2.0f); + Vector3f rightFootForce = rightFootVector.mult(rightFootMagnitude / 2.0f); + + // based off the acceleration of the com, get the force each foot is + // likely applying (the expected force sum should be equal to + // centerOfMassAcceleration since the mass is 1) + findForceVectors(leftFootForce, rightFootForce); + + // see if the force vectors found a reasonable solution + // if they did not we assume there is another force acting on the com + // and fall back to a low pressure prediction + if (detectOutsideForces(leftFootForce, rightFootForce)) + return FORCE_VECTOR_FALLBACK; + + // set the pressure to the force on each foot times the force to + // pressure scalar + leftFootPressure = leftFootForce.mult(FORCE_VECTOR_TO_PRESSURE).length(); + rightFootPressure = rightFootForce.mult(FORCE_VECTOR_TO_PRESSURE).length(); + + // distance from the ground is a factor in the pressure + // using the inverse of the distance to the ground scale the + // pressure + float leftDistance = (leftFootPosition.y > leftFloorLevel) + ? (leftFootPosition.y - leftFloorLevel) + : LegTweaks.NEARLY_ZERO; + leftFootPressure *= 1.0f / (leftDistance); + float rightDistance = (rightFootPosition.y > rightFloorLevel) + ? (rightFootPosition.y - rightFloorLevel) + : LegTweaks.NEARLY_ZERO; + rightFootPressure *= 1.0f / (rightDistance); + + // normalize the pressure values + float pressureSum = leftFootPressure + rightFootPressure; + leftFootPressure /= pressureSum; + rightFootPressure /= pressureSum; + + return new float[] { leftFootPressure, rightFootPressure }; + } + + // perform a gradient descent to find the force vectors that best match the + // acceleration of the com + private void findForceVectors(Vector3f leftFootForce, Vector3f rightFootForce) { + int iterations = 100; + float stepSize = 0.01f; + // setup the temporary variables + Vector3f tempLeftFootForce1 = leftFootForce.clone(); + Vector3f tempLeftFootForce2 = leftFootForce.clone(); + Vector3f tempRightFootForce1 = rightFootForce.clone(); + Vector3f tempRightFootForce2 = rightFootForce.clone(); + Vector3f error; + Vector3f error1; + Vector3f error2; + Vector3f error3; + Vector3f error4; + + for (int i = 0; i < iterations; i++) { + tempLeftFootForce1.set(leftFootForce); + tempLeftFootForce2.set(leftFootForce); + tempRightFootForce1.set(rightFootForce); + tempRightFootForce2.set(rightFootForce); + + // get the error at the current position + error = centerOfMassAcceleration + .subtract(leftFootForce.add(rightFootForce).add(GRAVITY)); + + // add and subtract the error to the force vectors + tempLeftFootForce1 = tempLeftFootForce1.mult(1.0f + stepSize); + tempLeftFootForce2 = tempLeftFootForce2.mult(1.0f - stepSize); + tempRightFootForce1 = tempRightFootForce1.mult(1.0f + stepSize); + tempRightFootForce2 = tempRightFootForce2.mult(1.0f - stepSize); + + // get the error at the new position + error1 = getForceVectorError(tempLeftFootForce1, rightFootForce); + error2 = getForceVectorError(tempLeftFootForce2, rightFootForce); + error3 = getForceVectorError(tempRightFootForce1, leftFootForce); + error4 = getForceVectorError(tempRightFootForce2, leftFootForce); + + // set the new force vectors + if (error1.length() < error.length()) { + leftFootForce.set(tempLeftFootForce1); + } else if (error2.length() < error.length()) { + leftFootForce.set(tempLeftFootForce2); + } + + if (error3.length() < error.length()) { + rightFootForce.set(tempRightFootForce1); + } else if (error4.length() < error.length()) { + rightFootForce.set(tempRightFootForce2); + } + } + } + + // detect any outside forces on the body such + // as a wall or a chair. returns true if there is a outside force + private boolean detectOutsideForces(Vector3f f1, Vector3f f2) { + Vector3f force = GRAVITY.add(f1).add(f2); + Vector3f error = centerOfMassAcceleration.subtract(force); + return error.lengthSquared() > FastMath.sqr(FORCE_ERROR_TOLLERANCE); + } + + // simple error function for the force vector gradient descent + private Vector3f getForceVectorError(Vector3f testForce, Vector3f otherForce) { + return centerOfMassAcceleration + .subtract(testForce.add(otherForce).add(GRAVITY)); + } } diff --git a/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweaks.java b/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweaks.java index 11e2f94a1..06d85106c 100644 --- a/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweaks.java +++ b/server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweaks.java @@ -1,8 +1,11 @@ package dev.slimevr.vr.processor.skeleton; +import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import dev.slimevr.vr.processor.TransformNode; + public class LegTweaks { // state variables @@ -28,8 +31,6 @@ public class LegTweaks { private Vector3f waistPosition = new Vector3f(); private Quaternion leftFootRotation = new Quaternion(); private Quaternion rightFootRotation = new Quaternion(); - private Vector3f leftWaistUpperLegOffset = new Vector3f(); - private Vector3f rightWaistUpperLegOffset = new Vector3f(); private Vector3f leftFootAcceleration = new Vector3f(); private Vector3f rightFootAcceleration = new Vector3f(); @@ -59,30 +60,44 @@ public class LegTweaks { */ // hyperparameters (clip correction) - private static final float DYNAMIC_DISPLACEMENT_CUTOFF = 0.8f; - private static final float MAX_DYNAMIC_DISPLACEMENT = 0.08f; - private static final float FLOOR_CALIBRATION_OFFSET = 0.02f; + private static final float DYNAMIC_DISPLACEMENT_CUTOFF = 1.0f; + private static final float MAX_DYNAMIC_DISPLACEMENT = 0.06f; + private static final float FLOOR_CALIBRATION_OFFSET = 0.015f; // hyperparameters (skating correction) - private static final float MIN_ACCEPTABLE_ERROR = 0.05f; - private static final float MAX_ACCEPTABLE_ERROR = LegTweakBuffer.SKATING_DISTANCE_CUTOFF; - private static final float CORRECTION_WEIGHT_MIN = 0.25f; + private static final float MIN_ACCEPTABLE_ERROR = 0.01f; + private static final float MAX_ACCEPTABLE_ERROR = 0.225f; + private static final float CORRECTION_WEIGHT_MIN = 0.40f; private static final float CORRECTION_WEIGHT_MAX = 0.70f; - - // hyperparameters (floating feet correction) - private static final float FOOT_Y_CORRECTION_WEIGHT = 0.45f; - private static final float FOOT_Y_MAX_ACCELERATION = 0.40f; - private static final float FOOT_Y_DIFF_CUTOFF = 0.15f; + private static final float CONTINUOUS_CORRECTION_DIST = 0.5f; + private static final int CONTINUOUS_CORRECTION_WARMUP = 175; // hyperparameters (knee / waist correction) private static final float KNEE_CORRECTION_WEIGHT = 0.00f; - private static final float KNEE_LATERAL_WEIGHT = 0.9f; + private static final float KNEE_LATERAL_WEIGHT = 0.8f; private static final float WAIST_PUSH_WEIGHT = 0.2f; + // hyperparameters (COM calculation) + // mass percentages of the body + private static final float HEAD_MASS = 0.082f; + private static final float CHEST_MASS = 0.25f; + private static final float WAIST_MASS = 0.209f; + private static final float THIGH_MASS = 0.128f; + private static final float CALF_MASS = 0.0535f; + private static final float UPPER_ARM_MASS = 0.031f; + private static final float FOREARM_MASS = 0.017f; + // hyperparameters (misc) - private static final float NEARLY_ZERO = 0.005f; + static final float NEARLY_ZERO = 0.001f; private static final float STANDING_CUTOFF_VERTICAL = 0.65f; private static final float MAX_DISENGAGMENT_OFFSET = 0.30f; + private static final float DEFAULT_ARM_DISTANCE = 0.15f; + + // counters + private int leftFramesLocked = 0; + private int rightFramesLocked = 0; + private int leftFramesUnlocked = 0; + private int rightFramesUnlocked = 0; // buffer for holding previus frames of data private LegTweakBuffer bufferHead = new LegTweakBuffer(); @@ -92,14 +107,6 @@ public class LegTweaks { this.skeleton = skeleton; } - // update the offsets for the waist and upper leg - // this is used for correcting the knee tracker position - public void updateOffsets(Vector3f upperLeftLeg, Vector3f upperRightLeg, Vector3f waist) { - // update the relevant leg data - this.leftWaistUpperLegOffset = upperLeftLeg.subtract(waist); - this.rightWaistUpperLegOffset = upperRightLeg.subtract(waist); - } - public Vector3f getLeftFootPosition() { return leftFootPosition; } @@ -270,7 +277,7 @@ public class LegTweaks { // tweaks active = isStanding(); - // if the buffer is invalid set it up + // if the buffer is invalid add all the extra info if (bufferInvalid) { bufferHead.setLeftFootPositionCorrected(leftFootPosition); bufferHead.setRightFootPositionCorrected(rightFootPosition); @@ -284,22 +291,18 @@ public class LegTweaks { bufferHead.setWaistPosition(waistPosition); bufferHead.setLeftLegState(LegTweakBuffer.UNLOCKED); bufferHead.setRightLegState(LegTweakBuffer.UNLOCKED); + + // if the system is active propulate the buffer with corrected floor + // clip feet positions + if (active && isStanding()) { + correctClipping(); + bufferHead.setLeftFootPositionCorrected(leftFootPosition); + bufferHead.setRightFootPositionCorrected(rightFootPosition); + } + bufferInvalid = false; } - // update offsets for knee correction if the knees are not null - if (kneesActive) { - Vector3f temp1 = new Vector3f(); - Vector3f temp2 = new Vector3f(); - Vector3f temp3 = new Vector3f(); - - // get offsets from the waist to the upper legs - skeleton.leftHipNode.localTransform.getTranslation(temp1); - skeleton.rightHipNode.localTransform.getTranslation(temp2); - skeleton.trackerWaistNode.localTransform.getTranslation(temp3); - updateOffsets(temp1, temp2, temp3); - } - // update the buffer LegTweakBuffer currentFrame = new LegTweakBuffer(); currentFrame.setLeftFootPosition(leftFootPosition); @@ -309,14 +312,15 @@ public class LegTweaks { currentFrame.setRightFootRotation(rightFootRotation); currentFrame.setRightKneePosition(rightKneePosition); currentFrame.setWaistPosition(waistPosition); + currentFrame.setCenterOfMass(computeCenterOfMass()); - this.bufferHead + currentFrame .setLeftFloorLevel( (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset())) - currentDisengagementOffset ); - this.bufferHead + currentFrame .setRightFloorLevel( (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset())) - currentDisengagementOffset @@ -335,10 +339,14 @@ public class LegTweaks { currentFrame.setDetectionMode(LegTweakBuffer.ANKLE_ACCEL); } + // update the buffer head and compute the current state of the legs currentFrame.setParent(bufferHead); this.bufferHead = currentFrame; this.bufferHead.calculateFootAttributes(active); + // update the lock duration counters + updateLockStateCounters(); + return true; } @@ -354,12 +362,13 @@ public class LegTweaks { if (floorclipEnabled) correctClipping(); - // calculate acceleration and velocity of the feet using the buffer - // (only needed if skating correction is enabled) - if (skatingCorrectionEnabled) { + // correct for skating if needed + if (skatingCorrectionEnabled) correctSkating(); - correctFloat(); - } + + // currently scuffed + // if (skatingCorrectionEnabled && floorclipEnabled) + // correctFloat(); // determine if either leg is in a position to activate or deactivate // (use the buffer to get the positions before corrections) @@ -532,22 +541,36 @@ public class LegTweaks { // if velocity and dif are pointing in the same direction, // add a small amount of velocity to the dif // else subtract a small amount of velocity from the dif - // calculate the correction weight + // calculate the correction weight. + // it is also right here where the constant correction is + // applied float weight = calculateCorrectionWeight( leftFootPosition, bufferHead.getParent().getLeftFootPositionCorrected(null) ); if (velocity.x * leftFootDif.x > 0) { - leftFootPosition.x += velocity.x * weight; - } else { - leftFootPosition.x -= velocity.x * weight; + leftFootPosition.x += (velocity.x * weight) + + (getConstantCorrectionQuantityLeft() + * (velocity.x > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); + } else if (velocity.x * leftFootDif.x < 0) { + leftFootPosition.x -= (velocity.x * weight) + + (getConstantCorrectionQuantityLeft() + * (velocity.x > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); } if (velocity.z * leftFootDif.z > 0) { - leftFootPosition.z += velocity.z * weight; - } else { - leftFootPosition.z -= velocity.z * weight; + leftFootPosition.z += (velocity.z * weight) + + (getConstantCorrectionQuantityLeft() + * (velocity.z > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); + } else if (velocity.z * leftFootDif.z < 0) { + leftFootPosition.z -= (velocity.z * weight) + + (getConstantCorrectionQuantityLeft() + * (velocity.z > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); } // if the foot overshot the target, move it back to the target @@ -610,15 +633,27 @@ public class LegTweaks { ); if (velocity.x * rightFootDif.x > 0) { - rightFootPosition.x += velocity.x * weight; - } else { - rightFootPosition.x -= velocity.x * weight; + rightFootPosition.x += (velocity.x * weight) + + (getConstantCorrectionQuantityRight() + * (velocity.x > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); + } else if (velocity.x * rightFootDif.x < 0) { + rightFootPosition.x -= (velocity.x * weight) + + (getConstantCorrectionQuantityRight() + * (velocity.x > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); } if (velocity.z * rightFootDif.z > 0) { - rightFootPosition.z += velocity.z * weight; - } else { - rightFootPosition.z -= velocity.z * weight; + rightFootPosition.z += (velocity.z * weight) + + (getConstantCorrectionQuantityRight() + * (velocity.z > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); + } else if (velocity.z * rightFootDif.z < 0) { + rightFootPosition.z -= (velocity.z * weight) + + (getConstantCorrectionQuantityRight() + * (velocity.z > 0 ? 1 : -1) + / bufferHead.getTimeDelta()); } // if the foot overshot the target, move it back to the target @@ -645,135 +680,6 @@ public class LegTweaks { } } - // check if the foot is begining to float over the floor when it should not - // be if it is move it back down to the floor - // note: very small corrections are all that is allowed to prevent momemnt - // that looks unrealistic - private void correctFloat() { - boolean correctLeft = true; - boolean correctRight = true; - - if (bufferHead.getLeftLegState() == LegTweakBuffer.LOCKED) { - float lastPositionY = bufferHead - .getParent() - .getLeftFootPositionCorrected(null).y; - if ( - Math.abs(leftFootPosition.y - lastPositionY) < FOOT_Y_DIFF_CUTOFF - && leftFootPosition.y > lastPositionY - && bufferHead.getLeftFootAccelerationY() < FOOT_Y_MAX_ACCELERATION - ) { - leftFootPosition.y = lastPositionY; - correctLeft = false; - } - } - - if (bufferHead.getRightLegState() == LegTweakBuffer.LOCKED) { - float lastPositionY = bufferHead - .getParent() - .getRightFootPositionCorrected(null).y; - - if ( - Math.abs(rightFootPosition.y - lastPositionY) < FOOT_Y_DIFF_CUTOFF - && rightFootPosition.y > lastPositionY - && bufferHead.getRightFootAccelerationY() < FOOT_Y_MAX_ACCELERATION - ) { - rightFootPosition.y = lastPositionY; - correctRight = false; - } - } - - // using the velocity correct the position - if (correctLeft) - correctLeftFootTrackerY(); - - if (correctRight) - correctRightFootTrackerY(); - } - - private void correctLeftFootTrackerY() { - Vector3f temp; - Vector3f leftFootDif = leftFootPosition - .subtract(bufferHead.getParent().getLeftFootPositionCorrected(null)); - - if (Math.abs(leftFootDif.y) > NEARLY_ZERO) { - temp = bufferHead - .getParent() - .getLeftFootPositionCorrected(null) - .subtract( - bufferHead - .getParent() - .getLeftFootPosition(null) - .subtract(bufferHead.getLeftFootPosition(null)) - ); - - float leftFloor = (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset())) - - currentDisengagementOffset; - - leftFootPosition.y = Math.max(temp.y, leftFloor); - - Vector3f velocity = bufferHead.getLeftFootVelocity(null); - - if (velocity.y * leftFootDif.y > 0) { - leftFootPosition.y += velocity.y * FOOT_Y_CORRECTION_WEIGHT; - } else { - leftFootPosition.y -= velocity.y * FOOT_Y_CORRECTION_WEIGHT; - } - - // check for overshoot and correct if necessary - if ( - checkOverShoot( - this.bufferHead.getLeftFootPosition(null).y, - this.bufferHead.getParent().getLeftFootPositionCorrected(null).y, - leftFootPosition.y - ) - ) { - leftFootPosition.y = bufferHead.getLeftFootPosition(null).y; - } - } - } - - private void correctRightFootTrackerY() { - Vector3f temp; - Vector3f rightFootDif = rightFootPosition - .subtract(bufferHead.getParent().getRightFootPositionCorrected(null)); - - if (Math.abs(rightFootDif.y) > NEARLY_ZERO) { - temp = bufferHead - .getParent() - .getRightFootPositionCorrected(null) - .subtract( - bufferHead - .getParent() - .getRightFootPosition(null) - .subtract(bufferHead.getRightFootPosition(null)) - ); - - float rightFloor = (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset())) - - currentDisengagementOffset; - - rightFootPosition.y = Math.max(temp.y, rightFloor); - - Vector3f velocity = bufferHead.getRightFootVelocity(null); - - if (velocity.y * rightFootDif.y > 0) { - rightFootPosition.y += velocity.y * FOOT_Y_CORRECTION_WEIGHT; - } else { - rightFootPosition.y -= velocity.y * FOOT_Y_CORRECTION_WEIGHT; - } - - // check for overshoot and correct if necessary - if ( - checkOverShoot( - this.bufferHead.getRightFootPosition(null).y, - this.bufferHead.getParent().getRightFootPositionCorrected(null).y, - rightFootPosition.y - ) - ) { - rightFootPosition.y = bufferHead.getRightFootPosition(null).y; - } - } - } - // returns true if it is likely the user is standing public boolean isStanding() { // if the waist is below the vertical cutoff, user is not standing @@ -796,8 +702,8 @@ public class LegTweaks { // move the knees in to a position that is closer to the truth private void solveLowerBody() { // calculate the left and right waist nodes in standing space - Vector3f leftWaist = waistPosition;// .add(leftWaistUpperLegOffset); - Vector3f rightWaist = waistPosition;// .add(rightWaistUpperLegOffset); + Vector3f leftWaist = waistPosition; + Vector3f rightWaist = waistPosition; Vector3f tempLeft; Vector3f tempRight; @@ -843,12 +749,12 @@ public class LegTweaks { private float getLeftFootOffset() { float offset = computeUnitVector(this.leftFootRotation).y; - return clamp(0, DYNAMIC_DISPLACEMENT_CUTOFF, offset); + return FastMath.clamp(offset, 0, DYNAMIC_DISPLACEMENT_CUTOFF); } private float getRightFootOffset() { float offset = computeUnitVector(this.rightFootRotation).y; - return clamp(0, DYNAMIC_DISPLACEMENT_CUTOFF, offset); + return FastMath.clamp(offset, 0, DYNAMIC_DISPLACEMENT_CUTOFF); } // calculate the weight of foot correction @@ -870,6 +776,129 @@ public class LegTweaks { * (CORRECTION_WEIGHT_MAX - CORRECTION_WEIGHT_MIN); } + // calculate the center of mass of the user for the current frame + // returns a vector representing the center of mass position + private Vector3f computeCenterOfMass() { + // perform a check to see if the needed data is available + if ( + skeleton.headNode == null + || skeleton.chestNode == null + || skeleton.waistNode == null + || skeleton.leftFootNode == null + || skeleton.rightFootNode == null + || skeleton.leftKneeNode == null + || skeleton.rightKneeNode == null + || skeleton.leftHipNode == null + || skeleton.rightHipNode == null + ) { + return null; + } + + // check if arm data is available + boolean armsAvailable = skeleton.hasLeftArmTracker + && skeleton.hasRightArmTracker; + + Vector3f centerOfMass = new Vector3f(); + + // compute the center of mass of smaller body parts and then sum them up + // with their respective weights + Vector3f head = skeleton.headNode.worldTransform.getTranslation(); + Vector3f chest = skeleton.chestNode.worldTransform.getTranslation(); + Vector3f waist = skeleton.waistNode.worldTransform.getTranslation(); + Vector3f leftCalf = getCenterOfJoint(skeleton.leftAnkleNode, skeleton.leftKneeNode); + Vector3f rightCalf = getCenterOfJoint(skeleton.rightAnkleNode, skeleton.rightKneeNode); + Vector3f leftThigh = getCenterOfJoint(skeleton.leftKneeNode, skeleton.leftHipNode); + Vector3f rightThigh = getCenterOfJoint(skeleton.rightKneeNode, skeleton.rightHipNode); + centerOfMass = centerOfMass.add(head.mult(HEAD_MASS)); + centerOfMass = centerOfMass.add(chest.mult(CHEST_MASS)); + centerOfMass = centerOfMass.add(waist.mult(WAIST_MASS)); + centerOfMass = centerOfMass.add(leftCalf.mult(CALF_MASS)); + centerOfMass = centerOfMass.add(rightCalf.mult(CALF_MASS)); + centerOfMass = centerOfMass.add(leftThigh.mult(THIGH_MASS)); + centerOfMass = centerOfMass.add(rightThigh.mult(THIGH_MASS)); + + if (armsAvailable) { + Vector3f leftUpperArm = getCenterOfJoint( + skeleton.leftElbowNode, + skeleton.leftShoulderTailNode + ); + Vector3f rightUpperArm = getCenterOfJoint( + skeleton.rightElbowNode, + skeleton.rightShoulderTailNode + ); + Vector3f leftForearm = getCenterOfJoint(skeleton.leftElbowNode, skeleton.leftHandNode); + Vector3f rightForearm = getCenterOfJoint( + skeleton.rightElbowNode, + skeleton.rightHandNode + ); + centerOfMass = centerOfMass.add(leftUpperArm.mult(UPPER_ARM_MASS)); + centerOfMass = centerOfMass.add(rightUpperArm.mult(UPPER_ARM_MASS)); + centerOfMass = centerOfMass.add(leftForearm.mult(FOREARM_MASS)); + centerOfMass = centerOfMass.add(rightForearm.mult(FOREARM_MASS)); + } else { + // if the arms are not avaliable put them slightly in front + // of the chest. + Vector3f chestUnitVector = computeUnitVector( + skeleton.chestNode.worldTransform.getRotation() + ); + Vector3f armLocation = chest.add(chestUnitVector.mult(DEFAULT_ARM_DISTANCE)); + centerOfMass = centerOfMass.add(armLocation.mult(UPPER_ARM_MASS * 2.0f)); + centerOfMass = centerOfMass.add(armLocation.mult(FOREARM_MASS * 2.0f)); + } + + // finally translate in to tracker space + centerOfMass = waistPosition + .add( + centerOfMass.subtract(skeleton.trackerWaistNode.worldTransform.getTranslation(null)) + ); + + return centerOfMass; + } + + // get the center of two joints + private Vector3f getCenterOfJoint(TransformNode node1, TransformNode node2) { + return node1.worldTransform + .getTranslation(null) + .add(node2.worldTransform.getTranslation(null)) + .mult(0.5f); + } + + // get the amount of the constant correction to apply. + private float getConstantCorrectionQuantityLeft() { + if (leftFramesUnlocked >= CONTINUOUS_CORRECTION_WARMUP) + return CONTINUOUS_CORRECTION_DIST; + + return CONTINUOUS_CORRECTION_DIST + * ((float) leftFramesUnlocked / CONTINUOUS_CORRECTION_WARMUP); + } + + private float getConstantCorrectionQuantityRight() { + if (rightFramesUnlocked >= CONTINUOUS_CORRECTION_WARMUP) + return CONTINUOUS_CORRECTION_DIST; + + return CONTINUOUS_CORRECTION_DIST + * ((float) rightFramesUnlocked / CONTINUOUS_CORRECTION_WARMUP); + } + + // update counters for the lock state of the feet + private void updateLockStateCounters() { + if (bufferHead.getLeftLegState() == LegTweakBuffer.LOCKED) { + leftFramesUnlocked = 0; + leftFramesLocked++; + } else { + leftFramesLocked = 0; + leftFramesUnlocked++; + } + + if (bufferHead.getRightLegState() == LegTweakBuffer.LOCKED) { + rightFramesUnlocked = 0; + rightFramesLocked++; + } else { + rightFramesLocked = 0; + rightFramesUnlocked++; + } + } + // check if the difference between two floats flipped after correction private boolean checkOverShoot(float trueVal, float valBefore, float valAfter) { return (trueVal - valBefore) * (trueVal - valAfter) < 0; @@ -879,9 +908,4 @@ public class LegTweaks { private Vector3f computeUnitVector(Quaternion quaternion) { return quaternion.getRotationColumn(2).normalize(); } - - // clamp a float between two values - private float clamp(float min, float max, float val) { - return Math.min(max, Math.max(min, val)); - } } diff --git a/server/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java b/server/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java index 633f8a983..5ef5656c2 100644 --- a/server/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java +++ b/server/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java @@ -37,6 +37,9 @@ public abstract class Skeleton { @VRServerThread public abstract void resetTrackersFull(); + @VRServerThread + public abstract void resetTrackersMounting(); + @VRServerThread public abstract void resetTrackersYaw(); diff --git a/server/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java b/server/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java index 018a52d52..0e5169eda 100644 --- a/server/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java +++ b/server/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java @@ -27,6 +27,7 @@ public class SkeletonConfig { protected final boolean autoUpdateOffsets; protected final SkeletonConfigCallback callback; + protected float userHeight; public SkeletonConfig(boolean autoUpdateOffsets) { this.autoUpdateOffsets = autoUpdateOffsets; @@ -105,6 +106,7 @@ public class SkeletonConfig { } } + // Calls callback if (callback != null) { try { callback @@ -114,6 +116,11 @@ public class SkeletonConfig { } } + // Re-calculate user height + userHeight = getOffset(SkeletonConfigOffsets.NECK) + + getOffset(SkeletonConfigOffsets.TORSO) + + getOffset(SkeletonConfigOffsets.LEGS_LENGTH); + return origVal; } @@ -130,6 +137,10 @@ public class SkeletonConfig { return val != null ? val : config.defaultValue; } + public float getUserHeightFromOffsets() { + return userHeight; + } + public Boolean setToggle(SkeletonConfigToggles config, Boolean newValue) { Boolean origVal = newValue != null ? configToggles.put(config, newValue) diff --git a/server/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java b/server/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java index 57a35fcc4..ac86bda6d 100644 --- a/server/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java +++ b/server/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java @@ -108,6 +108,10 @@ public class ComputedTracker implements Tracker, TrackerWithTPS { public void resetYaw(Quaternion reference) { } + @Override + public void resetMounting(boolean reverseYaw) { + } + @Override public TrackerPosition getBodyPosition() { return bodyPosition; diff --git a/server/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/server/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 44a9cd789..249e39c67 100644 --- a/server/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/server/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -277,6 +277,10 @@ public class IMUTracker // TODO Print "jump" length when correcting if it's more than 1 degree } + @Override + public void resetMounting(boolean reverseYaw) { + } + @Override public TrackerPosition getBodyPosition() { return bodyPosition; diff --git a/server/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java b/server/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java index aff4b7466..f09921fe4 100644 --- a/server/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java +++ b/server/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java @@ -1,5 +1,6 @@ package dev.slimevr.vr.trackers; +import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; import dev.slimevr.config.TrackerConfig; @@ -9,9 +10,15 @@ import dev.slimevr.vr.Device; public class ReferenceAdjustedTracker implements Tracker { public final E tracker; - public final Quaternion yawFix = new Quaternion(); + + // Vector for mounting rotation correction (tilt direction) + public final Vector3f rotVector = new Vector3f(); + public final Quaternion gyroFix = new Quaternion(); public final Quaternion attachmentFix = new Quaternion(); + public final Quaternion mountRotFix = new Quaternion(); + public final Quaternion yawFix = new Quaternion(); + protected float confidenceMultiplier = 1.0f; public ReferenceAdjustedTracker(E tracker) { @@ -34,8 +41,8 @@ public class ReferenceAdjustedTracker implements Tracker { } /** - * Reset the tracker so that it's current rotation is counted as (0, , 0). This allows tracker to be strapped to body at any pitch and + * Reset the tracker so that its current rotation is counted as (0, , 0). This allows the tracker to be strapped to body at any pitch and * roll. *

* Performs {@link #resetYaw(Quaternion)} for yaw drift correction. @@ -43,12 +50,12 @@ public class ReferenceAdjustedTracker implements Tracker { @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(); + + fixGyroscope(sensorRotation.clone()); + fixAttachment(sensorRotation.clone()); fixYaw(reference); } @@ -65,39 +72,69 @@ public class ReferenceAdjustedTracker implements Tracker { fixYaw(reference); } + private void fixGyroscope(Quaternion sensorRotation) { + sensorRotation.fromAngles(0, sensorRotation.getYaw(), 0); + gyroFix.set(sensorRotation).inverseLocal(); + } + + private void fixAttachment(Quaternion sensorRotation) { + gyroFix.mult(sensorRotation, sensorRotation); + attachmentFix.set(sensorRotation).inverseLocal(); + } + + @Override + public void resetMounting(boolean reverseYaw) { + tracker.resetMounting(reverseYaw); + + // Get the current calibrated rotation + Quaternion buffer = new Quaternion(); + tracker.getRotation(buffer); + gyroFix.mult(buffer, buffer); + buffer.multLocal(attachmentFix); + + // Reset the vector for the rotation to point straight up + rotVector.set(0f, 1f, 0f); + // Rotate the vector by the quat, then flatten and normalize the vector + buffer.multLocal(rotVector).setY(0f).normalizeLocal(); + + // Calculate the yaw angle using tan + // Just use an angle offset of zero for unsolvable circumstances + float yawAngle = FastMath.isApproxZero(rotVector.x) && FastMath.isApproxZero(rotVector.z) + ? 0f + : FastMath.atan2(rotVector.x, rotVector.z); + + // Make an adjustment quaternion from the angle + buffer.fromAngles(0f, reverseYaw ? yawAngle : yawAngle - FastMath.PI, 0f); + + Quaternion lastRotAdjust = new Quaternion(mountRotFix); + mountRotFix.set(buffer); + + // Get the difference from the last adjustment + buffer.multLocal(lastRotAdjust.inverseLocal()); + // Apply the yaw rotation difference to the yaw fix quaternion + yawFix.multLocal(buffer.inverseLocal()); + } + 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 targetRotation = new Quaternion(reference); + targetRotation.fromAngles(0, targetRotation.getYaw(), 0); Quaternion sensorRotation = new Quaternion(); tracker.getRotation(sensorRotation); gyroFix.mult(sensorRotation, sensorRotation); sensorRotation.multLocal(attachmentFix); + sensorRotation.multLocal(mountRotFix); - sensorRotation.toAngles(angles); - sensorRotation.fromAngles(0, angles[1], 0); + sensorRotation.fromAngles(0, sensorRotation.getYaw(), 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(); + yawFix.set(sensorRotation).inverseLocal().multLocal(targetRotation); } protected void adjustInternal(Quaternion store) { gyroFix.mult(store, store); store.multLocal(attachmentFix); + store.multLocal(mountRotFix); yawFix.mult(store, store); } diff --git a/server/src/main/java/dev/slimevr/vr/trackers/Tracker.java b/server/src/main/java/dev/slimevr/vr/trackers/Tracker.java index b870f59e4..840a2db0a 100644 --- a/server/src/main/java/dev/slimevr/vr/trackers/Tracker.java +++ b/server/src/main/java/dev/slimevr/vr/trackers/Tracker.java @@ -36,6 +36,8 @@ public interface Tracker { void resetYaw(Quaternion reference); + void resetMounting(boolean reverseYaw); + void tick(); TrackerPosition getBodyPosition();