Merge branch 'main' of github.com:SlimeVR/SlimeVR-Server into merge-g…

…ui-and-server

server/src/main/java/com/jme3/math/FastMath.java

@@ -70,9 +70,59 @@ private 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 @@ public static int nearestPowerOfTwo(int number) {
 	 * 
 	 * @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 @@ public static float interpolateLinear(float scale, float startValue, float endVa
 	 *
 	 * @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 @@ public static Vector3f interpolateLinear(
 	 *
 	 * @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 @@ public static Vector3f interpolateBezier(
 	}
 
 	/**
-	 * 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 @@ public static float getCatmullRomP1toP2Length(
 	}
 
 	/**
-	 * 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 @@ public static float pow(float fBase, float fExponent) {
 	/**
 	 * 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) {

server/src/main/java/com/jme3/math/Quaternion.java

@@ -279,7 +279,7 @@ public Quaternion fromAngles(float xAngle, float yAngle, float zAngle) {
 
 	/**
 	 * <code>toAngles</code> returns this quaternion converted to Euler rotation
-	 * angles (yaw,roll,pitch).<br/>
+	 * angles (pitch, yaw, roll).<br/>
 	 * Note that the result is not always 100% accurate due to the implications
 	 * of euler angles.
 	 * 
@@ -288,7 +288,7 @@ public Quaternion fromAngles(float xAngle, float yAngle, float zAngle) {
 	 * 
 	 * @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) {

server/src/main/java/dev/slimevr/VRServer.java

@@ -5,6 +5,7 @@
 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.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<Consumer<Tracker>> newTrackersConsumers = new FastList<>();
 	private final List<Runnable> onTick = new FastList<>();
 	private final List<? extends ShareableTracker> 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 VRServer(String configPath) {
 		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 VRServer(String configPath) {
 			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 void addSkeletonUpdatedCallback(Consumer<Skeleton> consumer) {
 	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 void run() {
 			for (Bridge bridge : bridges) {
 				bridge.dataWrite();
 			}
+			VRCOSCHandler.update();
 			// final long time = System.currentTimeMillis() - start;
 			try {
 				Thread.sleep(1); // 1000Hz
@@ -242,6 +258,43 @@ public void resetTrackersYaw() {
 		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 TrackersUDPServer getTrackersServer() {
 		return trackersServer;
 	}
 
+	public VRCOSCHandler getVRCOSCHandler() {
+		return VRCOSCHandler;
+	}
+
 	public DeviceManager getDeviceManager() {
 		return deviceManager;
 	}

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 String getResetBinding() {
 	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;
+	}
 }